移動機器人SLAM位姿估計的改進四元數(shù)無跡卡爾曼濾波

趙玏洋，閆 利

武漢大學(xué)測繪學(xué)院，湖北 武漢 430079

在傳統(tǒng)測繪產(chǎn)業(yè)數(shù)據(jù)獲取和數(shù)據(jù)處理模式中，大量重復(fù)性的人工工作還是占據(jù)主導(dǎo)地位，這種方式不僅效率極低，而且難以達(dá)到精度的要求，逐漸無法滿足智能化測繪階段時空信息獲取與應(yīng)用需求。隨著移動機器人技術(shù)的廣泛成熟以及智能化程度的提高，智能化測繪技術(shù)將改變目前數(shù)字化測繪以人為中心的方式，給測繪領(lǐng)域提供新的活力，成為未來發(fā)展的必然趨勢，突破原有數(shù)字化測繪由人為因素帶來的環(huán)境不可及和效率上的瓶頸。智能化測繪產(chǎn)業(yè)將以自主化、實時化為技術(shù)目標(biāo)，建立結(jié)合主動探索、實時感知和智能決策的技術(shù)體系。

近年來，隨著機器人技術(shù)的發(fā)展，機器人已經(jīng)被廣泛應(yīng)用于實際生產(chǎn)領(lǐng)域中，特別是功能強大的移動機器人在測繪領(lǐng)域中所扮演的作用越來越大。隨著移動測量技術(shù)走向成熟，特別是無人機測量、車載移動測量等的誕生，更是在一定程度上解決了動態(tài)測量技術(shù)問題[1-2]。如今，智能科技產(chǎn)業(yè)的發(fā)展已經(jīng)在社會生產(chǎn)和生活中掀起了重大的變革，擁有自主化、實時化能力的移動機器人為測繪產(chǎn)業(yè)帶來了巨大的發(fā)展機遇，利用機器代替人并完成測量行為的自主移動機器人也將是今后智能化測繪產(chǎn)業(yè)的發(fā)展方向。

自主移動機器人具有明顯的智能化和先進性特征，可以實現(xiàn)對于周圍信息的感知和處理，自主做出決策和規(guī)劃，并實現(xiàn)既定的移動目標(biāo)[3]。國外部分高校以及研究機構(gòu)很早開始了相關(guān)的研究，在20世紀(jì)60年代，斯坦福大學(xué)首次發(fā)布了機器人Shakey，其具備了一定的自主決策功能，引發(fā)了較大的關(guān)注，可以實現(xiàn)路徑規(guī)劃，并且在不同的環(huán)境中適用性良好[4]。進入21世紀(jì)，在機器學(xué)習(xí)以及大數(shù)據(jù)等技術(shù)持續(xù)發(fā)展的背景下，這些新技術(shù)被應(yīng)用于移動機器人的設(shè)計中。其中LAAS團隊研制的HILARE機器人[5]配備了視覺相機和激光測距儀等傳感系統(tǒng)，可以在已知場景內(nèi)實現(xiàn)路徑規(guī)劃和導(dǎo)航功能。移動機器人的發(fā)展從最初的工業(yè)機器人擴展到面向服務(wù)的自主移動機器人，在醫(yī)療保健、文化娛樂等生活方式中有很大的需求，顯示出較大的應(yīng)用前景。Massachusetts Institute of Technology的Wheelesley、德國Vamors-P和Caravelle systems、Carnegie Mellon University的CMU漫游者也在移動機器人領(lǐng)域有較高水平的研究成果[6]。

移動機器人在未知環(huán)境下從初始點開始運動到既定目標(biāo)的過程中，需要準(zhǔn)確獲取自身的位置以及環(huán)境中障礙物的位置。文獻[7]針對機器人的導(dǎo)航，將其總結(jié)為兩個問題，分別是“我處于何地?”“周圍環(huán)境如何?”，二者實際上對應(yīng)著定位、建圖的過程。1995年，文獻[8—10]提出“SLAM”一詞，并給出了移動機器人SLAM的基本技術(shù)框架。

SLAM根據(jù)傳感器類型的不同，可主要分為視覺SLAM[11-13]及激光SLAM[14]。移動機器人SLAM的研究主要包括數(shù)據(jù)關(guān)聯(lián)[15]、環(huán)境特征提取與表示[16]、目標(biāo)檢測[17]、閉環(huán)[18]等。移動機器人SLAM問題中存在許多不確定因素，例如與觀測模型以及所處環(huán)境的不確定性等因素有關(guān)[19]，由此逐步出現(xiàn)了基于概率的SLAM算法，具體可以將其劃分為不同的類型[20]，例如有基于EKF的SLAM算法、基于期望最大化(EM)的SLAM算法等。

卡爾曼濾波(KF)算法用狀態(tài)向量表示移動機器人的姿態(tài)和特征在地圖上的位置。在此算法中認(rèn)為是線性系統(tǒng)，噪聲以及狀態(tài)均滿足高斯分布，然而由于存在顯著的線性誤差，因此文獻[21—22]設(shè)計了EKF算法，該算法能夠?qū)崿F(xiàn)對于非線性系統(tǒng)的逼近。

為了減少EKF-SLAM算法的計算量，文獻[23]設(shè)計了無跡卡爾曼濾波(UKF)。在此方法中無須對Hessians矩陣進行計算，使用Sigma點集來近似目標(biāo)狀態(tài)的后驗概率密度。但UKF的計算量隨著觀測值的增加也會隨之增加。因此，擴展形式的Sigma點濾波方法也得到了廣泛的應(yīng)用。文獻[24]設(shè)計了Sigma點卡爾曼濾波的平方根。該方法可以達(dá)到較高的估計精度，并且穩(wěn)定性良好。由于濾波器的平方根具有保證協(xié)方差矩陣非負(fù)的優(yōu)點，因此文獻[25]在SLAM問題中設(shè)計了平方根無跡卡爾曼濾波，改善了算法的應(yīng)用效果。同時，文獻[25]改進了文獻[26]的成果，設(shè)計了一種迭代平方根的UKF-SLAM，文獻[27]設(shè)計了AUKF-SLAM。

根據(jù)現(xiàn)有的研究，四元數(shù)的應(yīng)用具有多方面的優(yōu)點，主要體現(xiàn)在計算量小，精度高，非奇異性和可全姿態(tài)工作等方面。部分學(xué)者基于四元數(shù)進行了研究，其中文獻[28]針對UKF計算量較大的問題,提出一種經(jīng)過改進的四元數(shù)無跡卡爾曼濾波算法。通過四元數(shù)UKF中四元數(shù)和修正羅德里格斯參數(shù)之間的線性變換,避免了頻繁切換四元數(shù)和誤差羅德里格斯參數(shù)所可能帶來的計算錯誤和測試量增大的現(xiàn)象。文獻[29]提出一種基于模糊邏輯四元數(shù)的平方根UKF姿態(tài)估計算法。采用模糊邏輯的平方根協(xié)方差形式作為更新參數(shù)，既降低了算法的計算量和復(fù)雜度，又保證了數(shù)值的穩(wěn)定性。文獻[30]提出一種用于磁角速率和重力傳感器的線性卡爾曼濾波器，該濾波器處理角速率、加速度和磁場數(shù)據(jù)，以獲得四元數(shù)表示中的方向估計。該算法將四元數(shù)方向計算為兩個代數(shù)四元數(shù)的組合。兩個四元數(shù)的解耦性質(zhì)使方向的滾轉(zhuǎn)和俯仰分量不受磁干擾的影響。文獻[31]基于UKF，提出一種四元數(shù)衍生無跡卡爾曼濾波的無人機姿態(tài)估計方法，該方法實時性及精度方面都強于無跡卡爾曼濾波。文獻[32]提出一種四元數(shù)狀態(tài)切換無跡卡爾曼濾波方法。通過羅德里格斯參數(shù)與四元數(shù)結(jié)合的無跡卡爾曼濾波，利用時間更新和量測更新的過程中切換濾波狀態(tài)來減少計算量，以此增加系統(tǒng)的實時性、精度和穩(wěn)定性。

本文旨在解決移動機器人運動過程中的非線性位姿估計問題。首先列出了擴展卡爾曼濾波和無跡卡爾曼濾波SLAM的推導(dǎo)方程。根據(jù)無跡卡爾曼濾波涉及高階矩陣運算導(dǎo)致時間計算增加的現(xiàn)狀，在無跡卡爾曼濾波的基礎(chǔ)上增加了平方根濾波。同時，利用四元數(shù)全姿態(tài)操作，準(zhǔn)確性高的特點，通過乘性誤差四元數(shù)對平均點和四元數(shù)點之間的距離進行描述，并降低濾波狀態(tài)的維數(shù)，減少計算耗時。

1 擴展卡爾曼濾波和無跡卡爾曼濾波

1.1 移動機器人SLAM的線性化

針對移動機器人SLAM的研究比較多，該問題可以描述為機器人從處于特定環(huán)境中的某位置開始運動，實時對附近環(huán)境信息進行探測和感知，并以增量方式形成地圖。對于SLAM問題，在時間k處定義以下狀態(tài)向量。

xr,k：描述機器人姿態(tài)的狀態(tài)向量。

uk：機器人從k-1至k時刻的控制矢量。

xfi：描述特征在時間i中位置的向量，且特征的真實位置不隨時間變化。

zk：機器人當(dāng)前位置到特征的觀測向量。

SLAM過程可以由運動方程和觀測方程來描述。假設(shè)在t=0至t=N的時間內(nèi)，有位姿x0至xN，并且有路標(biāo)y1,y2,…,yM。運動和觀測方程為

xk=f(xk-1,uk)+wkk=1,2，…,N

(1)

zk,j=h(yj,xk)+vk,jj=1,2，…,M

(2)

由于x、y均屬于需要估計的變量，因此可以將二者認(rèn)為是滿足特定概率分布的隨機變量。已知xk代表k時的全部未知量，對應(yīng)的全部觀測表示為zk

(xk=f(xk-1,uk)+wkk=1,2，…,N

(3)

zk=h(xk)+vk)

(4)

現(xiàn)在考慮第k時刻的情況。用過去0到k中的數(shù)據(jù)，來估計現(xiàn)在的狀態(tài)分布：那么SLAM問題可以用式(1)描述為

P(xr,k|z1:k,u1:k)

(5)

估計后驗概率密度分布P(xk|zk,uk)具體劃分為兩個不同的過程，分別是預(yù)測、更新，具體的過程如下所示。

(1) 預(yù)測階段：根據(jù)機器人的運動模型和當(dāng)前位置，預(yù)測機器人下一時刻的狀態(tài)。

機器人系統(tǒng)的先驗概率密度為

zk-1,uk-1)dxk-1

(6)

(2) 更新階段：在計算出觀測模型和先驗概率密度后，利用傳感器所獲取到的信息更新系統(tǒng)內(nèi)的狀態(tài)估計量。因此，系統(tǒng)的后驗概率密度為

(7)

P(zk|xk)是觀測模型。根據(jù)馬爾可夫假設(shè)，當(dāng)前狀態(tài)已知時，前一次觀測和當(dāng)前觀測是相互獨立的，因此P(zk|zk-1,uk)是歸一化常數(shù)。

如果運動模型和觀測模型滿足線性分布，則可采用卡爾曼濾波算法求解。當(dāng)不滿足時，積分計算很難實現(xiàn)，可采用擴展卡爾曼濾波進行求解。

1.2 擴展卡爾曼濾波

非線性離散系統(tǒng)的模型為

xk=f(xk-1)+wk-1

(8)

zk=h(xk)+vk

(9)

式中，wk和vk分別受到高斯白噪聲的影響。

(10)

(11)

忽略高階項的影響，式(7)可以簡化為

(12)

利用狀態(tài)轉(zhuǎn)移函數(shù)，狀態(tài)的單步預(yù)測值為

(13)

因此，對應(yīng)的誤差方差形式為

φk|k-1Pk|k-1Qk|k-1+Qk

(14)

(15)

從測量函數(shù)獲得的測量向量的單步預(yù)測值為

(16)

因此，對應(yīng)的預(yù)測誤差方差矩陣為

(17)

狀態(tài)增強矩陣為

(18)

因此，時間k的估計狀態(tài)向量為

(19)

狀態(tài)誤差協(xié)方差矩陣更新為

(20)

1.3 無跡卡爾曼濾波

傳統(tǒng)的EKF存在著兩個難以克服的問題：一是EKF算法的一階泰勒展開忽略了非線性函數(shù)高階導(dǎo)的影響，當(dāng)非線性度較強時，則容易降低濾波精度，特別是不低于二階的截斷誤差會產(chǎn)生更顯著的影響；二是有的非線性函數(shù)并沒有雅可比矩陣，這也讓EKF算法在某些條件下不可用。近來出現(xiàn)的無跡卡爾曼濾波方法在應(yīng)用中顯示出一定的優(yōu)勢。該算法根據(jù)系統(tǒng)狀態(tài)的先驗均值和方差選擇一組統(tǒng)計特征與之前的統(tǒng)計特征狀態(tài)一致的確定性Sigma采樣點。再通過線性傳遞函數(shù)得到一組新的Sigma點，在此基礎(chǔ)上可以得到采樣點的統(tǒng)計特征，即為對應(yīng)的后驗統(tǒng)計分布。從本質(zhì)上來看，該方法實際上逼近的是非線性函數(shù)的概率密度分布，所以無須對雅克比矩陣進行計算，以此可以實現(xiàn)對非線性函數(shù)的處理，相對于傳統(tǒng)的擴展卡爾曼濾波方法具備了明顯的優(yōu)勢。

UKF采用遞推估計的方式，求得狀態(tài)量的最優(yōu)方差估計值，避免了由高階項截斷而產(chǎn)生的系統(tǒng)截斷誤差，相較于EKF，提高了系統(tǒng)的解算精度。

對于無跡變換而言，需要確保在達(dá)到采樣均值x以及協(xié)方差P的基礎(chǔ)上選取合適的Sigma點，Sigma點數(shù)量取決于所選的采樣策略。目前較為常見的采樣策略是2n+1 Sigma點對稱采樣。采樣完成之后基于非線性變換方式對Sigma點進行處理，可以得到對應(yīng)的均值與協(xié)方差，然后將其應(yīng)用到各個Sigma點中，采用相同的處理方式可以得到新的Sigma點。該方法相對于擴展卡爾曼濾波可以得到更多的觀測假設(shè)。UT變換的應(yīng)用體現(xiàn)出明顯的優(yōu)勢，降低了截斷誤差對于數(shù)據(jù)融合所造成的影響，無須進行線性化處理的過程，可以達(dá)到更高的求解精度。因此，狀態(tài)統(tǒng)計特性的估計比EKF算法更準(zhǔn)確。

Sigma點的選擇和比例可根據(jù)以下公式選擇

λ=α2(n+χ)-n

(21)

Sigma點序列為

(22)

協(xié)方差的權(quán)重和Sigma點序列的估計計算為

(23)

式中，α代表與Sigma點擴散尺度有關(guān)的參數(shù)；β≥0；x服從高斯分布；n代表狀態(tài)向量維度。

在非線性系統(tǒng)中引入UT變換，可以得到無跡卡爾曼濾波，具體的過程如下。

(1) 初始化

(24)

(25)

(2) 時間更新。時間更新中Sigma點和權(quán)重的選擇為

(26)

(27)

(28)

利用非線性測量函數(shù)計算k時刻系統(tǒng)狀態(tài)、均值和協(xié)方差的單步預(yù)測χk|k-1(i)=f(χk-1(i))，

(29)

(30)

(3) 測量更新。測量更新中Sigma點和權(quán)重的選擇為

(31)

(32)

(33)

利用非線性測量函數(shù)計算k時刻系統(tǒng)狀態(tài)單步預(yù)測、均值和方差以及互協(xié)方差Zk|k-1(i)=h(ξk|k-1(i))，有

(34)

Zk|k-1(i))T]+Rk

(35)

Zk|k-1(i))T]+Rk

(36)

(4) 狀態(tài)增益。狀態(tài)增強矩陣、估計值和狀態(tài)誤差方差值為

(37)

(38)

(39)

無跡卡爾曼濾波相對于擴展卡爾曼濾波在精度上具有一定的優(yōu)勢，并且計算難度較小，無須計算雅克比矩陣，所以可以應(yīng)用到更多的場景中。

2 平方根無跡卡爾曼濾波

Sigma點的確定性采樣與協(xié)方差P的平方根運算有關(guān)，如果是高階矩陣則極易影響到計算的效率。其次，在運算過程中，受到數(shù)值舍入誤差的影響導(dǎo)致計算穩(wěn)定性降低。因此，文獻[34]在研究過程中設(shè)計了基于平方根的UKF算法。該方法在平方根計算過程中采用了Cholesky分解等方式，具體的內(nèi)容如下。

(1) 初始化

(40)

(41)

式中，chol{}表示矩陣的Cholesky分解。

(2) 時間更新。時間更新中Sigma點和權(quán)重的選擇為

(42)

(43)

(44)

利用非線性測量函數(shù)計算k時刻系統(tǒng)狀態(tài)、均值和互協(xié)方差的單步預(yù)測Zk|k-1(i)=h(ξk|k-1(i))，有

(45)

(46)

(47)

式中，qr{…}代表奇異值分解的過程；cholupdate{…}代表矩陣更新的過程。

(3) 測量更新。測量更新中Sigma點和權(quán)重的選擇為

(48)

(49)

(50)

利用非線性測量函數(shù)計算k時刻系統(tǒng)狀態(tài)、均值和互協(xié)方差的單步預(yù)測Zk|k-1(i)=h(ξk|k-1(i))。

(51)

(52)

(53)

(54)

(4) 狀態(tài)增益。狀態(tài)增強矩陣、估計值和狀態(tài)誤差方差值為

(55)

(56)

Uk=KkSzz,k

(57)

(58)

3 四元數(shù)平方根無跡卡爾曼濾波

傳統(tǒng)的非線性優(yōu)化方法在求解移動機器人的位姿估計時，往往面臨計算量大的問題，需要花費較多的時間才能夠完成計算。四元數(shù)則能夠有效地解決上述問題，其優(yōu)勢在于降低了計算量，提升了計算的精度，并且可以達(dá)到全姿態(tài)工作的目標(biāo)，所以廣泛應(yīng)用到了姿態(tài)表示中[35]。國外部分學(xué)者很早開始在此領(lǐng)域進行了研究，在19世紀(jì)，W.R.Hamilton提出了四元數(shù)，但是受到技術(shù)以及硬件條件等因素的影響并未引起廣泛的關(guān)注，也沒有得到較多的應(yīng)用。后來隨著在此領(lǐng)域研究的深入，以及計算機性能的不斷提升，對于四元數(shù)的研究和應(yīng)用逐步增多，可以充分發(fā)揮其應(yīng)用的優(yōu)勢與特色。另外，其相對于歐拉角更為簡單，大幅度降低了計算的復(fù)雜度。正是由于具備了上述優(yōu)勢，使得四元數(shù)在擴展卡爾曼濾波中有著廣泛的應(yīng)用。文獻[36]提出了一種基于四元數(shù)的二階EKF算法，計算了Taylor展開的二階精度，提高了姿態(tài)估計的精度，但計算量也有所增加。文獻[37]提出了一種基于修正Rodriguez參數(shù)和姿態(tài)四元數(shù)轉(zhuǎn)換的無跡Kalman濾波算法。該算法通過參數(shù)間的相互轉(zhuǎn)換有效地提高了姿態(tài)估計的精度，但這種相互轉(zhuǎn)換無疑增加了算法的復(fù)雜度。

本文提出一種四元數(shù)平方根無跡卡爾曼濾波(Quaternion Squareroot-UKF，QSR-UKF)算法。算法將四元數(shù)均值問題轉(zhuǎn)化為拉格朗日代價函數(shù)的極值問題，無須姿態(tài)參數(shù)切換，矢量部分屬于姿態(tài)變量，標(biāo)量則通過單位四元數(shù)計算。這種計算方式解決了四元數(shù)Sigma點在選取過程中的可能出現(xiàn)的干擾問題，降低了平方根無跡卡爾曼濾波狀態(tài)的維數(shù)。

在算法中利用了乘性誤差四元數(shù)，其可以對平均點和四元數(shù)點之間的距離進行描述，有助于解決奇異值分解的計算量大和特征向量法煩瑣的計算步驟等問題。除了上述優(yōu)勢之外，同樣有效地解決了四元數(shù)加權(quán)均值問題，可以保持清晰的計算流程，并得到精度較高的結(jié)果。

在UKF算法中加入四元數(shù)形成Q-UKF算法，同樣存在一定的不足，包括誤差協(xié)方差矩陣的對稱性等。本文中針對上述問題進行了分析和設(shè)計，在此基礎(chǔ)上設(shè)計了一種新的四元數(shù)平方根UKF(QSR-UKF)算法。

選定的狀態(tài)變量x=[ρT,βT]T，其中ρ是姿態(tài)四元數(shù)的矢量部分；β是陀螺漂移。

(1) 初始化

(59)

(60)

(2) 時間更新。Sigma點避免了狀態(tài)誤差方差矩陣的平方根運算

(61)

狀態(tài)Sigma點分為姿態(tài)部分和陀螺漂移部分

(62)

考慮到過濾器更新過程中經(jīng)過四元數(shù)，因此這里選擇Sigma點作為四元數(shù)點

(63)

(64)

(65)

選擇陀螺漂移Sigma點作為

(66)

(67)

(68)

時間更新主要劃分為兩部分，分別是四元數(shù)、非四元數(shù)，前者的具體表達(dá)式為

(69)

由于陀螺儀漂移是一個線性傳遞過程，其時間被更新為

(70)

(71)

(72)

(73)

對于漂移部分，狀態(tài)預(yù)測均值以及協(xié)方差依次為

(74)

(75)

(76)

(3) 測量更新。與時間更新的Sigma點選擇類似，測量更新的Sigma點為

(77)

分為四元數(shù)和陀螺漂移，具體為

(78)

測量更新中四元數(shù)的Sigma部分為

(79)

(80)

(81)

選擇測量更新中的陀螺漂移部分Sigma作為

(82)

(83)

(84)

四元數(shù)測量更新為

(85)

則四元數(shù)測量誤差協(xié)方差矩陣和狀態(tài)測量誤差方差矩陣為

(86)

(87)

(4) 狀態(tài)增益。由上可知，狀態(tài)增益矩陣為

(88)

由上可知，狀態(tài)增強矩陣為

(89)

因此，四元數(shù)狀態(tài)更新量和陀螺儀漂移更新為

(90)

(91)

狀態(tài)誤差方差矩陣為

Uk=KkSzz,k

(92)

(93)

4 仿真試驗

本文分別在公寓環(huán)境下以及迷宮環(huán)境下對移動機器人SLAM進行了測試，分別對EKF、SR-UKF和QSR-UKF進行了仿真。所有仿真試驗均在Ubuntu 18.04中使用ROS Melodic進行，在英特爾Core i7-8750HCPU上運行。在本次研究中已經(jīng)事先確定了地圖特征和觀測特征之間的關(guān)系，由此降低了數(shù)據(jù)關(guān)聯(lián)錯誤所產(chǎn)生的不利影響。然后針對3種算法的應(yīng)用效果進行了對比分析，通過指標(biāo)量化分析，包括運行時間和精度，繼而可以得到客觀的結(jié)果。

本文創(chuàng)建兩個試驗環(huán)境，公寓環(huán)境大小為15 m×15 m，迷宮環(huán)境大小為80 m×8 0m，共有18個導(dǎo)航標(biāo)記和28個靜止點的特征以及一些隨機設(shè)置的點特征。如圖1和圖6所示，移動機器人的初始位置為(0,0)移動速度為v=3 m/s。另外，激光雷達(dá)采樣時長為0.2 s，探測距離最大值是30 m，距離誤差為0.1 m，過程噪聲協(xié)方差矩陣Q為[1.900；01.20；0000.8]，觀測噪聲協(xié)方差矩陣R為[0.50；000.1]。

針對仿真中的干擾因素進行設(shè)計，在100個控制周期中任意設(shè)置外部干擾，具體為3—5次。通過這種方式可以對SLAM精度產(chǎn)生一定的影響。在本次試驗中，采用了3種算法來對移動路標(biāo)進行估計，分別是EKF-SLAM,SR-UKF-SLAM、QSR-UKF-SLAM算法。在各個參數(shù)設(shè)置完成之后可以進行仿真的過程，具體得到的結(jié)果如圖2—圖5、圖7—圖10所示，其中“+”“·”分別代表環(huán)境特征的真實以及估計的位置；而實際移動路線和估計路線分別為如圖1—圖5所示。

4.1 準(zhǔn)確度

根據(jù)3種方法得到的姿態(tài)誤差信息即為圖2—圖4及圖7—圖9中所示，而對應(yīng)的環(huán)境位置信息如圖5及圖10中所示。根據(jù)圖中所示的信息可知，相對于EKF-SLAM，其他兩種算法可以達(dá)到更高的精度要求。通過調(diào)整協(xié)方差矩陣和增加噪聲誤差，可以看出SR-UKF和QSR-UKF-SLAM具有更高的姿態(tài)估計精度。根據(jù)本文研究的結(jié)果可知，受到噪聲因素的影響，導(dǎo)致EKF容易發(fā)散，增大了EKF-SLAM控制的難度，而這種現(xiàn)象與噪聲的類型有關(guān)，如果不是零均值白噪聲即容易出現(xiàn)發(fā)散的問題。SR-UKF-SLAM則可以實現(xiàn)噪聲適應(yīng)，主要與無跡變換等處理過程有關(guān)。雖然3種算法與機器人真實的路徑都存在一定的誤差，但是QSR-UKFSLAM算法機器人軌跡估計和路標(biāo)估計的誤差都小于EKF-SLAM和SR-UKF-SLAM算法，所以QSR-UKFSLAM算法的位姿估計精度最高。

圖1 公寓仿真環(huán)境Fig.1 Simulation in apartment environment

圖2 擴展卡爾曼濾波在公寓內(nèi)的仿真結(jié)果Fig.2 Estimate result of EKF in apartment environmen

圖3 平方根無跡卡爾曼濾波在公寓內(nèi)的仿真結(jié)果 Fig.3 Estimate result of SR-UKF in apartment environment

圖4 四元數(shù)平方根無跡卡爾曼濾波在公寓內(nèi)的仿真結(jié)果 Fig.4 Estimate result of QSR-UKF in apartment environment

圖5 3種在公寓內(nèi)的仿真結(jié)果的比較Fig.5 Estimate result of three algorithms in apartment environment

圖6 迷宮仿真環(huán)境Fig.6 Simulation in maze environment

圖7 擴展卡爾曼濾波在迷宮內(nèi)的仿真結(jié)果Fig.7 Estimate result of EKF in maze environment

圖8 平方根無跡卡爾曼濾波在迷宮內(nèi)的仿真結(jié)果Fig.8 Estimate result of SRUKF in maze environment

圖9 四元數(shù)平方根無跡卡爾曼濾波在迷宮內(nèi)的仿真結(jié)果Fig.9 Estimate result of QSR-UKF in maze environment

圖10 3種在公寓內(nèi)的仿真結(jié)果的比較 Fig.10 Estimate result of three algorithms in maze environment

為了更詳細(xì)地比較3種算法在不同的運動噪聲和觀測噪聲下的姿態(tài)估計精度，在研究過程中通過RMSE來進行評價，針對3種算法的姿態(tài)估計精度進行了對比分析，為了提升仿真結(jié)果的準(zhǔn)確性與可靠性，將仿真次數(shù)設(shè)置為20次，最終得到的信息見表1。表1主要包括兩個指標(biāo)，分別是RMSE_P、RMSE_L，二者分別對應(yīng)著機器人、路標(biāo)位置估計。

根據(jù)表1和表2的信息可以明顯地看到，在公寓環(huán)境中，QSR-UKF-SLAM算法在機器人位置估計和路標(biāo)估計上的RMSE比EKF-SLAM分別降低了55.20%、47.29%，比SQ-UKF-SLAM算法在軌跡和路標(biāo)估計的RMSE分別下降了32.81%、31.57%。在迷宮環(huán)境中，QSR-UKF-SLAM算法在機器人位置估計和路標(biāo)估計上的RMSE比EKF-SLAM分別降低了64.78%、67.92%，比SQ-UKF-SLAM算法在軌跡和路標(biāo)估計的RMSE分別下降了20.08%、29.53%。3種算法在路標(biāo)位置的均方根誤差曲線如圖11和圖12所示，結(jié)合圖中所示的信息可知，基于SR-UKF、QSR-UKF的SLAM算法可以達(dá)到較高的位姿估計精度，估計誤差最低的是QSR-UKFSLAM算法，由此驗證了算法的應(yīng)用效果。

表1 3種算法在公寓環(huán)境中移動機器人位置估計以及路標(biāo)位置估計的均方根誤差Tab.1 RMSE of three algorithms in robot position estimation and landmark position estimation in maze environment

表2 3種算法在迷宮環(huán)境中移動機器人位置估計以及路標(biāo)位置估計的均方根誤差Tab.2 RMSE of three algorithms in robot position estimation and landmark position estimation in maze environment

圖11 在公寓環(huán)境中3種算法在路標(biāo)位置上的均方根誤差Fig.11 RMSE curve of three algorithms in landmark position

圖12 在迷宮環(huán)境中3種算法在路標(biāo)位置上的均方根誤差曲線圖Fig.12 RMSE curve of three algorithms in landmark position

表3顯示了在迷宮環(huán)境下，EKF、SR-UKF、QSR-UKF-SLAM 3種算法在相同條件下的X、Y以及姿態(tài)角方向的絕對誤差平均值以及方差的比較。同時從表中可以看出，SR-UKF-SLAM和QSR-UKF-SLAM計算精度在整體上要比EKF-SLAM高；SR-UKF-SLAM和QSR-UKF-SLAM計算精度相對比較接近，因此，SR-UKF-SLAM和QSR-UKF-SLAM能有效降低EKF所帶來的線性化誤差?？傮w而言，QSR-UKF提高了機器人運動過程的穩(wěn)定性，具體性能優(yōu)于EKF-SLAM和SR-UKF-SLAM。本文未比較在公寓環(huán)境下3種算法在X、Y以及姿態(tài)角方向的絕對誤差平均值以及方差。原因是公寓環(huán)境較小，X、Y以及姿態(tài)角方向的絕對誤差平均值以及方差都較小，可比性不足。

表3 EKF、SR-UKF、QSR-UKF-SLAM在X、Y和姿態(tài)角上的絕對誤差均值以及誤差方差Tab.3 Absolute error mean and error variance of EKF, SR-UKF and QSR-UKF-SLAM in X, Y and attitude angles

4.2 運算時間

基于QSR-UKF-SLAM與EKF-SLAM、SQ-UKF-SLAM算法在公寓以及迷宮環(huán)境下的運行時間見表4及表5，結(jié)合表中的信息可知，總運行時間最低的是基于QSR-UKF的SLAM算法。并且，QSR-UKF-SLAM算法在單步運行時間上也少于其他兩種算法。

表4 基于QSR-UKF-SLAM與EKF-SLAM、SQ-UKF-SLAM算法在公寓環(huán)境下的運行時間Tab.4 Running time of QSR-UKF-SLAM, EKF-SLAM and SQ-UKF-SLAM in apartment environment

表5 基于QSR-UKF-SLAM與EKF-SLAM、SQ-UKF-SLAM算法在迷宮環(huán)境下的運行時間Tab.5 Running time of QSR-UKF-SLAM, EKF-SLAM and SQ-UKF-SLAM in maze environment

5 結(jié) 論

本文對移動機器人SLAM算法的位姿估計與校正問題進行了研究，針對基于非線性優(yōu)化的擴展卡爾曼濾波和無跡卡爾曼濾波位姿優(yōu)化算法的局限性，給出了相應(yīng)的解決方案。本文在無跡卡爾曼濾波中加入Sigma點的平方根以提高其穩(wěn)定性和估計精度。其次，根據(jù)四元數(shù)具有計算量小、精度高的優(yōu)點，將其向量當(dāng)作姿態(tài)變量，解決了四元數(shù)Sigma在選取過程中的可能出現(xiàn)的干擾問題。并且相較于無跡卡爾曼濾波降低了濾波狀態(tài)的維數(shù)，減少了計算耗時。綜上，本文提出了四元數(shù)平方根UKFSLAM位姿估計算法。仿真試驗結(jié)果表明，與EKF和SR-UKF算法相比，QSR-UKF算法采用了相對簡單的奇異值分解和Cholesky分解來實現(xiàn)誤差方差矩陣的平方根運算，使得算法的數(shù)值運算特性得到了一定程度的改善，同時也提高了算法的穩(wěn)定性。