李一凡，杜世通，李爽，黃璐，楊子寒，陳沖

(1.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室,石家莊 050081；2.中國電科網(wǎng)絡(luò)通信研究院,石家莊 050081)

0 引言

同時定位和地圖構(gòu)建(simultaneous localization and mapping，SLAM)是移動機器人執(zhí)行導(dǎo)航和交互任務(wù)的關(guān)鍵技術(shù)之一.SLAM 框架通常包括前端里程計、后端和回環(huán)檢測.前端里程計是SLAM 系統(tǒng)的基礎(chǔ)，也是后端和回環(huán)檢測的前提.它使用環(huán)境感知傳感器，如視覺相機或激光雷達，獲取周圍環(huán)境的圖像和點云信息，并利用相鄰幀間的環(huán)境信息變化估計幀間的位姿變化，從而計算出前端里程計.

根據(jù)傳感器輸出數(shù)據(jù)的類型，前端里程計分為兩類: 基于視覺傳感器的視覺里程計(visual odometry，VO)[1]和基于激光雷達的激光里程計[2].視覺傳感器有許多缺陷，如受光照影響大，深度信息的尺度不確定性[3].相較之下，激光雷達不受環(huán)境光影響，具有厘米級的測距離精度，且抗干擾能力強，更具魯棒性和精確性[4].因此，基于激光雷達的SLAM 方法是自主機器人定位研究的一個熱點.

激光里程計是通過最小化相鄰幀點云之間關(guān)于位姿變化的誤差函數(shù)得到幀間位姿變化.基于激光雷達的SLAM 方法被分為三個不同的類別：基于點匹配的迭 代最近 點算法(iterative closest point，ICP)、基于概率分布的正態(tài)分布變換(normal distributions transform，NDT)算法和基于特征的匹配的算法[5].ICP算法具有實現(xiàn)簡單、運算速度快的優(yōu)點，對于較小的場景和運動較緩慢的情況下表現(xiàn)良好.但ICP 算法對噪聲和異常值較為敏感，且對預(yù)測值要求高，容易陷入局部最優(yōu)解.NDT 是一種基于高斯分布的概率匹配算法，它能夠更好地處理激光雷達傳感器噪聲和數(shù)據(jù)不完整性的問題，對于復(fù)雜場景下的建圖和定位表現(xiàn)優(yōu)秀.但NDT 算法對于場景的平滑度和連續(xù)性有一定要求，同時計算復(fù)雜度較高、精度較低.Zhang等[6]首先提出了基于特征的激光雷達里程計和建圖(LIDAR odometry and mapping，LOAM)算法，特征提取能夠?qū)?fù)雜的點云數(shù)據(jù)簡化為一組可用的特征點，從而減少數(shù)據(jù)量，提高算法效率，同時也為后續(xù)的點云匹配并最終得到激光里程計提供基礎(chǔ).LOAM 算法實現(xiàn)了實時的、低漂移的激光里程計，解決了ICP 算法對噪聲敏感、NDT 算法要求場景平滑的問題.但LOAM 算法中采用簡單的平滑度判定點云的特征，在噪聲、動態(tài)物體等復(fù)雜環(huán)境下容易出現(xiàn)漏檢或誤檢的情況，從而影響SLAM 的精度和魯棒性.因此，在復(fù)雜環(huán)境下的特征提取和匹配仍是一項極大的挑戰(zhàn).

為進一步提升激光里程計的魯棒性和精確性，研究者們提出了許多改進方法來解決特征提取的問題.Horache等[7]提出利用多尺度架構(gòu)和無監(jiān)督轉(zhuǎn)移學(xué)習(xí)的3D 點云配準方法，實現(xiàn)高精度的特征提取和匹配.Serafin等[8]提出了一種使用點鄰域的主成分分析(principal components analysis，PCA)方法，利用該方法得出數(shù)學(xué)原理的線和平面特征.Abedini等[9]提出引入激光雷達點云的強度數(shù)據(jù)，利用尺度不變特征變換(scale invariant feature transform，SIFT)算法從航空圖像中提取不同的特征.另外，Arshad等[10]提出利用仿射尺度不變特征變換、線性方程、粒子群優(yōu)化等方法提取優(yōu)化激光里程計特征提取算法.引入更多數(shù)據(jù)和更復(fù)雜的算法可以有效提高激光里程計算法中特征提取和匹配的準確性和魯棒性.然而，這些復(fù)雜的算法和需要處理的更多數(shù)據(jù)會對處理設(shè)備的要求產(chǎn)生較大壓力，難以在輕量化平臺上應(yīng)用.為了解決上述問題，本文提出了一種基于點云分割的激光里程計算法.該算法通過分析激光雷達點云數(shù)據(jù)特征，有針對性地對激光雷達點云進行分割并賦予標簽，并根據(jù)標簽與物體掃描情況提出了對應(yīng)的特征提取和匹配方法，從而提高了特征提取的準確性和里程計的精確性，并且占用較少的計算資源.

1 激光里程計

如圖1 所示，基于特征的激光雷達里程計算法通常包括三個模塊：點云分割、特征提取、特征匹配.點云分割將點云聚類并賦予點標簽.特征提取從點云中提取直線和平面這兩種特征點.隨后，每個直線和平面特征點會被分別投影到地圖特征點云中進行匹配，并計算點到直線的距離和點到平面的距離與位姿有關(guān)，累加這些距離的和即為與位姿相關(guān)的誤差函數(shù)，最小化誤差函數(shù)以得到激光里程計.

圖1 基于特征的激光里程計流程框圖

點云的特征提取是指對激光雷達獲取的點云數(shù)據(jù)進行處理，從中提取出具有定位特征的點.準確性和魯棒性對于激光里程計的性能至關(guān)重要.本文針對機械式激光雷達的工作原理及所得點云特點，提出了一種新的點云分割算法，該算法具有計算簡單、計算量小的優(yōu)勢，并且能夠判定并提取的特征點受噪點影響較小，具有更好的定位特性.

1.1 點云分割

在特征提取前利用點云分割將點云分割、聚類并賦予標簽[11].點云分割階段接收激光雷達獲取的點云數(shù)據(jù)，通過分割算法將點云分割成若干個子集，并為每個子集標記一個標簽.特征提取階段基于分割結(jié)果提取特征點云，通過對點云分割所得標簽判定物體掃描狀態(tài)，保留符合條件的特征點云，同時剔除噪點.相較于傳統(tǒng)的特征提取算法，本文提出的算法可以提高特征提取的準確性和魯棒性，并且能夠處理大規(guī)模點云數(shù)據(jù).

由于激光雷達機器本身的固有特性，其依靠內(nèi)部多對激光發(fā)射器360°水平旋轉(zhuǎn)掃描周圍環(huán)境，以獲取深度信息并形成點云.如圖2 所示，激光雷達的垂直分辨率取決于其掃描線數(shù).舉例來說，常用的16 線激光雷達垂直分辨率僅為2°.但由于激光發(fā)射器的高發(fā)射頻率，其水平分辨率可以高達0.18°.因此，通過對掃描線點云進行水平角度分割，可以更加精確地進行處理點云.

圖2 激光雷達工作原理示意圖

由于激光雷達點云數(shù)據(jù)中每個點的存儲順序與掃描順序相同，因此只需遍歷激光雷達點云中的每一個點，并使用式(1)計算其俯仰角度 α .然后，根據(jù)俯仰角度 α 將 點云按照掃描 線進行劃分.例 如針對16 線激光雷達，可以按照圖2 所示的掃描角度將點云劃分為16 份掃描線點云.

式中，x、y、z分別為點在激光雷達坐標系下的三維坐標.

后續(xù)的點云分割需使用點云的水平掃描角度，為降低計算資源的消耗，利用式(2)計算了點云的水平角度

由于反正弦函數(shù)取值范圍為 (-90,90)，因此當(dāng)(x,y) 處于第三、四象限時 β 還需增加180°.

在激光雷達運轉(zhuǎn)時，同一掃描線上相鄰掃描點的角度間隔極小(0.18°)，因此在激光雷達探測物體距離較近時，相鄰點僅存在兩種情況：1)兩點為同一物體上的點；2)兩點為不同物體上的點.如圖3 所示，通過計算相鄰掃描點連線所成向量，與兩點中較遠距離點坐標向量所成夾角φ，可以判斷相鄰點是否屬于同一物體[12].以相鄰點A、B為例，可以根據(jù)它們的水平掃描角度差和三維坐標進行計算，得到

圖3 激光雷達掃描線俯視示意圖

式中：x、y、z分為點在激光雷達坐標系下的三維坐標；ΔβAB為利用式(2)計算所得A、B兩點水平角差值.

為了將點云分割為不同的物體，本文按掃描線遍歷點云中的所有點，并計算每個點與其下一點之間的夾角φ.如果該夾角φ大于設(shè)定的閾值(在本文中為30°，即連續(xù)兩點橫向變化超過深度變化的兩倍)，則將這兩個連續(xù)的點視為不屬于同一物體.基于相鄰點是否屬于同一物體，本文將同一物體上的點標記為內(nèi)點，而不同物體上距離更近的點則被標記為物體邊緣點，距離稍遠的點則標記為離散點.通過這種方式，本文可以完成點云的分割.

1.2 特征提取

激光雷達通過發(fā)射激光并接收反射的激光測距形成點云，而由于激光會被物體遮擋，因此物體被掃描的區(qū)域會隨著激光雷達的運動而變化.如圖4 所示，黃色定位特征被激光雷達掃描的情況主要包括兩種：a) 完全掃描；b) 部分掃描.由于激光雷達是按一定順序旋轉(zhuǎn)掃描，物體被部分掃描時還需要區(qū)分左部分、右部分兩種情況，可通過遍歷掃描線中的每一個點，根據(jù)點云分割中賦予的標簽與表面點的順序判定左、右部分掃描的情況.

圖4 物體掃描情況示意圖

在城市場景中，物體通常呈現(xiàn)出結(jié)構(gòu)化的標準形狀，例如墻壁、汽車、房屋等可視為長方體、棱錐等的組合.物體的邊緣可視為直線，表面則可視為平面.被掃描區(qū)域較小的、或本身較小的物體難以從中提取準確的定位特征，因此僅從橫向掃描長度大于一定閾值(防止較小物體左右邊沿點相互干擾，設(shè)定最鄰近搜索的距離為0.5 m)的物體提取其邊緣點為直線特征，表面點為平面特征.而城市中含有許多橫向較小的物體，例如電線桿、樹干等，它們也是不可或缺的定位特征，可視為圓柱體，橫截面視為圓.因此對于完整掃描但物體橫向距離較小(與較小物體判斷相對應(yīng))的情況，若其表面點數(shù)量大于5 (圓心擬合最少5 個點可以判定擬合誤差)，則認為點云中存在足夠的點用于擬合，以保證擬合的可靠性.則將所有通過點帶入并優(yōu)化最小二乘式

式中：a、b分別為在物體掃描點最小、最大點的點云中的序號；xc、yc是所擬合圓心的坐標，；xi、yi為點的坐標；R為所擬合圓心的半徑.

若優(yōu)化上式所得圓的半徑小于物體兩邊邊緣點之間的距離，即所掃描物體具有圓柱屬性且掃描區(qū)域大于橫截面的三分之一，則認為擬合成功，并將圓心視為直線特征點.除上述符合條件的點作為定位特征點以外，其他點均視為離散點，不作為定位特征點，僅用作建圖.特征提取的具體流程如圖5 所示.

圖5 特征提取流程圖

1.3 特征匹配

在LOAM 算法中，特征點的匹配分為兩個步驟.首先，將當(dāng)前時刻點云中的特征點投影到歷史點云上，形成一組候選匹配點.然后，根據(jù)一定的距離閾值，選擇與每個候選點最近的點作為最終匹配點.特征點的匹配質(zhì)量將影響到后續(xù)的位姿估計和地圖構(gòu)建質(zhì)量，因此需要考慮匹配的可靠性和效率.本文也采用了類似的特征點匹配策略，對當(dāng)前時刻點云中提取的每一個特征點進行候選匹配點的搜索，進而確定最終匹配點.為了提高匹配效率和精度，本文對匹配點的搜索進行了優(yōu)化，使用了KD-Tree等數(shù)據(jù)結(jié)構(gòu)，加速匹配過程.假定激光雷達為均速運動，特征點的投影公式為

式中：為投影至地圖的位姿；Pi為特征點的三維坐標；Rk-1、tk-1分別為上一幀激光雷達的姿態(tài)變化矩陣和位移.

對于直線特征點，先將其投影到地圖的坐標系下，然后利用KD-tree 在地圖直線特征點云中搜索最鄰近的5 個點.接著，采用PCA 分析這5 個點所構(gòu)成的矩陣的最大特征值.如果最大特征值遠大于第二大特征值，就可視為最大特征值所對應(yīng)的向量是該5 個點所擬合直線的方向.否則應(yīng)該舍棄該特征點.該直線必過經(jīng)過該5 個點的平均坐標點，由此可求得點到直線的距離為

式中：di為點到直線的距離；為5 個點的平均坐標向量；vi為最大特征值對應(yīng)的特征向量；

同理，平面特征點也投影到地圖坐標系下，然后利用KD-tree 在地圖直線特征點云中搜索最鄰近的5 個點.由于PCA 算法耗時長，且平面特征點較多，因此采用最小二乘法將該5 個點擬合為一個平面.通過最小化帶入5 個點的坐標，最小化S為

式中：S為待優(yōu)化的量；A、B、C、D為平面標準方程的四參數(shù)；xl、yl、zl分別為點的三維坐標值.

即可得到標準平面方程，由此可計算點到平面的距離為

式中：dj為點到平面的距離；為地圖特征點的三維坐標；

將所有特征點中點到直線的距離、點到平面的距離累加即可得到關(guān)于位姿的誤差函數(shù)

將該誤差函數(shù)最小化即可得到當(dāng)前激光雷達姿態(tài)、位移的最優(yōu)估計.

2 實驗驗證

為了驗證本文提出的激光里程計算法的有效性、精確性和魯棒性，本文進行了實驗驗證.使用開源的KITTI 數(shù)據(jù)，同時在真實場景中模擬導(dǎo)致算法漂移的復(fù)雜情況，以對比本文提出的基于LOAM 算法和點云分割的激光里程計算法的性能.

2.1 開源數(shù)據(jù)驗證

KITTI00 數(shù)據(jù)集是在典型的城市環(huán)境中采集的，該環(huán)境的特征豐富且場景簡單，因此LOAM 算法和本文提出的算法都能夠魯棒地運行，里程計結(jié)果如圖6 所示.本文提出算法所得軌跡與基準軌跡的平均誤差為3.3 m，誤差始終保持較小水平，且最終誤差為11.43 m，而LOAM 算法的里程計平均誤差為6.8 m，軌跡因誤差累計導(dǎo)致逐漸偏離基準，最終誤差為62.17 m.由此可得，與LOAM 算法相比，本文提出的基于點云分割的激光里程計算法更具精確性.

圖6 實驗驗證硬件平臺圖

2.2 真實實驗驗證

圖7 為搭建的一個簡易組合導(dǎo)航系統(tǒng)硬件平臺.該硬件平臺包括16 線激光雷達、低成本微機械陀螺儀(micro electro mechanical systems，MEMS)、高精度光纖陀螺、高精度雙天線GNSS等傳感器以及一臺計算機.實驗運行環(huán)境為Ubuntu16.04+ROS 環(huán)境.平臺記錄了真實的激光雷達實驗數(shù)據(jù)包，并通過仿真驗證了本文算法的特征提取和激光里程計運算結(jié)果的優(yōu)越性.同時，本文使用雙天線高精度GNSS 和高精度光纖陀螺的組合導(dǎo)航結(jié)果作為激光里程計的基準，以評估算法的精確性.

如圖8 所示，實驗驗證環(huán)境為典型的城市環(huán)境，道路兩旁包含樓房、廣場以及樹林等場景，能夠良好、全面的檢驗算法的有效性.

圖8 真實實驗場景俯視圖

如圖9(a)所示，在房屋樓梯處，場景特征非常復(fù)雜，且存在大量難以提取的復(fù)雜定位特征.本文用綠色框標記出可用于定位的區(qū)域位置，由圖9(b)與圖9(c)的對比可知，本文提出的基于點云分割算法所提取的特征具有良好的直線特征，且特征清晰、離散點少，因此本文算法在特征提取方面優(yōu)于LOAM算法.此外，本文所提出的特征提取部分計算量更小，經(jīng)實驗驗證，在該數(shù)據(jù)集下的特征提取平均耗時為23.46 ms，而LOAM 算法為35.43 ms，本文算法更具實時性.如圖10 所示，當(dāng)機器人穿越這個復(fù)雜環(huán)境時，本文基于點云分割的激光里程計算法在復(fù)雜環(huán)境下依舊魯棒運行，而LOAM 算法發(fā)生了較大的漂移現(xiàn)象.由此可得，較LOAM 算法，本文提出的基于點云分割的激光里程計算法更具魯棒性.

圖9 特征提取對比圖

圖10 里程計軌跡對比圖

3 結(jié)束語

本文針對機械式激光雷達點云橫向分辨率遠高于豎直分辨率的特點，提出了一種根據(jù)掃描線分割點云并為其賦予標簽的方法.通過標簽的賦予，可以判定物體的掃描情況，從而提取出點云中的直線和平面特征點.最后，將這些特征點投影并搜索匹配地圖特征點，并分別擬合直線或平面.利用點到直線的距離和點到平面的距離，可以計算位姿誤差方程，從而得到對激光里程計的最優(yōu)估計.本文利用KITTI 數(shù)據(jù)集和真實實驗數(shù)據(jù)，通過ROSBAG 數(shù)據(jù)錄影并回訪進行了仿真驗證，結(jié)果表明本文提出的基于點云分割的激光里程計在特征提取方面比LOAM 算法更準確，并且具有更高的定位精度和魯棒性.