基于視覺激光慣性相結(jié)合的機(jī)器人SLAM算法

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

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

0 前言

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

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

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

(1)在視覺與雷達(dá)預(yù)處理階段，視覺提取點(diǎn)線特征，同時(shí)雷達(dá)幀間匹配過程采用激光點(diǎn)到其最近兩個(gè)點(diǎn)連線的距離構(gòu)建誤差方程，實(shí)現(xiàn)精度更高的匹配效果。

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

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

1 系統(tǒng)設(shè)計(jì)

1.1 視覺點(diǎn)線特征提取

視覺部分，構(gòu)建點(diǎn)線視覺里程計(jì)用于機(jī)器人定位。傳統(tǒng)基于點(diǎn)特征機(jī)器人定位算法，在弱紋理環(huán)境中單目視覺采集不到足夠特征，導(dǎo)致算法在機(jī)器人移動(dòng)過程中定位能力減弱。利用單目視覺作為里程計(jì)提供相對位姿，將一個(gè)局部時(shí)間窗口內(nèi)多個(gè)時(shí)刻得到的2D激光雷達(dá)數(shù)據(jù)融合成一個(gè)局部子圖。其中，視覺使用兩種特征結(jié)合方式，位置估計(jì)時(shí)獲得更多環(huán)境信息，雷達(dá)和慣性傳感器對單目視覺點(diǎn)線特征三角化深度信息進(jìn)行校準(zhǔn)，視覺為雷達(dá)提供更多有用的信息，采用多傳感器融合的方式進(jìn)行機(jī)器人定位，實(shí)現(xiàn)精準(zhǔn)的機(jī)器人定位。

1.2 雷達(dá)信息預(yù)處理

1.2.1 運(yùn)動(dòng)畸變模型

機(jī)器人采集激光點(diǎn)云數(shù)據(jù)不是瞬間獲得的，發(fā)射一束激光到接收激光數(shù)據(jù)的時(shí)間間隔伴隨著機(jī)器人運(yùn)動(dòng)和環(huán)境噪聲影響，當(dāng)激光幀率較低時(shí)，機(jī)器人運(yùn)動(dòng)產(chǎn)生雷達(dá)運(yùn)動(dòng)畸變不能被忽略。

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

圖1 激光雷達(dá)運(yùn)動(dòng)畸變原理

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

1.2.2 運(yùn)動(dòng)畸變補(bǔ)償

機(jī)器人采集到激光雷達(dá)數(shù)據(jù)同時(shí)里程計(jì)及IMU慣性信息也會(huì)被發(fā)送到數(shù)據(jù)處理中心，將3個(gè)數(shù)據(jù)信息進(jìn)行濾波處理，時(shí)間同步，數(shù)據(jù)對齊，構(gòu)建緊耦合模型。對里程計(jì)位姿分段線性近似及線性插值，最后將里程計(jì)及IMU坐標(biāo)系變換到基準(zhǔn)坐標(biāo)系，實(shí)現(xiàn)雷達(dá)信息精準(zhǔn)預(yù)處理。本文作者使用IMU和輪速里程計(jì)時(shí)時(shí)校準(zhǔn)雷達(dá)運(yùn)動(dòng)過程中產(chǎn)生的畸變，其中運(yùn)動(dòng)畸變校正傳感器對齊原理如圖2所示。

圖2 雷達(dá)運(yùn)動(dòng)畸變?nèi)コ?/p>

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

(1)

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

(2)

1.3 雷達(dá)點(diǎn)云匹配

激光雷達(dá)點(diǎn)云匹配算法為了匹配機(jī)器人在三維空間采集到的兩幀點(diǎn)云數(shù)據(jù)，計(jì)算其前后的累計(jì)位姿差形成機(jī)器人激光雷達(dá)里程計(jì)數(shù)據(jù)。文中相較于傳統(tǒng)算法，采用某一激光點(diǎn)云到其最近兩個(gè)點(diǎn)連線的距離構(gòu)建最優(yōu)匹配參數(shù)模型[11]，使用分段線性的方法對雷達(dá)掃描的點(diǎn)云曲面進(jìn)行近似處理，計(jì)算魯棒的幀間匹配誤差系數(shù)，實(shí)現(xiàn)基于PL-ICP的激光雷達(dá)里程計(jì)。

機(jī)器人激光雷達(dá)掃描得到激光點(diǎn)為真實(shí)環(huán)境曲面離散采樣，最優(yōu)距離誤差為激光點(diǎn)到實(shí)際曲面距離構(gòu)建誤差方程，PL-ICP匹配誤差方程更貼近于真實(shí)情況。如圖3所示，構(gòu)建激光點(diǎn)pi和pj匹配誤差方程為

圖3 改進(jìn)雷達(dá)點(diǎn)云匹配

(3)

式中：qk+1為下一時(shí)刻位置轉(zhuǎn)換矩陣；Ck為點(diǎn)到線的對應(yīng)關(guān)系；R(θk+1)為對應(yīng)旋轉(zhuǎn)角度θk+1的旋轉(zhuǎn)矩陣；tk+1為平移矩陣。

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

2.1 雷達(dá)里程計(jì)

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

2.2 信息融合

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

(4)

式中：rC為相機(jī)傳感器對應(yīng)殘差函數(shù)；rL為雷達(dá)傳感器對應(yīng)殘差函數(shù)；rB為IMU傳感器對應(yīng)殘差函數(shù)。三者為傳感器采集誤差與機(jī)器人狀態(tài)矩陣X相關(guān)的函數(shù)。通過最小化混合殘差實(shí)現(xiàn)位姿優(yōu)化，其中視覺里程計(jì)約束：使用點(diǎn)線視覺里程計(jì)，計(jì)算每個(gè)關(guān)鍵幀位姿，進(jìn)而得到相對位姿。激光里程計(jì)約束：使用PL-ICP匹配雷達(dá)里程計(jì)，計(jì)算每個(gè)關(guān)鍵幀位姿，進(jìn)而得到相對位姿[13]。IMU約束：在上一個(gè)關(guān)鍵幀的基礎(chǔ)上，進(jìn)行慣性積分，從而得到兩個(gè)關(guān)鍵幀相對位姿信息。

2.3 改進(jìn)柵格地圖的地圖構(gòu)建

傳統(tǒng)柵格地圖在應(yīng)用雷達(dá)信息建圖時(shí)被廣泛使用，機(jī)器人進(jìn)行同時(shí)定位與地圖構(gòu)建時(shí)可以構(gòu)建基于雷達(dá)信息柵格地圖，此過程由下式表示：

Mgrid={mg(x,y)}

(5)

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

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

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

(6)

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

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

M={m(x,y)}

(7)

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

(8)

有了柵格地圖的輔助，移動(dòng)機(jī)器人定位時(shí)不斷對準(zhǔn)地圖區(qū)域坐標(biāo)信息及柵格地圖識(shí)別的障礙物信息。視覺特征信息融入到柵格地圖，系統(tǒng)擁有更多且更豐富的信息用于回環(huán)檢測，減少系統(tǒng)累積誤差，也可以對導(dǎo)航和避障產(chǎn)生很大的幫助。

3 改進(jìn)算法流程

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

圖4 改進(jìn)算法流程

4 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證此改進(jìn)算法的有效性，本文作者分別在Gazebo構(gòu)建仿真環(huán)境和移動(dòng)機(jī)器人上進(jìn)行實(shí)驗(yàn)。通過弱紋理環(huán)境下機(jī)器人定位和建圖精確度來評(píng)價(jià)算法的性能。

4.1 雷達(dá)點(diǎn)云提取與匹配實(shí)驗(yàn)

圖5是分別用傳統(tǒng)雷達(dá)點(diǎn)云信息提取算法使用PL-ICP點(diǎn)云提取與雷達(dá)運(yùn)動(dòng)畸變補(bǔ)償后點(diǎn)云提取效果對比。

圖5 雷達(dá)點(diǎn)云去畸變結(jié)果

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

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

表1 4種雷達(dá)匹配算法的單幀匹配耗時(shí)對比

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

4.2 仿真環(huán)境實(shí)驗(yàn)

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

圖6 仿真實(shí)驗(yàn)

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

4.3 真實(shí)實(shí)驗(yàn)

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

圖7 算法融合前后對比

實(shí)驗(yàn)一：驗(yàn)證算法穩(wěn)定性。進(jìn)行弱紋理環(huán)境建圖實(shí)驗(yàn)，改進(jìn)的雷達(dá)去除運(yùn)動(dòng)畸變建圖效果對比如圖7(c)(d)所示，融合慣導(dǎo)、里程計(jì)信息進(jìn)行機(jī)器人建圖能提高系統(tǒng)穩(wěn)定性，優(yōu)質(zhì)的先驗(yàn)地圖有助于機(jī)器人定位。

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

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

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

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

5 結(jié)論

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