劉振宇， 惠澤宇*， 郭旭， 李剛

(1. 沈陽工業(yè)大學信息科學與工程學院， 沈陽 110870； 2. 沈陽新松機器人自動化股份有限公司， 沈陽 110169)

準確的位姿估計是移動機器人投入實際應用的關鍵，它確保機器人在運動中能夠計算出自己的位置和姿態(tài)，為同時定位與建圖(simultaneous localization and mapping，SLAM)和導航提供了數(shù)據(jù)支撐[1]。

在建立周圍場景時，根據(jù)所使用的傳感器不同，可分為激光SLAM和視覺SLAM[2]。三維激光傳感器根據(jù)其硬件結構，在無需提前布置場景的情況下可以直接獲取場景的三維點云信息，并且基于三維激光的SLAM技術在室外場景中不受到光照、尺度等因素影響，還可融合多傳感器加強定位精度，為后續(xù)移動機器人導航環(huán)節(jié)提供更為精確的先驗信息，因此成為當前定位方案中較為主流的新技術[3]。然而單獨的三維激光雷達傳感器的應用還不完善，從其本身來看，存在以下缺點：①垂直分辨率較低，獲取的稀疏點云提供的特征有限，使幀間匹配環(huán)節(jié)變得困難[4]；②當雷達安裝在移動機器人上，機器人運動時雷達獲取的點云存在運動畸變，直接影響傳感器數(shù)據(jù)精度；③更新頻率低，大概僅在10 Hz左右[5]。因此僅使用激光雷達進行位姿估計，輸出位姿速度低，且在范圍較大的室外場景，累積誤差造成的軌跡漂移無法滿足機器人快速定位的要求[6]。而慣性測量單元(inertial measurement unit，IMU)有著高達100 Hz角速度和加速度的更新頻率，可在激光雷達一個掃描周期內為其提供狀態(tài)信息，對單一傳感器定位的不足進行補償，提高精確度[7]。因此，現(xiàn)研究激光雷達與IMU融合進行數(shù)據(jù)融合的緊耦合SLAM定位算法，以期提升移動機器人位姿估計精度。

國內外許多學者對激光雷達與里程計的融合方案進行了研究，Zhang等[8]提出的激光雷達里程計和地圖繪制(lidar odometry and mapping，LOAM)方案，用IMU輔助激光雷達計算方向，并假設勻速運動，將激光雷達和慣性測量單元解耦，主要使用IMU作為整個系統(tǒng)的先驗，因此無法利用IMU對系統(tǒng)進行進一步優(yōu)化。Bavle等[9]通過對傳感器得到的圖像提取語義信息，提出一種輕量級和實時的語義SLAM框架，然而在面臨大型的復雜場景，會受到過多語義信息的干擾。Shan等[10]對LOAM進行優(yōu)化，提出Lego-LOAM方案，該方案對地面分割，并引入后端優(yōu)化和回環(huán)檢測模塊，是完整的SLAM系統(tǒng)。Ye等[11]提出的LIO-Mapping方案使用的是圖優(yōu)化的方法對激光和IMU進行緊耦合的里程計，然而處理速度慢，在運行時無法滿足里程計實時性的要求，所以對其進行了降采樣處理，室內隔一幀采一次、室外隔兩幀采一次。Qin等[12]提出的R-LINS方案選用了迭代誤差狀態(tài)卡爾曼濾波(error-state Kalman filter，ESKF)進行激光和IMU的數(shù)據(jù)融合，同樣屬于緊耦合方案。Yin等[13]融合神經(jīng)網(wǎng)絡對三維點云從旋轉和平移兩部分進行處理，提高整個SLAM系統(tǒng)的魯棒性。Lin等[14]在LOAM的基礎上針對小視場的不規(guī)則采樣做了優(yōu)化，提出了一種比LOAM更具魯棒性的里程計框架。Zhou等[15]通過改進的SUPER4PC和傳統(tǒng)ICP進行前端匹配，通過NDT匹配局部地圖實現(xiàn)精匹配。

受上述里程計方法的啟發(fā)，現(xiàn)選用以Lego-LOAM方案為基礎進行改進，以期改善里程計定位精度低、魯棒性差的問題。

1 系統(tǒng)框架及算法理論

在Lego-LOAM方案中，系統(tǒng)整體精度高，但里程計精度低，主要通過回環(huán)檢測模塊對全局位姿進行校正。但應用在大場景建圖時，里程計部分漂移過大后，當再次回到原點時，系統(tǒng)無法識別此時是一個回環(huán)，因此無法對全局位姿進行校正，主要原因如下。

(1)去除點云畸變時，采用松耦合方式進行修正，需要提供精確的角度初值，但IMU高頻噪聲沒有被預先處理，整體效果不理想。

(2)特征匹配時特征點較多，在相近區(qū)域內的特征點匹配時會產生錯誤匹配的現(xiàn)象，在相鄰幀計算位姿變換時產生較大的累積誤差。

(3)Lego-LOAM在里程計部分屬于IMU和激光的松耦合，數(shù)據(jù)間缺少相互校正的環(huán)節(jié)，在大場景建圖時，里程計誤差較大，后端優(yōu)化和回環(huán)檢測部分無法校正全局位姿。

針對Lego-LOAM方案在里程計部分存在的問題，提出如下4種解決方案。

方案1：引入IMU進行線性插值，消除點云畸變。

方案2：引入地面分割模塊，快速識別點云中的地面部分并提取。

方案3：以曲率為標準提取點云中的邊和面特征，按特性對提取的特征點進行分類，分別進行儲存；在匹配階段對邊和面特征按所分類別進行相應匹配，降低錯誤匹配的概率。

方案4：利用滑動窗口優(yōu)化的框架融合激光數(shù)據(jù)和IMU預積分數(shù)據(jù)，提高里程計位姿估計的準確率。

改進系統(tǒng)框架如圖1所示。

為加速度；為角速度；m為距離因子圖1 改進算法系統(tǒng)框架圖Fig.1 Improved algorithm odometer framework

2 里程計算法理論

2.1 數(shù)據(jù)預處理

(1)

(2)

(3)

式中：Δpij、Δvij、Δqij分別為從上一時刻i到當前時刻j的位置、速度、旋轉變化量；pi、pj分別為上一時刻與當前時刻的位置；gW為重力系數(shù)；ba、bg為IMU讀數(shù)的零偏與噪聲；vi、vj分別為上一時刻與當前時刻的速度；符號?表示四元數(shù)相乘。

圖2 線性插值Fig.2 Linear interpolation

定義當前激光點的時間戳為tL，curr，IMU在j幀時在世界坐標系的狀態(tài)為

(4)

(5)

(6)

(7)

2.2 地面分割

對于激光雷達掃描的點云數(shù)眾多，全部處理對于計算機資源消耗量大，因此將部分對于全局特征不明顯的點優(yōu)先提取出來，不參與后面的特征提取和相鄰幀匹配環(huán)節(jié)，這里采用點云的幾何特征進行地面分割，保證系統(tǒng)的實時性。

根據(jù)激光雷達的垂直分辨率為-15°～15°，選取0到7號線束所掃描的點云進行分類處理。

定義一個夾角β，已知激光雷達線數(shù)Rn、Rn-1對應的深度分別為OA、OB，垂直方向激光雷達線束與水平線的夾角為α1、α2，角度β的計算方法如式(8)所示。由于地面是一個水平面，根據(jù)它的特性，將所有β<5°且‖BM‖<5 cm所對應的點識別為地面點區(qū)域。地面分割算法設計示意圖如圖3所示。

(8)

圖3 地面分割算法設計Fig.3 Ground segmentation algorithm design

2.3 特征提取與分類

為提高計算效率，需要進行激光雷達的特征提取。主要提取的是平面上和邊上的點，因為這些點可以從激光的每次掃描中提取，通過曲率和距離變化來選擇，即

(9)

為了從各個方向上都能均勻地提取特征，將一圈點云平均分成6份，在每個區(qū)域內對點的曲率進行大小排序，選取曲率最大和最小的點，分別對應最像邊的點和最像平面的點。

需要說明的是，針對激光雷達一圈掃描的起始和結束分別對應的前后5個點是不參與運算的，因為這10個點沒有與其對應的前或后5個點，無法完成平滑度的計算。

將每個點所對應的平滑度與設定好的平滑度閾值進行比較。將平滑度小于閾值的點設置為平面特征，如果該點前后5個點已經(jīng)存在一個平面特征點，則跳過該點，并從上一平面特征點范圍外繼續(xù)選??；邊特征點是在平滑度大于閾值的范圍中選取，選取條件與平面特征點一致。

2.4 點云分割

在提取特征點的過程中，一些不穩(wěn)定的點同樣也會被提取，包括遮擋點和離群點，這類特征點在移動機器人運動過程中會消失，在特征匹配階段導致誤匹配，降低位姿估計的精度。

這里采用聚類的方法，將一部分有可能是同一物體的點云幾何特征進行標記，并在后續(xù)匹配過程中使用該標記的特征點云進行處理，主要分為兩部分，如圖4所示。

對于同一條水平激光線上的特征，首先選取相鄰且深度差在閾值以內的點，根據(jù)公式進行閾值判斷。

Δd=‖OFi，k‖-‖OFi，k+1‖=d1-d2cosα

(10)

(11)

式中：‖OFi，k‖為特征點深度值大的點記為d1；‖OFi，k+1‖為特征點深度值小的點記為d2；α為兩個相鄰特征點之間的分辨率。

經(jīng)過粗分割后，可將同類特征進行關聯(lián)處理，同時也有效地去除激光雷達掃描時由于物體材質原因生成的無效點。

按照深度值較大的特征點所對應的激光線深度到下一個相鄰特征點連線所對應的夾角β，對不同特征進行精分類，并設定一個閾值參數(shù)η用于判斷相鄰兩點特征是分割成兩個類別或者合并成一個類別。

d為特征點的深度；α為水平分辨率；β為相鄰特征點連線夾角； a、b、c為同類型特征點圖4 點云聚類Fig.4 Point Cloud Clustering

2.5 激光里程計

2.5.1 幀間匹配

圖5 優(yōu)化窗口Fig.5 Optimize window

通過建立局部地圖，可以找到當前時刻特征點FLα與局部地圖的對應關系，其中α∈{p+1，p+2，…，i}特征點包含之前特征提取部分的平面點和邊點。

(12)

對其中的每個特征點xLp擬合成平面，通過求解線性方程組的形式可以得到平面系數(shù)為

ωTx+d=0

(13)

式(13)中：ω為平面法向量；d為p時刻坐標系下的截距。

定義一個距離因子m=[x，ω，d]，記錄每個x∈FLα的特征點到面的相對測量值，其殘差可以表示為

(14)

2.5.2 數(shù)據(jù)融合

滑動窗口有助于限制計算量，當新的位姿約束出現(xiàn)后，邊緣化窗口中最早時刻的位姿約束，在窗口內從p時刻到i時刻有Na個IMU狀態(tài)信息，整體待估計的變量有

(15)

構建代價函數(shù)求解狀態(tài)變量X的全局位姿估計，表達式為

(16)

(17)

非線性最小二乘形式的代價函數(shù)可通過高斯牛頓法求解，這里通過谷歌的非線性優(yōu)化庫Ceres Solver進行求解。

3 實驗結果與分析

3.1 軟硬件平臺

實驗平臺的軟硬件配置如表1所示。其中，移動平臺如圖6所示。

表1 軟硬件配置Table 1 Hardware and software configuration

圖6 移動平臺Fig.6 Mobile platform

3.2 預處理結果分析

在該階段主要處理在移動機器人運動過程中造成的點云畸變，為后續(xù)點云處理提供一個良好的先驗條件。

根據(jù)自行錄制沈陽新松三期公司停車場數(shù)據(jù)集進行算法測試，根據(jù)圖7可以看到，該預處理算法可以有效降低點云畸變。

圖7 點云預處理Fig.7 Point cloud preprocessing

3.3 特征提取與地面分割結果分析

針對圖8(a)中預處理后的點云進行特征提取，圖8(b)～圖8(e)均為點云分割后的不同特征的結果。

地面特征提取效果如圖8(b)所示，其中不同顏色表示激光掃描物體的反射率；圖8(c)為地面點提取的特征，通過對地面特征僅計算每個均分區(qū)域內平滑度最大的那些點，并進行分類；通過點云的聚類，將樹干、路標和墻角這類邊特征進行分類，如圖8(d)中綠色的特征點為提取的邊特征；分類后的面特征如圖8(e)所示。

圖8 點云特征提取與分類Fig.8 Feature extraction and classification of point cloud

3.4 里程計精度分析

3.4.1 KITTI數(shù)據(jù)

使用KITTI00號數(shù)據(jù)集進行測試，分別利用Lego-LOAM、LIO-Mapping和改進方案LIO-SWO(sliding window odometer， SWO)對里程計輸出軌跡和真值進行精度評定，定位精度評價指標選取絕對位姿誤差(absolute pose error， APE)和均方根誤差(root mean square error， RMSE)，使用EVO插件對軌跡進行評定。

(18)

(19)

式中：Xi為算法估計出的位姿；Xt為GNSS的真值。

圖9為改進方案所建的KITTI00數(shù)據(jù)集的地圖，其中紅色三角形為數(shù)據(jù)錄制的起始點。

表2為三種方案在KITTI00數(shù)據(jù)集下建圖的絕對位姿誤差。

改進方案相比于兩種比對方案，絕對誤差的均值最小為1.14 m，相比于Lego-LOAM算法結果(2.86 m)和LIO-Mapping算法結果(1.53 m)分別提高60.1%和34.2%，但從均方根誤差的對比來看，略低于同樣使用緊耦合框架的LIO-Mapping方案。

圖9 KITTI00建圖Fig.9 KITTI00 mapping

表2 絕對位姿誤差Table 2 Absolute pose error

3.4.2 廠區(qū)數(shù)據(jù)

對改進算法的滑動窗口優(yōu)化位姿模塊進行測試，數(shù)據(jù)集選用廠區(qū)辦公樓，總長度為943 m，通過室外大場景測試改進算法的魯棒性。廠區(qū)地圖如圖10所示。

通過記錄優(yōu)化后的位姿輸出軌跡顯示，可以看出，在進行大場景的SLAM作業(yè)時，Lego-LOAM算法的累積誤差逐漸加大，如圖11(a)所示，且原有的回環(huán)檢測模塊也由于漂移過大，未能在回環(huán)處進行閉環(huán)以優(yōu)化位姿。

圖11(b)和圖11(c)分別對應LIO-Mapping軌跡和改進算法LIO-SWO軌跡，使用了緊耦合框架的兩種算法，在建圖距離較長時依舊可以維持較強的魯棒性；對比起點和終點軌跡，改進算法在累積漂移部分的處理略優(yōu)于LIO-Mapping。

圖10 廠區(qū)地圖Fig.10 Factory map

圖12為改進算法的相對誤差評估，通過計算里程計在相鄰時間戳位姿變化量與真值在相鄰時間戳位姿變化量，對得到的結果作差以獲得相對位姿誤差，在943 m的園區(qū)中，改進算法的漂移量誤差的最大值不到1 m。可以看出，基于優(yōu)化窗口的緊耦合里程計框架能夠在大范圍室外場景SLAM作業(yè)時，以較低漂移量完成定位建圖工作。

3.5 運行時間評估

前端激光里程計需要首先滿足快速性的要求，分別對Lego-LOAM、LIO-Mapping和改進方案進行逐個模塊的處理時間進行比對，采用的數(shù)據(jù)集包括16線激光雷達和IMU的數(shù)據(jù)。具體步驟為在每個模塊插入起始和結束的時間函數(shù)，用以記錄從數(shù)據(jù)進入該模塊到處理結束期間的時間戳。表3為上述方案的不同模塊消耗時間。

圖11 三種方案軌跡Fig.11 Trajectory of three schemes

在特征提取模塊，改進方案除了提取了邊面特征外，還融入了不同特征的聚類和分類存儲，耗時方

圖12 相對位姿誤差Fig.12 Relative pose error

表3 各個模塊數(shù)據(jù)處理用時Table 3 Data processing time of different schemes

面稍有增加。

在激光里程計模塊Lego-LOAM方案屬于輕量化，以稀疏的特征點云輸入里程計模塊進行處理，同時對優(yōu)化函數(shù)直接進行分解處理，沒有使用優(yōu)化庫，耗時為12.5 ms，改進方案采用特征分類匹配，同時引入IMU數(shù)據(jù)作為全局先驗，耗時較Lego-LOAM略有增加，但優(yōu)于采用多步優(yōu)化的LIO-Mapping方案。

優(yōu)化建圖模塊中，Lego-LOAM方案加入了回環(huán)檢測，通過對當前掃描與全局地圖進行匹配，判斷是否回到原點，耗時281.1 ms，改進方案使用滑窗掃描多幀特征建立局部地圖，通過幀與局部地圖的匹配，并在窗口中完成優(yōu)化，耗時較引入IMU數(shù)據(jù)優(yōu)化的LIO-Mapping方案稍有增加。

在整體定位環(huán)節(jié)中改進方案需要282.3 ms計算每幀位姿，Lego-LOAM和LIO-Mapping分別需要308.3 ms和233.6 ms，由于改進的LIO-SWO方案為了實現(xiàn)緊耦合構建基于IMU和激光雷達的優(yōu)化函數(shù)，提高整體定位精度，所以總體耗時較比對方案有所增加，但依舊小于激光雷達10 Hz的接收頻率，即激光雷達每掃描一圈獲取的數(shù)據(jù)時間為100 ms，滿足里程計實時性的要求。

4 結論

面向室外大場景，針對現(xiàn)有里程計在該場景中定位精度低、魯棒性不足的問題，提出一種三維激光和IMU慣導緊耦合的里程計方案，主要包括4個部分：數(shù)據(jù)預處理、地面點云提取、特征點云提取與分割、緊耦合激光里程計框架。

通過使用自行錄制的停車場數(shù)據(jù)集和公開的KITTI數(shù)據(jù)集分別對各個模塊處理效果和里程計精度進行評測，以及對運行時間進行分析得出以下結論。

(1)改進方案的數(shù)據(jù)預處理環(huán)節(jié)可以有效地去除激光雷達在運動時的點云畸變。

(2)通過對地面點的提取以及特征點的分割，對里程計部分的處理時間有小幅度提升，在幀間匹配環(huán)節(jié)也有效抑制了錯誤的匹配。

(3)建立局部地圖與優(yōu)化滑窗并且構建聯(lián)合優(yōu)化函數(shù)，提升了激光雷達和IMU數(shù)據(jù)的關聯(lián)性和里程計定位的精度，同時依舊滿足實時性的要求。

在未來的研究中，可從以下兩個方面進行改進。一方面，針對動態(tài)物體所造成的點云拖影問題，加入一個識別動態(tài)物體的模塊，并對其進行跟蹤，精確檢測和分割動態(tài)物體，優(yōu)化拖影問題；另一方面，在室外場景，通過提取關鍵幀，并計算關鍵幀位姿與GNSS真值在優(yōu)化窗口內協(xié)方差，當漂移過大時引入GNSS真值進行校正。