一種基于改進旋轉矢量的低成本多數據融合姿態更新算法

王君 趙苑辰 李濤

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

1 引言

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

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

2 姿態更新算法

捷聯慣導姿態更新的經典算法包括姿態更新四元數法、姿態更新旋轉矢量法等[1]。

2.1 四元數法

2.2 旋轉矢量法

通常載體角運動較為復雜，在更新周期內不是定軸轉動的。2.1節中，使用角增量直接求解姿態變換四元數，則會產生誤差。姿態更新的旋轉矢量算法可避免產生不可交換誤差，其原理是首先使用固聯于載體的傳感器得到角增量信息，其次利用同為描述轉動過程的角增量和等效旋轉矢量之間的關系求解后者，最后使用等效旋轉矢量對載體的姿態進行更新。

在姿態更新的旋轉矢量法中，可以使用多種方法利用角增量來構建旋轉矢量；最直觀的是以姿態更新周期內使用角增量數量進行區分，使用傳感器輸出的一次角增量結果信息構造旋轉矢量成為單子樣法，使用兩次更新結果為雙子樣法，使用三次更新結果為三子樣法，以此類推。

等效旋轉矢量φ 表示載體從tm-1時刻運動至tm時刻描述姿態變化的等效旋轉，由其定義可知轉動的方向為u=φ/φ，轉動大小為等效旋轉矢量φ 的模φ。當且僅當φ 為微小量時，可以對等效旋轉矢量的微分方程展開后的高階微小量進行省略；則微分方程近似為：[3]

觀察（4）式與（7），兩式為姿態變換四元數的不同構成形式。以發現兩者的主要區別是構造時四元數時使用的元素不同，（4）為使用角增量，（7）為使用旋轉矢量。若計算旋轉矢量使用常量擬合角速度，即單子樣旋轉矢量算法，則與四元數法等價。若使用一次項、二次項擬合角速度，可得雙子樣、三子樣算法構建的旋轉矢量：[1]

3 改進旋轉矢量姿態更新算法

在載體姿態存在復雜運動時，如存在周期性振動，則會誘發捷聯慣導姿態更新產生嚴重漂移，從而使姿態更新產生偏差。由于周期性振動可近似為圓錐運動，則可以使用基于圓錐誤差補償的多子樣算法進行優化。考慮到常用的慣性器件輸出為角速度，使用角速度構建旋轉矢量。

3.1 旋轉矢量優化方法

3.2 基于角速率輸出的姿態更新算法

4 低成本數據融合算法

載體的實時姿態可由姿態四元數進行表示，依據（1）式四元數運動學微分方程特性，載體的姿態估計是非線性問題。非線性系統的分析有多種經典方法，常用的包括擴展卡爾曼濾波、二階濾波、迭代濾波等。考慮到實際工程應用的制約，從運算量、算法復雜度和魯棒性方面考慮，選擇擴展卡爾曼濾波對姿態估計進行分析。擴展卡爾曼濾波的思路為將非線性系統在參考點處進行泰勒級數展開，略去高階項取一階線性部分作為近視，再開展線性卡爾曼濾波估計。

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

圖1：低成本數據融合的EKF 算法流程圖

4.1 擴展卡爾曼濾波狀態方程的建立

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

5 試驗仿真

5.1 圓錐運動仿真

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

圖2：兩種算法的姿態更新誤差曲線

5.2 跑車試驗

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

圖3：雙子樣與數據融合算法的姿態曲線

6 結束語

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