3D激光雷達的空間建模

趙貴平

（中煤晉中能源化工有限責任公司 山西省太原市 030006）

1 多線激光雷達工作原理

隨著智能化的時代到來，機器人等一些智能儀器與設(shè)備已經(jīng)融入了我們的生活與工作當中，在一些復(fù)雜或危險的場所，我們開始尋求一種無人化的工作模式；而掃地機器人、服務(wù)機器人等在生活中的應(yīng)用，也給人們帶來了便利。與此同時，怎樣做到精確的定位儀器設(shè)備是我們現(xiàn)在需要解決的難題。在已有的可實行定位方案中，通過激光雷達來構(gòu)建地圖同時基于構(gòu)建的地圖進行定位的方法逐漸成為了主流。激光雷達Lidar（Light Detection And Ranging）是將現(xiàn)代光學技術(shù)應(yīng)用到遙感技術(shù)當中的產(chǎn)物，由于激光的本質(zhì)是電磁波，具備高亮度性、高方向性以及高相干性等特點，所以激光雷達相較于傳統(tǒng)的雷達技術(shù)，在角分辨率、距離分辨率、速度分辨率與測速范圍等方面都具有一定的優(yōu)勢，并且還能抓取多種目標的圖像數(shù)據(jù)以及不易受外界的干擾。激光雷達的信息可以通過觀測的振幅、相位與偏振來搭建獲取，因此激光雷達不僅能在測量距離的時候做到精確，還能在測量速度與跟蹤目標時減少誤差[1]。

通過激光雷達測量距離的方法可以大致的分為TOF測距法和三角測距法。

TOF測距方法簡單的說就是通過速度和時間的乘積來求得距離，激光雷達通過激光發(fā)射器發(fā)射一個激光脈沖，與此同時，計時器記錄下此時的時間為初始時間，當激光脈沖照射到目標物后進行反射由激光雷達的接收器接收，計時器記錄當前的時間為接收時間，接收時間與初始時間的差值則是激光脈沖的“飛行時間”，由于光的速度是已知的，所以可以求出雷達與目標之間的距離，表達式如下：

其中D為測站點兩點間距離，c為光在大氣中傳播的速度，t為光往返一次所需的時間。

三角法測量的原理是通過激光雷達與線性CCD元件來完成的，當激光雷達的激光發(fā)射器發(fā)出一個激光脈沖后，碰到目標物體通過反射后被線性CCD所接收，而激光發(fā)射器和激光探測器之間存在間隔，通過光學原理，可以在CCD上獲取不同距離的物體信息，最后通過三角公式數(shù)學方法來計算被測目標的距離。

從測量距離、采樣率、精度、轉(zhuǎn)速等方面對兩種方法進行了比較，如表1所示，發(fā)現(xiàn)TOF雷達從性能上來說總體優(yōu)于三角雷達。

2 基于LOAM算法的空間建模方法

SLAM（Simultaneous Iocalization and Mapping）主要功能是在構(gòu)建環(huán)境地圖的同時，還能通過不間斷的估計自身的位姿信息來完成定位。當前激光SLAM框架主要內(nèi)容為前端掃描匹配和后端優(yōu)化。前端掃描匹配是激光SLAM整個流程當中的重點，其主要作用是在已知前一幀位姿時，利用相鄰幀之間的關(guān)系來估計當前幀的位姿；后端優(yōu)化是當長時間增量式掃描匹配后優(yōu)化里程計及地圖信息，其中前端掃描使用最多的算法為ICP算法與NDT算法，后端優(yōu)化主要基于粒子濾波與圖優(yōu)化兩種方法。其中激光雷達二維建模常用的算法有Hector算法、Gmapping算法以及Cartographer算法，三維建模常用LOAM算法[2]。

本文選擇LOAM算法進行三維空間建模，目標是使用一個三維空間中運動的兩軸單線激光雷達來構(gòu)建實時激光里程計并建圖，LOAM算法的創(chuàng)新點在于可以不通過高精度的測距和慣性測量方法就可以得到一個低漂移和低復(fù)雜度的數(shù)據(jù)信息[3]。該算法的核心是將定位和建圖兩個版塊分割開來解決，通過執(zhí)行高頻率的里程計信息和低精度的運動估計來進行空間定位，與此同時，通過比定位低一個數(shù)量級的頻率執(zhí)行匹配和注冊點云信息來實現(xiàn)環(huán)境地圖的構(gòu)建。將這兩個算法結(jié)合就獲得了高精度、實時性的激光里程計。

2.1 算法流程框圖

由流程圖1可知，整個工作主要分為四部分來完成。第一步是要獲得當前激光雷達坐標下的點云數(shù)據(jù)，接著把k次掃描所獲得的全部點云組成一幀數(shù)據(jù)pk。然后將得到的數(shù)據(jù)pk在建圖和定位兩個算法中進行處理。激光里程計的作用是獲取連續(xù)兩幀點云的運動信息，估計自身的位姿信息用于去除pk中的運動畸變。這個節(jié)點進行的頻率為10Hz，該節(jié)點使用地圖去匹配和注冊沒有畸變的點云數(shù)據(jù)并以1Hz的頻率輸出。最后由坐標轉(zhuǎn)換節(jié)點接收前面兩個節(jié)點輸出的信息并將其融合處理以10Hz的活動頻率輸出。

圖1：LOAM算法流程

表1：TOF測距法與三角測距法的比較

圖2：三視角下的室內(nèi)實時點云圖

2.2 環(huán)境特征點提取

使用圖1中的兩軸激光雷達獲得的點云數(shù)據(jù)提取特征點信息，由于兩個雷達的構(gòu)造會產(chǎn)生非均勻分布的點云數(shù)據(jù)。在一次掃描數(shù)據(jù)中點的分辨率為0.25度，并且這些點分布在一個掃描平面上。然而固定激光雷達的軸的轉(zhuǎn)速為180度每秒。而激光的頻率為40Hz，因此這個軸上點的分辨率為4.5度。考慮到點云數(shù)據(jù)的非均勻性，采取從一次掃描中獲取特征點。

特征點選取邊緣點和平面點，方法是得到我們想要求曲率點i周圍連續(xù)幾個點集合S用于求曲率。S中的點一半在i的一側(cè)從而避免雷達順時針和逆時針的影響。求取點的曲率的公式如下：

通過上式求出每個點的曲率c。當求得的曲率大于設(shè)定閾值時則歸類為邊緣點，當求得的曲率小于設(shè)定曲率閾值時則歸類為平面點。在選擇點時應(yīng)避免選擇離已選點距離過近的點和與激光光束相平行的平面上的點，同時我們也希望避免可能遮擋點。

2.3 尋找特征點的對應(yīng)關(guān)系

里程計算法估計獲取一幀點云時間內(nèi)的運動，使用tK表示第K次掃描開始的時間。在一次掃描結(jié)束時，這一次掃描獲得點云pK會有一個獲取時間tK+1。將投影后的點云記為，在下一次掃描時會使用做匹配求激光雷達的運動。將兩幀點云的特征點相匹配，匹配的邊角線和pK+1的邊角點，匹配的平面和pK+1的平面點，從而尋找到對應(yīng)關(guān)系。

在第K+1掃描開始計算，pK+1中起初沒有點云數(shù)據(jù)，當開始不斷掃描后，pK+1中的點云不斷累計。每次迭代的時候，都會將pK+1中的邊緣點εK+1和平面點HK+1投影到掃描起始點的坐標系下，用重新表示。將Pˉk中曲率大的點和曲率小的點保存在KD樹中，用于尋找對應(yīng)點。

點到線段的距離公式如下：

圖3：點云地圖

接下來求平面點到對應(yīng)平面的距離，距離公式如下：

2.4 運動估計

LOAM算法是在默認激光雷達是勻速運動的前提下工作的。因此當獲取了一幀數(shù)據(jù)的終止點相對于起始點的轉(zhuǎn)換矩陣的時候，就可以按照獲得點數(shù)據(jù)時相對于起始點的時間對這一幀數(shù)據(jù)中的任意點進行插值。獲得這一幀中每一個點的位姿。公式如下：

接著使用一個旋轉(zhuǎn)矩陣R和一個平移量T表示當前幀數(shù)據(jù)中的點和前一時刻幀數(shù)據(jù)中點的對應(yīng)關(guān)系。

用羅德里杰斯公式將旋轉(zhuǎn)矩陣R展開為：

對旋轉(zhuǎn)矩陣進行求導，有了點到線和面的距離，并獲得用于優(yōu)化的誤差函數(shù)：

f中的每一行代表一個特征點，接下就要求解雅克比矩陣，最后使用LM的方法進行優(yōu)化：

3 實驗驗證

3.1 硬件介紹

本實驗采用的儀器是VLP-16激光雷達，該激光雷達是Velodyne公司研發(fā)的體量最小的三維激光雷達，保留了電機轉(zhuǎn)速可調(diào)節(jié)的功能，通過5Hz-20Hz的頻率實時的傳輸環(huán)境測量值數(shù)據(jù)。VLP-16的測量距離最遠可達100多米，830克的輕重量使得使用和安裝時都非常的方便，適合安裝在小型無人機和小型移動機器人上進行實驗和數(shù)據(jù)測量。測量數(shù)據(jù)信息以點云的形式存儲，并且提供橫向360°，縱向±15°，每個點精度在2mm以內(nèi)的信息，每秒可以輸出30多萬個點云數(shù)據(jù)。

VLP-16型號一排16個IR激光器與IR檢測器配對，具有在垂直方向上16線的激光束，在采集三維數(shù)據(jù)時，每一步的旋轉(zhuǎn)（旋轉(zhuǎn)頻率可設(shè)定，頻率不同旋轉(zhuǎn)的步進角度不同）可在空間上采集16個點的三維數(shù)據(jù)，以測量到物體的距離。在垂直方向上的視角范圍-15°到+15°。該設(shè)備牢固的安裝在緊湊的耐候外殼內(nèi)。激光/檢測器對陣列在其固定外殼內(nèi)快速旋轉(zhuǎn)以掃描周圍的環(huán)境，每秒發(fā)射大約18000次激光，實時提供豐富的3D點數(shù)據(jù)集。先進的數(shù)字信號處理和波形分析可提供高精度的遠程感測以及校準后的反射率數(shù)據(jù)，可輕松檢測出路標，車牌和車道標志等后向反射器。將16對激光/檢測器組合成一個VLP-16傳感器，并以18.08kHz的頻率進行脈沖處理，每秒可進行多達30w個數(shù)據(jù)點的測量。

3.2 實驗結(jié)果

本文選用中國礦業(yè)大學（北京）沙河校區(qū)學8-413實驗室點云（由VLP-16激光雷達采集的模擬實驗室點云）進行算法驗證。實驗中，上位機選擇工控機，選用Ubuntu16.04系統(tǒng)和PCL點云庫來進行點云的采集與環(huán)境的構(gòu)建。激光雷達采用UDP協(xié)議傳輸數(shù)據(jù)（UDP稱為用戶數(shù)據(jù)報協(xié)議，提供了一種無需建立連接的封裝的IP數(shù)據(jù)報發(fā)送給應(yīng)用程序），由于網(wǎng)絡(luò)傳輸?shù)腢DP協(xié)議通過解析獲得的數(shù)據(jù)包是16進制數(shù)據(jù)，不能直觀的得到極坐標形式或者xyz空間坐標系，所以需要在ROS系統(tǒng)中利用PCL轉(zhuǎn)換成PCD文件來進行下一步的算法處理。實驗得到的實時點云和最后的點云地圖如圖2，圖3所示。