基于激光SLAM的移動機器人的改進實現

沈一鳴，趙希宇

(沈陽工業大學 機械工程學院，遼寧 沈陽 110870)

0 引言

移動機器人的同時定位與建圖(Simultaneous Localization And Mapping，SLAM)是將定位與地圖創建合二為一，是當今移動機器人研究的一個核心問題，也是后續路徑規劃與運動控制的基礎[1-3]。當前，與視覺SLAM相比，激光SLAM已經相對比較成熟，在移動機器人的實現上也比較方便。本文以實現激光SLAM的移動機器人平臺為基礎進行改進研究，分析比較以激光測距儀獲取掃描數據和以用Kinect獲取的深度圖像信息轉化為二維掃描數據來實現激光SLAM算法的優劣性。綜合考慮激光測距儀掃描數據的更高準確性以及Kinect對三維場景的認識，對基于激光SLAM的移動機器人的實現提出改進方案。當移動機器人同時配備激光測距儀和Kinect時，可以得到較高質量的環境地圖和較準確的定位信息以及通過復雜環境的能力。

1 SLAM的相關模型

1.1 移動機器人運動模型

本實驗選取差分驅動的移動機器人進行激光SLAM算法的實現，并考慮系統輸入噪聲。

在Δt時間內，以恒定線速度v和角速度ω作為系統輸入，用W(k)表示輸入的零均值的高斯白噪聲，X(k)表示機器人在k時刻的位姿(xk,yk,θk)T，X(k-1)表示機器人在上一時刻的位姿(xk-1,yk-1,θk-1)T，u(k)表示系統的控制輸入(v,ω)T，則它們滿足如下的關系：

X(k)=f(X(k-1),u(k))+W(k).

(1)

(2)

1.2 基于激光測距儀的觀測模型

若采用激光測距儀作為觀測傳感器，假設在k時刻移動機器人的位姿由(xt(k),yt(k),θt(k))T給定，此時機器人通過激光測距儀探測到的第i個特征位置用(xi,yi)表示，特征的總數為N，那么可以獲得的k時刻傳感器的觀測信息Z(k)為：

(3)

2 Kinect深度圖數據處理

2.1 深度圖像信息轉化為點云圖數據

圖1為透視投影關系，已知O-XYZ為相機坐標系，O1-xy為圖像坐標系，圖像坐標系到相機坐標系之間的距離為f，相機坐標系的原點定為相機中心點C，相機中心點C到圖像坐標系的垂線為相機主軸，相機主軸與圖像坐標系的交點c為主點。

假設圖像坐標系中的點p(x,y)是由場景空間中的點P(X,Y,Z)通過投影關系得到，水平像素焦距和垂直像素焦距分別為fx和fy，主點c的坐標為(cx,cy)。相機坐標系中的點可以經過一個旋轉變換和一個平移變換轉化到圖像坐標系中，由透視投影關系可以得到如下的關系式：

(4)

其中：S為比例系數；R為旋轉矩陣；T為平移矩陣；fx、fy、cx、cy均為相機的內部參數。

相機的內部參數可以通過對Kinect的標定來獲得，也可以取默認值：S=5 000，cx=320，cy=240，fx=fy=525，但是會與標定值有稍許偏差。

圖1 透視投影關系

2.2 深度圖像信息轉化為激光測距儀掃描信息

圖2為深度圖像信息轉化為激光測距儀掃描信息的原理，O′-MN平面是經過點P(X,Y,Z)且平行于O-XY平面的平面，其中點O′經過OZ軸，AP平行于OY軸，BP平行于OX軸。已知OO′的距離為dOO′，O′A的距離為dO′A，O′B的距離為dO′B，由三角關系可以求得：

∠AOO′=arctan(dO′A/dOO′).

(5)

假設激光數據的角度范圍為[α,β]，激光束按角度共被分為K份，用laser[m]表示第m條激光束的數據。由此可以得出點P對應的激光束的索引值mP和激光束具體的掃描數據值laser[mP]分別為：

mP=K(∠AOO′-α)/(β-α).

(6)

(7)

圖2 深度圖像信息轉化為激光測距儀掃描信息的原理

3 傳感器性能對比

移動機器人系統中，硬件的精度直接影響算法的效果。在相同的基于激光SLAM算法框架中，激光測距儀的精度直接影響后驗概率。激光測距儀的精度越高，那么基于傳感器的觀測信息越能表現出真實環境的信息，而移動機器人也能獲得對環境一個更好的認識。表1為URG-04LX-UG01型號的激光測距儀和Kinect的硬件參數對比。

由表1可以發現：基于水平面內的測量，激光測距儀推薦的測量距離范圍較Kinect的測量距離范圍廣，并且最近測量距離也比Kinect優越很多，有利于機器人通過傳感器獲取近距離的觀測信息，因此有利于后續移動機器人同時建圖與定位的實現；Kinect可以獲取三維信息，這是激光測距儀所不具有的功能。

表1 URG-04LX-UG01型號的激光測距儀和Kinect的硬件參數對比

4 基于不同傳感器的激光SLAM效果對比

為了比較激光測距儀和Kinect兩者應用于激光SLAM的效果，以FASTSLAM 2.0作為激光SLAM算法進行測試，分別比較分析以激光測距儀的掃描數據和由Kinect深度信息轉換而來的掃描數據作為系統的輸入獲得的環境地圖質量，以及對移動機器人定位準確性的影響。

移動機器人采用Kobuki基座進行運動控制，同時配備激光測距儀和Kinect傳感器，并分別記錄在相同系統狀態下激光測距儀掃描數據和由Kinect深度信息轉換而來的掃描數據。之后，把通過激光測距儀獲得的數據和Kinect獲得的數據作為不同觀測，基于FASTSLAM 2.0進行測試。

圖3和圖4分別為基于激光測距儀數據和Kinect數據獲得的環境地圖，測試的環境是一個樓梯間、兩個衛生間和一部分走廊。通過圖3與圖4的比較可以發現，基于激光測距儀數據獲得的環境地圖較基于Kinect數據獲得的環境地圖質量更好，更貼合實際環境。

圖3由激光測距儀數據獲得的環境地圖圖4由Kinect數據獲得的環境地圖

圖5為移動機器人實際位置以及對應點基于激光測距儀數據和Kinect數據的估計位置的采樣點形成的路徑曲線。由圖5可以發現，以激光測距儀數據估計的機器人位置與實際機器人位置的誤差比以Kinect數據估計的機器人位置與實際機器人位置的誤差小，前者的軌跡更接近于機器人實際的軌跡。因此，實驗表明，以激光測距儀數據作為觀測信息較以Kinect數據作為觀測信息能得到更好的定位效果。

綜合考慮采用不同傳感器數據作為觀測得到的移動機器人定位與建圖的效果，采用激光測距儀數據作為觀測時，機器人的定位與創建的環境地圖明顯優于采用Kinect數據作為觀測時的結果。實驗表明：激光測距儀與Kinect相比，更適合作為獲取激光SLAM觀測信息的傳感器。

5 導航

以往基于激光SLAM的移動機器人通常只配備激光測距儀，但激光測距儀存在局限性(如圖6所示)，它只能得到激光測距儀激光束所在平面內的障礙物信息，而不能考慮三維環境的信息。因此，在移動機器人導航時，當地面上存在激光測距儀不能檢測到的障礙物時，由于缺少環境的信息，移動機器人導航時會認為區域安全而撞上障礙物，最終導致導航失敗。

圖5 移動機器人的路徑曲線比較

圖6 激光測距儀局限性說明

為了克服激光測距儀不能獲取三維環境信息的缺點，將Kinect配置到移動機器人上，用Kinect將三維環境信息的點云數據轉化為激光測距儀對應的掃描數據，在導航時，結合激光測距儀的掃描數據和由Kinect轉化而來的掃描數據，使得機器人具備能夠順利通過復雜環境的能力，改進后移動機器人在已知環境中的導航結果如圖7所示。當機器人結合Kinect的數據時，會綜合考慮三維環境的信息，從而成功避開障礙物，最終實現有效的移動機器人導航。

圖7 移動機器人在已知環境中的導航結果

6 結語

本文將激光測距儀和Kinect同時配備在移動機器人上，作為獲取外界環境信息的手段。同時配備激光測距儀和Kinect的移動機器人繼承了基于激光測距儀數據的激光SLAM的優越性，同時彌補了激光測距儀不能獲取三維環境信息的不足。一方面，以激光測距儀數據作為基于激光SLAM的移動機器人的觀測信息，可以獲得較高的定位精度和建圖效果；另一方面，將激光測距儀數據和Kinect數據綜合考慮用于移動機器人導航，可以使移動機器人獲得通過復雜環境的能力。