特征序列數據關聯機器人同步定位與地圖構建*

弋英民,黃 瑩,王智敏,張孟志

(西安理工大學自動化與信息工程學院,西安 710048)

?

特征序列數據關聯機器人同步定位與地圖構建*

弋英民*,黃 瑩,王智敏,張孟志

(西安理工大學自動化與信息工程學院,西安 710048)

針對噪聲不確定性增大的數據關聯問題,提出特征點序列數據關聯機器人同步定位與地圖構建方法。根據機器人環境特征點的空間幾何信息,基于圖論建立特征點間的信息相關性。利用相鄰兩步的特征點觀測信息協方差的變化,轉化成求解特征點TSP問題和特征序列最大相關函數,以此確定所觀測特征點的數據關聯。實驗證明,提出的方法可在噪聲不確定性增大的情況下,保證同步定位與地圖構建算法的一致性。

特征序列;數據關聯;同步定位與地圖構建;機器人

數據關聯方法是機器人的同步定位與地圖構建(SLAM)的關鍵環節。Smith[1-4]等人提出的擴展卡爾曼濾波估計(EKF-SLAM)是解決SLAM問題的基礎理論方法。文獻[5-6]分析了EKF-SLAM和FastSLAM的一致性估計問題,研究表明數據關聯方法直接影響算法的一致性估計精度。數據關聯是確定傳感器接收到的量測信息與目標源對應關系的過程。文獻[5]研究表明數據關聯是解決SLAM問題的關鍵,不正確的數據關聯將使構圖和機器人的定位估計發散,甚至導致整個SLAM過程失敗。機器人SLAM研究所采用的數據關聯算法已取得一定成果,主要為三類,一是常用的最近鄰數據關聯[7];二是概率統計數據關聯[8];三是基于圖論的相關性數據關聯。文獻[9]提出的最近鄰數據關聯(NN),實現簡單但抗干擾能力差。概率數據關聯PDA方法適用于單目標問題。針對多目標問題的聯合概率數據關聯(JPDA)因其難以確切得到聯合事件與關聯事件的概率,并且由于回波密度增加引起組合爆炸,學者提出了折中近似算法。文獻[10]提出Takagi-Sugeno數據關聯(TSDA),相比JPDA提高了精度,降低了計算復雜度。文獻[11]提出了3SCAN-JPDA算法,可用于實時動態環境,降低了算法計算量。文獻[12]針對雜亂環境提出一種聯合相容分枝定界的數據關聯方法JCBB(Joint Compatibility Branch and Bound)。文獻[13]為獲得較高關聯準確率,提出一種基于蟻群、遺傳算法的多目標數據關聯算法(AC-GADA)。文獻[14-16]利用特征點特征、特征點群的布局以及特征點預測與觀測的偏差界限等信息,提出了概率數據關聯的改進算法。文獻[17-18]提出了基于圖論的數據關聯算法MCS(Maximum Common Subgraph),但是搜索兩幅完全圖的最大公共完全子圖的NP問題困難。MCS和JCBB都利用所有可用的相關信息進行批量數據關聯。JCBB通過預設假設并在搜索樹上解釋這一假設,以最優的解釋對應的假設為可信的數據關聯;MCS分為兩步,首先編制成對約束,之后為最大兼容約束組織搜索。對于JCBB、MCS和NN數據關聯方法,在噪聲不確定性增大時,由于觀測信息和特征點本身相互獨立,造成匹配不成功或估計誤差增大。

針對系統噪聲不確定性增大的情況,利用相鄰兩個時刻的信息協方差差異,提出一種特征序列數據關聯方法(LSDA)。方法要點是:解預測特征點和觀測特征點的TSP問題,獲取TSP序列,對兩組序列求最大相關函數,并標記已觀測特征點與新觀測特征點,最后進行特征點數據關聯,更新地圖。

1 特征序列數據關聯方法

機器人同步定位與地圖構建是以數據關聯為基礎。針對系統噪聲不確定性增大的情況,將求解特征點的TSP問題引入機器人的SLAM問題中,通過計算TSP序列的最大相關函數,獲得特征點數據關聯,更新地圖。

1.1 經典EKF-SLAM方法

基于EKF的機器人同步定位與地圖構建方法以最小均方差為準則,實現機器人位姿在時域的最優遞推過程。該方法主要分為預測和更新兩步。將控制信號或里程計的信息輸入到機器人系統的狀態方程,完成對位姿和地圖特征的預測;通過對環境特征的觀測和提取,來更新機器人位姿和特征地圖[4]。

預測:

(1)

Pxx,k|k-1=fPxx,k-1|k-1fT+Qk

(2)

更新:

(3)

(4)

其中

Sk=hPk|k-1hT+Rk

(5)

Wk=Pk|k-1

(6)

1.2 基于模擬退火方法的特征點TSP問題

機器人SLAM問題中的特征點可類比成TSP問題中的城市。特征點坐標表示城市所處位置,并可通過坐標計算城市間距離。SLAM問題中,總是假設特征點是靜止不動的。因此,特征點TSP問題的最優解是唯一的。由于存在觀測噪聲,觀測區域內的所有特征點作為已有條件定義TSP問題。模擬退火算法特別適合處理全局優化、離散變量優化問題[19-20]。特征點TSP問題的模擬退火方法示意圖如圖1所示。

圖1 基于模擬退火方法的特征點TSP問題

對預測特征點(觀測區域的已觀測特征點預測)求解TSP問題,得到最優的TSP路徑。對觀測特征點求解TSP問題,同樣可以得到一條最優的TSP路徑。

1.3 特征點序列的相關函數

將特征點的坐標看作自變量,特征點就是二維的離散點。根據TSP路徑,可以得到一組特征點的排列。每一個排列就是一條特征點序列,即特征點間的連線代表一個固定時間段。序列即信號,最大相關函數所對應的兩條特征點序列便可確定公共特征點的關聯。

1.4 特征點序列數據關聯方法

特征點序列的數據關聯方法(LSDA)主要過程是:求解預測特征點和觀測特征點的TSP問題,取得TSP序列。對兩組序列求最大相關函數,并標記已觀測特征點與新觀測特征點,最后進行特征點關聯,更新地圖。

具體步驟如下:

Step 1:初始化,數據關聯

預測已觀測過的特征點,保留在當前觀測區域的已觀測特征點,記為預測特征點。

(7)

對特征點進行觀測,得到觀測特征點。將特征點看作是TSP問題的城市,用特征點坐標表示城市所處位置并計算城市間距離。

Step 2:解特征點TSP問題

對預測特征點和觀測特征點兩種組合求解特征點TSP問題,得到特征點排列。將特征點排列看作關于坐標的二維序列,得到兩組序列。

Step 3:計算序列的相關函數

對特征點坐標的兩組序列求相關函數,取最大相關函數對應的序列。根據預測特征點組合獲得序列長度,將觀測特征點組合序列分為已觀測特征點zko和新觀測特征點zk,k-1。則

(8)

Step 4:解已觀測特征點TSP問題

對已觀測特征點求解特征點TSP問題,得到已觀測特征點序列排列。

Step 5:計算已觀測特征點序列

對已觀測特征點序列排列和預測特征點序列求相關函數,取最大相關函數對應的已觀測特征點序列。

Step 6:關聯地圖

將已觀測特征點序列和預測特征點序列進行地圖構建特征點關聯,并在地圖中加入新觀測特征點。

(9)

為了驗證算法的一致性估計問題,假設線性高斯濾波,采用NEES(Normalised Estimation Error Squared)來評價濾波估計的性能指標[21],即

(10)

(11)

2 實驗及分析

圖2是MT-R機器人實驗驗證平臺,為兩輪驅動的MT-R移動機器人,配備有兩自由度云臺攝像機、超聲波傳感器和測速編碼器等傳感器。在實驗中,記機器人位姿為(x,y,θ)T,分別對應平面坐標(x,y)和方向角θ。

圖2 實驗驗證平臺

實驗對幾種典型數據關聯方法進行對比分析。將NN、JCBB和LSDA方法分別應用到機器人SLAM。第一組低噪聲實驗初始條件為P0=diag[1e-4,1e-4,1e-4],Q0=diag[0.32,(3.0*pi/180)2],R=diag[0.12,(1.0*pi/180)2]。第二組高噪聲實驗初始條件為P0=10*diag[1e-4,1e-4,1e-4],Q0=10*diag[0.32,(3.0*pi/180)2],R=10*diag[0.12,(1.0*pi/180)2]。

實驗是噪聲不確定性增大時幾種數據關聯方法的機器人SLAM實驗數據。*表示地圖中的特征點。+表示地圖中的特征點預測位置,用橢圓表示特征點預測位置區間。線段表示方向點連線。曲線表示機器人實際路徑。圖3(a)是采用NN數據關聯的機器人SLAM構圖,圖3(b)是采用NN數據關聯的機器人SLAM局部放大圖;圖4(a)是采用JCBB數據關聯的機器人SLAM構圖,圖4(b)是采用JCBB數據關聯的機器人SLAM局部放大圖;圖5(a)是采用LSDA數據關聯的機器人SLAM構圖,圖5(b)是采用LSDA數據關聯的機器人SLAM局部放大圖。

圖3 高噪聲時NN-SLAM實驗

圖4 高噪聲時JCBB-SLAM實驗

圖5 高噪聲時LSDA-SLAM實驗

從圖5相比圖3和圖4可以看出,采用NN數據關聯和JCBB數據關聯方法機器人構建的地圖中,估計值與實際值有一定的偏差;而采用LSDA數據關聯的機器人SLAM構建的地圖中,估計值與實際特征點位置基本重合。表明提出的方法優于最近鄰數據關聯和JCBB數據關聯方法。

為驗證提出方法的一致性估計問題,對三種方法采用50次實驗,比較在不同噪聲條件下機器人位姿的一致性估計。圖6是在低噪聲條件下采用三種數據關聯方法的機器人位姿的NEES的實驗數據。

從圖6可以看出,在低噪聲條件下,采用NN數據關聯、JCBB數據關聯的機器人位姿的NEES曲線大部分不在置信區間內,可認為是保守估計;采用LSDA數據關聯方法的機器人位姿的NEES曲線大部分在置信區間內,可認為是樂觀估計。

圖7是在高噪聲條件下采用三種數據關聯方法的機器人位姿的NEES的實驗數據。

圖6 低噪聲下數據關聯方法的一致性估計

從圖7可以看出,在高噪聲情況下,采用NN數據關聯、JCBB數據關聯的機器人位姿的NEES曲線局部劇烈震蕩,大部分不在置信區間內,可認為是保守估計;采用LSDA數據關聯方法的機器人位姿的NEES曲線大部分在置信區間內,可認為是樂觀估計。

圖7 高噪聲下數據關聯方法的一致性估計

綜上實驗結果,所提出的方法在噪聲不確定性增大的情況下,機器人SLAM算法仍可保持樂觀的一致性估計,提出的方法總體上優于NN數據關聯和JCBB數據關聯方法。

3 總結

針對機器人SLAM系統噪聲不確定性增大的情況,提出了一種特征點序列數據關聯的機器人同步定位與地圖構建方法。利用類比城市間的TSP問題,求解特征點的相關函數進行觀測和預測特征點的數據關聯,并更新地圖。通過在機器人實驗平臺測試表明,提出的機器人特征序列數據關聯方法可在系統噪聲不確定性增大的情況下,保證算法是樂觀的一致性估計。

[1] Uyen H S V,Jeon J W. Combine Kalman Filter and Particle Filter to Improve Color Tracking Algorithm[C]//Proceedings of International Conference on Control,Automation and Systems 2007,2007:558-561.

[2]Anati R,Scaramuzza D,Derpanis K G,et al. Robot Localization Using Soft Object Detection[C]//2012 IEEE International Conference on Robotics and Automation(ICRA). 2012:4992-4999.

[3]Ivanjko E,Uasak M,Petrovic I. Kalman Filter Theory Based Mobile Robot Pose Tracking Using Occupancy Grid Maps[C]//International Conference on Control and Automation(ICCA2005),2005:869-874.

[4]Smith R,Chesseman P M. Estimating Uncertain Spatial Relationships in Robotics[J]. Ulncertainty in Artificial Intelligence,1988(2):435-461.

[5]Bailey T,Nieto J,Guivant J,et al. Consisteney of the EKF-SLAM Algorithm[C]//IEEE/RSJ Int Conf on Intelligent Robots and Systems. Beijing:IEEE,2006:3562-3568.

[6]Bailey T,Nieto J,Nebot E. Consistency of the Fast SLAM Algorithm[C]//Proc IEEE Int Conf on Robotics and Automation. Orlando:IEEE,2006:424-429.

[7]弋英民,劉丁. 有色過程噪聲下的輪式機器人同步定位與地圖構建[J]. 電子學報,2010,38(6):1339-1243.

[8]李輝,張安,沈瑩,等. 基于交互式自適應概率數據關聯的目標跟蹤算法[J]. 傳感技術學報,2007,20(1):172-176.

[9]Singer R A,Sea R G. A New Filter for Optimal Trackingin Dense Multitarget Environments[C]//Proceedingss of Theninth Allerton Conference Circuit and SystemTheory. Urbana-Champaign,USA. 1971:201-211.

[10]Watanabe K,Pathiranage C D,Izumi K. T-S Fuzzy Model Adopted SLAM Algorithm with Linear Programming Based Data Association for Mobile Robots[C]//ISIE 2009. IEEE International Symposium on Industrial Electronics,IEEE,2009:244-249.

[11]Wong R H,Xiao Jizhong,Joseph S L. A Robust Data Association for Simultaneous Localization and Mapping in Dynamic Environments[C]//2010 IEEE International Conference on Information and Automation(ICIA),IEEE,2010:470-475.

[12]Neira J,Tards J D. Data Association in Stochastic Mapping Using the Joint Compatibility Test. IEEE Transactions on Robotics and Automation,17(6):890-897,2001.

[13]袁述,袁東輝,孫基洲,等. 蟻群-遺傳算法在多傳感器多目標跟蹤技術中的應用[J]. 電子學報,2013,41(3):609-614.

[14]Maksarov D,Durrant-Whyte H. Mobile Vehicle Navigation in Unknown Environments:A Multiple Hypothesis Approach[J]. IEEE Proc:Control Theory Application,1995,142(4):385-391.

[15]Guivant J,Nebot E. Optimization of the Simultaneous Localization and Map-Building Algorithm for Real-Time Implementation[J]. IEEE Transactions on Robotics and Automation,2003,17(3):242-257.

[16]Castellanos J A,Montiel J M M,Neira J,et al. The SPmap:A Probabilistic Framework for Simultaneous Localization and Map Building[J]. IEEE Transactions on Robotics and Automation,1999,15(5):948-952.

[17]Bailey T,Nebot E. Localisation in Large-Scale Environments[J]. Robotics and Autonomous Systems,2001,37(4):261-281.

[18]Bailey T,Nebot E M,Rosenblatt J K,et al. Data Association for Mobile Robot Navigation:A Graph Theoretic Approach[C]//In IEEE International Conference on Robotics and Automation,volume 3,2000:2512-2517.

[19]Jeong C S,Kim M H. Fast Parallel Simulated Annealing for Traveling Salesman Problem[C]//IJCNN International Joint Conference. San Diego,CA. 1990:947-953.

[20]李芳芳,王靖. 一種基于模擬退火算法的無線傳感器網絡最優簇類求解方案[J]. 傳感技術學報,2011,24(6):900-904.

[21]Bar-Shalom Y,Li X R,Kirubarajan T. Estimation with Applications to Tracking and Navigation[M]. Toronto:John Wiley and Sons,2001:234-235.

弋英民(1976-),1998年于西安交通大學獲得學士學位,2004年于西安交通大學獲得碩士學位,現為西安理工大學副教授,研究方向為機器人同步定位與地圖構建,yiym@xaut.edu.cn。

LandmarkSequenceDataAssociationMethodforRobotSimultaneousLocalizationandMapBuilding*

YIYingmin*,HUANGYing,WANGZhimin,ZHANGMengzhi

(Faculty of Automation and Information Engineering,Xi’an University of Technology,Xi’an 710048,China)

For the noise uncertainty increases,a landmark sequence data association(LSDA)method for robot simultaneous localization and map building(SLAM)is proposed. As robot simultaneous localization and map building,the spatial geometry of the landmarks are considered. Then the correlation among landmarks based on graph theory is established. Between the adjacent two-step observations,the difference of innovation covariance is transformed into maximum correlation function of sequence by solving the TSP problem. Then landmark data association is performed. The experiments show that the proposed method can be to ensure the consistency of estimation in the case of uncertainty noise increasing.

landmark sequence;data association;simultaneous localization and mapping;robot

項目來源:國家自然科學基金項目(51275405);陜西省教育廳自然科學專項項目(2013Jk1078)

2014-07-10修改日期:2014-09-05

10.3969/j.issn.1004-1699.2014.11.014

TP24

:A

:1004-1699(2014)11-1517-05