有色量測(cè)噪聲下機(jī)器人同步定位與地圖構(gòu)建

1、計(jì) 算 機(jī) 工 程 第 35 卷 第24期Vol.35 No.24 Computer Engineering ·博士論文·文章編號(hào)：10003428(2009)24002904文獻(xiàn)標(biāo)識(shí)碼：A2009年12月December 2009中圖分類(lèi)號(hào)：TP242有色量測(cè)噪聲下機(jī)器人同步定位與地圖構(gòu)建弋英民，劉 丁(西安理工大學(xué)自動(dòng)化與信息工程學(xué)院，西安 710048)摘 要：針對(duì)有色量測(cè)噪聲模型，提出一種有色量測(cè)噪聲下的輪式機(jī)器人同步定位與地圖構(gòu)建算法。通過(guò)重新組合輪式機(jī)器人的過(guò)程模型和量測(cè)模型，將有色量測(cè)噪聲量測(cè)模型轉(zhuǎn)化為虛擬的白噪聲量測(cè)模型。為使過(guò)程噪聲和量測(cè)噪聲不相關(guān)，對(duì)過(guò)程

2、模型進(jìn)行不相關(guān)條件處理。算法按照構(gòu)造的虛擬過(guò)程模型和量測(cè)模型進(jìn)行濾波估計(jì)和地圖構(gòu)建。仿真結(jié)果驗(yàn)證了算法的一致性和魯棒性。 關(guān)鍵詞：有色量測(cè)噪聲；輪式機(jī)器人；同步定位與地圖構(gòu)建；算法一致性Simultaneous Localization and Mappingfor Robot under Colored Measurement NoiseYI Ying-min, LIU Ding(Faulty of Automation and Information Engineering, Xian University of Technology, Xian 710048)【Abstract】Aimi

3、ng at colored measurement noise model, this paper presents a Simultaneous Localization and Mapping(SLAM) algorithm for wheeled robot under colored measurement noise. Colored measurement noise model is converted into white measurement noise model by recombining the process model and the measurement m

4、odel for wheeled robot. In order to make the process noise and the measurement noise irrelevant each other, the process model is re-defined. Estimating state and mapping are conducted in accordance with the virtual process model and the virtual measurement model. In data association step, part obser

5、ved landmarks are processed as redundant landmarks. Some indicators of the filter are used to evaluate the performance the algorithm. Simulation results show that the algorithm is consistent and robust.【Key words】colored measurement noise; wheeled robot; Simultaneous Localization and Mapping(SLAM);

6、consistency of algorithm1 概述移動(dòng)機(jī)器人的自主定位與導(dǎo)航必須以可靠的環(huán)境信息感知為基礎(chǔ)。但是，由于傳感器自身的限制，各種傳感器量測(cè)信息容易受到各種噪聲的干擾，存在不同程度的不確定性。量測(cè)信息的不確定性必然導(dǎo)致系統(tǒng)模型的不確定，傳統(tǒng)的處理方法是用高斯白噪聲模型表征量測(cè)信息。但在實(shí)際問(wèn)題中，這種噪聲模型往往是通過(guò)模型近似得到的。機(jī)器人同步定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)問(wèn)題中常用的量測(cè)傳感器有視覺(jué)傳感器、激光傳感器和聲納傳感器。視覺(jué)傳感器通過(guò)攝像頭獲取環(huán)境的圖像信息，通過(guò)圖像信息與機(jī)器人本體的空間關(guān)系

7、計(jì)算相對(duì)距離和角度?；谝曈X(jué)的SLAM算法分為3類(lèi)：(1)stereo vision SLAM算法1，這類(lèi)方法的量測(cè)信息與攝像頭標(biāo)定的結(jié)果有關(guān)，攝像頭的物理參數(shù)、圖像處理算法精度都可能引起立體視覺(jué)量測(cè)為有色噪聲；(2)全景視覺(jué)算法2，這類(lèi)算法獲得的圖像產(chǎn)生很大的畸變，圖像的矯正精度和攝像頭的物理參數(shù)也會(huì)使量測(cè)為有色噪聲；(3)單目視覺(jué)算法3，這類(lèi)方法的量測(cè)與攝像頭的物理光心和焦距的標(biāo)定結(jié)果有關(guān)。因制造工藝和有限的圖像處理算法的限制，視覺(jué)量測(cè)模型已無(wú)法近似為高斯白噪聲模型。在激光傳感器和方向測(cè)量傳感器的量測(cè)系統(tǒng)中4，算法模型中假設(shè)激光傳感器中心和方向測(cè)量傳感器中心重合，但在實(shí)際問(wèn)題中，由于無(wú)法保

8、證中心重合，因此量測(cè)為有色噪聲。在基于超聲波傳感器的機(jī)器人SLAM系統(tǒng)中，傳感器往往安裝在機(jī)器人本體的四周5，算法模型中總是假設(shè)所有超聲波傳感器位于同一個(gè)輻射中心點(diǎn)，因在實(shí)際機(jī)械安裝裝配中無(wú)法滿足而使量測(cè)為有色噪聲。在這些問(wèn)題中，用高斯白噪聲模型近似實(shí)際的有色噪聲模型進(jìn)行機(jī)器人的同步定位與地圖構(gòu)建，誤差會(huì)隨時(shí)間累計(jì)，最終導(dǎo)致系統(tǒng)發(fā)散。針對(duì)實(shí)際量測(cè)為有色噪聲的情況，本文提出一種有色量測(cè)噪聲下的輪式機(jī)器人SLAM算法。算法適用于量測(cè)量為有色噪聲的情況，是對(duì)有色噪聲下機(jī)器人的同步定位與地圖構(gòu)建的有益探索。2 狀態(tài)空間和系統(tǒng)模型本文SLAM系統(tǒng)的狀態(tài)由機(jī)器人位姿和所量測(cè)到靜止環(huán)境特征點(diǎn)的坐標(biāo)組成。在k

9、時(shí)刻的聯(lián)合狀態(tài)向量表示為xTxk=xvk,yvk,vk,x1,y1,x2,y2,",xN,yN=vk (1)n 其中，xvk,yvk,vk分別表示機(jī)器人在二維空間的坐標(biāo)和方向角。地圖是靜止的，參數(shù)n=x1,y1,x2,y2,",xN,yN無(wú)時(shí)間下標(biāo)。機(jī)器人的運(yùn)動(dòng)模型為圓周的轉(zhuǎn)彎模型。xvk1+VkTcos(vk1+k)xk=fv(xvk1,uk)=yvk1+VkTsin(vk1+k) (2)VTvk1+sin(k) k基金項(xiàng)目：陜西省自然科學(xué)基金資助項(xiàng)目(SJ08-ZT13-11)；陜西省教育廳自然科學(xué)基金資助項(xiàng)目(08JK385)；西安市創(chuàng)新計(jì)劃基金資助項(xiàng)目(YF0702

10、1)作者簡(jiǎn)介：弋英民(1976)，男，博士研究生，主研方向：機(jī)器人同步定位，地圖構(gòu)建，信息處理；劉 丁，教授、博士生導(dǎo)師 收稿日期：2009-05-20 E-mail：yiym29其中，k1到k時(shí)刻的時(shí)間間隔為T(mén)；速度Vk和驅(qū)動(dòng)角k為常量，兩者組成控制量uk=Vk,Tk；機(jī)器人前后軸距為B。量測(cè)模型為zik=hi(xk)= (3) arctanyiyvkvk ivk量測(cè)噪聲為vk=k1vk1+k1其中，k1是矩陣；k是白噪聲過(guò)程；方差為R。3 有色量測(cè)噪聲下的機(jī)器人SLAM算法SLAM算法的主要思想：由量測(cè)模型構(gòu)造虛擬的高斯白噪聲量測(cè)模型，為滿足過(guò)程噪聲和量測(cè)噪聲不相關(guān)，對(duì)過(guò)程模型重新組合構(gòu)造

11、虛擬的過(guò)程模型。機(jī)器人的SLAM按照虛擬的過(guò)程模型和量測(cè)模型進(jìn)行狀態(tài)估計(jì)，完成地圖構(gòu)建。 3.1 有色量測(cè)模型的白化處理經(jīng)典的EKF-SLAM算法是假設(shè)量測(cè)模型為白噪聲情況的濾波估計(jì)。為了將式(3)的有色噪聲模型轉(zhuǎn)化白噪聲量測(cè)模型，對(duì)式(3)重新組合，構(gòu)造虛擬的白噪聲量測(cè)模型z*k1=zkk1zk1=(HkFk1k1Hk1)xk1+ H*kk1k1+k1=Hk1xk1+k1(4)其中，Hffk=k|xk|k1；Fk=k|xk1|k1；k=k|xk1|k1；*k1為白噪聲。由于虛擬的量測(cè)噪聲*k1與過(guò)程噪聲k1相關(guān)，因此要采用擴(kuò)展卡爾曼濾波必須對(duì)過(guò)程模型進(jìn)行去相關(guān)處理。將式(4)帶入過(guò)程模型，構(gòu)

12、造虛擬的過(guò)程模型x*k+1=Fkxk+kk+Jk(zkHkxkk)=(FJ*+(*kkHk)xk+JkzkkkJkk)= F*kxk+uk+k(5)其中，Jk是待定系數(shù)矩陣；k是方差為Q的零均值白噪聲；u*k看作虛擬的控制向量；Jk應(yīng)滿足虛擬的過(guò)程噪聲和量測(cè)噪聲不相關(guān)，即期望E*Tk(k)=0 (6)由式(6)求解JkJTHTQTT1k=kQkkk+1(Hk+1kkkHk+1+Rk) (7)將式(7)代入式(5)，即為構(gòu)造的虛擬過(guò)程模型。 3.2 有色量測(cè)噪聲的機(jī)器人SLAM算法CON-SLAM算法的遞推過(guò)程為狀態(tài)向量預(yù)測(cè)、量測(cè)、協(xié)方差預(yù)測(cè)、數(shù)據(jù)關(guān)聯(lián)、虛擬量測(cè)、更新和地圖構(gòu)建。(1)狀態(tài)向量預(yù)

13、測(cè)xk|k1=F*k1xk1 (8)(2)量測(cè)機(jī)器人的量測(cè)數(shù)據(jù)通過(guò)式(3)量測(cè)模型計(jì)算得到。(3)協(xié)方差預(yù)測(cè)由式(7)計(jì)算得到Jk，根據(jù)式(5)推導(dǎo)預(yù)測(cè)協(xié)方差為P=F*(F*T*TTk|k1kPk1|k1k)+JkHkPk1|k1(JkHk)+kQk (9)(4)數(shù)據(jù)關(guān)聯(lián) 算法獲得的地圖是二維的平面地圖，數(shù)據(jù)關(guān)聯(lián)采用文 獻(xiàn)6的最近鄰方法。數(shù)據(jù)關(guān)聯(lián)將量測(cè)z分解為關(guān)聯(lián)量測(cè)zk和新特征點(diǎn)的量測(cè)znk。對(duì)于式(4)的虛擬量測(cè)模型，量測(cè)過(guò)程中可能發(fā)生這樣的情況：在km時(shí)刻機(jī)器人觀測(cè)到圓圈中的特征點(diǎn)，在k時(shí)刻沒(méi)有觀測(cè)該點(diǎn)，在k+m時(shí)刻又觀測(cè)到該點(diǎn)。為滿足虛擬量測(cè)模型式(4)的關(guān)聯(lián)要求。將關(guān)聯(lián)量測(cè)zk分解為

14、zko和zk,k1，則30 zkozk=zk,k1znk其中，zko是過(guò)時(shí)的量測(cè)，在實(shí)際量測(cè)中占整體量測(cè)的比例很小，由文獻(xiàn)7可知，這部分可看作冗余量測(cè)舍棄。(5)虛擬量測(cè)由式(4)虛擬量測(cè)模型進(jìn)行虛擬量測(cè)。 (6)更新對(duì)關(guān)聯(lián)數(shù)據(jù)zk,k1進(jìn)行更新。殘差由虛擬量測(cè)模型得到。x*k|k=xk|k1+Wk(zkHkxk|k1) P*k|k=Pk|k1WkHkPk|k1 S=H*TkkPk|k1(Hk)+RkWTk=Pk|k1(H*)S1k (10)(7)地圖構(gòu)建將量測(cè)到的新特征點(diǎn)構(gòu)建到地圖中。xnewk=h1(znk,xvk)x=xkkxnewk(11) 3.3 算法的一致性檢驗(yàn)機(jī)器人進(jìn)行同步定位和

15、量測(cè)，提出的算法只對(duì)連續(xù)2次量測(cè)到的相同特征點(diǎn)進(jìn)行關(guān)聯(lián)和更新，其余特征點(diǎn)被視為冗余量測(cè)，不進(jìn)行更新。對(duì)于線性高斯濾波，可以通過(guò)NEES(Normalised Estimation Error Squared)評(píng)價(jià)濾波的性能指標(biāo)8。(xkxT1k=k|k)Pk|k(xkxk|k)Nk=1=1iki由文獻(xiàn)8可知，對(duì)于線性高斯濾波，Nk是具有N個(gè)自由度的2分布密度函數(shù)。機(jī)器人位姿是三維空間向量， 50次MonteCarlo仿真的概率為95%，置信區(qū)間為2.36, 3.72，如果大于區(qū)間上限為樂(lè)觀估計(jì)，小于下限為保守估計(jì)。4 仿真及分析機(jī)器人在60 m機(jī)分布 ×70 m的環(huán)境中進(jìn)行模擬實(shí)驗(yàn)，

16、環(huán)境中隨54個(gè)特征點(diǎn)。機(jī)器人的運(yùn)動(dòng)方向由6個(gè)方向標(biāo)記點(diǎn) 控制。實(shí)驗(yàn)仿真采用式(2)的模型，機(jī)器人的速度V=3 m/s，T=0.025s。數(shù)據(jù)關(guān)聯(lián)采用最近鄰方法，最大關(guān)聯(lián)距離為4 m，認(rèn)定新特征點(diǎn)的最小關(guān)聯(lián)距離為25 m。通過(guò)仿真實(shí)驗(yàn)對(duì)機(jī)器人的同步定位與地圖構(gòu)建進(jìn)行算法的一致性評(píng)價(jià)，并與相關(guān)的算法進(jìn)行定位精度比較。4.1 算法的一致性仿真分析為檢驗(yàn)算法中數(shù)據(jù)關(guān)聯(lián)對(duì)濾波估計(jì)的一致性影響，分別對(duì)機(jī)器人進(jìn)行非閉合和閉合運(yùn)動(dòng)仿真實(shí)驗(yàn)。 (1)非閉合運(yùn)動(dòng)仿真對(duì)機(jī)器人進(jìn)行非閉合路徑實(shí)驗(yàn)，分別在2種實(shí)驗(yàn)條件下進(jìn)行50次MonteCarlo仿真，實(shí)驗(yàn)1：有色量測(cè)噪聲為v0.020.02+0.04k=k；實(shí)驗(yàn)2：

17、有色量測(cè)噪聲為vk=0.04+k。 50次機(jī)器人位姿的NEES均值曲線見(jiàn)圖1(a)。為表征所有特征點(diǎn)的特性，取位于中心點(diǎn)兩側(cè)的2個(gè)特征點(diǎn)(-37.3, 38.2)和(13.6, -23)，50次MonteCarlo仿真2個(gè)特征點(diǎn)的聯(lián)合狀態(tài)NEES均值曲線如圖1(b)所示。(a)機(jī)器人位姿(b)2個(gè)特征點(diǎn)的聯(lián)合狀態(tài)圖1 非閉合路徑的NEES均值從圖1(a)和圖1(b)可知，機(jī)器人位姿的NEES均值服從2分布，實(shí)驗(yàn)二的量測(cè)噪聲均值為實(shí)驗(yàn)1的2倍，但是2條曲線仍在2.36, 3.72之間；2個(gè)特征點(diǎn)的聯(lián)合狀態(tài)NEES均值曲線在量測(cè)噪聲均值增大的情況下仍趨近于4。由文獻(xiàn)8估計(jì)理論可認(rèn)為該算法是一致性估

18、計(jì)。(2)閉合運(yùn)動(dòng)仿真機(jī)器人的閉合路徑實(shí)驗(yàn)見(jiàn)圖2。分別在2種實(shí)驗(yàn)條件下進(jìn)行MonteCarlo仿真50次，實(shí)驗(yàn)3：按照實(shí)驗(yàn)1的條件進(jìn)行閉合路徑實(shí)驗(yàn)；實(shí)驗(yàn)4：按照實(shí)驗(yàn)2的條件進(jìn)行閉合路徑實(shí)驗(yàn)。圖2 機(jī)器人的閉合運(yùn)動(dòng)路徑機(jī)器人位姿的NEES均值如圖3所示。50次MonteCarlo仿真的2個(gè)特征點(diǎn)的聯(lián)合狀態(tài)NEES均值如圖4所示。從圖3和圖4可知，實(shí)驗(yàn)4的量測(cè)噪聲均值為實(shí)驗(yàn)3的2倍，但是2種實(shí)驗(yàn)條件下機(jī)器人位姿的NEES均值曲線都處于2.36,3.72之間，2個(gè)特征點(diǎn)的聯(lián)合狀態(tài)NEES均值曲線處于 4附近。比較圖1(a)和圖3，閉合路徑的2分布曲線雖然比非閉合路徑的2分布曲線波動(dòng)大，但曲線基本處于

19、2.36, 3.72之間，由文獻(xiàn)8估計(jì)理論可認(rèn)為該算法是一致性估計(jì)。 4.2 有色噪聲條件下幾種算法的仿真分析為評(píng)價(jià)CON-SLAM算法優(yōu)劣，以機(jī)器人位姿的均方根誤差(RMS)作為評(píng)價(jià)依據(jù)。分別采用EKF-SLAM算法、Fast-SLAM算法和CON-SLAM算法進(jìn)行200次MonteCarlo仿真。實(shí)驗(yàn)條件1量測(cè)距離為有色，角度為白噪聲的情況 圖5是量測(cè)距離為有色噪聲情況下機(jī)器人位姿的RMS曲線。由圖5可知，在有色噪聲均值相對(duì)較小的情況下，采用Fast-SLAM算法機(jī)器人位姿的RMS曲線單調(diào)增大且發(fā)散；采用EKF-SLAM算法和CON-SLAM算法機(jī)器人位姿的RMS曲線都收斂，但在第800步

20、開(kāi)始分叉，說(shuō)明有色噪聲的誤差累積引起EKF-SLAM算法誤差增大；本文算法曲線完全位于EKF-SLAM算法下方，表明該算法具有更好的定位精度。圖5 量測(cè)距離為有色噪聲的機(jī)器人位姿RMS31實(shí)驗(yàn)條件2 量測(cè)距離為白噪聲，角度為有色噪聲的 情況圖6是量測(cè)角度為有色噪聲情況下機(jī)器人位姿的RMS曲線。從圖6可知，采用EKF-SLAM算法和Fast-SLAM算法機(jī)器人位姿的RMS曲線都單調(diào)增大且發(fā)散；采用CON-SLAM算法機(jī)器人位姿的RMS曲線收斂；本文算法具有更好的魯棒性。5 結(jié)束語(yǔ)本文提出一種針對(duì)有色量測(cè)噪聲下的機(jī)器人同步定位與地圖構(gòu)建算法。通過(guò)構(gòu)造虛擬的過(guò)程模型和量測(cè)模型，將有色噪聲模型轉(zhuǎn)化為白

21、噪聲模型。機(jī)器人SLAM算法按照虛擬 的過(guò)程模型和量測(cè)模型進(jìn)行同步定位與地圖構(gòu)建。仿真實(shí)驗(yàn)驗(yàn)證了數(shù)據(jù)關(guān)聯(lián)的冗余處理是濾波的一致性估計(jì)。與EKF-SLAM算法和Fast-SLAM算法相比，本文算法具有較好的魯棒性。參考文獻(xiàn)1 Gehrig S K, Stein F J. Dead Reckoning and Cartography UsingStereo Vision for an Autonomous CarC/Proceedings of the IEEE International Conference on Intelligent Robots and Systems. Piscataw

22、ay, NJ, USA: IEEE Press, 1999: 1507-1512.2 許俊勇, 王景川, 陳衛(wèi)東. 基于全景視覺(jué)的移動(dòng)機(jī)器人同步定位與地圖創(chuàng)建研究J. 機(jī)器人, 2008, 30(4): 289-297.3 Davison A J. Real-time Simultaneous Localization and Mappingwith a Single CameraC/Proceedings of the 9th IEEE International Conference on Computer Vision. Nice, France: s. n., 2003: 1403-1

23、410.4 Leung C, Huang Shoudong, Dissanayake G. Active SLAM inStructured EnvironmentsC/Proc. of 2008 IEEE International Conference on Robotics and Automation. Pasadena, CA, USA: s. n., 2008: 1989-1903.5 He Feng, Fang Yongchun, Wang Yutao, et al. Practical Feature-basedSimultaneous Localization and Map

24、ping Using Sonar DataC/ Proceedings of the 27th Chinese Control Conference. Kunming, China: s. n., 2008: 421-425.6 Singer R A, Sea R G. A New Filter for Optimal Tracking in DenseMulti-target EnvironmentC/Proceedings of the 9th Allerton Conference on Circuit and System Theory. Illinois, USA: s. n., 1

25、971: 201-211.7 Dissanayake G, Durrant-Whyte H, Bailey T. A ComputationallyEfficient Solution to the Simultaneous Localization and Map Building(SLAM) ProblemC/Proc. of IEEE International Conference on Robotics and Automation. S. l.: IEEE Press, 2000: 1009-1014.8 Bar-Shalom Y, Li Rong, Kirubarajan T. Estimation with Applicationsto Tracking and