一種基于動(dòng)態(tài)殘差的自適應(yīng)魯棒無(wú)跡卡爾曼濾波器定位算法

許 萬(wàn) 程 兆 夏瑞東 陳漢成

1.湖北工業(yè)大學(xué)機(jī)械工程學(xué)院,武漢,4300722.深圳海外裝飾工程有限公司,深圳,518031

0 引言

放線(xiàn)是完成建筑裝飾的基準(zhǔn)線(xiàn),對(duì)精度要求極高。相對(duì)于應(yīng)用在制造業(yè)、倉(cāng)儲(chǔ)業(yè)的移動(dòng)機(jī)器人,智能移動(dòng)放線(xiàn)機(jī)器人對(duì)軌跡跟蹤精度的要求極高,高定位精度和快速跟蹤自身位姿是實(shí)現(xiàn)高精度軌跡跟蹤的基礎(chǔ)。由于環(huán)境噪聲的存在,測(cè)量位姿與真實(shí)位姿之間存在偏差,而且智能移動(dòng)放線(xiàn)機(jī)器人工作的實(shí)際建筑施工現(xiàn)場(chǎng)為未整平的水泥地面,存在大量不規(guī)則分布的凹陷和凸起障礙物,具有很強(qiáng)的非線(xiàn)性,傳統(tǒng)定位算法無(wú)法滿(mǎn)足精度要求。

對(duì)于非線(xiàn)性系統(tǒng)的定位算法,卡爾曼濾波器[1](Kalman filter,KF)并不適用,常用于非線(xiàn)性系統(tǒng)定位的濾波算法有拓展卡爾曼濾波[2](extended Kalman filter,EKF)、粒子濾波[3](particle filter,PF)和無(wú)跡卡爾曼濾波[4](unscented Kalman filter,UKF)等。系統(tǒng)為強(qiáng)非線(xiàn)性系統(tǒng)時(shí),EKF的精度較差,且預(yù)測(cè)函數(shù)中的參數(shù)較多時(shí),其雅可比函數(shù)比較復(fù)雜、運(yùn)算量巨大、實(shí)時(shí)性較差[5]。對(duì)于非線(xiàn)性系統(tǒng)的定位問(wèn)題,PF具有較高的精度,但需要大量的樣本,且機(jī)器人面臨復(fù)雜情況時(shí)的算法復(fù)雜度高、計(jì)算量大[6]。相較于EKF,UKF具有更高的精度和穩(wěn)定性。相較于PF,UKF具有更小的計(jì)算量和更好的實(shí)時(shí)性[7]。UKF需要提前建立目標(biāo)的狀態(tài)方程并預(yù)設(shè)固定噪聲,而系統(tǒng)在實(shí)際運(yùn)行過(guò)程中的噪聲往往會(huì)不斷變化,且不與預(yù)設(shè)噪聲相同,因此UKF無(wú)法滿(mǎn)足智能移動(dòng)放線(xiàn)機(jī)器人對(duì)精度的要求,并且預(yù)測(cè)過(guò)程中可能出現(xiàn)預(yù)測(cè)位姿信息發(fā)散的問(wèn)題。智能移動(dòng)放線(xiàn)機(jī)器人對(duì)實(shí)時(shí)定位要求極高,UKF算法卻無(wú)法滿(mǎn)足實(shí)時(shí)性的要求。

針對(duì)UKF在定位中存在的問(wèn)題,研究者提出了許多改進(jìn)的UKF算法,LIU等[8]結(jié)合UT變換與EKF算法,提出了一種UEKF算法。相較于UKF算法,該算法的運(yùn)算速度得到極大的提升,但準(zhǔn)確率與魯棒性幾乎沒(méi)有提高。文獻(xiàn)[9-10]利用次優(yōu)Sage-Husa噪聲估計(jì)器對(duì)系統(tǒng)的噪聲統(tǒng)計(jì)特性進(jìn)行實(shí)時(shí)更新,獲取了更強(qiáng)的魯棒性,但定位過(guò)程中存在預(yù)測(cè)位姿信息發(fā)散的可能。 MENEGAZ等[11]提出了一種新的最小sigma集提取方法,減少了UKF算法的運(yùn)算量,在一些特定情況下可以獲得更高的運(yùn)算速度,但算法的運(yùn)用存在一定局限性。肖磊等[12]引入自適應(yīng)矩陣來(lái)修正系統(tǒng)噪聲協(xié)方差,提高了UKF算法的魯棒性,但該方法需要提前估計(jì)系統(tǒng)狀態(tài),增大了系統(tǒng)運(yùn)算量。

為滿(mǎn)足智能移動(dòng)放線(xiàn)機(jī)器人目標(biāo)實(shí)時(shí)定位和目標(biāo)位姿狀態(tài)準(zhǔn)確估計(jì)的要求,本文提出了一種基于動(dòng)態(tài)殘差的自適應(yīng)魯棒無(wú)跡卡爾曼濾波(adaptive robust unscented Kalman filter,ARUKF)算法。針對(duì)存在復(fù)雜外部干擾的系統(tǒng)擾動(dòng)問(wèn)題,結(jié)合UKF與抗差估計(jì)理論,利用濾波過(guò)程中的動(dòng)態(tài)殘差來(lái)減小系統(tǒng)擾動(dòng)對(duì)位姿信息預(yù)測(cè)的影響?？梢愿鶕?jù)動(dòng)態(tài)殘差計(jì)算自適應(yīng)加權(quán)因子,實(shí)時(shí)對(duì)預(yù)測(cè)矩陣和誤差協(xié)方差進(jìn)行加權(quán)處理,減小環(huán)境因素對(duì)系統(tǒng)穩(wěn)定性的影響,提高定位精度和魯棒性,同時(shí)通過(guò)采樣優(yōu)化來(lái)減小系統(tǒng)運(yùn)算量、提高系統(tǒng)實(shí)時(shí)性。仿真與實(shí)物實(shí)驗(yàn)驗(yàn)證了算法的可行性。

1 運(yùn)動(dòng)學(xué)模型及定位系統(tǒng)

1.1 智能移動(dòng)放線(xiàn)機(jī)器人的運(yùn)動(dòng)學(xué)模型與狀態(tài)方程

本文研究的智能移動(dòng)放線(xiàn)機(jī)器人是雙輪差速式移動(dòng)機(jī)器人[13]。圖1所示的機(jī)器人運(yùn)動(dòng)學(xué)模型中,大地坐標(biāo)系為OXY,將機(jī)器人前驅(qū)輪軸線(xiàn)的中心點(diǎn)OR定義為機(jī)器人的運(yùn)動(dòng)學(xué)中心,建立機(jī)器人的局部坐標(biāo)系ORXRYR。

圖1 智能移動(dòng)放線(xiàn)機(jī)器人運(yùn)動(dòng)學(xué)模型

智能移動(dòng)放線(xiàn)機(jī)器人的位姿為(x,y,θ)T,其中,(x,y)為機(jī)器人運(yùn)動(dòng)學(xué)中心在全局坐標(biāo)系中的坐標(biāo),θ為機(jī)器人局部坐標(biāo)系XR軸正方向與全局坐標(biāo)系X軸正方向之間的夾角。智能移動(dòng)放線(xiàn)機(jī)器人的運(yùn)動(dòng)狀態(tài)可由矢量U=(v,ω,a)T表示,其中,v、ω、a分別為機(jī)器人運(yùn)動(dòng)學(xué)中心的速度、角速度和加速度。相鄰兩時(shí)刻之間的時(shí)間間隔為Δt,智能移動(dòng)放線(xiàn)機(jī)器人的運(yùn)動(dòng)學(xué)模型滿(mǎn)足等式:

(1)

機(jī)器人實(shí)際位姿與參考位姿之間的誤差為

(2)

跟蹤誤差為

(3)

式中,eR為距離誤差;eψ為角度誤差。

根據(jù)圖1所示位置關(guān)系可以得到車(chē)輛的狀態(tài)方程:

(4)

1.2 定位系統(tǒng)與觀(guān)測(cè)方程

為滿(mǎn)足機(jī)器人高精度放線(xiàn)的要求,將全站儀作為外部測(cè)量傳感器。全站儀采集的數(shù)據(jù)為(r,φ)T,其中,r為距離,φ為全站儀在測(cè)量過(guò)程中的轉(zhuǎn)動(dòng)角度。全站儀具有極高的測(cè)量精度(100 m測(cè)量誤差小于3 mm),但無(wú)法滿(mǎn)足實(shí)時(shí)性要求(測(cè)量頻率僅為5 Hz),且無(wú)法獲得機(jī)器人的角度信息,因此綜合使用里程計(jì)、微型慣性測(cè)量單元和全站儀,以提高系統(tǒng)測(cè)量頻率,進(jìn)而提高系統(tǒng)測(cè)量精度。圖2為濾波補(bǔ)幀過(guò)程示意圖。

圖2 數(shù)據(jù)補(bǔ)幀處理示意圖

系統(tǒng)的觀(guān)測(cè)方程為

(5)

式中,(xm,ym)為全站儀在全局坐標(biāo)系中的坐標(biāo);arctan2(*)為四象限反正切函數(shù);εr、εφ分別為全站儀在測(cè)量過(guò)程中的距離測(cè)量噪聲和角度測(cè)量噪聲。

2 基于動(dòng)態(tài)殘差的ARUKF

智能移動(dòng)放線(xiàn)機(jī)器人在工作過(guò)程中可能會(huì)出現(xiàn)強(qiáng)非線(xiàn)性的位姿變化,UKF通過(guò)無(wú)跡變換的方法解決卡爾曼濾波在推演過(guò)程中出現(xiàn)的非線(xiàn)性傳遞問(wèn)題,對(duì)非線(xiàn)性運(yùn)動(dòng)的處理效果非常好。利用UKF需要提前預(yù)設(shè)系統(tǒng)噪聲與觀(guān)測(cè)噪聲,但智能移動(dòng)放線(xiàn)機(jī)器人的工作環(huán)境復(fù)雜、地面不平整,車(chē)體在運(yùn)動(dòng)過(guò)程中會(huì)持續(xù)抖動(dòng),很難預(yù)測(cè)多變的系統(tǒng)噪聲或觀(guān)測(cè)噪聲的誤差分布或誤差類(lèi)型,無(wú)法建立準(zhǔn)確的函數(shù)模型和隨機(jī)模型,因此,濾波過(guò)程需要減小更新參數(shù)的影響。

為解決該問(wèn)題,引入抗差估計(jì)理論[14],根據(jù)系統(tǒng)觀(guān)測(cè)量與預(yù)測(cè)量的動(dòng)態(tài)殘差來(lái)計(jì)算自適應(yīng)加權(quán)因子,并利用加權(quán)因子對(duì)每個(gè)離散時(shí)刻的預(yù)測(cè)狀態(tài)向量和預(yù)測(cè)誤差協(xié)方差進(jìn)行加權(quán)處理,從而使動(dòng)態(tài)模型信息與測(cè)量值信息達(dá)到平衡,可實(shí)時(shí)修改狀態(tài)方程以減小預(yù)設(shè)噪聲特性與實(shí)際不符的影響,降低濾波過(guò)程中預(yù)測(cè)位姿信息數(shù)據(jù)發(fā)散的可能性,提高算法精度和魯棒性。同時(shí),利用最小策略采樣減小系統(tǒng)的計(jì)算量,提高系統(tǒng)實(shí)時(shí)性。算法實(shí)現(xiàn)步驟如下:

(1)設(shè)定系統(tǒng)狀態(tài)估計(jì)矩陣初值X0和誤差協(xié)方差矩陣初值Σ0：

(6)

式中,Σx0、Σy0、Σθ0分別為系統(tǒng)X坐標(biāo)、Y坐標(biāo)和角度坐標(biāo)的初始方差。

xt-1,i=

(7)

W=diag(wc,1,wc,2,…,wc,n)

wc,i=w0ρ2(C-1[1]n×n(CT)-1)ii=1,2,…,n

完成采樣后,需對(duì)采集點(diǎn)進(jìn)行變換,將t-1時(shí)刻的系統(tǒng)狀態(tài)量xt-1,i在控制量ut的作用下轉(zhuǎn)換為t時(shí)刻的系統(tǒng)狀態(tài)量xt,i：

xt,i=g(ut,xt-1,i)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

式中,St為觀(guān)測(cè)量的協(xié)方差矩陣。

(5)計(jì)算動(dòng)態(tài)殘差

(16)

并設(shè)定自適應(yīng)比例因子

(17)

(6)為減小不準(zhǔn)確的預(yù)設(shè)噪聲對(duì)系統(tǒng)的影響,利用抗差估計(jì)理論對(duì)狀態(tài)量的估計(jì)值與估計(jì)協(xié)方差進(jìn)行魯棒性處理:

(18)

(19)

3 仿真與實(shí)地測(cè)試

3.1 仿真結(jié)果及分析

3.1.1仿真平臺(tái)與參數(shù)設(shè)置

為驗(yàn)證ARUKF算法的精度和魯棒性,借助MATLAB進(jìn)行仿真測(cè)試,將UKF、基于Sage-Husa的改進(jìn)無(wú)跡卡爾曼濾波(AUKF)和ARUKF在相同參數(shù)和控制量下進(jìn)行對(duì)比實(shí)驗(yàn)。

仿真路徑的第一段為直線(xiàn),起點(diǎn)為(0,0),終點(diǎn)為(9 m,0);第二段為圓弧,起點(diǎn)為(9 m,0),終點(diǎn)為(9 m,3.82 m),半徑為1.91m;第三段為直線(xiàn),起點(diǎn)為(9 m,3.82 m),終點(diǎn)為(0,3.82 m)。運(yùn)動(dòng)過(guò)程中,不斷對(duì)速度控制量U施加隨機(jī)擾動(dòng)來(lái)模擬地面不平整出現(xiàn)的車(chē)體抖動(dòng)。系統(tǒng)參數(shù)設(shè)置如表1所示。

表1 仿真實(shí)驗(yàn)系統(tǒng)參數(shù)

3.1.2仿真測(cè)試結(jié)果與分析

如圖3a所示,ARUKF最貼近真實(shí)值,AUKF次之,UKF偏差最大。圖3b中,UKF誤差最大、波動(dòng)較大,AUKF誤差較大、波動(dòng)較小,ARUKF誤差最小、波動(dòng)最小。圖3c中,AUKF誤差最大、波動(dòng)較大,UKF誤差較大、波動(dòng)較小,ARUKF誤差最小、波動(dòng)最小。圖3表明在受到外部擾動(dòng)影響時(shí),相較于UKF和AUKF,ARUKF的距離誤差和角度誤差的波動(dòng)更小,具有更好的魯棒性。

(a)位置估算圖

如表2所示,在相同的初始參數(shù)和控制量的作用下,ARUKF的距離誤差均值為UKF的42%,角度估算誤差均值為UKF的83%,ARUKF的距離誤差均值為AUKF的50%,角度估算誤差均值為AUKF的73%。這表明在受到外部擾動(dòng)影響時(shí),相較于UKF和AUKF,ARUKF的距離誤差和角度誤差更小,具有更高的精度。

表2 仿真實(shí)驗(yàn)結(jié)果均方根誤差

表3所示為ARUKF算法在直線(xiàn)運(yùn)動(dòng)和曲線(xiàn)運(yùn)動(dòng)時(shí)的均方根誤差。ARUKF算法在直線(xiàn)運(yùn)動(dòng)與曲線(xiàn)運(yùn)動(dòng)下的距離誤差和角度誤差相近,都具有較高的精度和較好的魯棒性。

表3 各運(yùn)動(dòng)狀態(tài)誤差對(duì)比圖

3.2 實(shí)地測(cè)試

3.2.1系統(tǒng)組成與實(shí)驗(yàn)設(shè)置

為驗(yàn)證智能移動(dòng)放線(xiàn)機(jī)器人的實(shí)際應(yīng)用效果,在某施工現(xiàn)場(chǎng)進(jìn)行實(shí)地測(cè)試。現(xiàn)場(chǎng)地面為未整平的水泥地面,存在大量不規(guī)則分布的凹陷和凸起障礙物。搭建的前驅(qū)式雙輪差速運(yùn)動(dòng)底盤(pán)搭載有輪式里程計(jì)、微型慣性測(cè)量單元和測(cè)量棱鏡,外置設(shè)備為全站儀天寶S5。主控制器為Beckhoff公司的工業(yè)PC,路徑設(shè)置的預(yù)處理計(jì)算機(jī)通過(guò)有線(xiàn)與控制器進(jìn)行數(shù)據(jù)交互。主控制器與驅(qū)動(dòng)器通過(guò)CANopen總線(xiàn)進(jìn)行控制。系統(tǒng)架構(gòu)如圖4所示。

圖4 系統(tǒng)結(jié)構(gòu)示意圖

實(shí)車(chē)測(cè)試平臺(tái)如圖5所示,移動(dòng)機(jī)器人的輪間距為245 mm,行駛速度為0.2 m/s,采樣周期為10 ms。

圖5 實(shí)車(chē)測(cè)試平臺(tái)

設(shè)定系統(tǒng)參數(shù)如下:

Σ0=diag(1,1,1,1)

Q=diag(0.001,0.001,0.001,0.001)

R=diag(0.0035,0.0036,0.0044,0.0045)

車(chē)體移動(dòng)速度由里程計(jì)和微型慣性測(cè)量單元獲取。實(shí)驗(yàn)過(guò)程中,根據(jù)實(shí)際工況完成一個(gè)完整U形運(yùn)動(dòng),運(yùn)動(dòng)包含三種運(yùn)動(dòng)基本狀態(tài)(直線(xiàn)、曲線(xiàn)和自旋)。通過(guò)對(duì)比實(shí)驗(yàn)來(lái)驗(yàn)證智能移動(dòng)放線(xiàn)機(jī)器人系統(tǒng)的精度和穩(wěn)定性,其中,直線(xiàn)段一起點(diǎn)為(2 m,0),終點(diǎn)為(4 m,0);曲線(xiàn)段起點(diǎn)為(4 m,0),終點(diǎn)為(4 m,3 m),半徑為1.5 m;直線(xiàn)段二起點(diǎn)為(4 m,3 m),終點(diǎn)為(2 m,3 m);在點(diǎn)(2 m,3 m)處進(jìn)行逆時(shí)針90°自旋。

實(shí)驗(yàn)1使用UKF算法進(jìn)行定位,實(shí)驗(yàn)2使用ARUKF算法進(jìn)行定位。兩組實(shí)驗(yàn)均采用基于前饋運(yùn)動(dòng)學(xué)和帶有干擾觀(guān)測(cè)器的解耦動(dòng)力學(xué)的混合控制算法[13]進(jìn)行軌跡跟蹤控制。兩組實(shí)驗(yàn)的參數(shù)與控制率完全相同。

3.2.2測(cè)試結(jié)果與分析

實(shí)車(chē)測(cè)試結(jié)果如圖6所示。圖6a中,ARUKF算法的定位精度更高,智能移動(dòng)放線(xiàn)機(jī)器人在直線(xiàn)段和曲線(xiàn)段的移動(dòng)軌跡都更貼近目標(biāo)軌跡。

(a)路徑跟蹤示意圖 (b)實(shí)驗(yàn)1的線(xiàn)速度和航向角示意圖

圖6b、圖6c中,智能移動(dòng)放線(xiàn)機(jī)器人由曲線(xiàn)段轉(zhuǎn)到直線(xiàn)段二時(shí),實(shí)驗(yàn)1的速度先跳至負(fù)值,然后緩慢增大,實(shí)驗(yàn)2并未出現(xiàn)此種情況。產(chǎn)生這種差異的原因是UKF算法的定位精度較差,在路徑銜接處出現(xiàn)較大誤差,智能移動(dòng)機(jī)器人出現(xiàn)過(guò)沖,運(yùn)動(dòng)超出預(yù)設(shè)位置后需要調(diào)整位姿。ARUKF算法的精度更高,運(yùn)動(dòng)過(guò)程中的誤差更小,速度更加平穩(wěn),車(chē)輛發(fā)生的抖動(dòng)更小,系統(tǒng)更穩(wěn)定。

如圖6d所示,實(shí)驗(yàn)1的直線(xiàn)段和曲線(xiàn)段的誤差都出現(xiàn)小范圍的劇烈波動(dòng),實(shí)驗(yàn)2的波動(dòng)更小、曲線(xiàn)更平滑。這是因?yàn)閷?shí)際施工環(huán)境中的地面不平整,智能移動(dòng)放線(xiàn)機(jī)器人在運(yùn)動(dòng)過(guò)程中出現(xiàn)車(chē)身抖動(dòng),而ARUKF具有更強(qiáng)的魯棒性,因此其抗干擾能力更強(qiáng)、數(shù)據(jù)波動(dòng)更小、曲線(xiàn)更平滑。實(shí)驗(yàn)1的自旋段距離誤差一直在零點(diǎn)附近波動(dòng),實(shí)驗(yàn)2則不存在此情況,這是因?yàn)锳RUKF根據(jù)動(dòng)態(tài)殘差實(shí)時(shí)調(diào)整了預(yù)測(cè)值和預(yù)測(cè)協(xié)方差,減少了系統(tǒng)噪聲對(duì)定位結(jié)果的影響。

圖6e為車(chē)體運(yùn)動(dòng)過(guò)程中的角度誤差示意圖,實(shí)驗(yàn)1的自旋段角度誤差為0.037 rad,實(shí)驗(yàn)2的自旋段角度誤差為0.017 rad。這是因?yàn)锳RUKF使用的采樣方法為最小采樣方法,減小了系統(tǒng)運(yùn)算量,提高了運(yùn)算速度和系統(tǒng)實(shí)時(shí)性。

實(shí)驗(yàn)結(jié)果中,距離誤差的均方根誤差如表4所示,角度誤差的均方根誤差如表5所示。由于移動(dòng)機(jī)器人的初始位姿會(huì)導(dǎo)致誤差數(shù)據(jù)波動(dòng),因此誤差數(shù)據(jù)取自系統(tǒng)穩(wěn)定運(yùn)行之后,以排除智能移動(dòng)放線(xiàn)機(jī)器人初始位姿差異產(chǎn)生的影響。

表4 實(shí)車(chē)測(cè)試距離誤差的均方根誤差

表5 實(shí)車(chē)測(cè)試角度誤差的均方根誤差

如表4、表5所示,UKF算法的定位精度較低,定位過(guò)程中數(shù)據(jù)出現(xiàn)發(fā)散現(xiàn)象,魯棒性不足,機(jī)器人出現(xiàn)過(guò)沖的問(wèn)題,實(shí)驗(yàn)的平均距離誤差為9 mm,最大距離誤差為13.6 mm,平均角度誤差為0.033 rad,最大角度誤差為0.62 rad;機(jī)器人在不同運(yùn)動(dòng)狀態(tài)時(shí)的定位精度波動(dòng)大,曲線(xiàn)運(yùn)動(dòng)的誤差遠(yuǎn)大于直線(xiàn)運(yùn)動(dòng)的誤差。ARUKF算法的定位精度較高,系統(tǒng)運(yùn)行穩(wěn)定,實(shí)驗(yàn)的平均距離誤差為2 mm,最大距離誤差為2.9 mm,平均角度誤差為0.015 rad,最大角度誤差為0.36 rad;機(jī)器人在不同狀態(tài)時(shí)的定位精度相近。

綜上所述,自適應(yīng)魯棒無(wú)跡卡爾曼濾波算法具有高精度、強(qiáng)魯棒性和高實(shí)時(shí)性的特點(diǎn)。雖然車(chē)體在運(yùn)動(dòng)過(guò)程中存在由地面不平整產(chǎn)生的抖動(dòng),但智能移動(dòng)機(jī)器人可以迅速調(diào)整自身狀態(tài)使自身狀態(tài)收斂并保持穩(wěn)定,這體現(xiàn)了ARUKF算法極高的定位精度與魯棒性。實(shí)驗(yàn)過(guò)程中的平均距離誤差小于3 mm,滿(mǎn)足建筑施工要求,可以用于地面環(huán)境復(fù)雜、存在大量分布不規(guī)則凹陷和凸起障礙物的實(shí)際施工現(xiàn)場(chǎng)。

4 結(jié)論

本文結(jié)合抗差估計(jì)理論,提出了一種自適應(yīng)魯棒無(wú)跡卡爾曼濾波算法。該算法利用系統(tǒng)觀(guān)測(cè)量與預(yù)測(cè)量之間的動(dòng)態(tài)殘差計(jì)算自適應(yīng)加權(quán)因子,進(jìn)而利用加權(quán)因子對(duì)每個(gè)離散時(shí)刻的預(yù)測(cè)狀態(tài)向量和預(yù)測(cè)誤差協(xié)方差進(jìn)行加權(quán)處理,從而使動(dòng)態(tài)模型信息與測(cè)量值信息達(dá)到平衡,可實(shí)時(shí)調(diào)整狀態(tài)方程,減小外部干擾對(duì)系統(tǒng)預(yù)測(cè)值的影響,通過(guò)減小采樣的運(yùn)算量來(lái)提高系統(tǒng)運(yùn)算的效率。仿真和施工現(xiàn)場(chǎng)測(cè)試結(jié)果表明,自適應(yīng)魯棒無(wú)跡卡爾曼濾波算法在系統(tǒng)存在不規(guī)則、不確定的擾動(dòng)時(shí)能快速收斂,并獲取精準(zhǔn)位姿。相較于無(wú)跡卡爾曼濾波算法和基于Sage-Husa的改進(jìn)無(wú)跡卡爾曼濾波算法,具有更高的精度、更強(qiáng)的魯棒性和更高的實(shí)時(shí)性。