王開宇

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

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

中圖分類號：TP242 文獻標識碼：A 文章編號：1672-3791（2016）06（b）-0125-04

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

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

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

1 UKF-SLAM原理介紹

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

1.1 UT變換

2 基于全景視覺機器人的改進UKF-SLAM算法

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

2.1 平方根UKF算法原理

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

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

（19）

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

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

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

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

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

3 實驗及分析

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

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

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

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

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

4 結(jié)語

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

參考文獻

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

[2] 吳葉斌.基于全景視覺的移動機器人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.