基于全景視覺機(jī)器人的改進(jìn)UKF—SLAM算法研究

王開宇

摘 要：標(biāo)準(zhǔn)UKF-SLAM算法根據(jù)協(xié)方差矩陣計(jì)算的Sigma點(diǎn)會(huì)逐漸偏離真實(shí)狀態(tài)估計(jì)值，影響定位精度。針對(duì)上述問題，該文引入平方根濾波的方法，在迭代更新過程中直接傳遞協(xié)方差矩陣的平方根，確保協(xié)方差矩陣的非負(fù)定性，提出了一種基于全景視覺的改進(jìn)UKF-SLAM算法。并通過仿真實(shí)驗(yàn)，驗(yàn)證了該文提出的改進(jìn)UKF-SLAM算法具有更高的定位精度。

關(guān)鍵詞：全景視覺機(jī)器人 同時(shí)定位和地圖創(chuàng)建 無跡卡爾曼濾波 平方根濾波

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1672-3791（2016）06（b）-0125-04

全景視覺傳感器不僅具有普通視覺傳感器信息量大、直觀性好和采樣周期短等優(yōu)點(diǎn)，而且還具備全向360°的感知范圍，在應(yīng)用于同時(shí)定位與地圖創(chuàng)建（SLAM）[1，2]是中時(shí)，視覺路標(biāo)可在其視野范圍內(nèi)停留更長的時(shí)間，增強(qiáng)了對(duì)路標(biāo)的連續(xù)觀測(cè)和跟蹤能力。

1988年，Smith，Self和Cheeseman在文獻(xiàn)[3]中首次提出EKF-SLAM算法，利用EKF算法對(duì)機(jī)器人位姿和地圖同時(shí)估計(jì)，奠定了移動(dòng)機(jī)器人SLAM的理論框架。但EKF算法在執(zhí)行時(shí)需要假設(shè)系統(tǒng)服從高斯分布，且存在線性化問題，無法滿足模塊化應(yīng)用。Wang等[4]將UKF算法用于解決SLAM問題，在保證計(jì)算復(fù)雜度同階的情況下，以對(duì)非線性函數(shù)的概率密度分布近似取代對(duì)非線性函數(shù)本身近似，解決了線性化問題。

然而UKF-SLAM算法中，每次迭代都需要傳遞協(xié)方差矩陣且根據(jù)協(xié)方差矩陣計(jì)算的Sigma點(diǎn)會(huì)逐漸偏離真實(shí)狀態(tài)估計(jì)值，影響定位精度。針對(duì)上述缺點(diǎn)，該文提出了一種直接傳遞協(xié)方差矩陣平方根的改進(jìn)UKF-SLAM算法，提高了算法的精度。

1 UKF-SLAM原理介紹

無跡卡爾曼濾波[5]（unscented Kalman filter，UKF）是一種利用采樣策略對(duì)非線性分布進(jìn)行逼近的方法，它以卡爾曼濾波框架為基礎(chǔ)，利用UT變換，通過一組確定性采樣點(diǎn)對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似。

1.1 UT變換

2 基于全景視覺機(jī)器人的改進(jìn)UKF-SLAM算法

在標(biāo)準(zhǔn)的UKF-SLAM 算法中，每次迭代都需要傳遞完整的協(xié)方差矩陣用于計(jì)算點(diǎn)。當(dāng)全景視覺機(jī)器人在多路標(biāo)特征環(huán)境下，產(chǎn)生的Sigma點(diǎn)會(huì)逐漸偏離真實(shí)狀態(tài)估計(jì)值，影響定位精度。平方根濾波可以解決狀態(tài)和參數(shù)的估計(jì)問題，提高UKF算法的計(jì)算效率。將平方根濾波思想引入U(xiǎn)KF-SLAM算法中，使其在迭代更新過程中，通過傳遞協(xié)方差矩陣的平方根代替直接傳遞協(xié)方差矩陣，既可以提高UKF-SLAM方法的魯棒性，同時(shí)也能保證協(xié)方差矩陣的非負(fù)定性。為此，該文提出了一種基于平方根濾波的UKF-SLAM算法，并用其解決全景視覺移動(dòng)機(jī)器人的SLAM問題。

2.1 平方根UKF算法原理

Rudolph和Eric等首先將平方根濾波的思想引入U(xiǎn)KF方法，并解決了狀態(tài)和參數(shù)的估計(jì)問題，平方根UKF算法中利用三種運(yùn)算技巧提高算法的運(yùn)行效率[6]，如下：

（1）分解。UKF算法中，若，可通過Cholesky分解得到，而采用QR分解可以提高算法的運(yùn)行效率。

（19）

采用分解不必先求出樣點(diǎn)的加權(quán)方差再進(jìn)行分解，還降低了計(jì)算量[7]。這對(duì)SLAM在真實(shí)環(huán)境實(shí)驗(yàn)的實(shí)時(shí)性要求是非常有必要的。

（2）Cholesky因子更新。若的Cholesky因子為，則的Cholesky 因子就可由表示。

（3）最小二乘法。UKF算法需求逆，因上三角矩陣是半正定的，所以引入回代法求解上式，以免去求逆運(yùn)算。

2.2 基于全景視覺機(jī)器人的平方根濾波UKF-SLAM算法

因?yàn)槠椒礁鵘KF算法的改進(jìn)，使原算法的狀態(tài)向量和協(xié)方差矩陣都不必在經(jīng)過噪聲增廣。假設(shè)系統(tǒng)噪聲和觀測(cè)噪聲均為高斯噪聲，下面對(duì)該文提出的基于全景視覺的改進(jìn)平方根UKF-SLAM算法進(jìn)行介紹。

3 實(shí)驗(yàn)及分析

該節(jié)分別對(duì)基于全景視覺移動(dòng)機(jī)器人的標(biāo)準(zhǔn)EKF-SLAM、標(biāo)準(zhǔn)UKF-SLAM這兩種傳統(tǒng)卡爾曼濾波框架下的SLAM算法和該文的改進(jìn)UKF-SLAM算法進(jìn)行仿真實(shí)驗(yàn)，通過對(duì)全景視覺移動(dòng)機(jī)器人定位精進(jìn)行比較分析，進(jìn)而驗(yàn)證該文所提出的基于全景視覺移動(dòng)機(jī)器人的改進(jìn)UKF-SLAM算法的優(yōu)勢(shì)。

仿真在200 m×200 m的矩形區(qū)域中進(jìn)行，該區(qū)域中分布著若干環(huán)境特征點(diǎn)，用藍(lán)色“*”表示；在實(shí)驗(yàn)中提前規(guī)劃了全景視覺機(jī)器人的行駛路線，全景視覺機(jī)器人的行駛路線（圖1、圖2、圖3中綠色線）由27個(gè)路徑點(diǎn)（用綠色“”表示）確定。

圖1、圖2和圖3為標(biāo)準(zhǔn)EKF-SLAM、標(biāo)準(zhǔn)UKF-SLAM同該文的改進(jìn)UKF-SLAM的仿真結(jié)果。圖1、圖2、圖3中，綠色線是按路徑信息設(shè)置好的全景視覺機(jī)器人運(yùn)動(dòng)軌跡；藍(lán)色虛線表示機(jī)器人實(shí)際運(yùn)動(dòng)軌跡；紅色線表示機(jī)器人利用SLAM算法之后得到的估計(jì)更新后的軌跡；藍(lán)色“×”表示預(yù)先設(shè)定好的地圖中的特征，即實(shí)際的路標(biāo)，是靜止的；紅色“+”表示機(jī)器人經(jīng)過SLAM算法得到的環(huán)境中路標(biāo)估計(jì)位置；紅色橢圓表示機(jī)器人對(duì)路標(biāo)誤差的估計(jì)，橢圓的大小代表了對(duì)應(yīng)路標(biāo)的不確定程度。矩形代表機(jī)器人，藍(lán)色的矩形表示機(jī)器人在控制信息下到達(dá)的實(shí)際位置，紅色矩形表示機(jī)器人在SLAM算法下所估計(jì)的位置。

機(jī)器人從（0，0）點(diǎn)出發(fā)，沿行駛路線按逆時(shí)針方向行駛1周。運(yùn)動(dòng)過程中利用外部全景視覺傳感器對(duì)周圍環(huán)境進(jìn)行掃描，獲取觀測(cè)信息，同時(shí)根據(jù)全景視覺移動(dòng)機(jī)器人的控制輸入得到自身軌跡狀態(tài)的預(yù)測(cè)信息，根據(jù)觀測(cè)信息和預(yù)測(cè)信息，利用SLAM算法最終得到全景視覺移動(dòng)機(jī)器人的運(yùn)行軌跡和環(huán)境地圖。

圖4、圖5為分別使用三種SLAM算法獲得的全景視覺機(jī)器人定位誤差對(duì)比情況。橫軸為SLAM的運(yùn)行時(shí)間，縱軸為全景視覺機(jī)器人在X和Y方向上的位置偏差?？傮w上從曲線中可以看出，三種算法都具有較小的估計(jì)誤差，且具有收斂性，由此表明，這三種SLAM算法能有效實(shí)現(xiàn)自主定位。對(duì)比三種算法，標(biāo)準(zhǔn)EKF-SLAM的定位誤差最大，X方向的最大誤差為2.63 m，Y方向的最大誤差為2.27 m；標(biāo)準(zhǔn)UKF-SLAM的定位精度高于標(biāo)準(zhǔn)EKF-SLAM，X方向的最大誤差為1.54 m ，Y方向的最大誤差為1.51 m；該文采用的改進(jìn)UKF-SLAM算法在X方向的最大誤差為0.54 m，Y方向的最大誤差為0.47 m，在整個(gè)全景視覺機(jī)器人運(yùn)行過程中其估計(jì)精度都保持在較高水平，算法的定位精度最高。仿真實(shí)驗(yàn)結(jié)果與前文的理論分析一致。

4 結(jié)語

該文主要研究小尺度室內(nèi)環(huán)境的全景視覺移動(dòng)機(jī)器人SLAM算法。針對(duì)UKF-SLAM中每個(gè)時(shí)刻都需要傳輸協(xié)方差矩陣計(jì)算的Sigma點(diǎn)會(huì)逐漸偏離真實(shí)狀態(tài)估計(jì)值的問題，引入平方根濾波的方法，提出了一種基于全景視覺的改進(jìn)UKF-SLAM算法。仿真實(shí)驗(yàn)表明，與標(biāo)準(zhǔn)EKF-SLAM和UKF-SLAM算法相比，該文提出的改進(jìn)UKF-SLAM在對(duì)全景視覺機(jī)器人自身的定位具有更好性能。

參考文獻(xiàn)

[1] 梁志偉，馬旭東，戴先中，等.基于分布式感知的移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建[J].機(jī)器人，2009，31（1）：33-39.

[2] 吳葉斌.基于全景視覺的移動(dòng)機(jī)器人SLAM方法研究[D].哈爾濱工程大學(xué)，2011.

[3] Smith R， Self M， Cheeseman P. Estimating Uncertain Spatial Relationships in Robotics[M].New York：Springer Verlag，1988：167-193.

[4] Wang H， Fu G， Li J， et al. An adaptive UKF based SLAM method for unmanned underwater vehicle[J]. Mathematical Problems in Engineering，2013（4）：1-12.

[5] 杜航原.自主式水下航行器同步定位與地圖構(gòu)建算法研究[D].哈爾濱工程大學(xué)，2012.

[6] Van D M R， Wan E A. The square-root unscented Kalman filter for state and parameter-estimation[C]// Icassp，2001：3461-3464.

[7 成蘭，謝愷.迭代平方根UKF[J].信息與控制，2008，37（4）：439-444.