移動機器人同時定位與建圖室內導航地圖研究

楊蘊秀， 韓寶玲*， 羅 霄

(1.北京理工大學機械與車輛學院， 北京 100081； 2. 北京理工大學計算機學院， 北京 100081)

機器人要實現自主運動就需要對周圍的場景及自身在場景中的位置具有感知能力。同時定位與建圖(SLAM)技術可以幫助機器人在未知環境中探索位置信息，提供運動軌跡，并對周圍環境進行場景描述[1]。二維占據柵格地圖是移動機器人在路徑規劃和自主導航中應用最為廣泛的地圖[2-4]。當前可以通過激光SLAM或者視覺SLAM對環境進行感知，進而構造環境二維柵格地圖。使用2D激光雷達SLAM可以直接創建二維柵格地圖，但是該地圖只包含環境在該平面上的幾何信息，不能完整表達空間中的障礙物信息[5-6]。使用3D激光雷達SLAM和視覺SLAM需要將生成的空間點云進行投影獲得二維柵格地圖[7]。由于點云對空間點云進行投影的關鍵在于將地面與障礙物進行分離。當前主要采取的做法是識別地面區域，使用隨機采樣一致性(random sample consensus，RANSAC)算法擬合求解地平面，將不滿足地面方程的點云視為障礙物，并沿地面平面的法向量進行投影[8-11]。Chen等[12]提出將3D點云用切片的方式進行高度投影得到鳥瞰圖，按圓柱坐標系進行投影得到前視圖。哈爾濱工程大學孫相宇在地圖中提取直線特征組成特征空間并進行基于傳統概率模型的模式識別對障礙物進行識別，將識別后的障礙物擬合為一個平面，并將柵格地圖投影到該平面上[13]。融合慣性測量單元(inertial measurement unit， IMU)的激光雷達系統，從點云數據構建出八叉樹，根據水平坐標對其姿態進行矯正，然后基于八叉樹從點云中分割出地面點，采用RANSAC算法提取地面部分的特征[14]。這些方法進行地面分離時需要場景中存在地平面或者關鍵幀需具有豐富的地面信息，然而室內移動機器人受限于其工作場景和自身結構，在某些視角下不能觀測到地面，采用地面擬合的方法無法有效進行地面障礙物的分離。現使用RGB-D(red green blue-depth)相機通過ORB_SLAM2(Oriented FAST and Rotated BRIEF_SLAM2)[15]算法生成三維稠密點云；從室內移動機器人的運動特點和結構特點出發，討論相機生成點云坐標系與真實空間地平面的關系，提出一種降維投影制備二維柵格地圖的方法。后期實現對當前SLAM的二維占據柵格地圖制備的有益補充。

1 SLAM三維點云地圖生成

使用RGB-D相機通過SLAM算法對相機的位姿進行準確的估計，并將采集到的帶有深度信息的圖像恢復出物體的位置和尺度，繼而生成稠密的三維點云地圖，為后續處理提供初始數據。

1.1 相機位姿估計

相鄰圖像進行信息對比，通過提取、匹配圖像特征點估計出兩幀之間的相機運動。對兩幅RGB-D圖像采用ORB特征進行特征角點提取，通過特征匹配得到兩組一一對應的特征點

(1)

(2)

式中：k、k+1為相鄰的前后兩張圖像；p為特征點的坐標；P為特征點的集合。兩幀之間的相機位姿變換用李代數表示:

(3)

對應匹配點之間的關系和重投影誤差為

(4)

(5)

由于誤匹配、噪聲等因素重投影誤差并不為0，尋找一個相機位姿使得所有點對的誤差平方和達到極小，即求解目標函數:

(6)

得到連續兩幀圖像之間的位姿變換關系。

1.2 SLAM后端優化

對重投影誤差迭代求解，可以得到一段時間內的相機運動軌跡，然而測量噪聲以及各種因素造成的不可避免的影響，使得誤差不斷的傳遞累計。較長時間內所得到的運動軌跡并不準確。SLAM過程中的運動和觀測方程為

(7)

式(7)中:ξk為k時刻相機的位姿；Pj為觀測到的空間路標點；zk，j表示在k時刻相機觀測到路標點pj產生的觀測數據；uk為k時刻傳感器的輸入；wk為k時刻運動的噪聲；vk，j為在k時刻觀測里的噪聲。假設位姿ξ和路標p為服從某種概率分布的隨機變量，均為待估計的變量，記k時刻的所有未知量為Xk：

(8)

得到新時刻的數據后需要對原有的概率分布進行估計。

由貝葉斯法則得:

P(Xk|X0，u1:k，z1:k)∝P(zk|Xk)P(Xk|X0，

u1:k，z1:k-1)

(9)

觀測數據是像素坐標

z?(us，vs)T

(10)

觀測的誤差為e=z-h(p，ξ)，整體的代價函數為

(11)

對所有待優化變量有一個整體增量ΔX，代價函數變為

FijΔξi+EijΔPj‖2

(12)

式(12)中：Fij為整個代價函數在當前狀態下對相機姿態的偏導;Eij為代價函數對路標點位置的偏導。對增量方程進行求解，得到更加精確的相機位姿ξ。

1.3 相機坐標系下三維點云的獲得

深度相機可以獲得像素深度，其成像模型如圖1所示，在相機坐標系中真實場景點P(xc，yc，zc)，經過相機投影得到RGB-D圖像中的坐標P′(u，v，d)對應成像平面的坐標為(x，y)。其中下標c表示是在相機坐標系下。

圖1 RGB-D針孔相機成像模型

由投影模型可得:

(13)

式(13)中:fx、fy、cx、cy為相機的內參；zc=d。

Pc的坐標如下:

(14)

2 三維點云的降維投影

2.1 點云的坐標轉換

將室內移動機器人在由視覺SLAM進行位姿估計和構建點云地圖過程中所接觸的地面視為平面。記世界坐標系Ow=[iw，jw，kw]T，相機坐標系Oc=[ic，jc，kc]T，初始時刻的相機坐標系為Oc0=[ic0，jc0，kc0]T，Oc0與Ow重合。車體中心坐標系Ob=[ib，jb，kb]T，車體在地面上行進時各坐標系的對應關系如圖2所示。

為了便于表述，用坐標原點代指相應的坐標系。OE是以地面為xoy平面，z軸向上的任意一個坐標系。

移動機器人的相機通常有兩種固定方式:①安裝在車體外殼的支架上；②安裝在移動機器人的機械臂關節末端執行器的某一固連支架處；對這兩種情況進行如下分析:

(15)

該過程中點云的坐標轉換如圖3中帶箭頭的紅色線條所示。圖3中車體中心坐標系Ob、相機坐標系Oc、世界坐標系Ow、自選坐標系OE分別用白色、深紅色、綠色、黑色的坐標系表示。

圖3 點云坐標轉換圖

將點云P在相機坐標下的位置帶入，即可得到

(16)

2.2 障礙物點云數據的篩選

首先需要根據機器人自身大小判斷空間中的物體是否處于其工作空間中，若在范圍以外可直接截去。記車體中心距離地面的高度為d，相機距離車體中心的高度為h，車體中心到機器人工作空間最高高度為l。初始時刻，相機位置為

POC=(xOC，yOC，h+d)T

(17)

空間中觀測到的任意點云位置為

Ppt=(x，y，z)T

(18)

為了能夠完整截取地平面，增加閾值σ。若移動平臺無越障能力，可將σ設置為較小的值，若有越障能力，可將σ設置為越障高度。當z∈ (σ，d+l+σ)，則Ppt為空間中的障礙物。

對障礙物范圍內的點云進行截取并投影，可以反映室內場景中障礙物的分布。先使用直通濾波器(pass through)截取再使用點云庫(point cloud library， PCL)中的參數投影模型(model coefficients)處理可保留環境三維信息，簡化后期需要處理的數據。

由于RGB-D相機的測量可靠性受深度范圍、外部環境和被測物體表面特性等諸多因素的影響，在預處理階段需要對點云進行去噪。若點云過于稀疏，則沒有實際的應用價值。采用半徑濾波器(radius outlier removal)，指定球體的半徑和球內點的數目分別為r和n，計算以點云中的某一點為球心畫圓落在此球體中的數量，若數量大于給定的值n則保留此點，否則剔除。通過設置單元空間內的點云數據濃度要求，篩選點云。其流程如圖4所示。

圖4 點云數據投影篩選流程

3 室內導航地圖制備

3.1 利用點云數據構建柵格地圖

根據RGB-D相機信息，將第2.1節得到的點云坐標轉化至柵格地圖。記柵格地圖中每一格的長度為R(R>r)，分辨率為1/R，則有

(19)

為對應障礙物在柵格地圖中的位置。由于點云是離散的，不能完整的表達障礙物的占據信息，需要根據該柵格內點云數量N的多少，判定其占據狀態。

對地圖中任意柵格的狀態s分兩種情況:有障礙物(occupied，記s= 1)，無障礙物(free，記s= 0)。

(20)

同時將該分辨率下的地圖保存為灰度圖，s= 1設置為黑色，s= 0設置為白色。

3.2 將地圖轉為機器人可用的柵格地圖

經過上述處理地圖信息得到完整保存。由于實際應用中移動機器人有一定的尺寸，需要以機器人的內切圓半徑為依據，對生成地圖里的障礙物進行膨脹，使得不可行區域的外圍有一定程度的擴展。最后利用四聯通的準則連接生成地圖，可以在該地圖上運行路徑規劃算法。

4 實驗與分析

以筆者所在工作室為實驗場所，空間為5.20 m×4.05 m，各種常用物品作為障礙物分散分布。采用的實驗設備為Intel的Realsense D435i深度相機，使用一對立體紅外相機和彩色相機輸出具有顏色和深度信息的圖像，標準鏡頭視角45°，測量的深度距離0.1～10 m。將相機固連于移動平臺，移動機器人環游一周構生成點云及二維柵格地圖。

4.1 視覺SLAM制備室內導航地圖

實驗采用ORB_SLAM2算法生成稠密地圖，將位姿估計過程中的關鍵幀及對應的位姿作為點云生成的初始數據。根據所用機器人平臺的高度，由第2.2節的參數設定取h=68 cm，l=20 cm，σ=3 cm。

4.1.1 地面與障礙物分離

使用視覺SLAM的三維點云生成二維柵格的一個主要問題在于分離地面和障礙物。當前常采用語義SLAM或地面提取的方法[8-11]進行分離。其中地面提取常采用RANSAC隨機采樣一致性算法，設置地平面模型，對地面點云進行平面擬合，實現地面點云和非地面點云的分離。現對本文方法和采用RANSAC算法提取地平面的分離效果進行試驗。

實驗場所如圖5(a)所示，對該環境中的地面和障礙物選取10個觀測點，其中4個觀測點位于地面，6個觀測點散布于障礙物和障礙物平面，如圖5(b)所示。使移動機器人從不同位置出發環游室內，創建點云地圖截去地面，各自重復10次。

圖5 實驗場景圖

使用本文方法，均能截去地面觀測點，保留障礙物觀測點，實現正確分離。采用RANSAC算法提取地面有三次截去了障礙物及相應平面的物體，遺漏了地面觀測點，正確分離的概率為70%。

4.1.2 柵格地圖創建

由第4.1.1節所得的點云俯視圖如圖6所示。

圖6 去除地面點云俯視圖

經過投影和柵格化后得到二維占據柵格地圖如圖7所示，可以看到地圖較為清楚和直觀地展示了室內障礙物和可通行區域的分布。

采用長度誤差和角度誤差作為地圖精度測量指標，從圖7中取出四個測量線段和兩個測量夾角，對比實際場景，結果如表1所示。實驗場地中障礙物真實占用率為τ，柵格占據地圖中障礙物占用率為τ′，使用τ′/τ判斷障礙物識別的完整度。參考地面分離原論文[9]的定量比較結果，針對不同建圖方法的對比如表2所示。

由表1可知，建圖長度誤差整體均較小，角度誤差在0.8以內。根據表2可知，本文建圖每米誤差低于0.015 5，相較激光雷達建圖具有較高精度。地面分離及障礙物截取較為完整，建圖角度誤差較小。

表2 不同建圖方法精度對比

4.2 地圖的路徑規劃應用

將此柵格地圖提供給移動平臺的自動導航系統，任意設置起始位置和初始姿態，選定終點位置和終止姿態，通過全局路徑規劃和局部優化算法，在仿真軟件中生成了一條綠色的虛擬軌跡，如圖8所示。在全局導航地圖已知，障礙物信息較少改變的環境中，機器人與障礙物不發生碰撞，依據生成的最優路徑繞過障礙物到達目標位置。其運動過程如圖9所示。

圖8 Rviz窗口仿真顯示及路徑規劃放大圖

圖9 移動平臺自動導航運動

上述實驗表明，采用本文方法生成的地圖適用于室內移動機器人的導航，作為路徑規劃的地圖依據。由視覺SLAM生成環境的柵格地圖，并對機器人的運動進行實時定位，可以為移動機器人的自主運動提供良好的信息輸入。

4.3 視覺SLAM構建柵格地圖實驗的分析

在地面和障礙物分離中采用RANSAC地面提取方法要求圖片中含有豐富的地面信息，同時存在將大面積的墻壁錯誤擬合的風險。根據移動機器人的結構和運動特點、機械臂運動學轉換關系，將點云坐標轉換到地面坐標，利用高度實現地面和障礙物的分離，是一種有效解決方法。使用視覺SLAM生成的二維柵格地圖還可有效識別懸垂障礙物，具有更高的障礙物識別度。

5 結論

從移動機器人在自主導航中對二維地圖的實際需求出發，通過視覺SLAM提出了一種導航地圖制備方法。依據室內移動機器人的運動特點和結構特點對點云進行坐標轉換，一方面實現了地平面的有效截取，另一方面對點云投影進行了有序柵格化。對地面截取的效率和創建柵格地圖的精度、完整度進行了實驗評估，并在所建柵格地圖上進行了仿真導航實驗。實驗結果表明，地面截取效率高于RANSAC地面擬合截取方法，建圖精度與完整度均高于傳統建圖方法。