基于雙微機電慣性測量單元的船用自主水平姿態估計算法

顧元鑫，吳文啟，王茂松，郭 航

（國防科技大學智能科學學院，長沙 410073）

船載慣性姿態基準系統應用廣泛[1]，與采用光纖陀螺、激光陀螺的系統相比，采用微機電慣性測量單元具有體積小、功耗低的優勢，由于成本低，可在船上不同區域分布式聯網布設多套系統，與少量的光學陀螺慣性姿態基準系統配合，建立高性價比的全船統一姿態參考基準。但微機電陀螺、加速度計精度較低，存在零偏誤差和高頻噪聲[2]，為解決這一問題，可以采用與衛星定位系統或其他導航傳感器組合[3]或引入慣性測量單元（IMU）旋轉調制技術的方案[4,5]，目前法國的MEMS姿態航向參考系統Ellipse-A（在衛星數據有效的情況下）的水平角精度可達到0.2°，航向角精度可以達到1°，文獻[6]中采用互補濾波與卡爾曼濾波的融合姿態測量算法，在衛星數據有效的情況下可以實現水平姿態精度0.2°，航向角精度0.5°。在實際應用中，有些應用背景要求能在衛星信號拒止的條件下實現自主姿態測量。另外，IMU旋轉調制雖然可以抑制陀螺、加速度計零偏誤差影響，但旋轉本身會造成系統水平姿態解算輸出結果出現鋸齒狀誤差，對于陀螺、加速度計零偏誤差較大的低精度系統更加顯著，且IMU轉動過程中，即使在有測角系統輔助的情況下所提供的載體姿態精度也往往會受到一定影響。

本文提出一種基于雙微機電IMU的水平姿態測量系統方案，系統中兩個IMU安裝在一起，一個慣性測量單元以捷聯方式與船體相固聯，為姿態輸出IMU；另一個慣性測量單元通過單軸旋轉機構可繞方位軸180 °轉位?；陔p微機電慣性測量單元相對關系約束，以兩套IMU之間相對位置、速度和姿態為觀測量[7]，采用狀態變換卡爾曼濾波算法[8-11]實現了無衛星輔助信息下的陀螺、加速度計零偏估計，提高了水平姿態動態測量精度；由于姿態輸出IMU與船體相固聯，克服了IMU旋轉調制造成的系統水平姿態解算輸出結果鋸齒狀誤差的缺陷。

在無外界參考速度信息時，姿態參考系統相當于工作在內阻尼模式中，傳遞函數分析與實驗結果表明，雖然船體機動時，未知的船體質心速度變化會影響水平姿態相對于當地水平面的絕對精度，但不會影響船體各分布水平姿態基準之間的相對水平姿態精度，而當艦船上有高精度主慣導系統可提供速度參考時，即使有常值速度偏差和舒勒周期速度誤差，也可確保絕對姿態測量精度。

1 基于雙微機電慣性測量單元相對關系約束的狀態變換卡爾曼濾波算法

1.1 基于狀態變換的系統誤差微分方程

與船體相固聯的MEMS-IMU記為捷聯慣性測量單元，可通過旋轉機構轉位的MEMS-IMU記為旋轉調制慣性測量單元，兩個慣性測量單元在北東地地理坐標系（NED）下的速度誤差微分方程、姿態誤差微分方程、位置誤差微分方程形式是一致的，式(1)為狀態變換后新的速度誤差向量與原速度誤差向量的關系[12]。狀態變換卡爾曼濾波通過將傳統濾波模型中的速度誤差狀態變換為新的考慮姿態誤差的速度誤差狀態，使狀態轉移矩陣中動態變化的比力項變換為相對穩定的重力項，從而使誤差狀態協方差矩陣的更新計算更準確，可以有效地克服濾波估計協方差與實際協方差不一致造成的濾波誤差狀態估計不準確的問題。

位置誤差向量定義為：

狀態變換后速度、姿態、位置誤差微分方程如式(3)-(5)所示[12]：

其中Frr和Frv如下所示：

由于微機電慣性測量單元的陀螺及加速度計零偏穩定性較差，建模為隨機常值不適合長時間連續工作的情況，因此將陀螺及加速度計的零偏建模為一階馬爾科夫過程：

其中τg、τa為相關時間常數，為過程噪聲。

將船舶質心運動的水平速度的估計值建模為一階馬爾科夫過程：

其中τov為一階馬爾科夫過程中相關時間常數，wovN、wovE為過程噪聲。

1.2 系統狀態方程

雙微機電慣性測量單元聯合的狀態變換卡爾曼濾波的系統方程可以表示為：

其中：

注意到兩個IMU安裝在一起，其速度、位置是一致的，則狀態轉移矩陣如下所示：

其中：

過程噪聲向量如下所示：

系統噪聲矩陣如下所示：

其中：

1.3 系統觀測方程

觀測方程可表示如下：

其中z(t)為觀測向量，H(t)為量測矩陣，υ（t）為量測噪聲向量。

其中觀測向量選取如下：

可通過求解式(30)得到：

可通過式(32)得到求解：

可通過式(34)，根據兩個微機電慣性測量單元1b和2b位置測量值進行求解：

υr12=[υr12Lυr12λυr12h]T為相應的觀測噪聲。

艦船在水面運動，對于艦船，觀測方程中IMU高度有外界參考信息，則可以得到高度觀測值則有：υh1為相應的觀測噪聲。

量測矩陣如下所示：

系統的量測噪聲向量如下所示：

在旋轉調制微機電慣性測量單元處于轉動狀態時，只進行濾波的時間更新方程解算，不進行測量更新方程解算；在0 °、180 °位置鎖定時，才進行測量更新方程解算。

2 基于傳遞函數的水平姿態動態誤差分析

狀態變換卡爾曼濾波穩定后，濾波增益矩陣近似為常值，可用傳遞函數分析水平通道的動態姿態誤差[13]。

以捷聯微機電慣性測量單元b1為對象，并忽略船的速度與姿態誤差的乘積，不區分RE，RN，均用R表示，則有：

圖1 東向水平通道誤差傳遞函數框圖Fig.1 Block diagram of error transfer function of eastbound horizontal channel

圖2 北向水平通道誤差傳遞函數框圖Fig.2 Block diagram of error transfer function of northbound horizontal channel

根據兩個微機電慣性測量單元b1和b2之間的相對位置、相對速度和相對姿態觀測信息，可估計出水平加速度計零偏誤差和水平陀螺零偏誤差。由于緯度及垂向速度誤差小，對水平姿態角測量誤差的影響可忽略。由傳遞函數可知，穩態后由初始速度及初始姿態角所引起的姿態誤差均為零。桿臂參數已知的情況下，桿臂速度所導致的水平姿態角誤差也可得到補償，因此姿態測量誤差的主要影響因素為艦船質心速度，下面分兩種情況進行研究：

1) 無主慣導提供速度參考信息：

此時系統實際上工作在內阻尼狀態，式(46)可簡化為如下所示：

其中

根據式(50)，只要船舶的質心運動的動態性足夠小，通過時域狀態變換卡爾曼濾波結合頻域設計優化濾波增益矩陣中參數KE1、KE2，即可在內阻尼狀態下仍能得到足夠精確的水平姿態測量結果。根據姿態基準的傳遞函數可得艦船質心正弦運動速度的幅值及角頻率對姿態測量精度的影響，如圖3所示。

圖3 船質心速度正弦運動幅值及角頻率對姿態測量精度的影響Fig.3 The influence of ship′s center of mass velocity sinusoidal motion amplitude and angular frequency on the accuracy of attitude measurement

由圖3可知，隨著船體質心正弦運動幅值的增加，姿態基準系統的姿態解算誤差逐漸變大。基于本文的微機電傳感器精度指標和濾波參數，當船質心運動角頻率為舒勒振蕩角頻率 1.24× 1 0-3rad/s 時（周期約為84.4×60 s），要想姿態測量絕對精度優于0.02°，則要求船體的質心正弦運動速度的幅值不超過2.76m/s。另隨著船體質心正弦運動角頻率的增加，姿態基準系統的姿態解算誤差幅值先逐漸變大后又逐漸變小最后趨于收斂，其衰減原因是水平姿態測量系統相當于帶通濾波器，當船體質心運動頻率高時會被過濾掉，基于本文的微機電傳感器精度指標和濾波參數，當船體質心運動角頻率約為 6× 1 0-3rad/s 時（約為5倍的舒勒角頻率，周期約為950秒），其姿態測量誤差幅值最大。當艦船質心運動導致姿態測量精度低于應用要求時，可通過不同位置處的姿態基準系統間求相對姿態的方法，獲得精度較高的相對姿態值，其分析過程如下，艦船上不同位置處水平姿態基準東向水平通道誤差傳遞函數如下：

式(52)及式(53)分別表示艦船上不同位置處姿態基準系統的水平通道東向回路誤差傳遞函數，可知其中造成姿態測量值差異的主要因素為傳遞函數中的參數KE1、KE2以及艦船東向質心速度，由于濾波算法及參數相同，且環境因素也幾乎相同，所以不同位置處水平姿態基準傳遞函數中的參數K1E、KE2分別近似一致，船上各處水平姿態基準可采用相同的艦船質心水平速度估計值，故濾波穩定后艦船質心速度所造成的動態姿態誤差是近似相同的，不同位置處水平姿態基準北向通道傳遞函數中的參數KN1、KN2也分別近似一致，有同樣結論，即有：

各分布式水平姿態基準之間的相對姿態精度，即使在內阻尼狀態下，其一致性也不受艦船的質心運動影響。2) 有主慣導提供速度參考信息：

由式(56)可知此時影響姿態測量精度的主要因素為主慣導系統的速度測量精度，姿態測量精度最差發生在主慣導無阻尼的情況下，主慣導速度誤差呈舒勒周期振蕩。根據傳遞函數式(56)可以計算出主慣導系統速度測量精度對姿態測量誤差的影響，如圖4所示。由于艦船上的主慣導系統的速度測量精度都高于2m/s，由圖4可知，當有艦船高精度主慣導系統提供的速度為觀測量時，姿態基準系統的水平姿態測量精度總可以優于 0.02°（1σ）。

圖4 主慣導速度誤差舒勒振蕩幅值對姿態測量精度影響Fig.4 The influence of main inertial navigation velocity error Schuler oscillation amplitude on attitude measurement accuracy

3 實驗系統設計

本姿態基準系統中的旋轉微機電慣性測量單元采用0 °→ 180 °→ 0°每個位置停留5min的旋轉方案，通過往復旋轉，以兩套慣性測量單元間的相對速度、相對位置、相對姿態為觀測量，實現兩套慣性測量單元陀螺、加速度計零偏的自主估計，進而減弱微機電慣性儀表零偏誤差對船體姿態測量精度的影響。系統采用的微機電慣性測量單元的性能指標如表1所示。

表1 微機電慣性測量單元性能指標參數Tab.1 Specifications of the MEMS-IMU

4 實驗結果與分析

4.1 海況模擬實驗設計

本節通過利用三軸角運動模擬轉臺來實時模擬艦船在海上的搖擺情況，進而驗證基于雙微機電慣性測量單元船用自主水平姿態基準系統的姿態測量精度。實驗裝置圖如圖5所示。

圖5 海況模擬實驗Fig.5 Sea state simulation experiment

本實驗的實驗過程如表2所示。

表2 海況模擬參數Tab.2 Sea state simulation parameter

圖5 中1為與運載體固聯的微機電慣性測量單元，2為單軸旋轉的微機電慣性測量單元，3為數據采集處理模塊。為進行動態姿態驗證，角運動模擬轉臺與微機電慣性測量單元已實現姿態數據時間同步。圖5中4為三軸角運動模擬轉臺。

4.2 海況模擬實驗結果

基于三軸角運動模擬轉臺按照上述實驗流程進行了三組海況模擬實驗，重點驗證無主慣導信息的惡劣情況。求取各階段實驗水平姿態角誤差均方差如表3所示。

表3 海況模擬實驗水平姿態角誤差均方差Tab.3 Mean square error of horizontal attitude angle in sea state simulation experiment

從實驗結果可知，本姿態測量方案所測得的三軸角運動模擬轉臺的水平姿態輸出值與三軸角運動模擬轉臺的實際水平姿態角輸出值間誤差的標準差小于0.02°。

4.3 仿真試驗

由于捷聯慣性測量單元北向水平通道與東向水平通道相似，下面以東向水平通道為例進行質心速度對姿態測量精度影響的仿真試驗。在姿態基準系統仿真試驗中對運載體加以0.1°的模擬載體初始緯度誤差及的船體東向速度，即海浪作用下，艦船質心運動周期為50 s。由于姿態基準系統輸出與捷聯微機電慣性測量單元輸出一致，因此下文中捷聯微機電慣性測量單元也代表姿態基準系統，由于捷聯微機電慣性測量單元與旋轉微機電慣性測量單元的基本指標相同，因此為了在陀螺零偏及加速度計零偏估計時將二者的相應零偏值更好地區分，這里對捷聯微機電慣性測量單元加以1°/h的常值陀螺偏移，以及1mg的常值加速度計偏移，對旋轉微機電慣性測量單元加以-1°/h 的常值陀螺偏移，以及-1mg的常值加速度計偏移，在引入上述誤差后姿態解算的結果如圖6所示。由圖6可知，一方面，由姿態基準系統所解算出的水平姿態角要比單獨采用旋轉微機電慣性測量單元解算出的水平姿態角的精度要高；另一方面，由于單軸旋轉測量單元0 °→ 180 °→ 0°往復旋轉，使得旋轉測量單元在其旋轉過程中所測得的水平姿態值不能反映船體實際姿態，而采用雙微機電協同轉位方案的姿態基準系統可以很好地解決這一問題，使得解算出的水平姿態角的精度及可信性更高。從圖6中可知，當仿真試驗條件與4.1節中海況模擬實驗一致，運載體存在質心速度，而質心加速度變化較小且緯度信息有誤差的情況下，姿態基準系統的水平姿態角測量精度可維持在 002°（1σ）以內，與前述載體無質心運動速度情況下實驗所得的結論相同。

圖6 姿態基準系統與旋轉慣性測量單元滾轉角誤差對比Fig.6 Comparison of roll angle error between attitude reference system and rotary inertial measurement unit

圖7 -8為捷聯微機電慣性測量單元及旋轉微機電慣性測量單元陀螺零偏、加速度計零偏的濾波估計值，可知微機電慣性測量單元的陀螺及加速度計零偏由于相對觀測量的引入均得到了較好地估計，其中由于天向陀螺的可觀測性較差，其估計的情況相較于東向和北向陀螺也較差，但最終也已經收斂。

圖7 陀螺零偏估計曲線Fig.7 Gyro bias estimation curve

圖8 加速度計零偏估計曲線Fig.8 Accelerometer bias estimation curve

在姿態基準系統試驗中，將船體質心速度的角頻率設置為造成最大姿態誤差的情況，將船體東向速度設置為其仿真試驗結果如圖9中紅色曲線所示。

由仿真試驗結果圖9中紅色曲線可知，當進一步加大運載體質心速度誤差后，姿態基準系統的水平姿態測量精度有所降低，其誤差超過了0.02°。由于在船上，縱使處在不同位置的姿態基準系統其所感應到的船體質心速度也應為同一值，因此當船體質心速度在一定范圍內增大時即使會對單套姿態基準系統的姿態測量精度產生影響，但對兩套姿態基準系統所測得的相對水平姿態值的精度應無影響，二者所測得的水平姿態角的差值應保持高精度。為驗證上述結論，取同一姿態基準系統不同組別實驗數據來模擬船上不同位置處的姿態基準系統，試驗中運載體的質心速度與前述一致，為緯度初始誤差為0.1°。位置1處及位置2處的滾轉角誤差值如圖9所示，二者間的相對滾轉角誤差如圖10所示。

圖9 姿態基準系統滾轉角誤差曲線Fig.9 Error curve of roll angle of attitude reference system

圖10 位置1與位置2間姿態基準系統相對滾轉角誤差Fig.10 Relative roll angle error of attitude reference system between position 1 and position 2

由圖9及圖10可知，即使由于載體質心速度加大導致單個姿態基準系統水平姿態測量精度下降，但不同的姿態基準系統間的相對姿態測量精度在1200 s濾波穩定后仍保持在相對較高的水平，在0.02°左右，造成這一相對姿態測量誤差的主要原因是由于船上不同位置處環境因素不同，對于船上不同位置處的姿態基準系統，艦船質心速度到水平姿態誤差間的傳遞函數中KE1、KE2、KN1、KN2分別存在差異，進而導致船上不同位置處姿態基準系統的水平姿態測量誤差值不同，最終導致了相對姿態測量誤差，但其精度仍可維持在較高的水平，這一結論將為后續的多套姿態基準系統協同工作提供一定的理論可行性基礎，也進一步說明了本姿態基準系統的自主性。

5 結 論

本文完成了一種不需要依賴外界信息的基于雙微機電慣性測量單元的船用自主式水平姿態基準系統算法設計，通過采用雙慣性測量單元協同轉位的方式，以兩套慣性測量單元間相對位置、速度、姿態關系為觀測約束，基于狀態變換卡爾曼濾波完成載體姿態測量，并通過基于三軸角運動模擬轉臺的海況模擬實驗進行了精度驗證。本姿態測量方案在船體質心加速度變化較小時，在不依賴外界信息的情況下可以實現水平姿態角測量絕對精度優于 0.02°（1σ），而在載體質心加速度變化較大時，雖然姿態基準系統的水平姿態角測量絕對精度超出 0.02°（1σ），但是不同姿態基準系統間的姿態測量相對精度仍可維持在 002°（1σ），進而可以利用兩套姿態基準系統來實現不同位置間相對水平姿態的測量。而當艦船上有主慣導系統可提供速度參考時，盡管主慣導存在常值速度誤差和舒勒周期速度誤差，微機電姿態基準系統的水平姿態測量精度仍可優于 0.02°（1σ）。綜上所述，采用本系統及相應算法可以在盡可能微型化、低功耗、低成本的前提下，實現在海況較差且無外界參考信息的情況下較好地完成艦船上子系統姿態的高精度測量，為艦船上子系統的高精度姿態測量提供了一種可行的技術方案。