單軸旋轉航位推算系統姿態誤差補償方法*

廉 璞，牟 東，青 澤，張延順

（1.中國工程物理研究院電子工程研究所，四川 綿陽 621999；2.北京航空航天大學儀器科學與光電工程學院，北京 100191）

0 引言

航位推算（Dead Reckoning DR）導航系統利用已知的航向、速度、時間等信息推算出新的航位，是一種自主導航定位系統［1-2］。與捷聯慣導系統相比，DR 導航系統誤差隨時間累積變化較?。?］，常與里程儀或多普勒測速儀（Doppler velocity log，DVL）組合用于地面車輛或水下運載體的導航定位中［4-7］。水下某運載體采用DVL/DR 組合導航方式，DVL 利用多普勒效應實現載體速度測量，DR 導航系統通過慣導解算的姿態角與測量速度進行航位推算，實現導航定位。姿態精度是影響定位精度的主要因素。而載體持續旋轉會使姿態誤差增大，進而引起導航系統定位誤差［8］。因此，需要對載體航行過程中的姿態誤差進行補償，提高旋轉航位推算導航系統的姿態角測量及解算精度。

針對旋轉運載體的姿態角解算與誤差補償，文獻［9］研究了旋轉軸不正交誤差對于水平方向上的姿態角影響，提出一種基于Kalman 濾波的水平姿態角誤差修正方法。文獻［10］提出通過BP 神經網絡對誤差建模，修正Kalman 濾波增益矩陣系數，提高旋轉彈姿態測量的方法。學者們針對旋轉載體提出基于磁強計測量的姿態角解算方法［11-12］。文獻［11］充分利用傳感器數據，實現了姿態更新與磁強計輸出同步。文獻［12］以地磁方位角為觀測量，通過彈體繞質心動力學方程估計彈體俯仰角和偏航角。文獻［13］把紅外與地磁復合的姿態測量技術用于旋轉彈的姿態獲取，并對彈上紅外與地磁測量誤差補償方法開展了研究工作。但是，地磁傳感器在使用過程中易受到干擾，并且在使用前需要進行磁校準。文獻［14］通過設計多加速度計的非質心配置方案，實現中空結構的MEMS 測量單元對高速旋轉體的滾轉角測量。

基于以上技術背景，本文在不改變傳感器配置的情況下，利用旋轉過程加速度計輸出信號周期性出現峰值的現象，提出了將加速度計輸出信息提取到的角度變化量與慣導更新角度進行融合的姿態角補償算法，并進行仿真研究。

1 航位推算導航系統定位誤差分析

1.1 航位推算導航定位方法

航位推算（DR）定位方法是根據捷聯慣導提供的載體實時姿態，對速度矢量進行投影和位置更新，從而得到載體位置信息。

選擇東-北-天地理坐標系為導航坐標系，記為n 系；將慣導與運載體固連，選擇右-前-上坐標系為載體坐標系，記為b 系。水下運載體沿載體軸向運動，載體速度可以表示為

將載體系下的速度vb投影到導航坐標系，得到

由式（2）和式（3）可見，速度誤差和姿態誤差是影響DR 導航系統誤差的兩個重要因素。本文主要研究姿態誤差影響及補償方法。

1.2 姿態四元數誤差分析

在姿態更新中，為避免歐拉角法存在的解算奇異問題，以及方向余弦法計算量大的缺點，本文采用四元數法完成姿態解算及分析［15］。

定義四元數q：

式中，q0表示四元數中的標量，后三項為四元數的矢量部分。

姿態更新四元數微分方程可以表示為

對式（6）兩邊求導，得

根據式（5）可得四元數估計值的微分方程為

式中，ω0為帶有測量誤差的角速度。

將式（5）和式（8）代入式（7）可得

將式（6）代入式（9）并消除同類項，可得

陀螺儀測量模型可以表示為

式中，εb為陀螺儀的漂移誤差，ωg為陀螺儀的均值白噪聲。

將式（11）代入式（10）可得

誤差四元數中第一項為1，對其求導為0，對姿態誤差分析無影響，因此，式（12）可以化簡為

由式（15）可以看出，姿態誤差主要是由陀螺漂移和陀螺測量噪聲引起。并且隨著時間的累積，陀螺漂移引起的姿態誤差將持續增大，因此，需要對該誤差項進行補償。

2 姿態誤差補償模型

2.1 濾波融合姿態補償方案

對于持續旋轉的水下運載體，當加速度計敏感軸垂直向下或向上時，其輸出信號會周期性地出現峰值，即相鄰兩峰值信號出現，對應著該段時間內滾動角轉過了180°。此信息可以作為量測量與陀螺儀求解的姿態融合，以補償陀螺漂移引起的姿態誤差。

該方法首先用捷聯慣導姿態更新算法求解載體的姿態角，然后利用加速度計隨載體運動周期性變化的特點，將加速度計輸出信號提取的角度增量與已解算得到的角度濾波融合，抑制姿態誤差的累積。采用濾波融合姿態補償方案如圖1 所示。

圖1 姿態補償方案原理框圖

2.2 基于加速度計輸出的姿態補償方法

加速度計ax、ay、az分別沿載體坐標系的3 個坐標軸安裝，如圖2 所示，其中加速度計ax、az與載體的滾轉軸垂直安裝。

圖2 加速度計安裝示意圖

當垂直于載體行進方向的z 軸加速度計垂直向上時，輸出信號最大，此時加速度計ax的輸出在0附近。載體繞y 軸旋轉90°，x 軸方向的加速度計ax垂直向下，輸出為極小值，az≈0。當載體繼續y 軸旋轉90°，z 軸加速度計az垂直向下，輸出信號最小，ax≈0。載體繼續旋轉，ax出現峰值，az≈0。隨著載體的持續滾轉，ax、az兩軸加速度計的輸出信號周期性出現峰值。

利用這一現象，可以建立加速度計輸出信息與橫滾角之間的關系，表示為

將加速度計信號提取得到的姿態角四元數與慣導陀螺解算的姿態角四元數進行濾波融合，首先建立系統的狀態方程和量測方程，再設計自適應濾波器實現濾波估計。

2.2.1 狀態方程

基于1.2 節推導得到的姿態四元數誤差方程，建立姿態濾波融合的狀態方程。

選擇狀態變量

2.2.2 量測方程

對加速度計輸出信號的分析可知，加速度計信號提取得到的姿態角不隨陀螺儀的漂移而發散，因此，可以將加速度計輸出量出現極值時對應的姿態四元數作為真值q，慣導陀螺儀解算可以得到姿態四元數估計值，將兩者的差值作為量測量建立系統的量測方程

2.2.3 Sage-Husa 自適應濾波算法

Sage-Husa 自適應濾波算法能利用量測量修正系統過程噪聲和量測噪聲，從而抑制濾波發散，提高濾波精度，該算法的原理簡單、實時性較好［16］。持續滾轉的運載體會引起陀螺儀和加速度計的測量噪聲，通過初始先驗值難以準確描述實際噪聲，因此，本文采用Sage-Husa 自適應濾波算法估計噪聲特性，實現姿態誤差的準確估計。

首先對連續的系統狀態方程和量測方程做離散化處理，得到離散的系統方程為

Sage-Husa 自適應濾波算法可以表示為［16］

3 仿真驗證

3.1 仿真條件

為檢驗姿態誤差補償算法的正確性和有效性，利用軌跡發生器產生仿真數據。

仿真初始條件：初始位置（緯度34°、經度116°、高度0），初始角度誤差（航向角1°、俯仰角0.5°、橫滾角0.5°）。

仿真軌跡：載體速度25 m/s，并且存在±1.5 m/s的隨機誤差，為驗證算法有效性，參考載體的實際運動狀態，分別設計了兩條路徑進行驗證，運動時間為400 s，具體的三維軌跡曲線和平面軌跡如圖3、圖4 所示。

圖3 路徑1 的軌跡曲線

圖4 路徑2 的軌跡曲線

慣導系統的仿真參數見表1。

表1 慣導仿真參數

利用本文提出的算法對軌跡發生器生成的數據進行導航解算及姿態補償，并以設定的導航真實值為基準［17-18］。

3.2 仿真結果

根據以上的仿真條件，分別采用未經姿態補償的旋轉DR 導航系統和基于姿態補償算法的旋轉DR 導航系統，對不同路徑進行仿真。

路徑1 的姿態誤差曲線和導航位置誤差如圖5、圖6 所示。圖中藍色虛線表示未采用姿態補償算法進行DR 導航的結果，紅色實線表示采用姿態角補償方法后DR 導航的結果。DR 導航誤差在運動400 s 時的統計結果如表2 所示。

圖5 路徑1 導航姿態誤差曲線

圖6 路徑1 導航位置誤差曲線

表2 路徑1 誤差統計表

從圖中可以看出，位置誤差曲線出現了3 次峰值，這與仿真路徑中存在的轉彎及環形運動有關，隨著時間的增加，采用姿態補償算法的導航效果越好。在姿態角誤差曲線中，俯仰角和橫滾角誤差經補償后減小顯著。

從誤差統計結果中可以看出，在運動400 s 時，俯仰角、橫滾角、航向角誤差分別由1.68°、-0.55°、-0.35°減小到0.74°、-0.06°-0.20°。位置誤差由補償前的53.4 m 減小到41.1 m。

路徑2 的姿態誤差曲線和導航位置誤差如圖7、下頁圖8 所示。導航的誤差統計結果如表3所示。

表3 路徑2 誤差統計表

圖7 路徑2 導航姿態誤差曲線

圖8 路徑2 導航位置誤差曲線

對于姿態角誤差，采用補償算法后，有效降低了陀螺儀漂移引起的姿態角誤差累積。俯仰角誤差和橫滾角誤差從補償前的1.46°和-1.1°降低到0.74°和-0.64°，分別減少了49.3 %和41.8 %。在400 s 時，位置誤差由134.1 m 減小到67.8 m，誤差減小了49.4%。

4 結論

本文針對旋轉過程中引起的航位推算導航系統定位誤差問題，在誤差機理分析的基礎上，提出了將加速度計輸出信息提取的姿態角作為量測的姿態誤差補償方法，以減小由陀螺漂移引起的姿態誤差和定位誤差。通過不同路徑下的仿真研究，驗證了本文提出的算法能降低陀螺漂移引起的姿態角誤差，提高了單軸旋轉DR 導航系統的姿態測量精度，以及導航定位精度。