張　鳳， 孫　陽， 袁　帥， 李昌國， 趙嵐光

(沈陽建筑大學(xué) 信息與控制工程學(xué)院， 沈陽 110168)

基于能觀測性分析的機(jī)器人EKF-SLAM算法*

張鳳， 孫陽， 袁帥， 李昌國， 趙嵐光

(沈陽建筑大學(xué) 信息與控制工程學(xué)院， 沈陽 110168)

針對傳統(tǒng)EKF-SLAM算法中存在狀態(tài)估計不一致的問題，從系統(tǒng)能觀測性角度分析，提出一種增加觀測性約束條件的算法，利用補償矩陣U最優(yōu)化求解約束條件，得到新的線性點，并通過優(yōu)化系統(tǒng)的雅克比矩陣重構(gòu)系統(tǒng)能觀測矩陣，使得EKF-SLAM系統(tǒng)與非線性SLAM系統(tǒng)觀測方程能觀矩陣的秩保持一致.結(jié)果表明，所提出算法在狀態(tài)估計的精確性和協(xié)方差一致性方面明顯優(yōu)于傳統(tǒng)的EKF-SLAM算法，研究工作和結(jié)論對車輛自主駕駛有一定的參考價值.

同時定位與建圖； 機(jī)器人控制； 擴(kuò)展卡爾曼濾波器； 能觀測性分析； 最優(yōu)估計； 數(shù)據(jù)融合； 估計不一致； 狀態(tài)方程

移動機(jī)器人SLAM(Simultaneous localization and mapping，SLAM)是指機(jī)器人通過傳感器測量值在創(chuàng)建環(huán)境地圖同時估計自身位置和方向的方法[1].Smith與Cheeseman[2]等人在1987年提出描述機(jī)器人與周圍路標(biāo)位置之間幾何特征關(guān)系及其不確定性的概率方法，并首次引入隨機(jī)建圖策略解決SLAM問題.SLAM問題研究已經(jīng)取得了飛速進(jìn)展，近年來，SLAM研究的熱點問題主要集中于最優(yōu)估計算法設(shè)計、精確數(shù)據(jù)融合及傳感器數(shù)據(jù)處理精度問題.對于在未知環(huán)境中自主駕駛的車輛，SLAM技術(shù)是車輛實現(xiàn)自主駕駛不可或缺的關(guān)鍵技術(shù)[3-13].

在解決SLAM問題的眾多算法中，EKF(Extended kalman filter，EKF)算法是應(yīng)用最廣泛的算法之一.Bar-Shalom等人提出：如果估計誤差均值為零，并且實際協(xié)方差小于或等于濾波器估算的協(xié)方差，則狀態(tài)估計是一致的，一致性是評價SLAM算法效果的主要標(biāo)準(zhǔn).大量研究仿真實驗結(jié)果[14-19]表明：傳統(tǒng)EKF-SLAM算法的狀態(tài)估計存在不一致性，針對該問題，本文從觀測方程的能觀測性角度出發(fā)，分析引起傳統(tǒng)EKF-SLAM算法產(chǎn)生狀態(tài)不一致的本質(zhì)原因，并提出一種解決方案.

通過分析傳統(tǒng)EKF-SLAM算法中觀測方程的雅克比矩陣得知該矩陣具有二維不可觀測子空間，而實際上非線性系統(tǒng)中觀測方程的雅克比矩陣具有三維不可觀測子空間，分別對應(yīng)全局坐標(biāo)系中機(jī)器人的位置(x，y)和方向θ，因此，傳統(tǒng)EKF-SLAM算法的不可觀測子空間與實際系統(tǒng)的不可觀測子空間維數(shù)不一致，導(dǎo)致協(xié)方差估計減小不合理，使得算法估算結(jié)果出現(xiàn)不一致.針對此問題，本文首先提出對傳統(tǒng)EKF-SLAM算法中觀測方程的觀測矩陣增加能觀測性約束條件，構(gòu)建補償矩陣U，使EKF-SLAM系統(tǒng)觀測方程能觀矩陣的秩與非線性SLAM系統(tǒng)觀測方程能觀矩陣的秩保持一致，然后，采用基于數(shù)據(jù)融合的狀態(tài)延時估計方法提高U矩陣中路標(biāo)初始位置的精確度，從而改善算法估計結(jié)果，再通過對約束條件最優(yōu)化求解得到算法線性化時新的線性點，并計算狀態(tài)模型和觀測模型的雅克比矩陣，重構(gòu)系統(tǒng)局部能觀測矩陣，最后通過仿真實驗，對比傳統(tǒng)算法與改進(jìn)算法的估算結(jié)果，驗證了改進(jìn)算法在精確性和協(xié)方差一致性方面明顯優(yōu)于傳統(tǒng)的EKF-SLAM算法.

1　傳統(tǒng)EKF-SLAM算法

文中以觀測單一路標(biāo)為例，對連續(xù)時間的非線性SLAM系統(tǒng)進(jìn)行分析，系統(tǒng)狀態(tài)預(yù)測方程表示為

(1)式中：v為線速度；ω為角速度.在傳統(tǒng)EKF-SLAM算法中，全局坐標(biāo)系下的狀態(tài)變量包含機(jī)器人位姿以及路標(biāo)位置.k時刻狀態(tài)變量定義為

(2)

移動機(jī)器人定位系統(tǒng)的預(yù)測方程及觀測方程可以表示為

xk=f(xk-1，k-1)+Γ(xk-1，k-1)wk-1

(3)

zk=h(xk，k)+vk

(4)

式中：wk、vk為零均值白噪聲序列；f、h、Γ為任意測量函數(shù)；zk為與相對距離和偏轉(zhuǎn)角度相關(guān)的測量值，可由測距傳感器獲得.

1.1EKF預(yù)測

預(yù)測過程主要是利用預(yù)測模型更新預(yù)測方程中當(dāng)前狀態(tài)的先驗值，實時推算下一時刻的狀態(tài)變量和誤差協(xié)方差，以便為下一個時刻最優(yōu)化估算奠定基礎(chǔ).由于路標(biāo)位置不發(fā)生變化，EKF預(yù)測方程形式為

(5)

(6)

(7)

算法非線性模型的狀態(tài)預(yù)測方程為

(8)

(9)

(10)

(11)

1.2EKF更新

EKF更新是通過預(yù)測方程與觀測方程對系統(tǒng)狀態(tài)變量進(jìn)行最優(yōu)估計.預(yù)測方程已經(jīng)在上文進(jìn)行了說明，觀測方程描述的是機(jī)器人與周圍環(huán)境路標(biāo)相對距離的觀測值.在EKF更新時，利用預(yù)測方程的先驗值與當(dāng)前路標(biāo)觀測的測量值以及測量協(xié)方差對機(jī)器人位姿狀態(tài)進(jìn)行最優(yōu)估計，即

zk=h(xk)+vk=h(RkPL)+vk

(12)

式中，RkPL=CT(φRk)(PL-PRk)，為k時刻路標(biāo)相對于機(jī)器人的位置.測量函數(shù)是非線性的，首先進(jìn)行線性化處理，線性測量誤差方程描述為

(13)

式中，HRk，HLk為關(guān)于h的雅克比矩陣，分別對應(yīng)機(jī)器人位姿和路標(biāo)位置，表示為

HRk=(

(14)

HLk=(

(15)

2　SLAM能觀測性分析

2.1非線性SLAM系統(tǒng)能觀測性分析

根據(jù)Hermann和Krener提出EKF-SLAM觀測方程的能觀測性秩條件，對連續(xù)時間條件下的非線性SLAM系統(tǒng)進(jìn)行能觀測性分析.機(jī)器人位姿狀態(tài)模型采用式(1)，測量值是路標(biāo)與機(jī)器人之間的相對位置，觀測模型表示為

z(t)=h(ρ，ψ)

(16)

(17)

ψ=atan2(yL-yR，xL-xR)-φR

(18)

式中，ρ和ψ為機(jī)器人與路標(biāo)的相對距離和相對方位角度，二者之間的相對距離關(guān)系表示為

(19)

則觀測模型可表示為

z(t)=CTφR(t)(PL(t)-PR(t))=

(20)

針對式(20)，采用李導(dǎo)數(shù)方法計算能觀測矩陣.由計算結(jié)果分析可知，n階李導(dǎo)數(shù)和一階李導(dǎo)數(shù)具有線性關(guān)系，為計算方便，采用一階李導(dǎo)數(shù)計算能觀測矩陣，去掉全0行，得到觀測矩陣為

(21)

(22)

式(21)是一個非滿秩矩陣，具有三個不可觀測自由度，即機(jī)器人在全局坐標(biāo)系下的狀態(tài)向量(位置與方向)在SLAM系統(tǒng)方程中是不可觀測的.

2.2EKF-SLAM能觀測性分析

由式(21)可知，非線性SLAM系統(tǒng)觀測方程具有三個不可觀測自由度，理論上應(yīng)用EKF算法對SLAM系統(tǒng)線性化后，系統(tǒng)應(yīng)仍然具有三個不可觀測自由度.分別對理想EKF-SLAM系統(tǒng)和傳統(tǒng)EKF-SLAM系統(tǒng)觀測方程的觀測矩陣維數(shù)進(jìn)行分析.

采用觀測矩陣對時變線性化誤差狀態(tài)系統(tǒng)進(jìn)行能觀測性分析，k到k+i時刻的觀測矩陣定義為

Diag(HLk，HLk+1，…，HLk+i)·

(23)

Diag(·)是一個區(qū)塊對角陣，當(dāng)且僅當(dāng)局部觀測矩陣N是滿秩矩陣時，系統(tǒng)從k到k+i時刻是局部能觀的.矩陣Diag(HLk，HLk+1，…，HLk+i)是非奇異的，顯而易見，Rank(N)=Rank(M)，矩陣N和M具有相同的秩，因此，分析矩陣M與分析矩陣N具有相同的意義.同時根據(jù)式(23)也可以得到觀測矩陣是一個關(guān)于線性點的函數(shù)，線性點選取精度對EKF線性誤差狀態(tài)方程的能觀測性產(chǎn)生影響.

2.3理想EKF-SLAM

(24)

由此觀測矩陣可表示為

[-I2，-J(PL-PRk)]

(25)

N=Diag(HLk，HLk+1，…，HLk+i)·

(26)

根據(jù)rank(M)=2得到rank(N)=2，理想EKF-SLAM系統(tǒng)的局部觀測矩陣秩為2，具有三個不可觀測自由度.

2.4傳統(tǒng)EKF-SLAM

傳統(tǒng)的EKF-SLAM的線性化點取在系統(tǒng)狀態(tài)變量的最優(yōu)估計值處，則有

ΦRk+i-1…ΦRk+1ΦRk=

(27)

由此觀測矩陣可表示為

(28)

(29)

根據(jù)rank(M)=3得到rank(N)=3，則理想EKF-SLAM系統(tǒng)的觀測矩陣秩為3，具有兩個不可觀測自由度.因此傳統(tǒng)EKF-SLAM算法在估計值位置估算的雅克比矩陣比理想SLAM系統(tǒng)中雅克比矩陣的觀測維數(shù)多了一維，引入虛假信息，導(dǎo)致系統(tǒng)狀態(tài)估計不一致.

為了使傳統(tǒng)EKF算法線性化后的SLAM系統(tǒng)中觀測方程的能觀矩陣秩保持不變，增加了能觀測約束條件，通過補充U矩陣重構(gòu)觀測矩陣，實現(xiàn)線性化前后的SLAM系統(tǒng)觀測矩陣秩不變.同時為了改善U矩陣的補償效果，采用基于數(shù)據(jù)融合的路標(biāo)位置延時狀態(tài)估計與最優(yōu)選取線性點位置的方法重構(gòu)觀測矩陣.

3　擴(kuò)展卡爾曼濾波器設(shè)計

3.1增加能觀測性約束條件的U矩陣補償

(30)

通過增加能觀測性約束條件，引入5×3的滿秩矩陣U，SLAM系統(tǒng)觀測矩陣的秩由3降為2，實現(xiàn)了線性化前后的SLAM系統(tǒng)具有相同的不可觀測自由度維數(shù).

證明當(dāng)SLAM系統(tǒng)滿足式(30)時，觀測矩陣所有行具有相同的不可觀測向量個數(shù)，該數(shù)量與U的列數(shù)相同.機(jī)器人和路標(biāo)的真實位置在未知環(huán)境中不能獲得，因此，選取路標(biāo)和機(jī)器人第一次被估計位置分別作為路標(biāo)和機(jī)器人的初始位置，仿照式(22)構(gòu)造U矩陣.

當(dāng)系統(tǒng)觀測單一路標(biāo)時，U的表達(dá)式為

(31)

當(dāng)系統(tǒng)同時觀測M個路標(biāo)時，U的表達(dá)式為

(32)

3.2基于數(shù)據(jù)融合的路標(biāo)位置延時狀態(tài)估計

傳感器測量誤差會降低路標(biāo)位置的初始估計精度，而直接使用此測量數(shù)據(jù)，會引入較大誤差.通過基于數(shù)據(jù)融合的延時狀態(tài)估計方法估算路標(biāo)位置初始值將提高其計算精度.

機(jī)器人在(x1，y1)，(x2，y2)兩個位置分別觀測同一個特征(x，y)，觀測值分別為r1，r2，則相應(yīng)觀測路標(biāo)的位置為PL1(xL1，yL1)，PL2(xL2，yL2).若定義兩個圓方程為

(33)

(34)

(35)

3.3線性化點最優(yōu)選取

(36)

并服從于

(37)

選取x*為線性點，并采用泰勒級數(shù)展開式

f(x)=f(x*)+f′(x*)(x-x*)+

(39)

根據(jù)二階導(dǎo)數(shù)的幾何意義，特征量表征了運動曲線的曲率.考慮到機(jī)器人運動軌跡的平滑性，運動曲線的曲率較小，若略去f″(ζ)對式(38)的影響，主要考慮(x-x*)2項，則式(38)可表示為

(40)

4　系統(tǒng)仿真實驗研究與分析

本實驗以MATLAB為平臺，在Tim Bailey等人提供的EKF-SLAM平臺基礎(chǔ)上進(jìn)行程序設(shè)計，實現(xiàn)上述算法功能.仿真基于200 m×200 m的室內(nèi)環(huán)境，圖1和圖2分別為同一環(huán)境條件下采用傳統(tǒng)算法與本文算法進(jìn)行實驗仿真的小車運動路徑(星號為地標(biāo)的理想位置，加號為經(jīng)標(biāo)準(zhǔn)卡爾曼濾波算法獲得的估計位置，粗線表示機(jī)器人估計路徑，細(xì)線表示機(jī)器人預(yù)計路徑).

圖1　EKF-SLAM仿真結(jié)果

圖2　改進(jìn)EKF-SLAM仿真結(jié)果

圖2與圖1相比，路標(biāo)位置的估計值更加靠近路標(biāo)真實值.當(dāng)采用傳統(tǒng)EKF-SLAM算法時，隨著小車不斷運動，算法線性化誤差不斷累計增加，路標(biāo)位置的觀測和小車自身位姿的估計誤差不斷積累，與預(yù)計值的偏離誤差較大.而本文提出的算法明顯提高了路標(biāo)位置與機(jī)器人位姿的估計精度.

圖3　位置估計偏差

圖4　EKF-SLAM航向偏差

從圖3中可以看出，本文所提算法的位置偏差明顯低于傳統(tǒng)EKF-SLAM算法.由此表明，附加的能觀測性限制及初始化有效提高了機(jī)器人位置的估計精度.圖4說明本文所提的算法改善了傳統(tǒng)EKF-SLAM算法中存在的狀態(tài)估計不一致問題.

機(jī)器人狀態(tài)協(xié)方差是衡量估計精度的重要指標(biāo)，隨機(jī)選取10組本文算法和EKF-SLAM算法的機(jī)器人狀態(tài)協(xié)方差進(jìn)行比較，結(jié)果如表1所示.

表1　機(jī)器人狀態(tài)協(xié)方差

從表1可以看出，本文提出的算法減小了機(jī)器人的協(xié)方差矩陣，即降低了機(jī)器人位置估計的不確定性，估計精度高于傳統(tǒng)EKF-SLAM.

5　結(jié)　論

傳統(tǒng)EKF-SLAM的狀態(tài)觀測方程具有二維不可觀測子空間，而SLAM非線性系統(tǒng)的狀態(tài)觀測方程具有三維不可觀測子空間，這是導(dǎo)致傳統(tǒng)EKF濾波算法估算結(jié)果不一致的主要因素.本文通過增加能觀測性約束條件的U矩陣補償，采用基于數(shù)據(jù)融合的路標(biāo)位置延時狀態(tài)估計方法，實現(xiàn)了線性化點最優(yōu)選取與觀測矩陣重構(gòu).仿真實驗結(jié)果說明，EKF系統(tǒng)的能觀測性會影響系統(tǒng)估算結(jié)果的一致性，同時驗證了本文提出算法在狀態(tài)估計精度和方差一致性方面優(yōu)于傳統(tǒng)EKF-SLAM算法，有效提高了建圖與定位的精度.

[1]陳長征，項宏偉，楊孔碩，等.可變形履帶機(jī)器人跨越臺階的動力學(xué)分析 [J].沈陽工業(yè)大學(xué)學(xué)報，2015，37(2)：165-170.

(CHEN Chang-zheng，XIANG Hong-wei，YANG Kong-shuo，et al.Dynamic analysis for variable tracked robot in process of climbing steps [J].Journal of Shenyang University of Technology，2015，37(2)：165-170.)

[2]Smith R，Cheeseman P.On the representation and estimation of spatial uncertainty [J].The International Journal of Robotics Research，1987，5(4)：56-68.

[3]Kim S J，Kim B K.Dynamic ultrasonic hybrid localization system for indoor mobile robots [J].IEEE Transactions on Industrial Electronics，2013，60(10)：4562-4573.

[4]Guerreiro B J N，Batista P，Silvestre C，et al.Globally asymptotically stable sensor-based simultaneous locali-zation and mapping [J].IEEE Transactions on Robotics，2013，29(6)：1380-1395.

[5]Auat F A，Pereira F M L，Sciascio F，et al.Autonomous simultaneous localization and mapping driven by Monte Carlo uncertainty maps-based navigation [J].The Knowledge Engineering Review，2013，28(1)：35-57.

[6]Souici A，Courdesses M，Ouldali A，et al.Full-observability analysis and implementation of the general SLAM model [J].International Journal of Systems Science，2013，44(3)：568-581.

[7]Oh S，Hahn M，Kim J.Simultaneous localization and mapping for mobile robots in dynamic environments [C]//International Conference on Information Science and Applications.Pattaya，Thailand，2013：1-4.

[8]Lee D，Kim D，Lee S，et al.Experiments on localization of an AUV using graph-based SLAM [C]//10th International Conference on Ubiquitous Robots and Ambient Intelligence.Jeju，Korea，2013：526-527.

[9]Louren?o P，Guerreiro B J，Batista P，et al.Preliminary results on globally asymptotically stable simultaneous localization and mapping in 3-D [C]//American Control Conference (ACC).Washington D C，America，2013：3087-3092.

[10]Pailhas Y，Capus C，Brown K，et al.Design of artificial landmarks for underwater simultaneous localisation and mapping [J].IET Radar，Sonar & Navigation，2013，7(1)：10-18.

[11]Yoon S，Hyung S，Lee M，et al.Real-time 3D simultaneous localization and map-building for a dynamic walking humanoid robot [J].Advanced Robotics，2013，27(10)：759-772.

[12]Ceriani S，Marzorati D，Matteucci M，et al.Single and multi-camera simultaneous localization and mapping using the extended Kalman filter [J].Journal of Mathematical Modelling and Algorithms in Operations Research，2014，13(1)：23.

[13]Chen Z，Dai X，Jiang L H，et al.Adaptive iterated square-root cubature Kalman filter and its application to SLAM of a mobile robot [J].Journal of Electrical Engineering，2013，11(12)：7213-7221.

[14]Oguz A E，Temeltas H.On the consistency analysis of A-SLAM for UAV navigation [C]//International Society for Optics and Photonics.Baltimore，USA，2014：90840R-90840R-12.

[15]Castellanos J A，Martinez C R，Tardós J D，et al.Robocentric map joining：improving the consistency of EKF-SLAM [J].Robotics and Autonomous Systems，2007，55(1)：21-29.

[16]Huang S，Dissanayake G.Convergence and consistency analysis for extended Kalman filter based SLAM [J].IEEE Transactions on Robotics，2007，23(5)：1036-1049.

[17]Huang G P，Mourikis A，Roumeliotis S.A quadratic-complexity observability-constrained unscented Kalman filter for SLAM [J].IEEE Transactions on Robotics，2013，29(5)：1226-1243.

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

[19]Gamage D，Drummond T.Reduced dimensionality extended Kalman filter for SLAM [C]//The 24th British Machine Vision Conference (BMVC 2013).Bristol，Britain，2013：1-11.

(責(zé)任編輯：景勇英文審校：尹淑英)

EKF-SLAM algorithm for robot based on observability analysis

ZHANG Feng, SUN Yang, YUAN Shuai, LI Chang-guo, ZHAO Lan-guang

(Information &Control Engineering Faculty, Shenyang Jianzhu University, Shenyang 110168, China)

In order to solve the problem that the state estimation inconsistency exists in the traditional EKF-SLAM (extended Kalman filter-simultaneous localization and mapping) algorithm, an algorithm which could increase the observability constraint condition was proposed from the perspective of system observability. In addition, the compensation matrixUwas optimized to solve the constrained condition, and the new linear points were obtained. Through optimizing the Jacobi matrix of system, the observability matrix of system was reconstructed, which could make the rank of local observability matrix of EKF-SLAM system be consistent with that of non-linear SLAM system. The results show that the proposed algorithm is superior to the traditional EKF-SLAM algorithm in terms of both state estimation accuracy and covariance consistency. The research work and conclusions have certain reference value for the vehicle autonomous driving.

simultaneous localization and mapping; robot control; extended Kalman filter; observability analysis; optimal estimation; data fusion; estimate inconsistency; state equation

2015-08-04.

國家青年基金資助項目(61305125).

張鳳(1972-)，女，遼寧沈陽人，副教授，主要從事移動機(jī)器人控制技術(shù)和智能控制等方面的研究.

10.7688/j.issn.1000-1646.2016.03.15

TP 242.6

A

1000-1646(2016)03-0319-07

*本文已于2016-03-02 16∶48在中國知網(wǎng)優(yōu)先數(shù)字出版. 網(wǎng)絡(luò)出版地址： http：∥www.cnki.net/kcms/detail/21.1189.T.20160302.1648.054.html

控制工程