基于MEMS-IMU 的動(dòng)態(tài)大失準(zhǔn)角STUKF 算法

顧元鑫，吳文啟，王茂松

（國防科技大學(xué) 智能科學(xué)學(xué)院，長(zhǎng)沙 410073）

微機(jī)電慣性測(cè)量單元（Micro-electro Mechanical Inertial Measurement Unit,MEMS-IMU）憑借其體積小、功耗低、價(jià)格低廉等優(yōu)勢(shì)，已經(jīng)在各類無人平臺(tái)的組合導(dǎo)航、動(dòng)態(tài)姿態(tài)測(cè)量等領(lǐng)域中得到廣泛應(yīng)用[1-3]，相關(guān)濾波算法的研究一直受到關(guān)注[4,5]。由于微機(jī)電陀螺、加速度計(jì)精度相對(duì)低，且MEMS-IMU應(yīng)用中經(jīng)常存在大失準(zhǔn)角姿態(tài)誤差情況[6]，在劇烈的角運(yùn)動(dòng)環(huán)境中，誤差影響顯著增大。對(duì)于導(dǎo)航級(jí)的慣導(dǎo)系統(tǒng)，可以采用慣性系粗對(duì)準(zhǔn)[7]的方法避免初始大失準(zhǔn)角情況。由于MEMS-IMU 陀螺精度較低，并不能通過慣性系粗對(duì)準(zhǔn)避免初始大失準(zhǔn)角情況，因此針對(duì)這種情況下的非線性濾波算法的研究是十分必要的。擴(kuò)展卡爾曼濾波器（Extended Kalman Filter,EKF）是應(yīng)用最為廣泛的傳統(tǒng)非線性濾波算法，但EKF 的一階截?cái)嗾`差較大，在大失準(zhǔn)角等強(qiáng)非線性應(yīng)用場(chǎng)景下EKF 會(huì)產(chǎn)生較大誤差甚至失效[8]。

針對(duì)大失準(zhǔn)角問題，無跡卡爾曼濾波（Unscented Kalman Filter,UKF）采用無跡變換（Unscented Transformation,UT）變換，應(yīng)用一系列精心選擇的點(diǎn)對(duì)系統(tǒng)的統(tǒng)計(jì)特性進(jìn)行近似，不需要像EKF 一樣將非線性模型進(jìn)行線性化，無需計(jì)算雅可比矩陣。與EKF相比，UKF 的模型更加準(zhǔn)確，在大失準(zhǔn)角等強(qiáng)非線性應(yīng)用場(chǎng)景中，其狀態(tài)估計(jì)的精度也就更高[9]；與粒子濾波相比，UKF 通過一定規(guī)則產(chǎn)生采樣點(diǎn)，不會(huì)出現(xiàn)粒子濾波中的粒子退化情況，且無跡卡爾濾波算法的計(jì)算量更小、更便于工程實(shí)現(xiàn)[10]。由于UKF 仍采用傳統(tǒng)慣導(dǎo)誤差模型，速度誤差微分方程中包含姿態(tài)矩陣與比力的乘積項(xiàng)。在劇烈的角運(yùn)動(dòng)環(huán)境中，姿態(tài)矩陣誤差和比力測(cè)量誤差會(huì)造成誤差狀態(tài)協(xié)方差矩陣的計(jì)算誤差，進(jìn)而導(dǎo)致狀態(tài)量估計(jì)不準(zhǔn)確。

文獻(xiàn)[11]提出了狀態(tài)變換擴(kuò)展卡爾曼濾波（State Transformation EKF,STEKF），使傳統(tǒng)慣導(dǎo)誤差模型中速度誤差微分方程中的比力項(xiàng)變換成較為穩(wěn)定的重力項(xiàng)，從而避免在動(dòng)態(tài)環(huán)境或慣性器件的比力誤差較大時(shí)，出現(xiàn)協(xié)方差矩陣計(jì)算不準(zhǔn)確所導(dǎo)致的狀態(tài)估計(jì)精度變差的情況[12,13]。但由于其誤差微分方程仍是在小失準(zhǔn)角的假設(shè)前提下推導(dǎo)出來的，本質(zhì)上仍存在截?cái)嗾`差，當(dāng)在動(dòng)態(tài)大初始失準(zhǔn)角等強(qiáng)非線性應(yīng)用場(chǎng)景中，由于其模型不準(zhǔn)確，仍會(huì)導(dǎo)致狀態(tài)估計(jì)精度變差。

本文提出了一種基于狀態(tài)變換的無跡卡爾曼濾波（State Transformation UKF,STUKF）算法，其基本思想是采用狀態(tài)變換的方法將UKF 速度誤差微分方程中的比力相關(guān)項(xiàng)轉(zhuǎn)換成了相對(duì)穩(wěn)定的重力項(xiàng)，避免了劇烈運(yùn)動(dòng)應(yīng)用場(chǎng)景中比力計(jì)算不準(zhǔn)確導(dǎo)致濾波精度下降的情形。相較于STEKF，由于本算法的誤差微分方程都是在大失準(zhǔn)角假設(shè)下推導(dǎo)獲得的，因此在初始大失準(zhǔn)角應(yīng)用場(chǎng)景中，其模型的準(zhǔn)確性以及濾波結(jié)果的可信性均優(yōu)于STEKF。

本文以基于MEMS-IMU 的小型無人船載航姿系統(tǒng)為例，經(jīng)海況數(shù)學(xué)仿真及三軸轉(zhuǎn)臺(tái)動(dòng)態(tài)實(shí)驗(yàn)驗(yàn)證，在初始大失準(zhǔn)角情況下，STUKF 的姿態(tài)角收斂精度及快速性均優(yōu)于現(xiàn)有的STEKF 及UKF。

1 大失準(zhǔn)角狀態(tài)變換非線性誤差微分方程

1.1 等效旋轉(zhuǎn)矢量非線性姿態(tài)誤差微分方程

理想情況下，捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)矩陣微分方程為[14]：

在實(shí)際應(yīng)用中，姿態(tài)矩陣微分方程是存在誤差的，即：

則大失準(zhǔn)角情況下的方向余弦誤差微分方程為：

將式(1)(2)代入式(4)，有：

又因?yàn)椋?/p>

令式(5)(6)右端相等，有：

大失準(zhǔn)角情況下等效旋轉(zhuǎn)矢量的非線性姿態(tài)誤差微分方程[15]如式(11)所示：

與傳統(tǒng)的歐拉角非線性姿態(tài)誤差微分方程[14]相比，沒有俯仰誤差角小于90°的限制；且當(dāng)姿態(tài)誤差角收斂到小角度時(shí)，非線性誤差模型可自然過渡到線性誤差模型。

1.2 狀態(tài)變換非線性速度誤差微分方程

將式(15)移項(xiàng)并代入式(16)，有：

其中，δ fb表示加速度計(jì)測(cè)量誤差，又有：

將式(15)移項(xiàng)并代入式(20)有：

其中，位置誤差向量定義為：

式(22)中，Lδ為緯度誤差；δλ為經(jīng)度誤差；hδ為高度誤差。

式(23)(24)中，RE及RN分別表示卯酉圈及子午圈曲率半徑；h表示當(dāng)?shù)馗叱?；L為當(dāng)?shù)鼐暥?。則：

式(26)中，ωie為地球自轉(zhuǎn)角速度。

位置誤差微分方程為：

2 STUKF 算法

2.1 狀態(tài)方程

根據(jù)上節(jié)中提出的大失準(zhǔn)角情況下基于等效旋轉(zhuǎn)矢量的狀態(tài)變換非線性誤差微分方程，可得STUKF系統(tǒng)的狀態(tài)方程：

狀態(tài)量x中的變量包括：

由于狀態(tài)方程為非線性，其狀態(tài)量與參數(shù)不可分，因此其狀態(tài)轉(zhuǎn)移矩陣無法明確寫出，以狀態(tài)轉(zhuǎn)移方程的形式給出：

系統(tǒng)噪聲矩陣為：

2.2 觀測(cè)方程

選取觀測(cè)量為：

觀測(cè)方程可表示為：

其中，υ為量測(cè)噪聲向量；H(x)為量測(cè)矩陣。有：

STUKF 的濾波過程與傳統(tǒng)的UKF 一致，與其相比，STUKF 將傳統(tǒng)的速度誤差替換成狀態(tài)變換后的速度誤差。變換后的誤差方程中不再含有與姿態(tài)及加速度計(jì)相關(guān)的比力項(xiàng)fn=，而是替換成相對(duì)穩(wěn)定的當(dāng)?shù)刂亓?xiàng)gn，減小了由于MEMS-IMU 姿態(tài)誤差和比力測(cè)量誤差大對(duì)協(xié)方差矩陣及狀態(tài)估計(jì)結(jié)果的影響。

STEKF 通過對(duì)非線性函數(shù)進(jìn)行泰勒展開，忽略其高階項(xiàng)，從而實(shí)現(xiàn)線性化，而STUKF 是通過對(duì)非線性函數(shù)的概率分布進(jìn)行近似，并且整個(gè)模型的推導(dǎo)過程均是在大失準(zhǔn)角的假設(shè)下完成的，因此大失準(zhǔn)角情況下STUKF 的模型準(zhǔn)確性要優(yōu)于STEKF。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 搖擺仿真實(shí)驗(yàn)

以小型無人船載MEMS-IMU/衛(wèi)星組合航姿系統(tǒng)為應(yīng)用場(chǎng)景，利用Matlab 生成仿真的載體姿態(tài)、速度和位置數(shù)據(jù)以及陀螺、加速度計(jì)理論采樣值，結(jié)合如表1 所示的實(shí)際MEMS-IMU 陀螺零偏及噪聲、加速度計(jì)零偏及噪聲，以及衛(wèi)星導(dǎo)航接收機(jī)的速度、位置測(cè)量噪聲，生成更符合真實(shí)情況的IMU 數(shù)據(jù)及速度位置觀測(cè)數(shù)據(jù)，選取的衛(wèi)星接收機(jī)的水平定位精度為2.5 m（1σ），測(cè)速精度為0.05 m/s（1σ）。

表1 模擬MEMS-IMU 性能指標(biāo)參數(shù)Tab.1 Specifications of the simulated MEMS-IMU

為提高M(jìn)EMS-IMU 水平加速度計(jì)零偏及等效東向陀螺零偏的可觀測(cè)性，實(shí)驗(yàn)中采用IMU 繞方位軸旋轉(zhuǎn)調(diào)制的方式，即 MEMS-IMU 繞方位軸采用0°→180°→ 0°每個(gè)位置停留5 min 的轉(zhuǎn)停方案，實(shí)現(xiàn)對(duì)MEMS-IMU 陀螺儀及加速度計(jì)零偏的估計(jì)和補(bǔ)償。

按照現(xiàn)行電工電子產(chǎn)品環(huán)境試驗(yàn)國家標(biāo)準(zhǔn)中典型搖擺實(shí)驗(yàn)嚴(yán)酷等級(jí)示例表中的指導(dǎo)參數(shù)進(jìn)行實(shí)驗(yàn)，載體搖擺方式中各參數(shù)定義如下：滾動(dòng)角正弦搖擺幅值為Aγ，正弦搖擺頻率為fγ；俯仰角正弦搖擺幅值為Aθ，正弦搖擺頻率為fθ；航向角正弦搖擺幅值為Aφ，正弦搖擺頻率為fφ。實(shí)驗(yàn)1-3 的參數(shù)選取如表2 所示。

表2 搖擺仿真實(shí)驗(yàn)Tab.2 Swing simulation experiments

記初始滾動(dòng)失準(zhǔn)角為Δγ，初始俯仰失準(zhǔn)角為Δθ，初始航向失準(zhǔn)角為Δφ。在實(shí)際應(yīng)用中，由于加速度計(jì)的精度一般較高，因此水平大失準(zhǔn)角估計(jì)相較于航向大失準(zhǔn)角更容易，所以實(shí)驗(yàn)中更側(cè)重航向大失準(zhǔn)角的情況。采用100 s 的慣性系粗對(duì)準(zhǔn)[7]時(shí)，三個(gè)陀螺5°/h 的零偏造成的航向誤差也不會(huì)超過60°。按照表3 所給定的大失準(zhǔn)角取值范圍均勻、隨機(jī)地選取50組初始失準(zhǔn)角進(jìn)行蒙特卡洛實(shí)驗(yàn)，實(shí)驗(yàn)中按失準(zhǔn)角較為惡劣的情況進(jìn)行假設(shè)，因此φN、φE、φD的濾波初始方差設(shè)置為[(45°)2,(45°)2,(60°)2]T，各次蒙特卡洛仿真實(shí)驗(yàn)的時(shí)長(zhǎng)為2000 s。根據(jù)表2 中實(shí)驗(yàn)2 的搖擺條件及表3 中小失準(zhǔn)角取值范圍均勻、隨機(jī)地選取50 組初始小失準(zhǔn)角進(jìn)行蒙特卡洛仿真，φN、φE、φD的濾波初始方差設(shè)置為[(0.1°)2,(0.1°)2,(0.5°)2]T，記為實(shí)驗(yàn)4。

表3 搖擺仿真實(shí)驗(yàn)初始失準(zhǔn)角設(shè)定Tab.3 Initial misalignment angle setting for swing simulation experiments

在上述給定初始失準(zhǔn)角取值規(guī)則及搖擺方式設(shè)定的基礎(chǔ)上，進(jìn)行50 次蒙特卡洛仿真。由于傳統(tǒng)EKF算法在動(dòng)態(tài)大失準(zhǔn)角情況下發(fā)散，不能正常工作，因此選取STUKF、STEKF、UKF 三種算法進(jìn)行比較，用姿態(tài)角估計(jì)誤差收斂后的值（600 s～2000 s）求取均方根值（Root Mean Square,RMS），判定算法的姿態(tài)角估計(jì)精度，實(shí)驗(yàn)結(jié)果如圖1-4 所示。

圖1 搖擺仿真實(shí)驗(yàn)1 的姿態(tài)誤差RMSFig.1 RMS of attitude error of swing simulation experiment 1

圖2 搖擺仿真實(shí)驗(yàn)2 的姿態(tài)誤差RMSFig.2 RMS of attitude error of swing simulation experiment 2

圖3 搖擺仿真實(shí)驗(yàn)3 的姿態(tài)誤差RMSFig.3 RMS of attitude error of swing simulation experiment 3

綜合圖1-3，在大初始失準(zhǔn)角情況下：在水平角方面，三種濾波算法的最終收斂精度相近，收斂值可維持0.01°（1σ）；在航向角方面，STUKF 的航向角估計(jì)精度的收斂值總可以優(yōu)于0.5°（1σ），UKF 的航向角估計(jì)精度的收斂值總可以優(yōu)于2°（1σ），STEKF的航向角估計(jì)精度的收斂值總可以優(yōu)于3°（1σ）。從總體上來說，STUKF 的姿態(tài)估計(jì)精度優(yōu)于UKF，UKF優(yōu)于STEKF。由圖4 可知，在小初始失準(zhǔn)角情況下，三種濾波算法的姿態(tài)估計(jì)精度均有提升：在水平角方面，三種算法的精度相近；在航向角方面，STUKF 與UKF 的姿態(tài)估計(jì)精度相近，均優(yōu)于STEKF，造成這一現(xiàn)象的主要原因是大陀螺零偏及加速度計(jì)零偏依然會(huì)導(dǎo)致實(shí)際系統(tǒng)模型非線性，進(jìn)而影響STEKF 的姿態(tài)估計(jì)精度。

圖4 搖擺仿真實(shí)驗(yàn)4 的姿態(tài)誤差RMSFig.4 RMS of attitude error of swing simulation experiment 4

選取實(shí)驗(yàn)2 中的搖擺條件及表1 中零偏初值進(jìn)行實(shí)驗(yàn)，比較UKF 與STUKF 的誤差狀態(tài)方差一致性，然后增大慣性器件誤差，設(shè)置x、y、z軸的陀螺及加速度計(jì)零偏為[3.5°/h,4°/h,4.5°/h]T和[4.5 mg,5 mg,5.5 mg]T，進(jìn)行誤差狀態(tài)方差一致性分析。實(shí)驗(yàn)中先進(jìn)行慣性系粗對(duì)準(zhǔn)，在粗對(duì)準(zhǔn)后大失準(zhǔn)角基礎(chǔ)上應(yīng)用UKF 及STUKF 進(jìn)行精對(duì)準(zhǔn)，以航向角誤差為例進(jìn)行分析。由于慣性系粗對(duì)準(zhǔn)后初始水平失準(zhǔn)角很小，為更好地觀察誤差狀態(tài)方差一致性，φN、φE、φD的濾波初始方差設(shè)置為[(1°)2,(1°)2,(60°)2]T，實(shí)驗(yàn)結(jié)果如圖5-8 所示。

圖5 增大慣性器件誤差前由濾波協(xié)方差陣得到的航向角誤差標(biāo)準(zhǔn)差估計(jì)曲線Fig.5 Estimation curves of yaw angle error standard deviation obtained in the filter covariance matrix before increasing the inertial sensor errors

圖6 增大慣性器件誤差前航向角誤差估計(jì)曲線Fig.6 Estimation curves of yaw angle error before increasing the inertial sensor errors

圖7 增大慣性器件誤差后由濾波協(xié)方差陣得到的航向角誤差標(biāo)準(zhǔn)差估計(jì)曲線Fig.7 Estimation curves of yaw angle error standard deviation obtained in the filter covariance matrix after increasing the inertial sensor errors

圖8 增大慣性器件誤差后航向角誤差估計(jì)曲線Fig.8 Estimation curves of yaw angle error after increasing the inertial sensor errors

從圖5-8 可知，在陀螺及加速度計(jì)零偏加大前后的兩組實(shí)驗(yàn)中，UKF 航向角誤差標(biāo)準(zhǔn)差估計(jì)曲線的收斂速度快于STUKF，但其實(shí)際誤差收斂速度卻比STUKF 慢，導(dǎo)致UKF 航向角誤差還未收斂但其誤差標(biāo)準(zhǔn)差估計(jì)值就已過度收斂，與實(shí)際誤差標(biāo)準(zhǔn)差不一致，表現(xiàn)出誤差標(biāo)準(zhǔn)差過度優(yōu)化現(xiàn)象，進(jìn)而導(dǎo)致UKF航向角誤差收斂精度及速度都比STUKF 差。

由于實(shí)際工作中航向失準(zhǔn)角的估計(jì)困難更大，且其收斂速度更慢，因此選取航向失準(zhǔn)角的誤差完全收斂到[-1°,1°] 范圍內(nèi)時(shí)記為算法航向誤差的收斂時(shí)間（若該次實(shí)驗(yàn)中航向誤差最終都未收斂到[-1°,1°]范圍內(nèi)，則記航向誤差收斂時(shí)間為2000 s）。取每組實(shí)驗(yàn)中50 次蒙塔卡洛實(shí)驗(yàn)收斂時(shí)間的均值作為該組實(shí)驗(yàn)濾波算法的收斂時(shí)間，實(shí)驗(yàn)結(jié)果如表4 所示。

表4 蒙特卡洛實(shí)驗(yàn)平均收斂時(shí)間Tab.4 Average convergence time of Monte Carlo experiments

由表4 可知，在大初始失準(zhǔn)角情況下，STUKF 航向角估計(jì)誤差的收斂速度優(yōu)于 UKF，UKF 優(yōu)于STEKF。

3.2 三軸轉(zhuǎn)臺(tái)實(shí)驗(yàn)

由于MEMS-IMU 在三軸角運(yùn)動(dòng)模擬轉(zhuǎn)臺(tái)上存在姿態(tài)安裝偏差，且由于三軸角運(yùn)動(dòng)模擬轉(zhuǎn)臺(tái)與實(shí)驗(yàn)系統(tǒng)未完全同步，因此選取初始小失準(zhǔn)角情況下長(zhǎng)時(shí)間對(duì)準(zhǔn)所獲得的姿態(tài)值作為姿態(tài)基準(zhǔn)。在實(shí)驗(yàn)中發(fā)現(xiàn)，三種算法在初始小失準(zhǔn)角情況下的姿態(tài)對(duì)準(zhǔn)誤差相近，只是在IMU 方位旋轉(zhuǎn)調(diào)制的轉(zhuǎn)位過程中每個(gè)轉(zhuǎn)停周期內(nèi)姿態(tài)誤差的具體變化存在一定差異，因此為避免各濾波算法誤差的具體變化對(duì)姿態(tài)基準(zhǔn)精度產(chǎn)生影響，選取三種濾波算法在初始小失準(zhǔn)角情況下且去除了初始陀螺、加速度計(jì)零偏的濾波結(jié)果均值作為姿態(tài)基準(zhǔn)。利用三軸角運(yùn)動(dòng)模擬轉(zhuǎn)臺(tái)進(jìn)行搖擺實(shí)驗(yàn)，實(shí)驗(yàn)中三軸轉(zhuǎn)臺(tái)的運(yùn)動(dòng)參數(shù)設(shè)置與仿真實(shí)驗(yàn)中實(shí)驗(yàn)2 的設(shè)置一致。實(shí)驗(yàn)在x、y、z軸陀螺及加速度計(jì)標(biāo)定后剩余零偏的基礎(chǔ)上分別加以[4.9°/h,5°/h,5.1°/h]T和[4.9 mg,5 mg,5.1 mg]T的常值零偏誤差。由于慣性系粗對(duì)準(zhǔn)后水平失準(zhǔn)角較小，因此φN、φE、φD的濾波初始方差設(shè)置為[(5°)2,(5°)2,(60°)2]T。首先采用慣性系粗對(duì)準(zhǔn)，然后在慣性系粗對(duì)準(zhǔn)后大失準(zhǔn)角誤差基礎(chǔ)上分別采用三種算法進(jìn)行精對(duì)準(zhǔn)。在三軸轉(zhuǎn)臺(tái)搖擺實(shí)驗(yàn)中，由于陀螺和加速度計(jì)的標(biāo)度因數(shù)誤差、溫度影響、殘余的桿臂誤差、速度觀測(cè)信息不準(zhǔn)確以及姿態(tài)基準(zhǔn)值本身不準(zhǔn)確等問題，導(dǎo)致三軸轉(zhuǎn)臺(tái)動(dòng)態(tài)實(shí)驗(yàn)中姿態(tài)角誤差值會(huì)比仿真實(shí)驗(yàn)中大。

濾波算法中陀螺及加速度計(jì)零偏估計(jì)值的濾波初值均設(shè)置為0。經(jīng)過濾波，陀螺及加速度計(jì)零偏的估計(jì)值逐漸逼近零偏誤差的實(shí)際值，圖9-10 即為估計(jì)所得的零偏值。從圖中可知，三種濾波算法可實(shí)現(xiàn)對(duì)除天向陀螺外的兩個(gè)水平陀螺零偏及三個(gè)加速度計(jì)零偏的有效估計(jì)，且三種濾波算法估計(jì)得到的零偏值相近。

圖9 轉(zhuǎn)臺(tái)實(shí)驗(yàn)陀螺零偏誤差估計(jì)曲線Fig.9 Gyroscope bias error estimation curves of turntable experiments

圖10 轉(zhuǎn)臺(tái)實(shí)驗(yàn)加速度計(jì)零偏誤差估計(jì)曲線Fig.10 Accelerometer bias error estimation curves of turntable experiments

由圖11-12 可知，在精對(duì)準(zhǔn)900 s 后，STUKF 的滾動(dòng)角和俯仰角估計(jì)誤差分別收斂在0.03°（1σ）和0.02°（1σ）；STEKF 的滾動(dòng)角和俯仰角估計(jì)誤差分別收斂在0.05°（1σ）和0.02°（1σ）；UKF 的滾動(dòng)角和俯仰角估計(jì)誤差分別收斂在0.03°（1σ）和0.04°（1σ）。由圖13 可知，在航向角方面，STUKF 精對(duì)準(zhǔn)900 s 后，航向角的估計(jì)誤差收斂到1°（1σ）以內(nèi)；UKF精對(duì)準(zhǔn)900 s后，航向角的估計(jì)誤差收斂到2°（1σ）以內(nèi)；STEKF 精對(duì)準(zhǔn)900 s 后，航向角的估計(jì)誤差收斂到3°（1σ）以內(nèi)。STUKF 對(duì)航向角估計(jì)誤差的收斂速度及精度優(yōu)于UKF，UKF 優(yōu)于STEKF。

圖12 轉(zhuǎn)臺(tái)實(shí)驗(yàn)俯仰角誤差Fig.12 Pitch angle error of turntable experiments

圖13 轉(zhuǎn)臺(tái)實(shí)驗(yàn)航向角誤差Fig.13 Yaw angle error of turntable experiments

綜上可以得出結(jié)論：在初始大失準(zhǔn)角動(dòng)態(tài)姿態(tài)對(duì)準(zhǔn)中，STUKF 的對(duì)準(zhǔn)精度及快速性優(yōu)于UKF，UKF 優(yōu)于STEKF，與仿真實(shí)驗(yàn)的結(jié)論一致，驗(yàn)證了仿真實(shí)驗(yàn)的可信性。

4 結(jié)論

以基于MEMS-IMU 的小型無人船載航姿系統(tǒng)為應(yīng)用背景，本文提出了一種STUKF 算法。通過狀態(tài)變換的方法將UKF 速度誤差微分方程中的比力項(xiàng)轉(zhuǎn)換為較為穩(wěn)定的重力項(xiàng)，克服了UKF 在劇烈運(yùn)動(dòng)情況下，姿態(tài)誤差與比力測(cè)量誤差導(dǎo)致的狀態(tài)估計(jì)不準(zhǔn)確的問題。相較于現(xiàn)有的STEKF 而言，在初始大失準(zhǔn)角以及MEMS-IMU 中陀螺和加速度計(jì)零偏較大的應(yīng)用場(chǎng)景中，考慮了非線性誤差項(xiàng)的STUKF 模型更加準(zhǔn)確，濾波精度更高。通過蒙特卡洛搖擺模擬仿真實(shí)驗(yàn)與實(shí)際三軸轉(zhuǎn)臺(tái)動(dòng)態(tài)實(shí)驗(yàn)驗(yàn)證了STUKF算法在初始大失準(zhǔn)角應(yīng)用場(chǎng)景中的優(yōu)越性，其收斂速度及姿態(tài)估計(jì)精度優(yōu)于UKF 及STEKF。同時(shí)，本文提出的等效旋轉(zhuǎn)矢量慣導(dǎo)大失準(zhǔn)角誤差方程也可推廣用于中、高精度慣導(dǎo)系統(tǒng)。