基于ROS的室內自主導航移動機器人系統實現

葉強強， 鄭明魁， 邱 鑫

(福州大學 物理與信息工程學院，福建 福州 350117)

0 引 言

國外對于移動機器人相關技術的研究起步較早。如20世紀70年代斯坦福研究院(SRI)研究出Shakey的移動機器人，它使用三角測距儀、碰撞傳感器等來實現自主定位與感知。受限于當時的計算機水平及相應的技術，Shakey往往需要數個小時才能完成相應的路徑規劃。隨著同時定位與建圖(simultaneous localization and mapping，SLAM)技術的發展，其技術迅速被運用到機器人身上。2019年,在美國的TechCrunch AI+機器人大會上波士頓創始人暨CEO Marc Raibert展示了即將量產化的機器人SpotMini[1],展示其靈活性及其適用場景。SpotMini使用雙目攝像頭配合IMU通過3D SLAM技術實現定位。

國內的移動機器人產業起步時間雖然較短，但是在這一方面也取得相當大的進步。特別是在室內移動機器人這一方面，商業化的步伐正在加快。然而室內移動機器人要想真正得走進人們的生活中，還有一些亟待解決的問題，如移動機器人準確的定位是其自主導航的基礎。此外，一套成熟、可移植性強的移動機器人方案也是實現商業化最重要的前提。

本文基于上述內容提出一種移植性強、定位性能優秀的室內移動機器人方案，在Ubuntu16.04環境搭載ROS系統上來實現室內移動機器人的建圖、循跡及自主導航系統，為室內移動機器人商業化提供一種可行性方案。

1 系統總體框架

1.1 移動機器人硬件框架

本文移動機器人硬件方面采用清華大學團隊研發的Autolabor Pro1[2]差速底盤機器人、瑞芬的Ah100b型號九軸慣性測量單元(inertial measurement unit,IMU),北科天繪Rfans—16的激光雷達。移動機器人系統的硬件框架如圖1所示，Autolabor Pro1是雙電機四輪驅動的模式。因此，同側車輪的速度是相同的，在發送右輪或者左輪的速度時，只需要發送同側一個車輪速度即可。激光雷達與IMU的數據放在PC端進行處理，對移動機器人的控制命令通過RS—232串口發給機器人底盤。

圖1 移動機器人的硬件框架

1.2 移動機器人軟件框架

移動機器人的軟件框架如圖2所示，由于GMapping[3]與AMCL只適用2D雷達數據。因此，使用ROS[4]的開源包Pointcloud_to_laserscan把3D PointCloud2轉為2D高質量的 LaserScan。為了讓GMapping建圖與AMCL重定位過程更加的精確。這里對2D的LaserScan數據進行運動畸變的矯正。另外，由于GMapping與AMCL嚴重依賴于里程計數據，若機器人處于打滑的情況，或者是地面不平坦環境下，會導致建圖與重定位的效果非常不理想。為改善這種情況，使用擴展卡爾曼濾波(extended Kalman filtering，EKF)[5]對輪式里程計與IMU數據進行融合，提升里程計數據的準確度。

圖2 GMapping建圖與AMCL定位框架

2 激光雷達數據預處理

圖3 移動機器人運動Δt后各個坐標系之間的關系

(1)

如果移動機器人是在靜止的情況下，運動畸變自然也不存在。但是，若移動機器人移動速度較快時，特別是在旋轉的情況下，這種雷達數據的運動畸變將嚴重影響到移動機器人的建圖、定位效果。

激光雷達數據的畸變矯正主要思想是估計出激光雷達在一幀數據時間內的運動情況，從而根據雷達數據一幀內運動情況對這一幀數據進行相應的矯正。估計幀內運動方法可以借助IMU[7]輔助測量得出，本文直接采用底盤里程計信息輔助方式來對激光數據進行運動畸變矯正。里程計信息更新頻率遠遠高于激光雷達的更新頻率，基本都能達到50 Hz左右。ROS環境下里程計發布的信息為機器人坐標系base_link在里程計坐標系odom下的位姿O=(xR,yR,θR)T，移動的速度v與角速度ω，在較短的時間內能正確反映機器人的運動情況。

假設一幀的激光雷達n個數據對應的時間點為{ts,ts+1,…,ts+n-2,te}，由于里程計的更新頻率遠遠高于激光雷的數據的發布頻率，在相同的時間間隔內，里程計的時間戳比激光雷達的時間戳更加的密集。如圖4所示，ts與te為一幀中激光數據開始點與結束點對應的時刻,豎直虛線代表擁有里程計信息的時間戳。把一幀的激光雷達按照時間間隔分成k個小區間，設移動機器人在每一個小區間內為勻加速運動的，如激光雷達的周期T=100 ms,k=10，則每個小區間的時間為10 ms，假設機器人在10 ms時間內運動狀態為勻加速運動是合理的。

圖4 激光和里程計信息對應的時刻

用Om表示m時刻對應的里程計的信息為Om,其中時刻tm

O(ts)=O(tm)+(O(tn)-O(tm))/(tn-tm)(ts-tm)

(2)

同理，通過{ts+k,ts+2k,…,ts+(k-1)k,te}最近時刻的里程計信息進行線性插值求出{O(ts+k),O(ts+2k),…,O(ts+(n-1)k),O(te)}的里程計信息。假設ts+2k與ts+3k中有p個激光數據點，在移動機器人從ts+2k到ts+3k時刻是勻加速運動的前提下,對p個激光點進行線性插值獲取p個點對應的里程計信息。同理，獲得這一幀激光數據n個點對應的里程計信息。使用對應的里程計信息根據上述方法把n個激光點轉換到同一坐標系上，重新封裝為激光數據再發布出去，即這一幀的運動畸變矯正完成。

3 GMapping建圖與AMCL重定位

3.1 GMapping建圖

GMapping主要基于RBPF粒子濾波的方法，GMapping中的每一個粒子都維持一幅地圖及相應的位姿。RBPF算法把SLAM問題分解為兩個部分:1)p(x1︰t′|z1︰t,u1︰t)機器人的定位;2)p(m|x1︰t,z1︰t)在已知機器人位姿的情況下建圖

{p(x1︰t,m|u1︰t,z1︰t)=p(x1︰t′|z1︰t,u1︰t)*p(m|x1︰t,|z1︰t)}

(3)

GMapping算法使用粒子濾波方式估計出機器人的軌跡p(x1︰t′|z1︰t,u1︰t)，該算法主要有以下4個步驟：

1)抽樣

(4)

(5)

2)重要性權重的計算

(6)

3)自適應重采樣

粒子濾波有個嚴重的缺陷，如果頻繁地進行重采樣的話，粒子的多樣性會散失。為緩解這種情況，GMapping實施自適應重采樣，當Neff[9]小于相應的閾值T時，則執行重采樣。

4)地圖估計

3.2 AMCL重定位

為提升移動機器人定位方面的魯棒性，實現從全局定位失效或者是機器人的綁架情況中恢復過來AMCL結合Augmented_MCL算法。Augmented_MCL使用指數平滑法思想通過跟蹤傳感器測量概率p(zt|z1︰t-1,u1︰t,m)的短期均值ωfast與長期均值ωslow來判斷在什么情況下增加粒子，以便加強定位的魯棒性。

AMCL如果單純的依賴由編碼器得出的里程計的信息的話，在小車行駛的過程中，特別是在轉彎的過程中(轉彎容易導致打滑)，從而導致粒子的估計不準確。通過EKF融合IMU與里程計的數據，能較好地維持粒子的收斂狀態。

4 移動機器人的路徑規劃

在移動機器人路徑規劃[11]這一方面，本文使用的是move_base包。在全局路徑規劃上使用帶有啟發式搜索的A*算法[12]。

在局部路徑規劃這一方面使用動態窗口法(dynamic window approach,DWA)[13]算法，DWA根據當前移動移動機器人的狀態，模擬出多條軌跡，如圖5所示。根據相應的評價標準如是否接近障礙物、是否靠近目標點等選擇一條最優的模擬軌跡。

圖5 DWA算法軌跡模擬情況

5 實驗結果

GMapping建圖效果如圖6所示，從整體上看建圖的效果還是非常不錯。但是由于建圖實際環境中存在玻璃，激光點透過玻璃，建圖在局部細節方面還略有瑕疵。

圖6 GMapping建圖結果

AMCL的定位效果如圖7所示。定位開始時，粒子處于散開狀態，隨著移動機器人的運動動，粒子慢慢收斂，說明移動機器人根據當前周圍環境的雷達數據信息逐漸確定自己的位置。

圖7 AMCL粒子收斂圖

為實現移動機器人的室內循跡，移動機器人每運動一段距離記錄下相應的x,y,yaw，并把這些小車的位姿信息存儲在文本文件下。循跡時，通過依次發布這些目標點，來實現小車的循跡功能。通過相應的設置，讓移動機器人既能依照軌跡回環循跡，又能讓小車往返循跡。在局部路徑規劃方面，小車使用的是DWA算法，在小車在循跡的過程中，也能實現避障的功能。移動機器人的循跡如圖8所示。路徑為繞障礙物一圈的長帶，可以看出，粒子一直收斂在機器人的周圍，定位狀態良好。

圖8 移動機器人的循跡實現

6 結束語

本文通過對激光雷達數據預處理，里程計融合IMU實現室內移動機器人的GMapping建圖、AMCL重定位、自主導航與循跡等功能。經過實驗認證，移動機器人在室內自主導航，循跡效果表現良好。而且，本文方案的可移植性非常的強，如需要移植到阿克曼轉向型或是麥克納姆輪移動機器人上，只需要更改其相應的里程計模型即可。基于本方案，之后可以逐步融合視覺等方面以增加相應的功能，提升其魯棒性，使其真正商業化落地。