一種基于改進(jìn)旋轉(zhuǎn)矢量的低成本多數(shù)據(jù)融合姿態(tài)更新算法

王君 趙苑辰 李濤

（北京航天長征飛行器研究所 北京市 100076）

1 引言

在導(dǎo)航系統(tǒng)的應(yīng)用中，考慮到低成本、高環(huán)境適應(yīng)性等客觀條件，通常使用低精度MEMS 慣組作為傳感器來進(jìn)行捷聯(lián)導(dǎo)航解算。此類型應(yīng)用中慣性測量器件與載體固連，當(dāng)載體存在較大機(jī)動(dòng)時(shí)，姿態(tài)精度對導(dǎo)航算法的要求較高。捷聯(lián)慣性導(dǎo)航系統(tǒng)的數(shù)據(jù)更新算法一般包括姿態(tài)更新、速度更新和位置更新算法；其中姿態(tài)更新中的誤差會(huì)加快速度誤差、位置誤差增加的速度，因此姿態(tài)精度對系統(tǒng)精度的影響最大，是算法研究和設(shè)計(jì)的核心[1]。在自捷聯(lián)慣性導(dǎo)航系統(tǒng)的概念提出以來，對捷聯(lián)導(dǎo)航姿態(tài)更新算法己經(jīng)形成了以四元數(shù)導(dǎo)航算法、多子樣旋轉(zhuǎn)矢量圓錐補(bǔ)償算法、劃船補(bǔ)償算法為代表的捷聯(lián)慣性導(dǎo)航姿態(tài)更新算法。

國內(nèi)外許多學(xué)者提出了不同的旋轉(zhuǎn)矢量算法[2][3]，都是基于陀螺的角增量信息，然而對于目前的常用慣性器件輸出的角速率信息，無法直接使用。因此本文針對以MEMS 陀螺、MEMS 加表為代表的低精度慣性器件，使用角速率信息推導(dǎo)出一種改進(jìn)旋轉(zhuǎn)矢量的表達(dá)式，增加了基于單基線GNSS 位置速度測量的角速度卡爾曼濾波估計(jì)補(bǔ)償方法。經(jīng)過數(shù)學(xué)仿真研究和跑車試驗(yàn)數(shù)據(jù)分析，試驗(yàn)結(jié)果表明本文提出的數(shù)據(jù)融合算法，能夠結(jié)合改進(jìn)雙子樣姿態(tài)更新和單基線GNSS 的優(yōu)勢，通過多數(shù)據(jù)融合算法有效提高載體姿態(tài)估計(jì)精度，具備較高的工程應(yīng)用價(jià)值。

2 姿態(tài)更新算法

捷聯(lián)慣導(dǎo)姿態(tài)更新的經(jīng)典算法包括姿態(tài)更新四元數(shù)法、姿態(tài)更新旋轉(zhuǎn)矢量法等[1]。

2.1 四元數(shù)法

2.2 旋轉(zhuǎn)矢量法

通常載體角運(yùn)動(dòng)較為復(fù)雜，在更新周期內(nèi)不是定軸轉(zhuǎn)動(dòng)的。2.1節(jié)中，使用角增量直接求解姿態(tài)變換四元數(shù)，則會(huì)產(chǎn)生誤差。姿態(tài)更新的旋轉(zhuǎn)矢量算法可避免產(chǎn)生不可交換誤差，其原理是首先使用固聯(lián)于載體的傳感器得到角增量信息，其次利用同為描述轉(zhuǎn)動(dòng)過程的角增量和等效旋轉(zhuǎn)矢量之間的關(guān)系求解后者，最后使用等效旋轉(zhuǎn)矢量對載體的姿態(tài)進(jìn)行更新。

在姿態(tài)更新的旋轉(zhuǎn)矢量法中，可以使用多種方法利用角增量來構(gòu)建旋轉(zhuǎn)矢量；最直觀的是以姿態(tài)更新周期內(nèi)使用角增量數(shù)量進(jìn)行區(qū)分，使用傳感器輸出的一次角增量結(jié)果信息構(gòu)造旋轉(zhuǎn)矢量成為單子樣法，使用兩次更新結(jié)果為雙子樣法，使用三次更新結(jié)果為三子樣法，以此類推。

等效旋轉(zhuǎn)矢量φ 表示載體從tm-1時(shí)刻運(yùn)動(dòng)至tm時(shí)刻描述姿態(tài)變化的等效旋轉(zhuǎn)，由其定義可知轉(zhuǎn)動(dòng)的方向?yàn)閡=φ/φ，轉(zhuǎn)動(dòng)大小為等效旋轉(zhuǎn)矢量φ 的模φ。當(dāng)且僅當(dāng)φ 為微小量時(shí)，可以對等效旋轉(zhuǎn)矢量的微分方程展開后的高階微小量進(jìn)行省略；則微分方程近似為：[3]

觀察（4）式與（7），兩式為姿態(tài)變換四元數(shù)的不同構(gòu)成形式。以發(fā)現(xiàn)兩者的主要區(qū)別是構(gòu)造時(shí)四元數(shù)時(shí)使用的元素不同，（4）為使用角增量，（7）為使用旋轉(zhuǎn)矢量。若計(jì)算旋轉(zhuǎn)矢量使用常量擬合角速度，即單子樣旋轉(zhuǎn)矢量算法，則與四元數(shù)法等價(jià)。若使用一次項(xiàng)、二次項(xiàng)擬合角速度，可得雙子樣、三子樣算法構(gòu)建的旋轉(zhuǎn)矢量：[1]

3 改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法

在載體姿態(tài)存在復(fù)雜運(yùn)動(dòng)時(shí)，如存在周期性振動(dòng)，則會(huì)誘發(fā)捷聯(lián)慣導(dǎo)姿態(tài)更新產(chǎn)生嚴(yán)重漂移，從而使姿態(tài)更新產(chǎn)生偏差。由于周期性振動(dòng)可近似為圓錐運(yùn)動(dòng)，則可以使用基于圓錐誤差補(bǔ)償?shù)亩嘧訕铀惴ㄟM(jìn)行優(yōu)化?？紤]到常用的慣性器件輸出為角速度，使用角速度構(gòu)建旋轉(zhuǎn)矢量。

3.1 旋轉(zhuǎn)矢量優(yōu)化方法

3.2 基于角速率輸出的姿態(tài)更新算法

4 低成本數(shù)據(jù)融合算法

載體的實(shí)時(shí)姿態(tài)可由姿態(tài)四元數(shù)進(jìn)行表示，依據(jù)（1）式四元數(shù)運(yùn)動(dòng)學(xué)微分方程特性，載體的姿態(tài)估計(jì)是非線性問題。非線性系統(tǒng)的分析有多種經(jīng)典方法，常用的包括擴(kuò)展卡爾曼濾波、二階濾波、迭代濾波等?？紤]到實(shí)際工程應(yīng)用的制約，從運(yùn)算量、算法復(fù)雜度和魯棒性方面考慮，選擇擴(kuò)展卡爾曼濾波對姿態(tài)估計(jì)進(jìn)行分析。擴(kuò)展卡爾曼濾波的思路為將非線性系統(tǒng)在參考點(diǎn)處進(jìn)行泰勒級(jí)數(shù)展開，略去高階項(xiàng)取一階線性部分作為近視，再開展線性卡爾曼濾波估計(jì)。

由于低精度MEMS 傳感器的零偏量級(jí)大，必須進(jìn)行補(bǔ)償才能提高姿態(tài)更新精度。若載體使用場景為車載、穩(wěn)定平臺(tái)、航行器等，可依據(jù)載體機(jī)動(dòng)設(shè)計(jì)加速度輸出的機(jī)動(dòng)判斷，進(jìn)而對水平姿態(tài)進(jìn)行修正；同時(shí)采用單基線GNSS 測量的速度矢量，求解載體與地理北向的方位角，估計(jì)載體的姿態(tài)信息。低成本數(shù)據(jù)融合的EKF 算法流程圖如圖1所示。

圖1：低成本數(shù)據(jù)融合的EKF 算法流程圖

4.1 擴(kuò)展卡爾曼濾波狀態(tài)方程的建立

4.2 觀測方程的建立和濾波算法流程

5 試驗(yàn)仿真

5.1 圓錐運(yùn)動(dòng)仿真

搭建試驗(yàn)工況對比不同算法的精度，依據(jù)第3 節(jié)中提出的基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法進(jìn)行仿真，仿真條件為（仿真參數(shù)Ω=1Hz，條件，慣組更新周期2ms，姿態(tài)更新周期4ms）。姿態(tài)更新的雙子樣算法與改進(jìn)型算法的姿態(tài)更新誤差如圖2所示。可觀察到改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法的誤差比經(jīng)典雙子樣算法減小一倍。

圖2：兩種算法的姿態(tài)更新誤差曲線

5.2 跑車試驗(yàn)

為驗(yàn)證實(shí)際工況下數(shù)據(jù)融合算法的可靠性和姿態(tài)精度，開展了地面跑車試驗(yàn)進(jìn)行驗(yàn)證。為了獲得比較基準(zhǔn)，在試驗(yàn)車上搭載了高精度慣導(dǎo)系統(tǒng)（航向姿態(tài)精度0.3°，3σ；水平姿態(tài)精度0.15°，3σ）。高精度慣導(dǎo)的數(shù)據(jù)更新頻率為200Hz；低精度MEMS 慣組主要器件精度為：陀螺零偏60°/h，3σ；加表零偏6mg，3σ，數(shù)據(jù)更新頻率為200Hz（姿態(tài)更新頻率為100Hz）。由于對高精度慣導(dǎo)和低精度MEMS 的輸出沒有同步信號(hào)，對高精度關(guān)組的輸出結(jié)果進(jìn)行擬合，與低精度MEMS 進(jìn)行比較。圖3 為跑車試驗(yàn)姿態(tài)角試驗(yàn)結(jié)果。從結(jié)果中可看出，在約600s 的實(shí)驗(yàn)中，改進(jìn)數(shù)據(jù)融合算法航向角的偏差在1°以內(nèi)，水平姿態(tài)偏差在0.5°以內(nèi)；雙子樣算法姿態(tài)精度漂移6°左右；改進(jìn)數(shù)據(jù)融合算法的精度有明顯的提高。由于本方法中采用了加速度計(jì)的輸出進(jìn)行機(jī)動(dòng)判斷，判斷的準(zhǔn)確度決定了水平修正的正確性；航向修正也需要結(jié)合GNSS 的輸出進(jìn)行機(jī)動(dòng)判斷，當(dāng)且僅當(dāng)載體進(jìn)行直線運(yùn)動(dòng)時(shí)才能進(jìn)行航向的修正，判斷的準(zhǔn)確度也決定了航向角修正的正確性。為解決航向角估計(jì)的動(dòng)態(tài)限制，可采用其他傳感器提供航向信息，例如參考文獻(xiàn)[5]提出的基于磁力計(jì)的方法。

圖3：雙子樣與數(shù)據(jù)融合算法的姿態(tài)曲線

6 結(jié)束語

本文提出了一種基于改進(jìn)旋轉(zhuǎn)矢量的低成本多數(shù)據(jù)融合姿態(tài)更新算法。為提高姿態(tài)更新精度，在雙子樣旋轉(zhuǎn)矢量系數(shù)補(bǔ)償?shù)幕A(chǔ)上，增加前一時(shí)刻角速度信息建立改進(jìn)旋轉(zhuǎn)矢量更新公式。并增加了基于單基線GNSS 速度測量的卡爾曼濾波角速度估計(jì)補(bǔ)償。通過數(shù)學(xué)仿真分析，結(jié)果表明在圓錐運(yùn)動(dòng)條件下，利用前周期角速率的優(yōu)化雙子樣算法，比傳統(tǒng)算法的姿態(tài)精度更高。結(jié)合跑車實(shí)驗(yàn)，驗(yàn)證了基于改進(jìn)旋轉(zhuǎn)矢量的多數(shù)據(jù)融合姿態(tài)更新算法的正確性和有效性，結(jié)合過載、GNSS 對載體機(jī)動(dòng)進(jìn)行判斷，提高了姿態(tài)角修正的正確性。本文提出的算法具有較高的應(yīng)用價(jià)值，同時(shí)可以推廣至其它傳感器提供航向數(shù)據(jù)的信息融合算法。