移動機器人多信標SLAM技術

高沛林，高 赟

(西安科技大學 電氣與控制工程學院，陜西 西安 710054)

0 引 言

隨著計算機技術和人工智能的發展成熟，針對機器人進行的研究得到越來越多的關注和投入。無論是哪一種工作原理、工作目的的移動機器人，其移動都不是盲目的。在機器人的移動過程中需要考慮移動路徑、避障、移動地圖、路徑規劃、導航定位等許多實際問題[1-3]。對于在已知環境中的移動機器人自主定位和已知機器人位置的地圖創建，已有了一些實際的解決方法。然而在很多情況下，需要人工進入變電站、發電廠、煤礦井下進行巡檢，巡檢困難、危險性高，難免會發生漏檢、誤檢事件[4-7]。針對類似于變電站、發電廠、井下這類高溫、高危、高輻射地區，機器人往往因為信號干擾不能利用GPS全局定位系統，這時就需要機器人在自身位置不確定、地圖環境未知的情況下完成導航與定位，這就是移動機器人同時制圖與定位問題，即SLAM(Simultaneous Localization and Mapping)問題[8]。

到目前為止，SLAM問題得到機器人界眾多學者的重視與研究。Menegatti E等人構建了無線傳感器網絡，機器人通過接收到信號強度的不同與距離量測，使用REKF(魯棒卡爾曼濾波：Robuts Extended Kalman Filter)解決了SLAM問題[9]。劉明雍等人研究了基于隨機信標的水下SLAM問題，解決了信標位置不固定時的水下航行器定位問題，并將該方法的定位精度與最小二乘法進行了比較[10]。Dauson A J等人針對攜帶單目攝像頭的機器人，設計了一種可以復現其在未知場景里隨機運動的軌跡的算法，該工作重點在于深入分析單目攝像頭的工作原理與工作方式，如何將攝像頭采集到的數據進行更好地復現，并將復現后的數字信號應用于解決SLAM問題[11]。以上這些文章都針對SLAM問題的不同方面進行了研究與解決，但SLAM問題針對煤礦機器人的應用少之又少。2019年1月，國家煤監局發布了《煤礦機器人重點研發目錄》，鼓勵支持煤礦企業與國內外科研單位、機器人制造企業開展合作，大力研發應用煤礦機器人。其中就包括具備遠程無線通信、自主行走、導航避障功能的露天礦卡車無人駕駛系統。

因此，針對大型煤礦中的露天移動機器人沒有攜帶慣性導航設備，也無先驗地圖的情況[12-13]，通過自身攜帶的傳感器與環境中隨機布放的信標進行通信，在實現移動機器自身導航定位的基礎上，對環境中隨機信標的位置也進行了估計。同時，還分析了不同量測噪聲對SLAM定位算法的影響。研究表明，基于卡爾曼濾波的SLAM算法很好地控制了移動機器人在未知環境中的定位誤差，有效提升了機器人的定位精度。

1 露天移動機器人多信標SLAM問題

在某一大型露天礦場中存在一露天移動機器人，移動機器人在未知環境中，從一個未知位置開始移動，在移動過程中根據自身攜帶的傳感器與環境中隨機布放的信標進行應答式通信，建立增量式地圖，進行同時制圖與定位，從而完成不需要任何預先定義的地圖或預先設置的位置已知的信標便可實現的機器人自主導航與定位[14-15]。露天移動機器人SLAM定位如圖1所示。

圖1 露天移動機器人SLAM定位Fig.1 Schematic diagram of SLAM for mobile robot in the open air

將露天礦場環境簡化為一個二維空間，在該空間內隨機分布著多個位置未知的信標，露天機器人在該空間內移動。當機器人移動到路徑中的某一點X時，此時可以觀測到信標1、信標2與信標5.但信標3，信標4此時未被觀測到。隨著機器人的移動，信標3與信標4先后被觀測到，從而將信標3，信標4的位置信息加入增廣的觀測矩陣中。對SLAM問題而言，機器人在環境特征未知的一個二維空間中運動，根據已知的運動模型，利用攜帶的傳感器觀測附近信標的位置，從而通過觀測到的信標位置確定自身位置，實現導航與定位[16-17]。用數學方法將SLAM問題描述為

機器人的狀態，被稱為States，用Xp描述。

機器人的運動模型，被稱為Motion，即XP(k)=f(XP,uk)+vk，其中，uk為機器人在k時刻的控制輸入；vk為服從均值為零的高斯白噪聲，且協方差矩陣為Qk.

2 基于Kalman濾波的SLAM定位算法

SLAM定位算法對露天移動機器人的狀態與隨機信標的位置同時進行估計，首先需要確定移動機器人的運動模型與觀測模型。為便于計算機處理與仿真，離散化后的運動模型和觀測模型可表示為[18]

運動模型：Xp(k+1)=F(·)XP(k)+u(k)+v(k)，F(·)為移動機器人的狀態轉移矩陣。

觀測模型：Zj(k)=H(·)XP(k)+w(k)，其中j=1,…,m，H(·)為移動機器人的觀測矩陣。

2.1 預測Prediction

預測的SLAM算法可以表示為

(1)

Zj(k+1|k)=H(k)·Xp(k+1|k)

(2)

P(k+1|k)=F(k)·P(k|k)·FT(k)+Q(k)

(3)

機器人在移動過程中，用相鄰時刻的距離變化量作為被估計的速度值。除了速度更新時，下一時刻對狀態的預測等同于本時刻系統的狀態，則移動機器人的狀態轉移矩陣可表示為

因為要求隨機信標個數m≥2所以Fk中應至少包含2個信標的位置信息。

2.2 觀測Observation

觀測的SLAM算法可以表示為

Zj(k+1)=H(k)·XP(k+1)+w(k)

(4)

觀測新息

vj(k+1)=Zj(k+1)-Zj(k+1|k)

(5)

新息的協方差矩陣

(6)

觀測的目的是根據第k+1時刻對第j個信標的觀測Zj(k+1)值，構造觀測新息vj(k+1)與新息的協方差矩陣Sj(k+1).

2.3 更新Update

預測及觀測后，對系統狀態的更新可表示為

XP(k+1|k+1)=XP(k+1|k)+wj(k+1)·vj(k+1)

(7)

(8)

更新的目的是更新機器人當前的狀態[22-24]。根據預測與觀測完成更新后，即通過式(1)～式(8)，使機器人在移動過程中完成自我定位與新環境特征的采集，遞推地估計出自身狀態與信標位置[25]。

3 仿真與分析

假設在某一露天礦場，露天移動機器人的活動范圍是100×100的區域，在該區域隨機分布m個信標，且m≥2.當移動機器人在該區域活動時，無論是哪種移動方式，都是直線運動的排列組合。因此，針對露天移動機器人沿一直線路徑行進時，信標個數不同的情況進行仿真，并對仿真結果進行分析。

在仿真過程中，定義信標量測噪聲為r，距離量測噪聲為rd，速度量測噪聲為rv，過程噪聲為rp.在Kalman濾波的SLAM算法中，定義初始協方差矩陣為Pk，量測噪聲協方差矩陣為Rk，過程噪聲協方差矩陣為Qk，分別表示為

Pk=

在設定好的初始條件下，在露天礦場100×100的區域內隨機分布2個信標，露天移動機器人在該區域內沿直線路徑移動。仿真時間600 s，仿真結果如圖2所示。

圖2 SLAM仿真結果(landmarks=2)Fig.2 SLAM simulation results(landmarks=2)

從圖2可以看出，隨機分布著2個信標，綠色曲線表示機器人移動的真實路徑，紅色曲線表示SLAM算法估計出的機器人移動路徑。從圖2中的放大圖可以清楚地看出，SLAM導航軌跡與真實軌跡的趨勢相同，但存在一定誤差。對機器人位置、速度及信標位置的估計誤差進行仿真分析后仿真結果如圖3～圖5所示。

圖3 SLAM位置誤差(landmarks=2)Fig.3 SLAM position error diagram(landmarks=2)

圖4 SLAM速度誤差(landmarks=2)Fig.4 SLAM velocity error diagram(landmarks=2)

圖5 SLAM信標誤差(landmarks=2)Fig.5 SLAM beacon error diagram(landmarks=2)

圖3～圖5表示SLAM算法對露天移動機器人的狀態，即移動位置、速度，信標位置的估計。從圖3～圖5可以看出，在仿真的600 s內，SLAM算法的x軸定位誤差保持在±1 m以內、y軸定位誤差保持在±0.5 m以內，速度定位誤差保持在±0.2 m/s以內。對信標的位置定位誤差最終保持在±2 m以內。仿真結果表示，基于卡爾曼濾波的SLAM算法很好地控制了露天移動機器人在未知環境中的定位誤差，保證了機器人的定位精度。

為了進一步探究信標個數對定位精度的影響，在相同的礦場環境與移動路徑下，隨機分布著20個信標，信標分布如圖6所示。仿真結果如圖7，圖8所示。

圖6 隨機信標位置分布(landmarks=20)Fig.6 Random beacons location(landmarks=20)

圖7 SLAM仿真結果(landmarks=20)Fig.7 SLAM simulation results(landmarks=20)

圖8 SLAM位置誤差(landmarks=20)Fig.8 SLAM position error diagram(landmarks=20)

圖7～圖8為信標landmarks=20時的SLAM算法仿真圖。圖7中綠色曲線為機器人移動的真實路徑，紅色曲線為SLAM算法估計出的機器人移動路徑。圖7中圍繞著各個信標逐漸密集的圓圈表示隨著機器人的移動、對環境特征量的不斷觀測與算法的迭代，對信標的定位誤差也逐漸減小，越來越接近信標的真實位置。圖8為landmarks=20時，SLAM算法的路徑定位誤差圖。相較于landmarks=2時，信標個數的增多，無論是x軸方向還是y軸方向的定位誤差都控制在±0.5 m以內，有效提高了定位精度。

除了隨機信標的個數，在仿真過程中，信標量測噪聲r，距離量測噪聲rd，速度量測噪聲rv，過程噪聲rp都將直接影響初始協方差矩陣Pk，量測噪聲矩陣Rk，過程噪聲矩陣Qk，從而對SLAM算法的定位精度造成影響。而量測噪聲對初始協方差矩陣Pk，量測噪聲矩陣Rk的影響尤為嚴重。因此文中對相同前提下的不同距離量測噪聲rd，速度量測噪聲rv進行了仿真分析，仿真中假設隨機信標個數為2個，仿真時間600 s，分析結果如圖9，圖10所示。

圖9 不同量測噪聲下的SLAM位置誤差曲線Fig.9 SLAM position error with different measurement noises

圖10 不同量測噪聲下的SLAM速度誤差曲線Fig.10 SLAM velocity error with different measurement noises

圖9，圖10針對不同距離量測噪聲rd，速度量測噪聲rv的情況進行了仿真分析。從圖中可以看出，當保持速度量測噪聲rv不變，距離量測噪聲rd增大時，將圖9和圖10中的紅色曲線與綠色曲線做比較，抑或將兩圖中的藍色曲線與黑色曲線作比較，都能發現隨著距離量測噪聲rd增大，無論在x軸方向還是y軸方向，SLAM算法的路徑定位誤差都增大。同樣，當保持距離量測噪聲rd不變，速度量測噪聲rv增大時，將圖9和圖10中的紅色曲線與黑色曲線做比較，抑或將兩圖中的藍色曲線與綠色曲線作比較，都能發現隨著速度量測噪聲rv的增大，SLAM算法的速度定位誤差也增大。由此可見，通過降低量測噪聲，在一定程度上可以提高SLAM算法的定位精度，但傳感器噪聲受限于傳感器的制造工藝、使用環境等因素。因此，如何有效控制傳感器噪聲帶來的影響可作為下一步的研究重點。

4 結 論

1)基于Kalman濾波的SLAM算法能很好地估計出露天移動機器人的位置信息。并針對信標個數不同、信標位置隨機的情況進行了仿真分析，該方法克服了露天移動機器人無慣導設備與先驗地圖情況下無法導航的困難。

2)給出系統的初始協方差矩陣、量測噪聲協方差矩陣、過程噪聲協方差矩陣的表達式。并對不同距離量測噪聲、速度量測噪聲的情況進行了仿真分析。研究表明，基于卡爾曼濾波的SLAM算法很好地控制了移動機器人在未知環境中的定位誤差，保證了機器人的定位精度。

3)不足之處在于，該方法沒有針對如何更精確地確定信標量測位置，現有的研究也僅限于理論分析，后續研究應使用真實傳感器以獲得更準確的數據，在這些數據的基礎上通過改進算法致力于降低導航誤差，也應針對系統噪聲未知、量測噪聲多變的現實環境進行后續研究。