移動(dòng)機(jī)器人障礙物地圖構(gòu)建研究

程天明，陳元電，蘇成悅,，徐勝，楊尚儒，劉拔

（1.廣東工業(yè)大學(xué)信息工程學(xué)院，廣州510000；2.廣東工業(yè)大學(xué)物理與光電工程學(xué)院，廣州510000）

0 引言

移動(dòng)機(jī)器人行駛過程中對(duì)路面不平和障礙物的檢測(cè)是環(huán)境感知技術(shù)領(lǐng)域的重要研究方向。研究包括基于立體視覺的方法[1-2]、基于激光雷達(dá)的方法[3-4]和基于神經(jīng)網(wǎng)絡(luò)的方法[5-6]。室外檢測(cè)的主要目標(biāo)為建筑物、行人和車輛等，室內(nèi)檢測(cè)的主要目標(biāo)為墻壁和桌椅等。實(shí)際環(huán)境中的障礙物種類繁多，如減速帶和凹坑等處于移動(dòng)機(jī)器人盲區(qū)不能被識(shí)別。有學(xué)者利用圖像和結(jié)構(gòu)光對(duì)室外水體、凹凸障礙物特征進(jìn)行分析[7]，但易受光照等環(huán)境因素影響。3D激光雷達(dá)分布式安裝進(jìn)行凹障礙物特征檢測(cè)經(jīng)濟(jì)性和運(yùn)算量都存在問題[8]。上述研究均只能處理檢測(cè)而不具備構(gòu)建凹凸障礙物地圖的算法和能力。

移動(dòng)機(jī)器人同步定位與地圖構(gòu)建（SLAM）主流有激光SLAM和視覺同步定位與地圖構(gòu)建（VSLAM）兩種，激光SLAM又分2D和3D，其中2D激光SLAM利用2D激光雷達(dá)直立安裝檢測(cè)同水平面內(nèi)的障礙物并構(gòu)建環(huán)境地圖，但它存在路面盲區(qū)，忽略了地面平整性。3D激光SLAM和VSLAM都用于構(gòu)建三維點(diǎn)云地圖[9-10]，但點(diǎn)云地圖的目的是提高定位精度，避障導(dǎo)航則是使用點(diǎn)云地圖提取大型障礙物信息轉(zhuǎn)換而成2D柵格地圖，去掉了地面點(diǎn)云的平整性信息[11]。3D激光雷達(dá)和深度相機(jī)或雙目相機(jī)與2D激光雷達(dá)相比運(yùn)算開銷大，行內(nèi)多采用成熟的2D激光雷達(dá)技術(shù)[12]。目前無論哪種技術(shù)都沒有實(shí)現(xiàn)路面實(shí)時(shí)平整性檢測(cè)和凹凸障礙物地圖。

針對(duì)現(xiàn)有方法實(shí)時(shí)性弱、可靠性差和缺乏平整性信息等問題，本文提出一種用激光雷達(dá)實(shí)時(shí)檢測(cè)路面平整性和凹凸障礙物地圖構(gòu)建方法。通過對(duì)地面探測(cè)的數(shù)據(jù)進(jìn)行空間變換、標(biāo)簽并濾波獲得有效數(shù)據(jù)，對(duì)路面平整性進(jìn)行預(yù)測(cè)，結(jié)合慣性測(cè)量單元（IMU）和視覺里程計(jì)的最優(yōu)位姿，構(gòu)造凹凸障礙物地圖。

1 單線激光投影變換

平整性檢測(cè)使機(jī)器人在不同位姿下，通過多線激光測(cè)距進(jìn)行路面檢測(cè)，實(shí)時(shí)計(jì)算路面平整性用于即時(shí)避障，并提供構(gòu)建凹凸障礙物地圖的數(shù)據(jù)。多線激光是單線激光進(jìn)行剛性變換后的擴(kuò)展，以單線激光檢測(cè)作為最小檢測(cè)單元，建立數(shù)學(xué)模型計(jì)算理論距離與實(shí)際測(cè)量的比值，判斷測(cè)量點(diǎn)是凹陷障礙物還是凸起障礙物。假設(shè)世界坐標(biāo)系為(Xw,Yw,Zw)，機(jī)器人坐標(biāo)系為(Xr,Yr,Zr)；單線激光測(cè)距的激光束為世界坐標(biāo)系中兩點(diǎn)P0(x0,y0,z0)和P1(x1,y1,z1)所確定的一條射線P0P1，其中P0為射線的起始點(diǎn)，P1為預(yù)測(cè)的測(cè)量點(diǎn)，如圖1所示。

圖1 算法應(yīng)用示意圖

當(dāng)機(jī)器人坐標(biāo)系與世界坐標(biāo)系之間的投影僅有繞Zw軸旋轉(zhuǎn)和XwYw軸平移時(shí)，射線P0T P1T在XwYw平面上的投影長(zhǎng)度不變，此時(shí)LP0P1=LP0TP1'。機(jī)器人前輪最低點(diǎn)所在的XwYw平行面視為平整路面，雷達(dá)安裝高度z0為固定值，Zw軸平移量為0，只需計(jì)算XwYw方向上的旋轉(zhuǎn)變換，旋轉(zhuǎn)角由IMU測(cè)量獲得。

令P0P1變換后為：

由此確定射線P0TP1T的參數(shù)方程：

式（2）中，P為未知量，k為參數(shù)值。求射線P0TP1T與XwYw軸所成平面相交的點(diǎn)P1'T=k[P0T-P1T]+P1T|z=0（當(dāng)z=0時(shí)求得的值），求得平坦時(shí)的理論距離

設(shè)實(shí)際測(cè)得的距離為L(zhǎng)meas。①Lmeas>LP0TP'1時(shí)，機(jī)器人前方路面為凹陷路面；②Lmeas=LP0TP'1時(shí)，機(jī)器人前方路面為平坦路面；③Lmeas

圖2 單線投影算法流程

2 多線檢測(cè)與構(gòu)圖

在上文提出基本單元的平整性檢測(cè)算法基礎(chǔ)上，擴(kuò)展成多線激光測(cè)距模塊或激光雷達(dá)，結(jié)合IMU和視覺里程計(jì)位姿建立凹凸障礙物地圖。上文的P0和P1點(diǎn)為參考點(diǎn)和其他激光束存在變換關(guān)系A(chǔ)j：

由于IMU二次積分導(dǎo)致變換矩陣誤差大，所以構(gòu)圖中需結(jié)合視覺里程計(jì)獲取變換矩陣T。令第i時(shí)刻的IMU位姿xpose(i)為預(yù)測(cè)數(shù)據(jù)，視覺里程計(jì)位姿zobs(i)為觀測(cè)數(shù)據(jù)，利用非線性最小二乘計(jì)算最優(yōu)位姿。令f(xpose(i))=zobs(i)，i=0,1,2,3,…，目標(biāo)為最小化預(yù)測(cè)和觀測(cè)的差：

假設(shè)誤差服從高斯分布，令其對(duì)應(yīng)的信息矩陣為Ωi，觀測(cè)值誤差的平方定義為：

非線性最小二乘的目標(biāo)函數(shù)為：

求解目標(biāo)函數(shù)得到各時(shí)刻的最優(yōu)位姿，Topt(i)代表求得的第i時(shí)刻的最優(yōu)位姿，結(jié)合點(diǎn)云數(shù)據(jù)[PmeasT]i投影到世界坐標(biāo)中以獲得點(diǎn)云集合：

式（7）得到了原始的點(diǎn)云地圖。將所有激光測(cè)距的初始點(diǎn)[P0T,P1T]j，j=0,1,2,3,…根據(jù)各時(shí)刻的最優(yōu)位姿進(jìn)行單線投影變換計(jì)算出每個(gè)點(diǎn)云數(shù)據(jù)的理論距離LP0TP'1T實(shí)際距離Lmeas的比值，將該數(shù)據(jù)點(diǎn)定義為凹陷、凸起、平坦三種情況，并對(duì)每一個(gè)數(shù)據(jù)添加凹陷、凸起或平坦的標(biāo)簽屬性。根據(jù)標(biāo)簽篩選出所有的凹陷和凸起障礙物，獲得凹凸障礙物地圖。其算法流程如圖3所示。

圖3 多線構(gòu)圖算法流程

3 實(shí)驗(yàn)

3.1 平整性檢測(cè)

本研究用于實(shí)時(shí)檢測(cè)凹凸障礙物時(shí)，室內(nèi)的門檻、地面線槽和向下樓梯等可類比室外凹坑減速帶等凹凸障礙物，檢測(cè)原理一致。基于此原則，本研究在測(cè)試平臺(tái)上運(yùn)行Gazebo仿真軟件加載測(cè)試模型，添加實(shí)驗(yàn)障礙物，進(jìn)行仿真實(shí)驗(yàn)，測(cè)試平臺(tái)硬件信息如表1所示。

表1 測(cè)試平臺(tái)硬件信息

根據(jù)單線投影變換原理標(biāo)志后得到激光點(diǎn)云數(shù)據(jù)，利用標(biāo)簽進(jìn)行濾波，保留凹凸障礙物信息，其效果圖4所示。

圖4 投影變換效果圖

激光數(shù)據(jù)投影只提取表征凹凸障礙物距離的激光數(shù)據(jù)。圖4（b）是對(duì)圖4（a）的原始數(shù)據(jù)效果，經(jīng)過平整性檢測(cè)和濾波后的數(shù)據(jù)如圖4（c）。進(jìn)一步對(duì)不同類型的凸起障礙物進(jìn)行檢測(cè)，結(jié)果如下所述。

（1）對(duì)圖5（a）（b）（c）凸起障礙物檢測(cè)試驗(yàn)結(jié)果如圖6（a）（b）（c）所示，本研究能檢測(cè)出移動(dòng)機(jī)器人前進(jìn)方向中處于激光雷達(dá)安裝位置下方的凸起障礙物，包括圖6（a）木板、圖6（b）圓臺(tái)障礙物和圖6（c）小球。此木板等類障礙物類比門檻、地毯和玩具等室內(nèi)障礙物，亦可類比減速帶和石頭等室外障礙物。

圖5 Gazebo凸起障礙物仿真場(chǎng)景

圖6 凸起障礙物可視化檢測(cè)效果

（2）對(duì)圖7（a）（b）凹陷障礙物檢測(cè)效果如圖8（a）（b）所示，本研究能檢測(cè)出路面凹陷障礙物。選取的圖7（a）長(zhǎng)方體凹陷、圖7（b）半球體凹陷類比凹坑和路面斷層等室外凹陷障礙物。

圖7 Gazebo凹陷障礙物仿真場(chǎng)景

圖8 凹陷障礙物可視化激光檢測(cè)效果

3.2 構(gòu)建地圖

（1）本研究在Gazebo的大型和凹凸障礙物仿真場(chǎng)景地圖構(gòu)建效果如圖9（a）（b）所示。在如圖9（a）仿真環(huán)境中，根據(jù)本文方法進(jìn)行凹凸障礙物檢測(cè)后構(gòu)建的地圖包含所有物體，如圖9（b）。

圖9 本算法建圖效果

（2）在相同仿真場(chǎng)景下直立安裝的2D激光雷達(dá)的地圖構(gòu)建效果，如圖10（a）（b）所示。在圖10（b）的障礙物地圖中僅存在一個(gè)障礙物，不能表征整個(gè)仿真環(huán)境。

圖10 直立安裝2D激光雷達(dá)效果

（3）在仿真場(chǎng)景下深度相機(jī)的地圖構(gòu)建效果，如圖11（a）（b）（c）所示。在圖11（b）的3D點(diǎn)云地圖中可見兩種凸起障礙物的模型，兩種凹陷障礙物存在形狀上的空缺，沒有點(diǎn)云數(shù)據(jù)。在圖11（c）障礙物地圖中也僅包含一個(gè)障礙物，不能表征整個(gè)仿真環(huán)境。

圖11 深度相機(jī)效果

各方法的建圖實(shí)驗(yàn)結(jié)果如表2所示。

表2 實(shí)驗(yàn)結(jié)果對(duì)比

2D激光雷達(dá)傳感器基于gmapping算法創(chuàng)建的障礙物地圖，只包含雷達(dá)安裝平面的大型障礙物，缺失了地面小型和凹陷障礙物。深度相機(jī)基于rtabmap在3D點(diǎn)云地圖中包含了門和球形兩類障礙物體，缺少凹陷障礙物，轉(zhuǎn)換成障礙物地圖時(shí)，由于球形障礙物體積小，凹陷障礙物點(diǎn)云缺失，只保留了大型障礙物。本算法的障礙物地圖除包含大型障礙物外，還包含了地面小型凹凸障礙物。實(shí)驗(yàn)結(jié)果表明，前面兩傳感器和它們的算法不具備構(gòu)建包含小型和凹陷障礙物的導(dǎo)航地圖的能力；本算法應(yīng)用于機(jī)器人平整性檢測(cè)和地圖構(gòu)建時(shí)，具備構(gòu)建小型和凹陷障礙物的導(dǎo)航地圖的能力，比其他算法含有更多的障礙物信息，具備更強(qiáng)大的導(dǎo)航能力。

4 結(jié)語

在實(shí)際應(yīng)用中，移動(dòng)機(jī)器人在建圖導(dǎo)航時(shí)僅考慮XY坐標(biāo)和偏航（Yaw），缺乏地面平整性和凹凸障礙物信息，這對(duì)機(jī)器人移動(dòng)過程中的位姿將產(chǎn)生重要影響。室內(nèi)的門檻和往下樓梯等區(qū)域不包含在導(dǎo)航地圖中，對(duì)移動(dòng)機(jī)器人存在威脅；室內(nèi)外的不平路面對(duì)機(jī)器人移動(dòng)位姿的干擾同樣也會(huì)影響建圖精度。本文從移動(dòng)機(jī)器人不同姿態(tài)時(shí)對(duì)地面凹凸障礙物檢測(cè)的數(shù)學(xué)原理和計(jì)算流程開展研究，首次提出構(gòu)建凹凸障礙物地圖并實(shí)現(xiàn)路面實(shí)時(shí)平整性預(yù)測(cè)。與其他方法相比，本研究使機(jī)器人在復(fù)雜的環(huán)境中能夠感知更多的障礙物信息，增加了小型和凹型障礙物的信息，并直接納入導(dǎo)航地圖，使移動(dòng)機(jī)器人可依靠障礙物地圖避開危險(xiǎn)區(qū)域。本研究提供的方法計(jì)算量少，經(jīng)濟(jì)性和實(shí)時(shí)性好，很好地解決了導(dǎo)航地圖的信息缺失問題。