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

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

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

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

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

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

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

同時定位與建圖； 機器人控制； 擴展卡爾曼濾波器； 能觀測性分析； 最優估計； 數據融合； 估計不一致； 狀態方程

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

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

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

1　傳統EKF-SLAM算法

文中以觀測單一路標為例，對連續時間的非線性SLAM系統進行分析，系統狀態預測方程表示為

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

(2)

移動機器人定位系統的預測方程及觀測方程可以表示為

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

(3)

zk=h(xk，k)+vk

(4)

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

1.1EKF預測

預測過程主要是利用預測模型更新預測方程中當前狀態的先驗值，實時推算下一時刻的狀態變量和誤差協方差，以便為下一個時刻最優化估算奠定基礎.由于路標位置不發生變化，EKF預測方程形式為

(5)

(6)

(7)

算法非線性模型的狀態預測方程為

(8)

(9)

(10)

(11)

1.2EKF更新

EKF更新是通過預測方程與觀測方程對系統狀態變量進行最優估計.預測方程已經在上文進行了說明，觀測方程描述的是機器人與周圍環境路標相對距離的觀測值.在EKF更新時，利用預測方程的先驗值與當前路標觀測的測量值以及測量協方差對機器人位姿狀態進行最優估計，即

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

(12)

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

(13)

式中，HRk，HLk為關于h的雅克比矩陣，分別對應機器人位姿和路標位置，表示為

HRk=(

(14)

HLk=(

(15)

2　SLAM能觀測性分析

2.1非線性SLAM系統能觀測性分析

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

z(t)=h(ρ，ψ)

(16)

(17)

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

(18)

式中，ρ和ψ為機器人與路標的相對距離和相對方位角度，二者之間的相對距離關系表示為

(19)

則觀測模型可表示為

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

(20)

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

(21)

(22)

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

2.2EKF-SLAM能觀測性分析

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

采用觀測矩陣對時變線性化誤差狀態系統進行能觀測性分析，k到k+i時刻的觀測矩陣定義為

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

(23)

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

2.3理想EKF-SLAM

(24)

由此觀測矩陣可表示為

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

(25)

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

(26)

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

2.4傳統EKF-SLAM

傳統的EKF-SLAM的線性化點取在系統狀態變量的最優估計值處，則有

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

(27)

由此觀測矩陣可表示為

(28)

(29)

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

為了使傳統EKF算法線性化后的SLAM系統中觀測方程的能觀矩陣秩保持不變，增加了能觀測約束條件，通過補充U矩陣重構觀測矩陣，實現線性化前后的SLAM系統觀測矩陣秩不變.同時為了改善U矩陣的補償效果，采用基于數據融合的路標位置延時狀態估計與最優選取線性點位置的方法重構觀測矩陣.

3　擴展卡爾曼濾波器設計

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

(30)

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

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

當系統觀測單一路標時，U的表達式為

(31)

當系統同時觀測M個路標時，U的表達式為

(32)

3.2基于數據融合的路標位置延時狀態估計

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

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

(33)

(34)

(35)

3.3線性化點最優選取

(36)

并服從于

(37)

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

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

(39)

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

(40)

4　系統仿真實驗研究與分析

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

圖1　EKF-SLAM仿真結果

圖2　改進EKF-SLAM仿真結果

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

圖3　位置估計偏差

圖4　EKF-SLAM航向偏差

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

機器人狀態協方差是衡量估計精度的重要指標，隨機選取10組本文算法和EKF-SLAM算法的機器人狀態協方差進行比較，結果如表1所示.

表1　機器人狀態協方差

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

5　結　論

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

[1]陳長征，項宏偉，楊孔碩，等.可變形履帶機器人跨越臺階的動力學分析 [J].沈陽工業大學學報，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.

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

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-)，女，遼寧沈陽人，副教授，主要從事移動機器人控制技術和智能控制等方面的研究.

10.7688/j.issn.1000-1646.2016.03.15

TP 242.6

A

1000-1646(2016)03-0319-07

*本文已于2016-03-02 16∶48在中國知網優先數字出版. 網絡出版地址： http：∥www.cnki.net/kcms/detail/21.1189.T.20160302.1648.054.html

控制工程