改進(jìn)的平方根容積模糊自適應(yīng)卡爾曼濾波SLAM算法

李 俊,舒志兵,王蘇洲

(南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院，南京 211816)

改進(jìn)的平方根容積模糊自適應(yīng)卡爾曼濾波SLAM算法

李 俊,舒志兵,王蘇洲

(南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院，南京 211816)

針對移動機(jī)器人SLAM算法存在系統(tǒng)噪聲對定位精度影響嚴(yán)重，特征點的增加導(dǎo)致軌跡偏移等現(xiàn)象。文章將迭代思想與時變漸消因子引入平方根容積卡爾曼濾波中，通過動態(tài)調(diào)節(jié)新息均值和協(xié)方差的方式，建立模糊自適應(yīng)模型調(diào)整噪聲權(quán)值，改善系統(tǒng)中存在的運動噪聲和觀測噪聲。該算法相對于以往算法只能解決單一問題而言，具有更好的兼容性與魯棒性。通過實驗仿真結(jié)果可以看出，該算法相對于以往算法在X方向、Y方向和位姿偏移角的誤差分別減小了21.59%、36.45%、32.97%。將此算法應(yīng)用于實際中，具有良好的地圖重建效果。

SLAM算法；迭代平方根容積；時變漸消因子；模糊自適應(yīng)

0 引言

同時定位與地圖重建是指移動機(jī)器人在未知環(huán)境下通過激光雷達(dá)進(jìn)行環(huán)境地圖的重建并在重建后的地圖中表示出該機(jī)器人位姿狀態(tài)的過程[1]。SLAM算法在移動機(jī)器人中的應(yīng)用是該領(lǐng)域的研究熱點。傳統(tǒng)的算法將擴(kuò)展卡爾曼濾波引入到SLAM領(lǐng)域，其本質(zhì)就是使用卡爾曼濾波對線性化的模型進(jìn)行處理[2]。擴(kuò)展的卡爾曼濾波(EKF)雖然能夠解決非線性系統(tǒng)問題，但是不能很好的處理算法中的數(shù)據(jù)關(guān)聯(lián)且精度不高[3-5]。在此基礎(chǔ)之上，近年來許多研究學(xué)者提出了各種改進(jìn)的新型算法。針對地圖重建過程中受外界環(huán)境影響較大的問題[6]，涂剛毅等人提出了一種超聲概率柵格地圖環(huán)境特征點提取算法[7]。趙琳等人提出了迭代測量更新的UKF算法，有效解決了無跡卡爾曼濾波(UKF)中Sigma點會逐漸偏離真實狀態(tài)估計值的問題[8]，但機(jī)器人在同時定位與地圖重建過程中系統(tǒng)往往存在噪聲，隨著地圖特征點的增加系統(tǒng)的觀測維度也會隨之增加，導(dǎo)致定位效果不佳[9-10]。

在分析了以上研究成果后提出一種改進(jìn)的平方根容積模糊自適應(yīng)卡爾曼濾波SLAM算法，該算法將迭代方式與強(qiáng)跟蹤相結(jié)合，并且針對機(jī)器人存在運動噪聲和觀測噪聲這一問題，采用改進(jìn)的模糊自適應(yīng)方式對不同的噪聲進(jìn)行自適應(yīng)動態(tài)調(diào)整。該算法的優(yōu)點在于大大降低了算法復(fù)雜度，解決了采樣點在非線性情況下存在失真的問題，能夠很好的校正因特征點增加引起的軌跡偏移現(xiàn)象，提高了位姿精度。

1 融合模糊自適應(yīng)與平方根容積的SLAM算法

1.1 模糊自適應(yīng)噪聲動態(tài)調(diào)整算法

(1)

對移動機(jī)器人進(jìn)行建模，其動力學(xué)模型和觀測模型如下：

(2)

式中：Uk為nu維運動信息向量，Zk為nz維的觀測向量。Qk、Rk分別表示運動噪聲和觀測噪聲。

對于式(2)中的運動噪聲和觀測噪聲，對其設(shè)置控制權(quán)值，則式(2)變換為：

(3)

其中，ak、bk分別表示控制的權(quán)值。

模糊自適應(yīng)算法通過動態(tài)調(diào)整新息均值和方差的方式對噪聲的權(quán)值進(jìn)行了模糊調(diào)整。則新息和新息方差定義如下：

(4)

在時間窗M內(nèi)新息和方差的均值如下所示：

(5)

根據(jù)新息和方差的均值得到其置信度為：

(6)

為了提高模糊自適應(yīng)模型的適應(yīng)性，本文引入高斯函數(shù)來動態(tài)的確定ak，bk的值，高斯函數(shù)如下所示：

(7)

式中，c表示高斯函數(shù)的中心，σ表示高斯函數(shù)的寬度。

由式(6)、(7)得到自適應(yīng)控制ak、bk的值為：

(8)

式中，η為[0,1]之間的常數(shù)，表示對運動噪聲和觀測噪聲的調(diào)節(jié)率，σ的值通過公式(6)可得：

(9)

由于高斯值是有界的，所以ak、bk的波動范圍比較小，所以在解決濾波器出現(xiàn)發(fā)散問題時具有良好的效果。通過自適應(yīng)的方式調(diào)整高斯函數(shù)曲線形狀，從而增加了系統(tǒng)的穩(wěn)定性與精確性。其模糊自適應(yīng)控制模型結(jié)構(gòu)如圖1所示。

圖1 模糊自適應(yīng)控制模型結(jié)構(gòu)

如圖1所示，通過新息和方差均值的置信度q1(k)和q2(k)的變化在線監(jiān)測濾波器的穩(wěn)定性。當(dāng)q1(k)和q2(k)偏離穩(wěn)定值時，則通過模糊自適應(yīng)的方式動態(tài)調(diào)整ak和bk的值來改變系統(tǒng)噪聲與觀測噪聲的權(quán)重，進(jìn)而改善濾波器的性能。

1.2 強(qiáng)跟蹤迭代平方根容積卡爾曼濾波

移動機(jī)器人在同時定位與地圖重建過程中隨著特征點數(shù)的增加，系統(tǒng)的觀測維度也會隨之增加，這將導(dǎo)致容積點偏離預(yù)期的位置，為了避免此情況的發(fā)生，在平方根容積卡爾曼濾波的基礎(chǔ)上增加迭代的方式。利用貝葉斯濾波求出K時刻的先驗概率分布和后驗概率分布，其算法分為預(yù)測階段和更新階段。

1.2.1 預(yù)測階段

(10)

(11)

通過k-1時刻的機(jī)器人位姿信息預(yù)測k時刻機(jī)器人位姿信息。其容積點先驗估計和位姿信息公式如下：

(12)

(13)

又可得地圖特征誤差向量如下所示：

(14)

1.2.2 更新階段

(15)

經(jīng)過迭代后的容積點公式變?yōu)椋?/p>

(16)

計算第l次迭代后的卡爾曼增益為：

(17)

計算觀測誤差向量為：

(18)

根據(jù)式(14)、式(18)可以解得狀態(tài)預(yù)測協(xié)方差陣、地圖特征誤差向量與觀測誤差向量的交互協(xié)方差矩陣以及觀測新息協(xié)方差平方根因子如下所示：

(19)

因此卡爾曼增益矩陣為：

(20)

最后計算迭代l+1次后的機(jī)器人位姿矩陣和平方根因子矩陣公式如下所示：

(21)

(22)

當(dāng)達(dá)到預(yù)先設(shè)定的迭代次數(shù)后，數(shù)據(jù)更新如下：

(23)

位姿更新：

(24)

在預(yù)測更新過程中經(jīng)常會由于系統(tǒng)的不穩(wěn)定導(dǎo)致濾波器出現(xiàn)發(fā)散等問題，因此本文通過引入漸消因子來改變數(shù)據(jù)權(quán)重從而提高系統(tǒng)的穩(wěn)定性與定位精度。強(qiáng)跟蹤濾波時變漸消因子公式如下所示：

(25)

其中,

(26)

(27)

(28)

(29)

式中，Hk和Fk|k-1分別表示測量方程與狀態(tài)方程對于狀態(tài)變量的一階偏導(dǎo)矩陣，β為弱化因子，ρ為遺忘因子。

將式(19)帶入式(27)、式(28)得到：

(30)

(31)

通過引入漸消因子后的狀態(tài)協(xié)方差預(yù)測平方根因子為：

(32)

最后將此平方根因子重新帶入預(yù)測更新過程中進(jìn)行迭代運算。

2 系統(tǒng)建模與實驗仿真

將本文提出的算法在MATLAB下進(jìn)行的仿真，使用250m×200m的環(huán)境地圖，選擇17個確定的路徑點和35個地圖特征點。具體仿真參數(shù)如表1所示。

表1 仿真參數(shù)

仿真實驗中運動模型：

(33)

輸入：xv(k)為機(jī)器人的位姿，ΔT為傳感器采樣時間，v(k)為速度，a(k)為機(jī)器人在k時刻的方向角，B為兩軸間的軸距。輸出：xv(k+1)為機(jī)器人的新位姿。

仿真實驗中觀測模型：

(34)

輸入：(xi,yi) 為探測到的第i個特征的位置坐標(biāo)，xv(k)為位姿。輸出：通過激光雷達(dá)等傳感器測得的特征點與移動機(jī)器人的距離x和特征點與移動機(jī)器人前進(jìn)方向的夾角θ。

通過仿真得到傳統(tǒng)SR-CKF-SLAM算法與改進(jìn)的SR-CKF-SLAM算法移動機(jī)器人軌跡圖，圖中“虛線”表示機(jī)器人估計軌跡，“實線”表示機(jī)器人實際軌跡，“*”表示地圖特征點，“+”表示SLAM預(yù)估計的特征點位置。

圖3 改進(jìn)的SR-CKF-SLAM算法

由圖2、圖3所知，本文提出的改進(jìn)的SR-CKF-SLAM算法有效改善了因為特征點的增加導(dǎo)致的軌跡偏移現(xiàn)象，為了更好的驗證本文算法的可靠性，將此算法與STF-SRCKF-SLAM算法和ISR-CKF-SLAM算法進(jìn)行誤差對比，在存在系統(tǒng)噪聲情況下得到X軸、Y軸以及角度位姿誤差對比圖如圖4所示。

圖4 X軸方向誤差對比

圖5 Y軸方向誤差對比

圖6 角度誤差對比

由圖4～圖6可以看出，隨著時間的增加，移動機(jī)器人的定位誤差逐漸減小，由于本文算法引入了迭代與時變漸消因子相結(jié)合的方式，再加上針對存在系統(tǒng)噪聲這一問題，在算法中動態(tài)調(diào)節(jié)新息均值和方差，使得整個算法具有較好的模糊自適應(yīng)性。

為了更加直觀的看出改進(jìn)后算法的優(yōu)點，3種算法性能的定量比較如表2所示。

表2 算法誤差統(tǒng)計

相比于性能較好的ISR-CKF算法，改進(jìn)的SR-CKF算法分別在X方向、Y方向和角度位姿上的誤差降低了21.59%、36.45%和32.97%。

將改進(jìn)后的SR-CKF應(yīng)用于實際中，其硬件部分和實驗重建的地圖如圖7、圖8所示，圖8中左邊為實際場景的地圖，右邊是算法重建的地圖，可以看出本文算法具有良好的地圖重建功能。

圖7 移動機(jī)器人硬件平臺

圖8 算法重建的地圖

3 結(jié)論

本文提出的改進(jìn)的平方根容積模糊自適應(yīng)算法融合了STF-SRCKF和ISR-CKF的優(yōu)點，有效改善了因特征點增加導(dǎo)致的軌跡偏移現(xiàn)象，并且采用動態(tài)調(diào)節(jié)新息和協(xié)方差的方式改善了系統(tǒng)中存在的運動噪聲和觀測噪聲帶來的定位誤差較大等問題。通過實驗仿真結(jié)果可以看出該算法有效提高了定位精度，具有很好的穩(wěn)定性與自適應(yīng)性，在實際應(yīng)用中具有良好的地圖重建功能。

[1] Durrant-Whyte H,Bailey T. Simultaneous localization and mapping:Part I[J].IEEE on Robotics& Automation Magazine, 2006, 13(2): 99- 108.

[2] 陶明，凌有鑄，陳孟元，等.迭代的平方根容積卡爾曼濾波SLAM算法[J].計算機(jī)工程，2015，41(9):317-321.

[3] 歐陽廣帥，周晶.基于卡爾曼濾波的高精度彈道濾波算法研究[J]．電子測量技術(shù)，2014，37(11):16-19.

[4] 劉丹，段建民，于宏嘯.基于自適應(yīng)漸消 EKF的FastSLAM算法[J].系統(tǒng)工程與電子技術(shù)，2016，38(3):644-651.

[5] 張文玲，朱明清，陳宗海.基于強(qiáng)跟蹤 UKF的自適應(yīng)SLAM算法[J].機(jī)器人，2010，32(2):190-195.

[6] 周武，趙春霞，沈亞強(qiáng)，等．基于全局觀測地圖模型的SLAM研究[J].機(jī)器人，2010，32(5):647-653.

[7] 涂剛毅，金世俊，祝雪芬，等.基于粒子濾波的移動機(jī)器人SLAM算法[J].東南大學(xué)學(xué)報(自然科學(xué)版)，2010，40(1):117-122.

[8] 趙琳,閆鑫,郝勇.基于迭代平方根UKF的SLAM算法[J].彈箭與制導(dǎo)學(xué)報，2011,31(2):157-161.

[9] 戴雪梅，郎朗，陳孟元.強(qiáng)跟蹤平方根容積卡爾曼濾波SLAM算法[J].電子測量與儀器學(xué)報，2015，29(10):1493-1499.

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

ImprovedSquareRootVolumeFuzzyAdaptiveKalmanFilterSLAMAlgorithm

LI Jun,SHU Zhi-bing,WANG Su-zhou

(College of Electrical Engineering and Control Science ,Nanjing Tech University, Nanjing 211816,China)

SLAM algorithm for mobile robot system is severely affected by the noise on the positioning accuracy, the phenomenon such as the increase of feature point lead to track migration. In this paper, the iterative thoughts and time-varying fading factor is introduced into the square root kalman filtering in the volume, and through dynamic adjust the way of the new interest rates mean and covariance, and fuzzy adaptive adjust the weights of noise model, which improved the system noise and observation noise existing in the movement.The algorithm is compared with the previous algorithm can only solve a single problem, which has better compatibility and robustness. The simulation results show that the proposed algorithm compared with previous algorithms in theXdirection,Ydirection error and position deviation angle decreases by 21.59%, 36.45%, 32.97%. This algorithm is applied in practice, and has a good map reconstruction effect.

SLAM algorithm;iterative square root volume;time-varying fading factor;adaptive fuzzy

TH166;TG506

A

1001-2265(2017)12-0029-04

10.13462/j.cnki.mmtamt.2017.12.007

2017-01-15；

2017-02-17

李俊(1993—)，男，江蘇揚州人，南京工業(yè)大學(xué)碩士研究生，研究方向為地圖重建與導(dǎo)航，圖像處理，運動控制技術(shù)，(E-mail)lj893434@163.com。

(編輯李秀敏)