面向高精度城市測繪的激光緊耦合SLAM方法

孫喜亮，關(guān)宏?duì)N，蘇艷軍，徐光彩，郭慶華

1. 中國科學(xué)院植物研究所植被與環(huán)境變化國家重點(diǎn)實(shí)驗(yàn)室，北京 100093； 2. 中國科學(xué)院大學(xué)，北京 100049； 3. 北京大學(xué)城市與環(huán)境學(xué)院，北京 100871

近年來，集成了激光雷達(dá)(light detection and ranging,LiDAR)、慣性測量單元(inertial measurement unit,IMU)和全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)的移動激光雷達(dá)平臺被廣泛用于獲取城市三維地理信息數(shù)據(jù)[1-2]。GNSS/IMU組合導(dǎo)航是目前移動測量平臺最常用的定位方法，然而由于高樓、橋梁、隧道、行道樹等城市地物對GNSS信號的遮擋，城市中易存在大量弱、無GNSS信號區(qū)域，僅憑這種定位方法難以有效實(shí)現(xiàn)大范圍城市場景的高精度制圖[3]。激光同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)技術(shù)可利用激光雷達(dá)增量式構(gòu)建地圖的同時(shí)，實(shí)現(xiàn)移動測量平臺自身的相對定位，為解決弱、無GNSS信號環(huán)境下的制圖問題提供了有效的解決方案[4]，從而被廣泛應(yīng)用于高精度制圖中[5-6]。

目前，激光SLAM方法按照激光和IMU的融合方式可分為松耦合和緊耦合[7-9]。松耦合方法獨(dú)立計(jì)算激光和IMU各自的運(yùn)動量，然后，對它們的位姿估計(jì)結(jié)果進(jìn)行融合。文獻(xiàn)[10]提出的LOAM方法是目前較為經(jīng)典的松耦合方法，該方法通過結(jié)合高頻里程計(jì)粗定位和低頻建圖精定位的方式，實(shí)現(xiàn)了基于旋轉(zhuǎn)2D LiDAR的同步定位和制圖。文獻(xiàn)[11]在LOAM的基礎(chǔ)上，引入視覺里程計(jì)作為激光里程計(jì)的前端，形成了V-LOAM方法，進(jìn)一步提高了SLAM的精度。文獻(xiàn)[12]在LOAM基礎(chǔ)上，利用點(diǎn)云分割降低特征數(shù)量，結(jié)合兩步配準(zhǔn)和閉環(huán)約束削弱累積誤差，形成了適合地面無人車的LeGO-LOAM方法。然而，松耦合方法未能聯(lián)合優(yōu)化各傳感器的觀測量，在特征缺乏區(qū)域易存在累積誤差大、穩(wěn)健性差等問題[4]。

與松耦合方法相比，緊耦合方法直接融合了激光原始特征信息和慣性單元測量值等輸出運(yùn)動狀態(tài)量，具有穩(wěn)健性強(qiáng)且制圖精度高的優(yōu)點(diǎn)[8]。近年來，多種緊耦合方法被相繼提出。例如文獻(xiàn)[13]將IMU預(yù)積分值應(yīng)用到點(diǎn)云的運(yùn)動畸變補(bǔ)償中，實(shí)現(xiàn)了LiDAR和IMU的外參校正；文獻(xiàn)[14]基于視覺慣性緊耦合SLAM方法引入了一種緊耦合的激光慣導(dǎo)SLAM框架，聯(lián)合優(yōu)化激光雷達(dá)特征點(diǎn)和IMU的觀測量，與LOAM相比獲得了更準(zhǔn)確的結(jié)果，但是其效率較低，慣導(dǎo)初始化條件復(fù)雜，場景特征缺乏區(qū)域穩(wěn)健性較差。文獻(xiàn)[15]提出了一種以機(jī)器人為中心的激光雷達(dá)慣性狀態(tài)估計(jì)器R-LINS，該方法利用迭代誤差狀態(tài)卡爾曼濾波器修正了機(jī)器人的狀態(tài)估計(jì)，但存在長期運(yùn)行會產(chǎn)生漂移的問題。文獻(xiàn)[16]提出一種面向移動機(jī)器人的激光慣導(dǎo)緊耦合實(shí)時(shí)定位方法，該方法具有較好的實(shí)時(shí)性，但在開闊的室外環(huán)境下定位精度達(dá)分米級。文獻(xiàn)[17]構(gòu)建了基于因子圖的緊耦合激光慣導(dǎo)里程計(jì)框架(LIO-SAM)，將不同來源的IMU預(yù)積分因子、激光里程計(jì)因子、GNSS因子和閉環(huán)環(huán)因子進(jìn)行聯(lián)合優(yōu)化，取得了較高的測圖精度，但該方法只是簡單地將GNSS因子加入聯(lián)合優(yōu)化中，未考慮復(fù)雜環(huán)境下GNSS異常對SLAM結(jié)果的影響。總體而言，目前已有方法主要針對小范圍的室外街道或室內(nèi)建筑場景，應(yīng)用的地理覆蓋范圍有限，在場景復(fù)雜、GNSS信號不穩(wěn)定、制圖范圍廣且精度要求高的城市測繪應(yīng)用中仍存在著累積誤差大、穩(wěn)健性差的問題[18-19]。

為了解決城市移動測量制圖的難點(diǎn)，本文在LOAM和LeGO-LOAM的基礎(chǔ)上，融合了差分GNSS軌跡、慣性測量單元位姿、激光序列幀數(shù)據(jù)開展高精度SLAM制圖。本文的主要貢獻(xiàn)可歸納為：①針對城市地物形狀特性，聯(lián)合桿狀和面狀特征點(diǎn)，采用先高程方向后水平方向的兩步優(yōu)化策略實(shí)現(xiàn)激光序列幀的相對位姿估計(jì)，提高了局部地圖配準(zhǔn)精度，降低了城市環(huán)境下SLAM的累積誤差。②針對GNSS信號不穩(wěn)定的特點(diǎn)，在全局地圖構(gòu)建及優(yōu)化過程中增量式的加入GNSS角點(diǎn)構(gòu)成的位置因子約束，實(shí)現(xiàn)了復(fù)雜城市場景下厘米級的高精度制圖。

1 總體框架設(shè)計(jì)

本文提出的緊耦合激光SLAM算法在LOAM和LeGO-LOAM的基礎(chǔ)上，融合了IMU預(yù)積分因子和GNSS因子，并在激光序列幀數(shù)據(jù)的初始相對位姿估計(jì)和全局地圖構(gòu)建及優(yōu)化中分別引入了桿狀和面狀特征點(diǎn)和多因子聯(lián)合優(yōu)化的策略。如圖1所示，本文提出的緊耦合激光SLAM系統(tǒng)可分為7個(gè)模塊。

(1) IMU預(yù)積分。根據(jù)IMU運(yùn)動方程對采樣間隔的觀測量進(jìn)行積分，獲取激光點(diǎn)云序列幀時(shí)刻對應(yīng)位姿。

(2) 激光運(yùn)動補(bǔ)償和特征提取。利用IMU積分得到的相對運(yùn)動位姿修正點(diǎn)云的運(yùn)動畸變，隨后提取桿狀和面狀特征點(diǎn)。

(3) 特征匹配和相對位姿估計(jì)。匹配序列幀同名特征，利用Levenberg-Marquardt(L-M)方法求解獲取相鄰幀的相對運(yùn)動位姿。

(4) 局部地圖構(gòu)建及優(yōu)化。聯(lián)合序列幀同名特征點(diǎn)、初始相對位姿、IMU預(yù)積分因子優(yōu)化構(gòu)建局部地圖。

(5) 閉環(huán)檢測。新的局部地圖構(gòu)建完成后與歷史局部地圖進(jìn)行特征匹配檢測閉環(huán)。

(6) GNSS位置因子構(gòu)建。依據(jù)GNSS軌跡方位變化提取出GNSS角點(diǎn)構(gòu)成GNSS位置因子。

(7) 全局地圖構(gòu)建及優(yōu)化。當(dāng)前局部地圖與歷史地圖存在閉環(huán)，則聯(lián)合局部地圖相對位姿、GNSS位置因子、IMU預(yù)積分因子、閉環(huán)因子等優(yōu)化構(gòu)建全局地圖，不存在閉環(huán)則直接更新全局地圖。

圖1 系統(tǒng)框架Fig.1 Framework of the proposed method

2 基于特征點(diǎn)匹配的相對位姿估計(jì)

準(zhǔn)確估計(jì)激光序列幀的相對位姿是構(gòu)建局部地圖的必要前提。本文采用IMU預(yù)積分的方式獲得激光序列幀對應(yīng)時(shí)刻的相對運(yùn)動[13-14]，基于此消除移動平臺位姿變換所引起的運(yùn)動畸變。在實(shí)現(xiàn)各單幀點(diǎn)云的畸變糾正后，通過配準(zhǔn)得到激光序列幀數(shù)據(jù)的初始相對位姿估計(jì)。在點(diǎn)云配準(zhǔn)中，迭代最近點(diǎn)(iterative closest point,ICP)算法是目前最常用的配準(zhǔn)方法，然而由于城市場景移動測量數(shù)據(jù)采集中易存在大量的移動目標(biāo)，且往返區(qū)域遮擋現(xiàn)象嚴(yán)重，直接基于原始激光點(diǎn)數(shù)據(jù)的ICP配準(zhǔn)失敗率高，因此本文采用基于特征點(diǎn)的配準(zhǔn)方法，通過特征匹配得到同名特征點(diǎn)后，利用L-M法迭代求解激光序列幀間的相對位姿。

2.1 特征點(diǎn)提取

考慮到相對位姿估計(jì)中地面點(diǎn)對于垂直方向的約束較好，而桿狀點(diǎn)和面狀點(diǎn)則對水平方向的平移和旋轉(zhuǎn)的約束較好，本文先進(jìn)行地面點(diǎn)和非地面點(diǎn)的區(qū)分，隨后進(jìn)一步在非地面點(diǎn)中提取桿狀和面狀特征點(diǎn)[20-21]，具體處理流程如下：

(1) 首先將單幀掃描獲取激光點(diǎn)集ρt={ρ1,ρ2,…,ρn}生成深度圖像(c,r)，其中每個(gè)點(diǎn)ρi對應(yīng)深度圖像的一個(gè)像素，c和r分別為圖像的列數(shù)和行數(shù)，圖像深度值為ρi到傳感器中心的距離。

(3) 獲取角度圖像后進(jìn)一步利用Savitsky-Golay濾波算法進(jìn)行平滑處理以削弱噪聲的影響。最后將最低行中角度小于45°的所有像素標(biāo)記為初始地面，并利用廣度優(yōu)先算法(breadth-first search,BFS)在角度圖像上搜索與地面像素間角度差值小于5°的像素，將其標(biāo)記為地面像素[22]。

(4) 根據(jù)地面像素，將其對應(yīng)的點(diǎn)云標(biāo)記為地面特征點(diǎn)fg，其余為非地面點(diǎn)，并對各地面點(diǎn)計(jì)算其法向nfg。

(1)

(2)

2.2 特征匹配和相對位姿估計(jì)

本文通過不同幀間特征點(diǎn)的匹配得到同名特征點(diǎn)[25]，對于每一個(gè)特征點(diǎn)，本文在其相鄰幀的特征點(diǎn)集中利用KDtree搜索其最鄰近的同屬性特征點(diǎn)，并將法向和主方向夾角均小于5°，且距離小于1 m的特征點(diǎn)對視為同名特征點(diǎn)[26-27]。同名特征提取后，聯(lián)合點(diǎn)到線距離和點(diǎn)到面距離最小構(gòu)建殘差方程，利用L-M方法求解獲取相鄰幀的相對運(yùn)動位姿[28]?？紤]地面點(diǎn)對于垂直方向的約束較好，而桿狀點(diǎn)和面狀點(diǎn)則對水平方向的平移和旋轉(zhuǎn)的約束較好，與直接求解6自由度位姿不同，本文采用了兩步優(yōu)化求解的策略，先利用地面點(diǎn)估算Z方向、橫滾角、俯仰角的變化量，再利用桿狀點(diǎn)和面狀點(diǎn)估算X方向、Y方向、方位角的變化量，最終聯(lián)合兩步優(yōu)化獲取序列幀的相對位姿[12]。由于單幀點(diǎn)云稀疏且場景多樣，無法保證配準(zhǔn)的穩(wěn)健性，且序列幀配準(zhǔn)及IMU測量信息存在累積誤差，因此本文選取1 s的點(diǎn)云數(shù)據(jù)建立局部地圖，在獲取激光序列幀相對位姿的基礎(chǔ)上，融合IMU預(yù)積分測量信息，完成局部地圖的構(gòu)建，并將其與歷史局部地圖集合進(jìn)行閉環(huán)檢測，實(shí)現(xiàn)累積誤差的削弱。

3 多因子聯(lián)合的全局地圖構(gòu)建及優(yōu)化

3.1 GNSS角點(diǎn)提取

全局地圖構(gòu)建及優(yōu)化是生成高精度的全局一致性地圖的關(guān)鍵步驟。GNSS位置因子可有效約束長距離軌跡(無閉環(huán)情況下)的漂移誤差，但在城市復(fù)雜環(huán)境中，GNSS存在著位置精度不穩(wěn)定的現(xiàn)象。此外，由于GNSS位置信息和激光慣導(dǎo)SLAM軌跡間沒有初始的轉(zhuǎn)換關(guān)系，其僅能約束XYZ位置而不能約束姿態(tài)，且在直線分布的軌跡中易存在歧義解。

為了提高GNSS位置因子的穩(wěn)定性，本文在基于GNSS定位標(biāo)準(zhǔn)差剔除誤差較大的GNSS點(diǎn)的基礎(chǔ)上，進(jìn)一步結(jié)合相同時(shí)間段內(nèi)SLAM的軌跡距離和GNSS點(diǎn)位移動距離的差值約束剔除了GNSS中的虛假高精度點(diǎn)。此外，針對GNSS位置因子僅能約束XYZ，特別是在直線分布的軌跡中易存在歧義解的問題，本文提出了基于GNSS角點(diǎn)的位置因子約束方法，與直接使用GNSS位置因子不同的是，本文將GNSS位置段方位變化超過30°的GNSS點(diǎn)視為角點(diǎn)，在隨后的全局優(yōu)化中，增量式的加入GNSS角點(diǎn)，構(gòu)成GNSS位置因子約束。

3.2 全局地圖構(gòu)建及優(yōu)化

(3)

(4)

(5)

Eglobal=arcmin(rloop+rM+rimu+rgnss)

(6)

4 試驗(yàn)與分析

4.1 試驗(yàn)設(shè)備及數(shù)據(jù)介紹

為驗(yàn)證本文方法的有效性，采用了集成Velodyne VLP-16 LiDAR、Xsens MTI-100 IMU和Novatel 718D GNSS系統(tǒng)的背包式激光掃描平臺對4種典型城市場景(開放園區(qū)、地下車庫、城市公園、街區(qū)道路)進(jìn)行了數(shù)據(jù)獲取(圖3)。

試驗(yàn)區(qū)中，開放公園整體為大型回環(huán)，場景中包含大量的建筑立面和少量移動車輛，軌跡總長度約為2.7 km，數(shù)據(jù)采集過程中存在少量往返采集。地下車庫場景面積較小，軌跡總長度約為800 m，數(shù)據(jù)采集過程中場景內(nèi)無移動目標(biāo)干擾，但該場景GNSS信號較弱，存在GNSS信號丟失現(xiàn)象。城市公園場景內(nèi)地物以樹木為主，由于樹木枝葉對激光掃描的遮擋，該場景內(nèi)存在大量往返掃描。街區(qū)道路場景為筆直的城市主干道，掃描過程中存在大量移動車輛和行人的干擾，行走軌跡不存在閉環(huán)。各場景復(fù)雜情況和數(shù)據(jù)采集信息見表1。

表1 數(shù)據(jù)集概況

在上述4個(gè)場景中，研究將本文所提方法與目前主流的LOAM、LeGO-LOAM、LIO-SAM方法進(jìn)行了對比，通過計(jì)算SLAM軌跡的絕對位置誤差(absolute position error,APE)評定各方法的定位精度。同時(shí)，本文研究還對僅利用激光慣導(dǎo)進(jìn)行SLAM制圖(our-odom)和增加GNSS角點(diǎn)位置約束后的SLAM制圖(our-method)結(jié)果進(jìn)行了對比，以表明GNSS位置因子約束的有效性。

除定位精度的評定外，本文還在開放園區(qū)場景中，利用RTK(real-time kinematic)采集了20個(gè)控制點(diǎn)，用于評定點(diǎn)云制圖的絕對坐標(biāo)精度。

4.2 僅利用激光慣導(dǎo)的本文方法與不同SLAM方法的精度對比

不同場景下各SLAM方法的軌跡平面位置對比結(jié)果如圖4所示。在開放園區(qū)場景中，局部放大圖顯示LeGO-LOAM方法的軌跡存在明顯斷裂(圖4(a))。地下車庫場景的局部放大圖分別展示了車庫入口和出口處軌跡結(jié)果，同樣顯示出LeGO-LOAM方法存在明顯的軌跡跳變(圖4(b))。城市公園場景中，由于LOAM和LeGO-LOAM方法精度較差，圖中僅展示了LIO-SAM和本文方法的對比結(jié)果(圖4(c))。街區(qū)道路場景中，結(jié)合表2的APE統(tǒng)計(jì)結(jié)果和圖4(d)所示，可發(fā)現(xiàn)LIO-SAM方法APE誤差主要出現(xiàn)在了Z方向上。

各場景下4種SLAM方法的軌跡APE隨時(shí)間的變化情況如圖5所示。在開放園區(qū)場景地下車庫場景中，LeGO-LOAM方法出現(xiàn)點(diǎn)云配準(zhǔn)失敗情況后，誤差直線上升(圖5(a)和5(b))。城市公園場景中,LeGO-LOAM、LIO-SAM和本文方法的軌跡APE均隨時(shí)間變化存在起伏(圖5(c))，說明3種方法的閉環(huán)約束均削弱了累積誤差的影響，且本文方法的累積誤差最小。街區(qū)道路場景中，LIO-SAM存在較為嚴(yán)重的退化現(xiàn)象，APE隨時(shí)間的增加顯著上升。

精度評定結(jié)果表明(表2)，LOAM方法在唯一成功的城市公園場景中，軌跡APE的RMSE高達(dá)32.406 m。LeGO-LOAM方法雖較LOAM方法有所提升，但其RMSE均在數(shù)十米量級。LIO-SAM方法在4個(gè)場景下均成功實(shí)現(xiàn)了制圖，但最優(yōu)精度出現(xiàn)在地下車庫場景中，RMSEAPE為0.505 m。在開放園區(qū)、地下車庫、城市公園、街區(qū)道路場景中，本文僅利用激光慣導(dǎo)的方法較LIO-SAM方法分別提升0.772 m、0.014 m、0.126 m和71.117 m。本文僅利用激光慣導(dǎo)的方法與LIO-SAM在街區(qū)道路和開放園區(qū)場景下的差異較大，其主要原因是兩個(gè)場景中均存在較多的移動車輛，致使LIO-SAM方法點(diǎn)云配準(zhǔn)發(fā)生退化現(xiàn)象，結(jié)果均表現(xiàn)為Z方向出現(xiàn)大的偏差。而本文方法提取了更趨近于靜態(tài)目標(biāo)的桿狀和面狀特征，并結(jié)合兩步優(yōu)化，有效地提高了點(diǎn)云配準(zhǔn)的精度，削弱了累積誤差。

4.3 GNSS角點(diǎn)位置約束的影響分析

如圖5所示，加入GNSS角點(diǎn)位置約束有效降低了本文方法在4個(gè)場景下的累積誤差。精度評價(jià)結(jié)果表明，4個(gè)場景的平均RMSEAPE由13.449 m提升至0.062 m(表3)，其中在街區(qū)道路場景中的提升最為顯著，主要原因在于該場景存在大量移動目標(biāo)且移動軌跡不存在閉環(huán)，致使SLAM累積誤差較大，此外，近似直線分布的GNSS點(diǎn)無法有效約束相應(yīng)時(shí)刻的姿態(tài)，本文方法利用提出的GNSS角點(diǎn)約束有效提高了城市場景制圖的穩(wěn)健性和精度。

圖2 全局地圖構(gòu)建及優(yōu)化Fig.2 Global map construction and optimization

圖3 試驗(yàn)區(qū)概況Fig.3 Overview of the study area

圖4 SLAM軌跡平面位置對比Fig.4 Comparison between plane positions of SLAM trajectories

圖5 SLAM軌跡APE對比Fig.5 Comparison between APE of SLAM trajectories

4.4 本文方法的點(diǎn)云制圖精度分析

本文以開放園區(qū)為例，進(jìn)一步展開本文方法點(diǎn)云制圖結(jié)果的精度驗(yàn)證工作。試驗(yàn)采集了均勻分布于開放園區(qū)場景測區(qū)的20個(gè)有效的檢查點(diǎn)(圖6(a))，用于點(diǎn)云制圖結(jié)果的絕對坐標(biāo)精度驗(yàn)證。驗(yàn)證結(jié)果表明，本文所提方法制圖結(jié)果的平面中誤差為0.041 m，高程中誤差為0.024 m，整體中誤差為0.047 m(表4)，檢查點(diǎn)的平面和高程誤差分布均為殘留的隨機(jī)誤差(圖6(b))，誤差分布相對較大的點(diǎn)基本位于移動軌跡出現(xiàn)急劇方位變化之處，原因在于該處相對于平滑運(yùn)動的軌跡而言仍有部分姿態(tài)誤差殘留，本文方法可有效提升SLAM制圖的絕對坐標(biāo)精度，滿足復(fù)雜城市場景下的高精度制圖要求。

圖6 檢查點(diǎn)及誤差分布Fig.6 Distribution of check points in the accuracy analysis

表2 SLAM軌跡APE統(tǒng)計(jì)結(jié)果

表3 有無GNSS角點(diǎn)位置因子約束的軌跡APE統(tǒng)計(jì)結(jié)果對比

表4 檢查點(diǎn)精度統(tǒng)計(jì)結(jié)果

5 結(jié) 論

本文面向復(fù)雜城市場景的高精度制圖要求，提出了一種LiDAR、IMU、GNSS緊耦合SLAM方法，該方法利用桿狀和面狀特征進(jìn)行序列幀間、激光掃描幀和局部地圖間及局部地圖間的點(diǎn)云配準(zhǔn)，并采用GNSS角點(diǎn)位置約束SLAM，有效降低了城市環(huán)境下SLAM的累積誤差，提高了全局地圖構(gòu)建的準(zhǔn)確性。試驗(yàn)結(jié)果表明，LOAM和LeGO-LOAM方法穩(wěn)健性及精度均較差，本文方法僅利用激光慣導(dǎo)進(jìn)行SLAM制圖時(shí)，在開放園區(qū)、地下車庫、城市公園和街區(qū)道路場景中的精度(RMSEAPE=13.449 m)顯著高于目前主流的緊耦合LIO-SAM(RMSEAPE=31.456 m)方法，結(jié)合GNSS位置因子約束后，精度提升至厘米級(RMSEAPE=0.062 m)，其中開放園區(qū)中基于控制點(diǎn)的點(diǎn)云絕對坐標(biāo)精度評價(jià)結(jié)果表明，本文方法(RMSE=0.047 m)可有效支持厘米級高精度城市制圖。