基于激光SLAM的移動(dòng)機(jī)器人的改進(jìn)實(shí)現(xiàn)

沈一鳴，趙希宇

(沈陽(yáng)工業(yè)大學(xué) 機(jī)械工程學(xué)院，遼寧 沈陽(yáng) 110870)

0 引言

移動(dòng)機(jī)器人的同時(shí)定位與建圖(Simultaneous Localization And Mapping，SLAM)是將定位與地圖創(chuàng)建合二為一，是當(dāng)今移動(dòng)機(jī)器人研究的一個(gè)核心問(wèn)題，也是后續(xù)路徑規(guī)劃與運(yùn)動(dòng)控制的基礎(chǔ)[1-3]。當(dāng)前，與視覺(jué)SLAM相比，激光SLAM已經(jīng)相對(duì)比較成熟，在移動(dòng)機(jī)器人的實(shí)現(xiàn)上也比較方便。本文以實(shí)現(xiàn)激光SLAM的移動(dòng)機(jī)器人平臺(tái)為基礎(chǔ)進(jìn)行改進(jìn)研究，分析比較以激光測(cè)距儀獲取掃描數(shù)據(jù)和以用Kinect獲取的深度圖像信息轉(zhuǎn)化為二維掃描數(shù)據(jù)來(lái)實(shí)現(xiàn)激光SLAM算法的優(yōu)劣性。綜合考慮激光測(cè)距儀掃描數(shù)據(jù)的更高準(zhǔn)確性以及Kinect對(duì)三維場(chǎng)景的認(rèn)識(shí)，對(duì)基于激光SLAM的移動(dòng)機(jī)器人的實(shí)現(xiàn)提出改進(jìn)方案。當(dāng)移動(dòng)機(jī)器人同時(shí)配備激光測(cè)距儀和Kinect時(shí)，可以得到較高質(zhì)量的環(huán)境地圖和較準(zhǔn)確的定位信息以及通過(guò)復(fù)雜環(huán)境的能力。

1 SLAM的相關(guān)模型

1.1 移動(dòng)機(jī)器人運(yùn)動(dòng)模型

本實(shí)驗(yàn)選取差分驅(qū)動(dòng)的移動(dòng)機(jī)器人進(jìn)行激光SLAM算法的實(shí)現(xiàn)，并考慮系統(tǒng)輸入噪聲。

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

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

(1)

(2)

1.2 基于激光測(cè)距儀的觀測(cè)模型

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

(3)

2 Kinect深度圖數(shù)據(jù)處理

2.1 深度圖像信息轉(zhuǎn)化為點(diǎn)云圖數(shù)據(jù)

圖1為透視投影關(guān)系，已知O-XYZ為相機(jī)坐標(biāo)系，O1-xy為圖像坐標(biāo)系，圖像坐標(biāo)系到相機(jī)坐標(biāo)系之間的距離為f，相機(jī)坐標(biāo)系的原點(diǎn)定為相機(jī)中心點(diǎn)C，相機(jī)中心點(diǎn)C到圖像坐標(biāo)系的垂線為相機(jī)主軸，相機(jī)主軸與圖像坐標(biāo)系的交點(diǎn)c為主點(diǎn)。

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

(4)

其中：S為比例系數(shù)；R為旋轉(zhuǎn)矩陣；T為平移矩陣；fx、fy、cx、cy均為相機(jī)的內(nèi)部參數(shù)。

相機(jī)的內(nèi)部參數(shù)可以通過(guò)對(duì)Kinect的標(biāo)定來(lái)獲得，也可以取默認(rèn)值：S=5 000，cx=320，cy=240，fx=fy=525，但是會(huì)與標(biāo)定值有稍許偏差。

圖1 透視投影關(guān)系

2.2 深度圖像信息轉(zhuǎn)化為激光測(cè)距儀掃描信息

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

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

(5)

假設(shè)激光數(shù)據(jù)的角度范圍為[α,β]，激光束按角度共被分為K份，用laser[m]表示第m條激光束的數(shù)據(jù)。由此可以得出點(diǎn)P對(duì)應(yīng)的激光束的索引值mP和激光束具體的掃描數(shù)據(jù)值laser[mP]分別為：

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

(6)

(7)

圖2 深度圖像信息轉(zhuǎn)化為激光測(cè)距儀掃描信息的原理

3 傳感器性能對(duì)比

移動(dòng)機(jī)器人系統(tǒng)中，硬件的精度直接影響算法的效果。在相同的基于激光SLAM算法框架中，激光測(cè)距儀的精度直接影響后驗(yàn)概率。激光測(cè)距儀的精度越高，那么基于傳感器的觀測(cè)信息越能表現(xiàn)出真實(shí)環(huán)境的信息，而移動(dòng)機(jī)器人也能獲得對(duì)環(huán)境一個(gè)更好的認(rèn)識(shí)。表1為URG-04LX-UG01型號(hào)的激光測(cè)距儀和Kinect的硬件參數(shù)對(duì)比。

由表1可以發(fā)現(xiàn)：基于水平面內(nèi)的測(cè)量，激光測(cè)距儀推薦的測(cè)量距離范圍較Kinect的測(cè)量距離范圍廣，并且最近測(cè)量距離也比Kinect優(yōu)越很多，有利于機(jī)器人通過(guò)傳感器獲取近距離的觀測(cè)信息，因此有利于后續(xù)移動(dòng)機(jī)器人同時(shí)建圖與定位的實(shí)現(xiàn)；Kinect可以獲取三維信息，這是激光測(cè)距儀所不具有的功能。

表1 URG-04LX-UG01型號(hào)的激光測(cè)距儀和Kinect的硬件參數(shù)對(duì)比

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

為了比較激光測(cè)距儀和Kinect兩者應(yīng)用于激光SLAM的效果，以FASTSLAM 2.0作為激光SLAM算法進(jìn)行測(cè)試，分別比較分析以激光測(cè)距儀的掃描數(shù)據(jù)和由Kinect深度信息轉(zhuǎn)換而來(lái)的掃描數(shù)據(jù)作為系統(tǒng)的輸入獲得的環(huán)境地圖質(zhì)量，以及對(duì)移動(dòng)機(jī)器人定位準(zhǔn)確性的影響。

移動(dòng)機(jī)器人采用Kobuki基座進(jìn)行運(yùn)動(dòng)控制，同時(shí)配備激光測(cè)距儀和Kinect傳感器，并分別記錄在相同系統(tǒng)狀態(tài)下激光測(cè)距儀掃描數(shù)據(jù)和由Kinect深度信息轉(zhuǎn)換而來(lái)的掃描數(shù)據(jù)。之后，把通過(guò)激光測(cè)距儀獲得的數(shù)據(jù)和Kinect獲得的數(shù)據(jù)作為不同觀測(cè)，基于FASTSLAM 2.0進(jìn)行測(cè)試。

圖3和圖4分別為基于激光測(cè)距儀數(shù)據(jù)和Kinect數(shù)據(jù)獲得的環(huán)境地圖，測(cè)試的環(huán)境是一個(gè)樓梯間、兩個(gè)衛(wèi)生間和一部分走廊。通過(guò)圖3與圖4的比較可以發(fā)現(xiàn)，基于激光測(cè)距儀數(shù)據(jù)獲得的環(huán)境地圖較基于Kinect數(shù)據(jù)獲得的環(huán)境地圖質(zhì)量更好，更貼合實(shí)際環(huán)境。

圖3由激光測(cè)距儀數(shù)據(jù)獲得的環(huán)境地圖圖4由Kinect數(shù)據(jù)獲得的環(huán)境地圖

圖5為移動(dòng)機(jī)器人實(shí)際位置以及對(duì)應(yīng)點(diǎn)基于激光測(cè)距儀數(shù)據(jù)和Kinect數(shù)據(jù)的估計(jì)位置的采樣點(diǎn)形成的路徑曲線。由圖5可以發(fā)現(xiàn)，以激光測(cè)距儀數(shù)據(jù)估計(jì)的機(jī)器人位置與實(shí)際機(jī)器人位置的誤差比以Kinect數(shù)據(jù)估計(jì)的機(jī)器人位置與實(shí)際機(jī)器人位置的誤差小，前者的軌跡更接近于機(jī)器人實(shí)際的軌跡。因此，實(shí)驗(yàn)表明，以激光測(cè)距儀數(shù)據(jù)作為觀測(cè)信息較以Kinect數(shù)據(jù)作為觀測(cè)信息能得到更好的定位效果。

綜合考慮采用不同傳感器數(shù)據(jù)作為觀測(cè)得到的移動(dòng)機(jī)器人定位與建圖的效果，采用激光測(cè)距儀數(shù)據(jù)作為觀測(cè)時(shí)，機(jī)器人的定位與創(chuàng)建的環(huán)境地圖明顯優(yōu)于采用Kinect數(shù)據(jù)作為觀測(cè)時(shí)的結(jié)果。實(shí)驗(yàn)表明：激光測(cè)距儀與Kinect相比，更適合作為獲取激光SLAM觀測(cè)信息的傳感器。

5 導(dǎo)航

以往基于激光SLAM的移動(dòng)機(jī)器人通常只配備激光測(cè)距儀，但激光測(cè)距儀存在局限性(如圖6所示)，它只能得到激光測(cè)距儀激光束所在平面內(nèi)的障礙物信息，而不能考慮三維環(huán)境的信息。因此，在移動(dòng)機(jī)器人導(dǎo)航時(shí)，當(dāng)?shù)孛嫔洗嬖诩す鉁y(cè)距儀不能檢測(cè)到的障礙物時(shí)，由于缺少環(huán)境的信息，移動(dòng)機(jī)器人導(dǎo)航時(shí)會(huì)認(rèn)為區(qū)域安全而撞上障礙物，最終導(dǎo)致導(dǎo)航失敗。

圖5 移動(dòng)機(jī)器人的路徑曲線比較

圖6 激光測(cè)距儀局限性說(shuō)明

為了克服激光測(cè)距儀不能獲取三維環(huán)境信息的缺點(diǎn)，將Kinect配置到移動(dòng)機(jī)器人上，用Kinect將三維環(huán)境信息的點(diǎn)云數(shù)據(jù)轉(zhuǎn)化為激光測(cè)距儀對(duì)應(yīng)的掃描數(shù)據(jù)，在導(dǎo)航時(shí)，結(jié)合激光測(cè)距儀的掃描數(shù)據(jù)和由Kinect轉(zhuǎn)化而來(lái)的掃描數(shù)據(jù)，使得機(jī)器人具備能夠順利通過(guò)復(fù)雜環(huán)境的能力，改進(jìn)后移動(dòng)機(jī)器人在已知環(huán)境中的導(dǎo)航結(jié)果如圖7所示。當(dāng)機(jī)器人結(jié)合Kinect的數(shù)據(jù)時(shí)，會(huì)綜合考慮三維環(huán)境的信息，從而成功避開(kāi)障礙物，最終實(shí)現(xiàn)有效的移動(dòng)機(jī)器人導(dǎo)航。

圖7 移動(dòng)機(jī)器人在已知環(huán)境中的導(dǎo)航結(jié)果

6 結(jié)語(yǔ)

本文將激光測(cè)距儀和Kinect同時(shí)配備在移動(dòng)機(jī)器人上，作為獲取外界環(huán)境信息的手段。同時(shí)配備激光測(cè)距儀和Kinect的移動(dòng)機(jī)器人繼承了基于激光測(cè)距儀數(shù)據(jù)的激光SLAM的優(yōu)越性，同時(shí)彌補(bǔ)了激光測(cè)距儀不能獲取三維環(huán)境信息的不足。一方面，以激光測(cè)距儀數(shù)據(jù)作為基于激光SLAM的移動(dòng)機(jī)器人的觀測(cè)信息，可以獲得較高的定位精度和建圖效果；另一方面，將激光測(cè)距儀數(shù)據(jù)和Kinect數(shù)據(jù)綜合考慮用于移動(dòng)機(jī)器人導(dǎo)航，可以使移動(dòng)機(jī)器人獲得通過(guò)復(fù)雜環(huán)境的能力。