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

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

0 引言

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

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

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

1 單線激光投影變換

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

圖1 算法應用示意圖

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

令P0P1變換后為：

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

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

設實際測得的距離為Lmeas。①Lmeas>LP0TP'1時，機器人前方路面為凹陷路面；②Lmeas=LP0TP'1時，機器人前方路面為平坦路面；③Lmeas

圖2 單線投影算法流程

2 多線檢測與構圖

在上文提出基本單元的平整性檢測算法基礎上，擴展成多線激光測距模塊或激光雷達，結合IMU和視覺里程計位姿建立凹凸障礙物地圖。上文的P0和P1點為參考點和其他激光束存在變換關系Aj：

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

假設誤差服從高斯分布，令其對應的信息矩陣為Ωi，觀測值誤差的平方定義為：

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

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

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

圖3 多線構圖算法流程

3 實驗

3.1 平整性檢測

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

表1 測試平臺硬件信息

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

圖4 投影變換效果圖

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

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

圖5 Gazebo凸起障礙物仿真場景

圖6 凸起障礙物可視化檢測效果

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

圖7 Gazebo凹陷障礙物仿真場景

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

3.2 構建地圖

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

圖9 本算法建圖效果

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

圖10 直立安裝2D激光雷達效果

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

圖11 深度相機效果

各方法的建圖實驗結果如表2所示。

表2 實驗結果對比

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

4 結語

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