高沛林，高 赟

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

0 引 言

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

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

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

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

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

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

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

機器人的狀態(tài)，被稱為States，用Xp描述。

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

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

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

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

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

2.1 預(yù)測Prediction

預(yù)測的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)

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

因為要求隨機信標個數(shù)m≥2所以Fk中應(yīng)至少包含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)

新息的協(xié)方差矩陣

(6)

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

2.3 更新Update

預(yù)測及觀測后，對系統(tǒng)狀態(tài)的更新可表示為

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

(7)

(8)

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

3 仿真與分析

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

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

Pk=

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

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

從圖2可以看出，隨機分布著2個信標，綠色曲線表示機器人移動的真實路徑，紅色曲線表示SLAM算法估計出的機器人移動路徑。從圖2中的放大圖可以清楚地看出，SLAM導(dǎo)航軌跡與真實軌跡的趨勢相同，但存在一定誤差。對機器人位置、速度及信標位置的估計誤差進行仿真分析后仿真結(jié)果如圖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算法對露天移動機器人的狀態(tài)，即移動位置、速度，信標位置的估計。從圖3～圖5可以看出，在仿真的600 s內(nèi)，SLAM算法的x軸定位誤差保持在±1 m以內(nèi)、y軸定位誤差保持在±0.5 m以內(nèi)，速度定位誤差保持在±0.2 m/s以內(nèi)。對信標的位置定位誤差最終保持在±2 m以內(nèi)。仿真結(jié)果表示，基于卡爾曼濾波的SLAM算法很好地控制了露天移動機器人在未知環(huán)境中的定位誤差，保證了機器人的定位精度。

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

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

圖7 SLAM仿真結(jié)果(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中圍繞著各個信標逐漸密集的圓圈表示隨著機器人的移動、對環(huán)境特征量的不斷觀測與算法的迭代，對信標的定位誤差也逐漸減小，越來越接近信標的真實位置。圖8為landmarks=20時，SLAM算法的路徑定位誤差圖。相較于landmarks=2時，信標個數(shù)的增多，無論是x軸方向還是y軸方向的定位誤差都控制在±0.5 m以內(nèi)，有效提高了定位精度。

除了隨機信標的個數(shù)，在仿真過程中，信標量測噪聲r，距離量測噪聲rd，速度量測噪聲rv，過程噪聲rp都將直接影響初始協(xié)方差矩陣Pk，量測噪聲矩陣Rk，過程噪聲矩陣Qk，從而對SLAM算法的定位精度造成影響。而量測噪聲對初始協(xié)方差矩陣Pk，量測噪聲矩陣Rk的影響尤為嚴重。因此文中對相同前提下的不同距離量測噪聲rd，速度量測噪聲rv進行了仿真分析，仿真中假設(shè)隨機信標個數(shù)為2個，仿真時間600 s，分析結(jié)果如圖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的情況進行了仿真分析。從圖中可以看出，當(dāng)保持速度量測噪聲rv不變，距離量測噪聲rd增大時，將圖9和圖10中的紅色曲線與綠色曲線做比較，抑或?qū)蓤D中的藍色曲線與黑色曲線作比較，都能發(fā)現(xiàn)隨著距離量測噪聲rd增大，無論在x軸方向還是y軸方向，SLAM算法的路徑定位誤差都增大。同樣，當(dāng)保持距離量測噪聲rd不變，速度量測噪聲rv增大時，將圖9和圖10中的紅色曲線與黑色曲線做比較，抑或?qū)蓤D中的藍色曲線與綠色曲線作比較，都能發(fā)現(xiàn)隨著速度量測噪聲rv的增大，SLAM算法的速度定位誤差也增大。由此可見，通過降低量測噪聲，在一定程度上可以提高SLAM算法的定位精度，但傳感器噪聲受限于傳感器的制造工藝、使用環(huán)境等因素。因此，如何有效控制傳感器噪聲帶來的影響可作為下一步的研究重點。

4 結(jié) 論

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

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

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