高沛林 高赟

摘 要：針對(duì)大型煤礦中的露天移動(dòng)機(jī)器人自主行走問(wèn)題，應(yīng)用基于卡爾曼濾波的SLAM算法，研究了移動(dòng)機(jī)器在不攜帶慣性導(dǎo)航設(shè)備，也無(wú)先驗(yàn)地圖的情況下，通過(guò)自身攜帶的傳感器與環(huán)境特征量進(jìn)行應(yīng)答式通信，建立環(huán)境地圖，并利用該地圖計(jì)算自身位置，從而實(shí)現(xiàn)自主導(dǎo)航與定位。仿真結(jié)果顯示，SLAM算法的定位誤差保持在±1 m以?xún)?nèi)，速度誤差保持在±0.2 m/s以?xún)?nèi)，對(duì)環(huán)境特征量的定位誤差隨著機(jī)器人的移動(dòng)逐漸減小，最終保持在±2 m以?xún)?nèi)。同時(shí)，通過(guò)對(duì)不同距離量測(cè)噪聲與速度量測(cè)噪聲的情況也進(jìn)行了分析。仿真結(jié)果顯示，當(dāng)保持距離量測(cè)噪聲不變，增大速度量測(cè)噪聲時(shí)，或保持速度量測(cè)噪聲不變，增大距離量測(cè)噪聲時(shí)，SLAM算法的定位精度均會(huì)下降。研究表明，基于卡爾曼濾波的SLAM算法很好地控制了移動(dòng)機(jī)器人在未知環(huán)境中的定位誤差，保證了機(jī)器人的定位精度。

關(guān)鍵詞：露天移動(dòng)機(jī)器人;同時(shí)制圖與定位;導(dǎo)航定位;隨機(jī)信標(biāo)

中圖分類(lèi)號(hào)：TD 05 ? ? ? ? ? ?文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1672-9315（2019）05-0905-07

Abstract：This article aims at the problem of autonomous walking of outdoor mobile robots in large coal mines.It focuses on a situation that a robot，which doesn’t carry any inertial navigation equipment or without a priori map，communicates with the beacons，establishes the environmental maps，which is used to calculate its own position.The simulation results show that the positioning error of SLAM algorithm is keep within ±1 m，the velocity error is kept within ±0.2 m/s，the beacon positioning error is decreasing while the robot is moving，and eventually kept within ±2 m.The study also finds out that distance measurement noise or velocity measurement noise will directly affect the position accuracy of SLAM algorithm.Therefore，the influence of different measurement noises is also analyzed.The accuracy of SLAM algorithm is decrease while the distance measurement noise is increased or the velocity measurement noise is increased.The simulation results show that the SLAM algorithm based on Kalman Filtering controls the position error of mobile robot in the unknown environment well and improves the positioning accuracy of the robot effectively.

Key words：mobile robots in the open air;SLAM;navigation;random beacon

0 引 言

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

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

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

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

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

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

信標(biāo)的位置，被稱(chēng)為L(zhǎng)andmarks，用XjL描述，其中，j=1，2，…，m.

機(jī)器人的運(yùn)動(dòng)模型，被稱(chēng)為Motion，即XP（k）=f（XP，uk）+vk，其中，uk為機(jī)器人在k時(shí)刻的控制輸入;vk為服從均值為零的高斯白噪聲，且協(xié)方差矩陣為Qk.

機(jī)器人的觀測(cè)模型，被稱(chēng)為Observation，即Zj（k）=h（XP，XjL）+wk，表示機(jī)器人在第k時(shí)刻觀測(cè)到的第j個(gè)信標(biāo)的位置;wk為服從均值為零的高斯白噪聲，且協(xié)方差矩陣為Rk.

在SLAM問(wèn)題中，指導(dǎo)機(jī)器人運(yùn)動(dòng)的信息有輸入uk及觀測(cè)量Zj（k），根據(jù)uk，Zj（k）求解XP，XjL的過(guò)程，就是解決SLAM問(wèn)題的過(guò)程。

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

圖7～圖8為信標(biāo)landmarks=20時(shí)的SLAM算法仿真圖。圖7中綠色曲線為機(jī)器人移動(dòng)的真實(shí)路徑，紅色曲線為SLAM算法估計(jì)出的機(jī)器人移動(dòng)路徑。圖7中圍繞著各個(gè)信標(biāo)逐漸密集的圓圈表示隨著機(jī)器人的移動(dòng)、對(duì)環(huán)境特征量的不斷觀測(cè)與算法的迭代，對(duì)信標(biāo)的定位誤差也逐漸減小，越來(lái)越接近信標(biāo)的真實(shí)位置。圖8為landmarks=20時(shí)，SLAM算法的路徑定位誤差圖。相較于landmarks=2時(shí)，信標(biāo)個(gè)數(shù)的增多，無(wú)論是x軸方向還是y軸方向的定位誤差都控制在±0.5 m以?xún)?nèi)，有效提高了定位精度。

除了隨機(jī)信標(biāo)的個(gè)數(shù)，在仿真過(guò)程中，信標(biāo)量測(cè)噪聲r(shí)，距離量測(cè)噪聲r(shí)d，速度量測(cè)噪聲r(shí)v，過(guò)程噪聲r(shí)p都將直接影響初始協(xié)方差矩陣Pk，量測(cè)噪聲矩陣Rk，過(guò)程噪聲矩陣Qk，從而對(duì)SLAM算法的定位精度造成影響。而量測(cè)噪聲對(duì)初始協(xié)方差矩陣Pk，量測(cè)噪聲矩陣Rk的影響尤為嚴(yán)重。因此文中對(duì)相同前提下的不同距離量測(cè)噪聲r(shí)d，速度量測(cè)噪聲r(shí)v進(jìn)行了仿真分析，仿真中假設(shè)隨機(jī)信標(biāo)個(gè)數(shù)為2個(gè)，仿真時(shí)間600 s，分析結(jié)果如圖9，圖10所示。

圖9，圖10針對(duì)不同距離量測(cè)噪聲r(shí)d，速度量測(cè)噪聲r(shí)v的情況進(jìn)行了仿真分析。從圖中可以看出，當(dāng)保持速度量測(cè)噪聲r(shí)v不變，距離量測(cè)噪聲r(shí)d增大時(shí)，將圖9和圖10中的紅色曲線與綠色曲線做比較，抑或?qū)蓤D中的藍(lán)色曲線與黑色曲線作比較，都能發(fā)現(xiàn)隨著距離量測(cè)噪聲r(shí)d增大，無(wú)論在x軸方向還是y軸方向，SLAM算法的路徑定位誤差都增大。同樣，當(dāng)保持距離量測(cè)噪聲r(shí)d不變，速度量測(cè)噪聲r(shí)v增大時(shí)，將圖9和圖10中的紅色曲線與黑色曲線做比較，抑或?qū)蓤D中的藍(lán)色曲線與綠色曲線作比較，都能發(fā)現(xiàn)隨著速度量測(cè)噪聲r(shí)v的增大，SLAM算法的速度定位誤差也增大。由此可見(jiàn)，通過(guò)降低量測(cè)噪聲，在一定程度上可以提高SLAM算法的定位精度，但傳感器噪聲受限于傳感器的制造工藝、使用環(huán)境等因素。因此，如何有效控制傳感器噪聲帶來(lái)的影響可作為下一步的研究重點(diǎn)。

4 結(jié) 論

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

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

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

參考文獻(xiàn)（References）：

[1] Dissanayake M W M G，Newman P，Clark S，et al.A solution to the simultaneous localization and map building （SLAM） problem[J].Csorba IEEE Transactions on Robotics and Automation，2001，17（3）：229-241.

[2]Bailey T，Durrant-Whyte H.Simultaneous localization and mapping（SLAM）：part Ⅱ[J].IEEE Robotics & Automation Magazine，2006，13（3）：108-117.

[3]Durrant-Whyte，Hugh，Tim Bailey.Simultaneous localization and mapping：part Ⅰ[J]. Robotics & Automation Magazine，IEEE，2006，13（2）：99-110.

[4]薛永勝.變電站巡檢機(jī)器人SLAM算法及其應(yīng)用研究[D].重慶：西南科技大學(xué)，2015.

XUE Yong-sheng.Substation inspection robot SLAM algorithm and its application research[D].Chongqing：Southwest University of Science and Technology，2015.

[5]張志超，鄭之增，方海峰，等.礦井救災(zāi)機(jī)器人的導(dǎo)航定位研究[J].煤礦機(jī)械，2008，29（11）：41-43.

ZHANG Zhi-chao，ZHENG Zhi-zeng，F(xiàn)ANG Hai-feng，et al.Research on navigation and localization technologies for mine rescue robots[J].Coal Mine Machinery，2008，29（11）：41-43.

[6]孫 振，胡金磊，羅建軍，等.變電站智能巡檢機(jī)器人導(dǎo)航定位技術(shù)設(shè)計(jì)[J].自動(dòng)化技術(shù)與應(yīng)用，2018，37（11）：86-89.

SUN Zhen，HU Jin-lei，LUO Jian-jun，et al.Design of navigation and positioning technology for intelligent inspection robot in substation[J].Techniques of Automation and Application，2018，37（11）：86-89.

[7]耿世毅.煤礦井下機(jī)器人準(zhǔn)確定位研究[D].鄭州：河南理工大學(xué)，2014.

GENG Shi-yi.Research on accurate localization of underground robot[D].Zhengzhou：Henan Polytechnic University，2014.

[8]王 怡.機(jī)器人同時(shí)定位與地圖創(chuàng)建研究[D].上海：復(fù)旦大學(xué)，2009.

WANG Yi.Research on simultaneous localization of robots and construction of land maps[D].Shanghai：Fudan University，2009.

[9]Menegatti E，Zanella A，Zilli S，et al.Range-only SLAM with a mobile robot and a Wireless Sensor Netw[J].IEEE International Conference on Robotics & Automation，2009：1109-1115.

[10]劉明雍，董婷婷，張立川.隨機(jī)信標(biāo)的水下SLAM導(dǎo)航方法[J].系統(tǒng)工程與電子技術(shù)，2015，37（12），2830-2834.

LIU Ming-yong，DONG Ting-ting，ZHANG Li-chuan.Underwater SLAM navigation algorithm based on random beacons[J].Systems Engineering and Electronics，2015，37（12）：2830-2834.

[11]Davison A J，Reid I D，Molton N D，et al.Mono SLAM：Real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis & Machine Intelligence，2007，29（6）：1052-1067.

[12]Micha R Nowicki，Dominik Belter，Aleksander Kostusiak，et al.An experimental study on feature-based SLAM for multi-legged robots with RGB-D sensors[J].Industrial Robot，2017，44（4）：428-441.

[13]Smith，Randall，Matthew Self，et al.Estimating uncertain spatial relationships in robotics[M].Springer New York：Autonomous Robot Vehicles，1990.

[14]周 武，趙春霞，沈亞強(qiáng)，等.基于全局觀測(cè)地圖模型的SLAM研究[J].機(jī)器人，2010，32（5）：647-654.

ZHOU Wu，ZHAO Chun-xia，SHEN Ya-qiang et al.SlAM research based on global observation map model[J].Robot，2010，32（5）：647-654.

[15]程見(jiàn)童，張為華，江振宇.同時(shí)定位與制圖輔助的GPS/DR組合導(dǎo)航[J].國(guó)防科技大學(xué)學(xué)報(bào)，2014，36（3）：72-76.

CHENG Jian-tong，ZHANG Wei-hua，JIANG Zhen-yu.GPS/DR integrated navigation aided by simultaneous localization and mapping[J].Journal of National University of Defense Technology，2014，36（3）：72-76.

[16]Zhang J，Shi C，Sun D，et al.High-precision，limited-beacon-aided AUV localization algorithm[J].Ocean Engineering，2018，149：106-112.

[17]王宏健，王 晶，邊信黔，等.基于組合EKF的自主水下航行器SLAM[J].機(jī)器人，2012，34（1）：56-64.

WANG Hong-jian，WANG Jing，BIAO Xin-qian，et al.Slam of AUV based on the combined EKF[J].Robot，2012，34（1）：56-64.

[18]季秀才，鄭志強(qiáng)，張 輝.SLAM問(wèn)題中機(jī)器人定位誤差分析與控制[J].自動(dòng)化學(xué)報(bào)，2008，34（3）：323-330.

JI Xiu-cai，ZHENG Zhi-qiang，ZHANG Hui.Analysis and control of Robot Position Error in SLAM[J].Acta Automatica Sinica，2008，34（3）：323-330.

[19]曹 軍，曾 碧，何元烈.基于路標(biāo)觀測(cè)的改進(jìn)EKF-SLAM算法[J].自動(dòng)化與信息工程，2014（1）：21-26.

CAO Jun，ZENG Bi，HE Yuan-lie.Improved EKF-SLAM algorithm based on road sign observation[J].Automation and Information Engineering，2014（1）：21-26.

[20]厲茂海，洪炳館，羅榮華.移動(dòng)機(jī)器人的同時(shí)定位和地圖創(chuàng)建方法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2004，36（7）：874-876.

LI Mao-hai，HONG Bing-guan，LUO Rong-hua.Simultaneous localization and map building for mobile robot[J].Journal of Harbin Institute Technology，2004，36（7）：874-876.

[21]陳衛(wèi)東，張 飛.移動(dòng)機(jī)器人的同步自定位與地圖創(chuàng)建研究進(jìn)展[J].控制理論與應(yīng)用，2005，22（3）：455-460.

CHEN Wei-dong，ZHANG Fei.Review on achievements in simultaneous location and map building for mobile robot[J].Control Theory and Applications，2005，22（3）：455-460.

[22]Thrun S，Liu Y.Multi-robot SLAM with sparse extended information filers[J].Robotics Research Sienna，2005，15（5）：254-266.

[23]付夢(mèng)印.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

FU Meng-yin.Kalman filtering theory and its application in navigation system[M].Beijing：Science Press， 2003.

[24]Chen S Y.Kalman filter for robot vision：a survey[J].IEEE Transactions on Industrial Electronics，2012，59（11）：4409-4420.