基于全景視覺機器人的改進UKF—SLAM算法研究

王開宇

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

關鍵詞：全景視覺機器人 同時定位和地圖創建 無跡卡爾曼濾波 平方根濾波

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

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

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

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

1 UKF-SLAM原理介紹

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

1.1 UT變換

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

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

2.1 平方根UKF算法原理

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

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

（19）

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

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

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

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

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

3 實驗及分析

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

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

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

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

圖4、圖5為分別使用三種SLAM算法獲得的全景視覺機器人定位誤差對比情況。橫軸為SLAM的運行時間，縱軸為全景視覺機器人在X和Y方向上的位置偏差。總體上從曲線中可以看出，三種算法都具有較小的估計誤差，且具有收斂性，由此表明，這三種SLAM算法能有效實現自主定位。對比三種算法，標準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，在整個全景視覺機器人運行過程中其估計精度都保持在較高水平，算法的定位精度最高。仿真實驗結果與前文的理論分析一致。

4 結語

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

參考文獻

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

[2] 吳葉斌.基于全景視覺的移動機器人SLAM方法研究[D].哈爾濱工程大學，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] 杜航原.自主式水下航行器同步定位與地圖構建算法研究[D].哈爾濱工程大學，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.