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

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

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

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

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

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

1 基于雙微機電慣性測量單元相對關(guān)系約束的狀態(tài)變換卡爾曼濾波算法

1.1 基于狀態(tài)變換的系統(tǒng)誤差微分方程

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

位置誤差向量定義為：

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

其中Frr和Frv如下所示：

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

其中τg、τa為相關(guān)時間常數(shù)，為過程噪聲。

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

其中τov為一階馬爾科夫過程中相關(guān)時間常數(shù)，wovN、wovE為過程噪聲。

1.2 系統(tǒng)狀態(tài)方程

雙微機電慣性測量單元聯(lián)合的狀態(tài)變換卡爾曼濾波的系統(tǒng)方程可以表示為：

其中：

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

其中：

過程噪聲向量如下所示：

系統(tǒng)噪聲矩陣如下所示：

其中：

1.3 系統(tǒng)觀測方程

觀測方程可表示如下：

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

其中觀測向量選取如下：

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

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

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

υr12=[υr12Lυr12λυr12h]T為相應(yīng)的觀測噪聲。

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

量測矩陣如下所示：

系統(tǒng)的量測噪聲向量如下所示：

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

2 基于傳遞函數(shù)的水平姿態(tài)動態(tài)誤差分析

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

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

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

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

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

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

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

其中

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

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

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

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

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

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

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

3 實驗系統(tǒng)設(shè)計

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

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

4 實驗結(jié)果與分析

4.1 海況模擬實驗設(shè)計

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

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

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

表2 海況模擬參數(shù)Tab.2 Sea state simulation parameter

圖5 中1為與運載體固聯(lián)的微機電慣性測量單元，2為單軸旋轉(zhuǎn)的微機電慣性測量單元，3為數(shù)據(jù)采集處理模塊。為進行動態(tài)姿態(tài)驗證，角運動模擬轉(zhuǎn)臺與微機電慣性測量單元已實現(xiàn)姿態(tài)數(shù)據(jù)時間同步。圖5中4為三軸角運動模擬轉(zhuǎn)臺。

4.2 海況模擬實驗結(jié)果

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

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

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

4.3 仿真試驗

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

圖6 姿態(tài)基準系統(tǒng)與旋轉(zhuǎn)慣性測量單元滾轉(zhuǎn)角誤差對比Fig.6 Comparison of roll angle error between attitude reference system and rotary inertial measurement unit

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

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

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

在姿態(tài)基準系統(tǒng)試驗中，將船體質(zhì)心速度的角頻率設(shè)置為造成最大姿態(tài)誤差的情況，將船體東向速度設(shè)置為其仿真試驗結(jié)果如圖9中紅色曲線所示。

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

圖9 姿態(tài)基準系統(tǒng)滾轉(zhuǎn)角誤差曲線Fig.9 Error curve of roll angle of attitude reference system

圖10 位置1與位置2間姿態(tài)基準系統(tǒng)相對滾轉(zhuǎn)角誤差Fig.10 Relative roll angle error of attitude reference system between position 1 and position 2

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

5 結(jié) 論

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