李 杰，楊雁宇，馮凱強，李小燕，鄭 濤，杜思遠，許廷金

（中北大學 電子測試技術科技重點實驗室，太原 030051）

現階段船載姿態(tài)參考系統(tǒng)內大多為光纖陀螺或者激光陀螺，雖能達到很高的精度，但相對而言，MEMS器件具有體積小、功耗低、價格便宜的優(yōu)點，尤其在民用領域有著廣闊的前景[1]。法國SBG基礎型組合導航產品ellipse-a水平姿態(tài)角精度0.2°以內，航向角精度在0.5°以內。國內相對出色的船用MEMS組合導航產品以西安精準測控公司的 pa-gs為代表，水平姿態(tài)角精度 0.3°，航向角精度 0.6°，但在無衛(wèi)星可見的情況下精度急劇下降。因此本文在使用低成本 MEMS器件的前提下，提出一種融合卡爾曼濾波和互補濾波的算法，滿足各種情況下姿態(tài)的高精度輸出。

1 基礎算法設計

針對航向角可觀性弱的問題以及惡劣海況 GPS無信號的情形，常規(guī)算法無法完成高精度姿態(tài)輸出。本算法采用多種濾波方式相融合，以卡爾曼濾波為主，互補濾波為輔的設計。通過互補濾波進行原始數據的初步處理，求得陀螺誤差，然后利用卡爾曼濾波將IMU和GPS 數據進行融合，達到更好的濾波效果，尤其在無 GPS信號的情況下可以通過互補濾波保證水平姿態(tài)角的高精度輸出。

1.1 卡爾曼濾波算法

使用線性隨機方程來描述系統(tǒng)的狀態(tài)誤差和量測誤差為：

式中：X(t)和Z(t)分別是系統(tǒng)的狀態(tài)量和量測量；分別是系統(tǒng)的轉移矩陣、噪聲矩陣、量測矩陣；W(t)和V(t)為系統(tǒng)的噪聲向量[2-3]。

1.1.1 狀態(tài)方程的建立

選擇慣導系統(tǒng)的姿態(tài)失準角φ、地理坐標下東北天方向的速度誤差 δvn、經度緯度高度三個位置誤差、三軸陀螺相關漂移、三軸加速度計相關偏置為狀態(tài)量：

其中，系統(tǒng)狀態(tài)轉移矩陣為：

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

1.1.2 量測方程的建立

針對航向角的可觀性比較差的問題，引入雙天線GPS輸出的航向角，但GPS的輸出頻率最高只能達到10 Hz，所以通過與IMU進行組合，既解決可觀性差的問題，同時在較高動態(tài)下也可精確輸出航向角。

為構建量測方程，首先推導平臺誤差角與失準角的關系，由于解算誤差的存在，故從c系（計算坐標系）到n系（導航坐標系）需乘一個姿態(tài)轉換矩陣又因誤差角很少，故可用小角度姿態(tài)矩陣進行描述：

因誤差角為小角（取θ為例），對公式(7)移項后，兩邊同時求正余弦函數得：

對應角誤差泰勒展開后近似可得：

公式(9)代入公式(8)可得：

將式(10)帶入式(12)中后，又有

聯(lián)立(11)(13)后對應項相等略去高次項后可得姿態(tài)誤差角和平臺失準角的關系[4]：

只取航向角誤差關系式，定義航向角誤差和三個失準角之間的系數為M，作為下文中量測方程的系數。

以GPS和INS速度差ΔV，GPS和INS位置差ΔP，GPS和 INS的航向角差Δφ作為觀測量并建立量測方程[5-7]：

以上，狀態(tài)方程與量測方程構建完畢。

1.2 互補濾波算法

IMU內包含三個相互正交的加速度計和三個相互正交的陀螺儀，將陀螺儀測得的角速率積分后得到角度的變化，在短時間內可得到比較精確的結果，但陀螺儀誤差會隨著時間增加不斷積累，而加速度計在短時間內由于噪聲的影響精度不高，但是不會隨著時間的增加產生誤差積累，尤其在低頻狀態(tài)測傾角會有理想的效果，因此利用這兩種傳感器在頻域上的互補特性，通過設計互補濾波器融合兩種傳感器的數據，達到提高精度的目的[8]。

通常在4級海況下，船體不會出現劇烈的單方向振動，而且晃動頻率相對較低，故可以在衛(wèi)星信號丟失的狀況或者速度位置沒有明顯變化時，有效利用加速度計的低頻高精度特性，對陀螺儀進行不斷修正，使俯仰和滾動角仍然保持高精度輸出。

然后利用式(13)得到的角速度誤差通過互補濾波器來補償角速度矢量。

其中，bω為陀螺儀在載體坐標系下測得的角速度，通過調整PI調節(jié)器中的kp能快速地糾正陀螺儀的誤差，調節(jié)ki能消除穩(wěn)態(tài)誤差。

2 互補/卡爾曼算法融合過程

當量測信息有效時，通過互補濾波將加速度計和陀螺儀進行融合，初步計算出陀螺積分誤差。接下來將IMU速度、位置、航向角之差以及陀螺積分誤差作為觀測量，進行卡爾曼濾波，最終將得到的各部分誤差進行對應補償，得到精確的導航參數信息。

當量測信息無效時，直接通過預先給定的kp和ki確定修正量的大小，保證水平姿態(tài)角的準確輸出。算法融合流程圖如圖1所示。

圖1 算法融合流程圖Fig.1 Procedure of the algorithm

由公式(6)以及等效旋轉矢量與方向余弦陣關系式得：

代入公式

得到

由姿態(tài)微分方程：

寫成包含誤差量的形式為

新的量測方程中bΔω系數為：

因此，量測方程系數H更新為：

3 算法驗證平臺

IMU內三軸加速度計型號為MS9005，三軸陀螺儀型號為 CRM100。雙天線 GPS采用北斗星通公司BDM680板卡，最高可以輸出10Hz的航向、位置、三維速度。為實現實時解算與數據存儲，選取方案為DSP和FPGA相結合，利用FPGA對IMU輸出的模擬信號進行采集控制，并將DSP解算后的姿態(tài)結果通過串口發(fā)送出來同時存儲到系統(tǒng)內部FLASH當中，因此本系統(tǒng)既可以實時輸出導航信息，同時也可以存儲傳感器原始數據以及解算結果，極大地便利了算法開發(fā)與驗證。

圖2 系統(tǒng)硬件平臺Fig.2 Platform of system hardware

4 實驗驗證及分析

為了驗證上述算法的有效性，搭建了無衛(wèi)星下以高精度三軸溫控速率轉臺模擬船載環(huán)境的實驗平臺進行了動態(tài)實驗，室外空曠環(huán)境下進行了跑車動態(tài)實驗。

4.1 高精度三軸速率轉臺實驗

在常見4級海況的情形下，船體的響應運動和性能參數為：橫搖±15°，縱搖±10°，最快運動周期為5 s。

本實驗設置高精度三軸溫控速率轉臺搖擺頻率為0.5 Hz，幅值為10°的俯仰滾動搖擺運動。由圖3可明顯看出，在模擬的搖擺環(huán)境下130 s后能穩(wěn)定輸出俯仰角和滾動角。該算法有效抑制了MEMS陀螺的漂移，以轉臺反饋回的數據作為基準，進行RMSE為標準的誤差計算，俯仰角精度0.34269°，滾動角精度0.31147°。

圖3 動態(tài)搖擺實驗Fig.3 Dynamic swing experiment

圖4 轉臺試驗平臺Fig.4 Platform of the turntable test

4.2 車載動態(tài)實驗

為驗證算法在室外高機動環(huán)境下的性能，進行了跑車動態(tài)實驗。采用加拿大NovAtel公司高精度光纖組合導航系統(tǒng)span-lci為參考基準，考核系統(tǒng)的姿態(tài)精度。

實驗地點為中北大學先進制造中心樓后，將樣機和 span-lci系統(tǒng)捷聯(lián)于同一平面進行跑車試驗，實驗設備安裝如圖5所示。分別上電并配置使兩系統(tǒng)均以50 Hz輸出姿態(tài)角，然后通過上位機同時采集兩路信號保證數據的同步便于后期數據精度的計算與對比。

圖5 跑車試驗平臺Fig.5 Platform of the vehicle running

4.2.1 丟失GPS信號跑車實驗

圖6和圖7分別為俯仰角和滾動角在拔掉GPS天線時的跑車試驗結果，通過觀察發(fā)現純慣性積分計算誤差逐漸變大，而融合算法跟蹤效果很好，經計算該算法水平角誤差在0.35°以內（RMSE標準），有效抑制了陀螺漂移帶來的誤差。

圖6 無衛(wèi)星俯仰角對比圖Fig.6 Comparison of the pitch without satellite

圖7 無衛(wèi)星滾動角對比圖Fig.7 Comparison of the roll without satellite

4.2.2 GPS信號良好跑車實驗

通過圖8和圖11可以看到，融合算法雖然在初始航向角誤差較大，但在150 s時就收斂到1°內，由于加速度計不能修正天向陀螺，故航向角誤差隨時間累積。圖8直觀反映出融合算法在向基準收斂，互補算法逐漸發(fā)散。

觀察圖9～11可以發(fā)現，在車載環(huán)境下因路況較顛簸產生有害加速度進而影響到了互補濾波，因此會出現在真實值附近波動的毛刺現象，而融合后的算法很好地展現了卡爾曼濾波的跟蹤性，得到良好的結果。

圖9 滾動角對比結果Fig.9 Comparison results of the roll

圖10 俯仰角對比結果Fig.10 Comparison results of the pitch

圖11 姿態(tài)角實時絕對誤差Fig.11 The curve of attitude error

本次試驗選用通用標準 RMSE進行評估，航向角在160 s后精度可以達到0.5°以內，俯仰角和滾動角可達到0.2°以內，基本上能夠滿足小型化船載姿態(tài)測試要求。

5 結 論

本文基于卡爾曼濾波與互補濾波原理，融合二者設計并實現了船載姿態(tài)測量算法。推導了姿態(tài)角和失準角之間的數學關系，并建立量測模型。通過實際的轉臺及跑車實驗可得到以下結論：

1）利用加速度計和陀螺儀的高低頻互補特性使該系統(tǒng)在無衛(wèi)星的情況下仍然可以維持俯仰角和滾動角的高精度輸出。

2）融合互補濾波和卡爾曼濾波并針對航向角的可觀性弱的問題，在卡爾曼濾波的量測方程里加了雙GPS天線的航向角和角速度誤差為觀測量，提高了系統(tǒng)的航向的精度，同時有效提升了水平姿態(tài)的精度。

3）通過高精度轉臺的動態(tài)實驗以及車載動態(tài)實驗，驗證該算法能有效地將航向角精度穩(wěn)定提高到0.5°以內，水平姿態(tài)角穩(wěn)定在0.2°以內。

參考文獻（References）：

[1]張麗杰, 常佶. 微小型航姿測量系統(tǒng)及其數據融合方法[J]. 中國慣性技術學報, 2011, 19(3): 307-311.Zhang L J, Chang J. Attitude and heading measurement system and its data fusion method[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 307-311.

[2]Chen R. K-means aided Kalman Filter noise estimation calibration for integrated GPS/INS Navigation[C]//IEEE International Conference on Intelligent Transportation Engineering. 2016.

[3]Zhao L, Qiu H, Feng Y. Analysis of a robust Kalman filter in loosely coupled GPS/INS navigation system[J]. Measurement, 2016, 80: 138-147.

[4]丁濤, 趙忠. 捷聯(lián)慣導系統(tǒng)的誤差角關系與推導[J]. 兵工自動化, 2007(12): 66-67.Ding T, Zhao Z. Angles error relationship and deduction of SINS[J]. Ordnance Industry Automation, 2007(12):66-67.

[5]Wu J, Zhou Z, Chen J, et al. Fast complementary filter for attitude estimation using low-cost MARG sensors[J]. IEEE Sensors Journal, 2016, 16(18): 6997-7007.

[6]Yoo T S, Hong S K, Yoon H M, et al. Gain-scheduled complementary filter design for a MEMS based attitude and heading reference system[J]. Sensors, 2011, 11(4):3816.

[7]Wu Z, Yao M, Ma H, et al. Low-cost attitude estimation with MIMU and two-antenna GPS for Satcom-on-themove[J]. GPS Solutions, 2013, 17(1): 75-87.

[8]Feng K, Li J, Zhang X, et al. A new quaternion-based Kalman filter for real-time attitude estimation using the two-step geometrically-intuitive correction algorithm[J].Sensors, 2017, 17(9): 2146.

[9]Chang L B, Zha F, Qin F J. Indirect Kalman filtering based attitude estimation for low-cost attitude and heading reference systems[J]. IEEE/ASME Transactions on Mechatronics, 2017, PP(99): 1850-1858.

[10]Wang Y L, Soltani M, Hussain D M A. An attitude heading and reference system for marine satellite tracking antenna[J]. IEEE Transactions on Industrial Electronics,2017, PP(99): 3095-3104.