單軸旋轉(zhuǎn)航位推算系統(tǒng)姿態(tài)誤差補償方法*

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

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

0 引言

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

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

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

1 航位推算導航系統(tǒng)定位誤差分析

1.1 航位推算導航定位方法

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

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

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

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

1.2 姿態(tài)四元數(shù)誤差分析

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

定義四元數(shù)q：

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

姿態(tài)更新四元數(shù)微分方程可以表示為

對式（6）兩邊求導，得

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

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

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

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

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

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

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

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

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

2 姿態(tài)誤差補償模型

2.1 濾波融合姿態(tài)補償方案

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

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

圖1 姿態(tài)補償方案原理框圖

2.2 基于加速度計輸出的姿態(tài)補償方法

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

圖2 加速度計安裝示意圖

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

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

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

2.2.1 狀態(tài)方程

基于1.2 節(jié)推導得到的姿態(tài)四元數(shù)誤差方程，建立姿態(tài)濾波融合的狀態(tài)方程。

選擇狀態(tài)變量

2.2.2 量測方程

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

2.2.3 Sage-Husa 自適應濾波算法

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

首先對連續(xù)的系統(tǒng)狀態(tài)方程和量測方程做離散化處理，得到離散的系統(tǒng)方程為

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

3 仿真驗證

3.1 仿真條件

為檢驗姿態(tài)誤差補償算法的正確性和有效性，利用軌跡發(fā)生器產(chǎn)生仿真數(shù)據(jù)。

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

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

圖3 路徑1 的軌跡曲線

圖4 路徑2 的軌跡曲線

慣導系統(tǒng)的仿真參數(shù)見表1。

表1 慣導仿真參數(shù)

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

3.2 仿真結(jié)果

根據(jù)以上的仿真條件，分別采用未經(jīng)姿態(tài)補償?shù)男D(zhuǎn)DR 導航系統(tǒng)和基于姿態(tài)補償算法的旋轉(zhuǎn)DR 導航系統(tǒng)，對不同路徑進行仿真。

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

圖5 路徑1 導航姿態(tài)誤差曲線

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

表2 路徑1 誤差統(tǒng)計表

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

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

路徑2 的姿態(tài)誤差曲線和導航位置誤差如圖7、下頁圖8 所示。導航的誤差統(tǒng)計結(jié)果如表3所示。

表3 路徑2 誤差統(tǒng)計表

圖7 路徑2 導航姿態(tài)誤差曲線

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

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

4 結(jié)論

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