基于視覺激光慣性相結合的機器人SLAM算法

王立玲，朱旭陽，馬東，王洪瑞

(1.河北大學電子信息工程學院，河北保定 071002；2.河北大學機器人技術研究中心，河北保定 071002)

0 前言

近幾年來隨著智能駕駛、機器人避障導航和自動泊車等技術的不斷發展，攝像頭作為重要的視覺傳感器被廣泛應用于自動駕駛等領域。通過視覺捕獲機器人周圍環境相關信息，作為下一步判斷與決策的依據，因此，視覺傳感器進行機器人同時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)技術成為研究熱點。

相比于單傳感器視覺SLAM，多傳感器融合進行機器人定位和繪圖精度大大提高。單目相機和單線激光雷達是當前室內移動機器人SLAM領域最具代表性、應用范圍最廣的兩類傳感器。劉諍軒等[1]融合雙目和2D激光雷達進行室外定位，克服雷達點云的稀疏性，取得了較好效果。王祝等人[2]通過障礙物點云聚類分割提高雷達測距和定位穩定性。姜祚鵬、梅天燦[3]通過不同點云匹配方式設計兩種激光里程計進行失效矯正和融合，實現高魯棒的定位。MU等[4]設計2D激光雷達融合深度相機，在保證實時性的基礎上實現精準定位。CHEN等[5]提出基于單目視覺和2D激光雷達的城市機器人搜救系統，實現精準建圖。但國內外對單目融合單線雷達研究較少，在弱紋理環境(存在大面積特征較為單一環境)中激光雷達可以估計精準位置，但只能獲得部分信息[6]。與激光雷達相比，相機可以獲得更豐富信息，相機和雷達結合實現完美互補。因此，本文作者提出一種單目視覺、2D激光雷達、慣性信息融合策略，使用3種傳感器系統用于移動機器人同時定位與地圖構建，并展開研究。

本文作者針對弱紋理環境下單一傳感器機器人定位和建圖性能較差，使用視覺、激光雷達和IMU慣性元件多傳感器融合進行機器人SLAM。針對雷達運動估計存在激光點漂移、運動畸變問題，使用改進PL-ICP算法(An Iterative Closest Point Variant Using a Point-to-Line Metric)，采用激光點到其最近兩個點連線的距離，并使用IMU和輪速計去除雷達運動畸變。在視覺激光慣性融合方法中，采取如下策略，有利于實現精準定位和建圖：

(1)在視覺與雷達預處理階段，視覺提取點線特征，同時雷達幀間匹配過程采用激光點到其最近兩個點連線的距離構建誤差方程，實現精度更高的匹配效果。

(2)針對單線雷達運動畸變問題，采用慣性傳感器與輪速里程計進行雷達運動畸變校正[7]，同時雷達估計的信息為單目點線特征三角化提供良好的深度值。

(3)針對單一傳感器進行機器人SLAM穩定性較差，將視覺特征定位信息融入到柵格地圖，利用多傳感器融合[8-10]提高機器人SLAM精確度。

1 系統設計

1.1 視覺點線特征提取

視覺部分，構建點線視覺里程計用于機器人定位。傳統基于點特征機器人定位算法，在弱紋理環境中單目視覺采集不到足夠特征，導致算法在機器人移動過程中定位能力減弱。利用單目視覺作為里程計提供相對位姿，將一個局部時間窗口內多個時刻得到的2D激光雷達數據融合成一個局部子圖。其中，視覺使用兩種特征結合方式，位置估計時獲得更多環境信息，雷達和慣性傳感器對單目視覺點線特征三角化深度信息進行校準，視覺為雷達提供更多有用的信息，采用多傳感器融合的方式進行機器人定位，實現精準的機器人定位。

1.2 雷達信息預處理

1.2.1 運動畸變模型

機器人采集激光點云數據不是瞬間獲得的，發射一束激光到接收激光數據的時間間隔伴隨著機器人運動和環境噪聲影響，當激光幀率較低時，機器人運動產生雷達運動畸變不能被忽略。

假設激光雷達的掃描幀率為10 Hz，機器人運動時一幀數據首尾時間差100 ms，若機器人以0.5 m/s的速度向前移動，掃描前面的障礙物，那么100 ms后尾部的測量距離和首部的測量距離在x方向上相差5 cm，如圖1所示。

圖1 激光雷達運動畸變原理

綠色箭頭表示激光雷達旋轉方向，黑色箭頭表示激光雷達運動方向。顯然，可以看出每個激光束距離數據都是在不同位姿下采集而來，如橙色、灰色、藍色點位姿所示。然而，一般激光雷達驅動封裝數據時，默認一幀激光雷達數據的所有激光束是在同一位姿下瞬時獲得的，也就是所有激光束都是從橙色點獲得的數據，這樣實際使用的數據和真實數據有明顯差距，如5 cm。所以，對于低幀率激光雷達，運動畸變明顯是個需要被解決的問題。

1.2.2 運動畸變補償

機器人采集到激光雷達數據同時里程計及IMU慣性信息也會被發送到數據處理中心，將3個數據信息進行濾波處理，時間同步，數據對齊，構建緊耦合模型。對里程計位姿分段線性近似及線性插值，最后將里程計及IMU坐標系變換到基準坐標系，實現雷達信息精準預處理。本文作者使用IMU和輪速里程計時時校準雷達運動過程中產生的畸變，其中運動畸變校正傳感器對齊原理如圖2所示。

圖2 雷達運動畸變去除原理

假設對第n個激光雷達掃描點進行畸變校正。如圖2所示，機器人雷達數據幀開始時刻為tlidar，采樣周期為 Δt，則對應掃描點采樣時刻為tn=tlidar+(n-1)Δt。經查詢知該掃描點相鄰兩幀里程計及IMU數據采樣時刻與姿態分別為tm、qm、tm-1、qm-1，則計算可得旋轉四元數之間平滑差值系數：

(1)

根據球面線性插值公式可以得到qn=Slerp(qm,qm-1,μ)，已知該雷達掃描點相對于激光雷達坐標系的空間坐標pn=[xn,yn,zn]T，并將四元數q0、qn分別轉換至R0、Rn。最后，通過系統坐標變換公式計算得到該掃描點校正后的坐標：

(2)

1.3 雷達點云匹配

激光雷達點云匹配算法為了匹配機器人在三維空間采集到的兩幀點云數據，計算其前后的累計位姿差形成機器人激光雷達里程計數據。文中相較于傳統算法，采用某一激光點云到其最近兩個點連線的距離構建最優匹配參數模型[11]，使用分段線性的方法對雷達掃描的點云曲面進行近似處理，計算魯棒的幀間匹配誤差系數，實現基于PL-ICP的激光雷達里程計。

機器人激光雷達掃描得到激光點為真實環境曲面離散采樣，最優距離誤差為激光點到實際曲面距離構建誤差方程，PL-ICP匹配誤差方程更貼近于真實情況。如圖3所示，構建激光點pi和pj匹配誤差方程為

圖3 改進雷達點云匹配

(3)

式中：qk+1為下一時刻位置轉換矩陣；Ck為點到線的對應關系；R(θk+1)為對應旋轉角度θk+1的旋轉矩陣；tk+1為平移矩陣。

2 基于視覺激光慣性的SLAM算法

2.1 雷達里程計

在采集到雷達數據后，使用雷達點云信息每一幀之間的累計位姿構建雷達里程計。傳統輪速里程計是基于安裝在機器人車輪上編碼器來跟蹤每個車輪轉數，轉數被整合到車輛運動模型中，以確定機器人相對于起始點的當前位置。在機器人長期移動過程中，由于軌跡漂移可能造成較大的累積誤差。代替輪速里程計使用激光雷達的點云深度值構建雷達里程計為視覺特征點提供深度信息，可以提供更高精度的位姿估計和更高質量的地圖。

2.2 信息融合

此算法采用雷達及慣性信息輔助視覺深度提取、融合補償進行機器人定位和建圖，將雷達采集點云信息映射到視覺圖像特征中，并基于此特征融合進行機器人定位。雷達幀間匹配對傳感器信息初始值更敏感，將單目相機采集到的特征信息與雷達融合有效避免了雷達因初始值不理想陷入局部循環的問題，通過構建最小化誤差方程實現更加精準的軌跡定位。視覺雷達慣性加權融合[12]，構建視覺雷達慣性混合殘差：

(4)

式中：rC為相機傳感器對應殘差函數；rL為雷達傳感器對應殘差函數；rB為IMU傳感器對應殘差函數。三者為傳感器采集誤差與機器人狀態矩陣X相關的函數。通過最小化混合殘差實現位姿優化，其中視覺里程計約束：使用點線視覺里程計，計算每個關鍵幀位姿，進而得到相對位姿。激光里程計約束：使用PL-ICP匹配雷達里程計，計算每個關鍵幀位姿，進而得到相對位姿[13]。IMU約束：在上一個關鍵幀的基礎上，進行慣性積分，從而得到兩個關鍵幀相對位姿信息。

2.3 改進柵格地圖的地圖構建

傳統柵格地圖在應用雷達信息建圖時被廣泛使用，機器人進行同時定位與地圖構建時可以構建基于雷達信息柵格地圖，此過程由下式表示：

Mgrid={mg(x,y)}

(5)

機器人建圖時雷達柵格信息grid(x,y)被占用不確定性用mg(x,y)表示。為了簡便，記mg(x,y)取值為0或1，0代表機器人移動時此柵格沒有被占用，反之1代表柵格信息已被占用。

基于單目視覺獲取圖像中點線特征地圖，可以用下面表達式來表示：

Mfeature={f(x,y,z)}

(6)

其中點線特征信息用f(x,y,z)表示，利用PNP求解進行位姿估計。

作者將視覺采集到的點線信息、二維激光雷達和IMU慣性單元的深度信息整合到一張地圖中，視覺為雷達提供良好初值，IMU、雷達信息校準視覺深度信息，構建柵格地圖。可以用下列式子描述這個過程：

M={m(x,y)}

(7)

m(x,y)={mg(x,y),feature_list(x,y)}

(8)

有了柵格地圖的輔助，移動機器人定位時不斷對準地圖區域坐標信息及柵格地圖識別的障礙物信息。視覺特征信息融入到柵格地圖，系統擁有更多且更豐富的信息用于回環檢測，減少系統累積誤差，也可以對導航和避障產生很大的幫助。

3 改進算法流程

在移動機器人SLAM過程中，改進算法主要分為兩個部分，圖4為改進算法流程框架。第一部分：通過視覺提取點線特征構建單目視覺里程計，達到弱紋理環境機器人定位穩定性效果；第二部分：通過改進雷達點云匹配構建雷達慣性融合生成局部子圖；第三部分：通過先驗地圖、視覺局部子圖和激光慣性局部子圖融合[14]、軌跡對齊實現位姿校正，軌跡更新。采用多傳感器融合方法，視覺為雷達信息定位提供良好初值，雷達、慣性傳感器深度信息為視覺點線特征三角化提供深度對照[15]，實現弱紋理環境魯棒軌跡定位和建圖。

圖4 改進算法流程

4 實驗結果與分析

為驗證此改進算法的有效性，本文作者分別在Gazebo構建仿真環境和移動機器人上進行實驗。通過弱紋理環境下機器人定位和建圖精確度來評價算法的性能。

4.1 雷達點云提取與匹配實驗

圖5是分別用傳統雷達點云信息提取算法使用PL-ICP點云提取與雷達運動畸變補償后點云提取效果對比。

圖5 雷達點云去畸變結果

從圖5可以看出：在場景(a)中，受機器人移動影響，傳統雷達點云檢測算法存在較大運動畸變及激光漂移等問題，長時間累計誤差過大導致機器人定位失??；在場景(b)中，使用慣性元件和輪速里程計對雷達進行運動畸變補償，通過點云分段線性近似、線性插值，找到激光束原點近似里程計位姿和近似激光雷達坐標系角度。將所有激光束從里程計及慣性坐標系變換到基準坐標系，實現里程計、慣性傳感器輔助單線雷達去除運動畸變。

根據以上實驗理論分析，分別對3種傳統雷達幀間匹配算法和改進后融合慣導、里程計數據的 IOPL-ICP(An Iterative Closest Point variant using a point-to-line metric，IMU and odometer)雷達匹配算法進行實驗對比和數據分析，并完成激光雷達幀間匹配實驗，雷達匹配耗時及精度如表1所示。

表1 4種雷達匹配算法的單幀匹配耗時對比

結果表明：文中改進雷達匹配算法相較于其他3種算法整體性能最好，耗時最少情況下能夠更加完整、精準地估計機器人位置，并且在不同場景下機器人定位和建圖準確率都能維持在一個較高的水平，具有較強的適應能力。

4.2 仿真環境實驗

為驗證算法在弱紋理環境下的穩定性與魯棒性，建立基于Gazebo仿真實驗。實驗采用Cartgrapher的 SLAM 算法，對移動機器人采集激光點數據，根據改進PL-ICP算法進行幀間點云匹配求解位姿，然后通過局部子圖融合進行局部和全局位姿優化，減少位姿誤差，并解決定位不準確而導致 SLAM 構建柵格地圖發生畸變的問題。雷達融合視覺里程計信息、IMU慣性信息進行定位，采用地圖回環檢測進行軌跡更新、地圖更新，最終定位軌跡精度測評和構建柵格地圖如圖6所示。

圖6 仿真實驗

圖6(c)(d)分別為僅使用雷達與視覺雷達慣性融合建圖效果對比，如圖6(c)中紅綠色圈出假墻和假影，圖6(d)融合后被有效去除。

4.3 真實實驗

為驗證文中改進算法對機器人SLAM的可行性，進行弱紋理環境實驗，如圖7所示。以麥克納姆輪式機器人作為硬件平臺，如圖7(a)所示。采用機器人單目攝像頭、單線激光雷達和IMU慣性測量單元融合進行實驗。調用Ubuntu系統的ROS(Robot Operating System)機器人操作系統，將實時定位和建圖數據發送到遠程可視化界面進行處理。

圖7 算法融合前后對比

實驗一：驗證算法穩定性。進行弱紋理環境建圖實驗，改進的雷達去除運動畸變建圖效果對比如圖7(c)(d)所示，融合慣導、里程計信息進行機器人建圖能提高系統穩定性，優質的先驗地圖有助于機器人定位。

實驗二：驗證定位精確性。進行弱紋理環境定位實驗，將視覺里程計、雷達里程計、視覺雷達慣性3種定位方法進行對比。如圖8可以看出：單一傳感器進行機器人定位精確性較差，改進算法能夠有效克服弱紋理環境單一傳感器機器人SLAM魯棒性較差的問題，采用多傳感器融合的方式進行定位大大提高了系統魯棒性。

圖8 融合前后定位精度對比

實驗三：驗證建圖準確性。進行樓道走廊實驗，在弱紋理大廳環境下對機器人算法融合前后建圖精度進行對比，如圖9所示。加入了視覺里程計后，融合算法有效提高弱紋理環境長走廊建圖效果，實現魯棒的機器人SLAM，提高算法精確性。

圖9 融合前(a)、后(b)建圖精度對比

5 結論

提出一種基于視覺激光慣性融合的機器人定位算法，并且將所提方法在Gazebo仿真環境和真實弱紋理環境中分別進行了測試和實驗。對比實驗結果表明：改進算法能夠快速準確地進行機器人同時定位與地圖構建，有效克服了弱紋理環境單一傳感器進行機器人SLAM魯棒性較差的問題，在保證實時性的同時對弱紋理環境具有較強的適應性，定位和建圖精度滿足實際需求，為后續機器人避障導航提供了有力保障。