一種船舶漂浮狀態(tài)下捷聯(lián)慣導(dǎo)快速自對(duì)準(zhǔn)方法

劉瑞鑫，劉付成，陳紹紅，李傳江

（1.哈爾濱工業(yè)大學(xué)，哈爾濱 150001；2.上海航天技術(shù)研究院，上海 201109；3.上海新躍聯(lián)匯電子科技有限公司，上海 200233）

隨著捷聯(lián)慣導(dǎo)系統(tǒng)在船舶上的廣泛應(yīng)用，船用捷聯(lián)系統(tǒng)的初始對(duì)準(zhǔn)對(duì)船舶的機(jī)動(dòng)性起著重要的作用。對(duì)于初始對(duì)準(zhǔn)問(wèn)題，不僅要求對(duì)準(zhǔn)達(dá)到一定的精度，還要求其具有快速性。一般的初始對(duì)準(zhǔn)的收斂主要是方位失準(zhǔn)角的收斂，在粗對(duì)準(zhǔn)階段或者精對(duì)準(zhǔn)階段采用優(yōu)化的算法加快方位失準(zhǔn)角的收斂。傳統(tǒng)的粗對(duì)準(zhǔn)方案是利用重力和地球自轉(zhuǎn)角速度在地理坐標(biāo)系和載體坐標(biāo)系下的投影不同推算出來(lái)的。文獻(xiàn)[1]提出了新的粗對(duì)準(zhǔn)算法——凝固對(duì)準(zhǔn)法，即通過(guò)觀測(cè)不同時(shí)刻重力加速度在不同坐標(biāo)系中的積分矢量計(jì)算初始姿態(tài)矩陣。近年來(lái)以凝固對(duì)準(zhǔn)法為基礎(chǔ)，不斷出現(xiàn)了新的變種。文獻(xiàn)[2]闡述了凝固對(duì)準(zhǔn)法通過(guò)引入外部速度，如：里程計(jì)速度、DVL 速度、GNSS 速度等輔助，可以大大提高動(dòng)基座粗對(duì)準(zhǔn)精度。文獻(xiàn)[3][4]結(jié)合回溯算法通過(guò)多級(jí)凝固法進(jìn)行動(dòng)基座粗對(duì)準(zhǔn)，提高了粗對(duì)準(zhǔn)精度。文獻(xiàn)[5]將Wahba 問(wèn)題以QUEST 算法實(shí)現(xiàn)，充分利用了粗對(duì)準(zhǔn)時(shí)間段內(nèi)所有時(shí)刻的重力積分矢量。在精對(duì)準(zhǔn)階段，一般建立小方位失準(zhǔn)角誤差模型，采用羅經(jīng)法或者卡爾曼濾波方法。羅經(jīng)法，通過(guò)合理設(shè)置時(shí)變控制參數(shù)，在精對(duì)準(zhǔn)初期采取較大的閉環(huán)系統(tǒng)振蕩頻率加速失準(zhǔn)角收斂，在對(duì)準(zhǔn)后期逐漸切換到較小的振蕩頻率提高對(duì)準(zhǔn)精度[6]。在理想的慣性器件和穩(wěn)定的外部環(huán)境條件下，卡爾曼濾波有著最優(yōu)的對(duì)準(zhǔn)性能。但慣性器件不可避免地受到外界各種環(huán)境因素影響，導(dǎo)致系統(tǒng)參數(shù)不穩(wěn)定，從而大大影響濾波性能。文獻(xiàn)[7-10]針對(duì)建模和外部運(yùn)動(dòng)環(huán)境的不確定性，在卡爾曼濾波的基礎(chǔ)上進(jìn)行了自適應(yīng)的改進(jìn)，以提高系統(tǒng)的魯棒性。隨著計(jì)算機(jī)技術(shù)的發(fā)展，導(dǎo)航計(jì)算機(jī)的性能大幅提升，逐漸產(chǎn)生了重復(fù)利用已經(jīng)存儲(chǔ)的傳感器數(shù)據(jù)的回溯導(dǎo)航算法，大大縮短了對(duì)準(zhǔn)時(shí)間[2][11]。上述方法存在如下問(wèn)題：采用凝固對(duì)準(zhǔn)法進(jìn)行粗對(duì)準(zhǔn)，在動(dòng)基座情況下如果沒(méi)有外部速度輔助，精度大大下降從而影響精對(duì)準(zhǔn)性能；采用非線性大失準(zhǔn)角的UKF及其自適應(yīng)類的變種，工程實(shí)踐難度增大；同樣的，回溯導(dǎo)航算法需要存儲(chǔ)大量傳感器數(shù)據(jù)，工程實(shí)踐難度也大大增加。

與凝固對(duì)準(zhǔn)法粗對(duì)準(zhǔn)加卡爾曼濾波精對(duì)準(zhǔn)的方案不同，本文通過(guò)建立非線性的大方位失準(zhǔn)角模型，直接采用雙通道時(shí)變參數(shù)的羅經(jīng)法進(jìn)行粗對(duì)準(zhǔn)，在快速達(dá)到一定的穩(wěn)態(tài)階段，失準(zhǔn)角已經(jīng)減小到較小的程度，切換到標(biāo)準(zhǔn)線性卡爾曼濾波精對(duì)準(zhǔn)狀態(tài)，通過(guò)提高羅經(jīng)法對(duì)準(zhǔn)的精度，間接縮短標(biāo)準(zhǔn)卡爾曼濾波精對(duì)準(zhǔn)的時(shí)間。本文最后通過(guò)一段船舶漂浮數(shù)據(jù)進(jìn)行仿真，表明該方案的有效性。

1 大方位失準(zhǔn)角誤差模型與控制回路對(duì)準(zhǔn)

經(jīng)典控制回路水平對(duì)準(zhǔn)收斂較快，這里不再闡述，后面的分析均建立在水平姿態(tài)失準(zhǔn)角為小角度這一條件下。由于初始方位角未知，因此有必要建立大方位失準(zhǔn)角誤差模型[2]，如下

其中，

φ為姿態(tài)失準(zhǔn)角，δV為速度誤差，下標(biāo)和上標(biāo)中的i、e、n、n′分別表示慣性坐標(biāo)系、地球坐標(biāo)系、導(dǎo)航坐標(biāo)系、計(jì)算導(dǎo)航坐標(biāo)系，表示測(cè)量比力，分別表示測(cè)量導(dǎo)航系相對(duì)慣性系的角速度、測(cè)量地球自轉(zhuǎn)角速度、測(cè)量導(dǎo)航系相對(duì)地球坐標(biāo)系的角速度，分別表示三者偏差，ε、?分別表示陀螺和加速度計(jì)零偏。

考慮在船舶漂浮狀態(tài)下載體加速度可近似為0，展開(kāi)式(1)并忽略天向速度與高階分量，得誤差方程：

其中，L為緯度，為地球半徑，東向干擾加速度和北向干擾加速度分別包含在東向加速度計(jì)零偏和和北向加速度計(jì)零偏中。

圖1 方位對(duì)準(zhǔn)控制回路Fig.1 Azimuth alignment control loop

由圖1 中控制回路可列出控制回路方程

其中0()sΔ為特征多項(xiàng)式

初始方位失準(zhǔn)角未知，應(yīng)按照大值進(jìn)行評(píng)估，水平對(duì)準(zhǔn)結(jié)束后水平姿態(tài)角和水平速度誤差的影響相對(duì)于大方位失準(zhǔn)角可忽略不計(jì)。由于初始方位失準(zhǔn)角未知，和未知，是的函數(shù)，分別采用常值和代替，這樣式(5)應(yīng)增加相關(guān)的誤差項(xiàng)，忽略式(5)中與水平失準(zhǔn)角、水平速度誤差和零偏項(xiàng)，式(5)變?yōu)?/p>

其中

其中()sΔ 為特征多項(xiàng)式。式(8)中等式右邊第二項(xiàng)為初始方位失準(zhǔn)角引起的響應(yīng)，隨著時(shí)間的推移逐漸衰減；第一項(xiàng)由和的取值偏差造成的的收斂過(guò)程主要取決于特征多項(xiàng)式中的大小，這一項(xiàng)只影響幅值超調(diào)，當(dāng)逐漸收斂，接近1，接近0，即東向通道作用逐漸消失，此時(shí)可斷開(kāi)東向通道的計(jì)算，圖1 中的虛線代表的是東向通道的斷開(kāi)。方便起見(jiàn)，在對(duì)準(zhǔn)初期取

既保證在對(duì)準(zhǔn)初期快速收斂，又保證在對(duì)準(zhǔn)末期的方位失準(zhǔn)角振蕩幅度不至于過(guò)大。方位對(duì)準(zhǔn)結(jié)束時(shí)刻由dminω確定，這里直接引用文獻(xiàn)[7]對(duì)羅經(jīng)法收斂時(shí)間分析的結(jié)論：對(duì)于較大的方位失準(zhǔn)角，經(jīng)過(guò)約1.4個(gè)阻尼振蕩周期就能達(dá)到穩(wěn)態(tài)精度。

2 捷聯(lián)慣導(dǎo)誤差模型與卡爾曼濾波

第1 節(jié)中的控制回路對(duì)準(zhǔn)方法能快速使方位失準(zhǔn)角收斂，但未考慮到加速度計(jì)和陀螺零偏以及漂浮狀態(tài)下微小的晃動(dòng)速度與角速度，對(duì)準(zhǔn)精度不夠，本文考慮在方位角收斂到小范圍之后進(jìn)入卡爾曼濾波精對(duì)準(zhǔn)狀態(tài)。

對(duì)于系統(tǒng)離散方程

捷聯(lián)慣導(dǎo)誤差模型如下：

取狀態(tài)變量

其中?包含加速度計(jì)本身的零偏和漂浮狀態(tài)下引起的等效隨機(jī)加速度零偏。

誤差方程寫(xiě)成狀態(tài)方程的形式：

其中w為系統(tǒng)過(guò)程噪聲。對(duì)晃動(dòng)基座，以速度誤差為觀測(cè)量，則觀測(cè)方程為：

其中v為測(cè)量噪聲，觀測(cè)量。狀態(tài)初值和狀態(tài)協(xié)方差矩陣初值可根據(jù)具體的慣導(dǎo)精度進(jìn)行設(shè)置，一般采用相對(duì)較大的初值。

3 對(duì)準(zhǔn)方案與仿真驗(yàn)證

整個(gè)對(duì)準(zhǔn)方案如圖2所示。其中水平回路對(duì)準(zhǔn)持續(xù)20 s，快參數(shù)對(duì)準(zhǔn)持續(xù)10 s，變參數(shù)方位對(duì)準(zhǔn)和慢參數(shù)方位對(duì)準(zhǔn)時(shí)間由快參數(shù)振動(dòng)頻率、慢參數(shù)振動(dòng)頻率和變化速率確定。經(jīng)過(guò)約1.4 個(gè)慢參數(shù)振動(dòng)周期，方位對(duì)準(zhǔn)結(jié)束，進(jìn)入卡爾曼濾波精對(duì)準(zhǔn)狀態(tài)。

圖2 對(duì)準(zhǔn)方案Fig.2 The plan of alignment

這里采用某型陀螺零偏穩(wěn)定性為0.01 °/h、加速度計(jì)零偏為5 ×10-5g的船用光纖慣組在水上漂浮試驗(yàn)的一段數(shù)據(jù)進(jìn)行仿真。試驗(yàn)地點(diǎn)位于 N 43.7127 ° E 126.6952 °，漂浮試驗(yàn)過(guò)程中風(fēng)速約0.3 m/s，GNSS漂浮軌跡如圖3，圖中“×”表示對(duì)準(zhǔn)起點(diǎn)，“°”表示對(duì)準(zhǔn)結(jié)束時(shí)刻。

圖3 光纖慣導(dǎo)水上漂浮軌跡圖Fig.3 The drifting trace picture of optical fiber inertial navigation system

試驗(yàn)初始姿態(tài)角均為0 °，進(jìn)行了三組仿真試驗(yàn)。

仿真一：0～300 s 時(shí)間段進(jìn)行基于重力加速度的凝固對(duì)準(zhǔn)法粗對(duì)準(zhǔn)，300 s～900 s 時(shí)間段進(jìn)行卡爾曼濾波精對(duì)準(zhǔn)。仿真一采用的方法與實(shí)際測(cè)試方案一致，該試驗(yàn)用作參考。

仿真二：300 s～ 600 s 時(shí)間段進(jìn)行凝固對(duì)準(zhǔn)法粗對(duì)準(zhǔn)，600 s～900 s 時(shí)間段進(jìn)行卡爾曼濾波精對(duì)準(zhǔn)。該試驗(yàn)用作對(duì)比。

仿真三：300 s～Ts 時(shí)間段采用羅經(jīng)法對(duì)準(zhǔn)，Ts～900 s 時(shí)間段進(jìn)行卡爾曼濾波精對(duì)準(zhǔn)（時(shí)間T由羅經(jīng)對(duì)準(zhǔn)參數(shù)確定）。該試驗(yàn)用于驗(yàn)證本方案。

其中仿真三的羅經(jīng)對(duì)準(zhǔn)參數(shù)取值為

仿真對(duì)準(zhǔn)姿態(tài)角如圖4～6所示。圖中體現(xiàn)的是300 s～900 s 時(shí)間段對(duì)準(zhǔn)姿態(tài)角曲線。其中藍(lán)色虛線、綠色點(diǎn)劃線、紅色實(shí)線分別表示仿真一、仿真二、仿真三的對(duì)準(zhǔn)姿態(tài)角曲線圖。仿真一中事先進(jìn)行了300 s粗對(duì)準(zhǔn)，隨著卡爾曼濾波的進(jìn)行，姿態(tài)角逐漸接近真值，因此后面兩組仿真越接近仿真一的結(jié)果越能體現(xiàn)方案的對(duì)準(zhǔn)精度。

由圖4 和圖5 以及其中的放大圖可看出，本對(duì)準(zhǔn)方案中水平姿態(tài)角很快接近仿真一的對(duì)準(zhǔn)結(jié)果，且隨著對(duì)準(zhǔn)的進(jìn)行最后三組仿真水平姿態(tài)角基本重合，這里不再列舉具體數(shù)值。

由圖6 可看出，本方案中的方位失準(zhǔn)角由一個(gè)大值快速收斂，在160 s 以后方位角相對(duì)于仿真一方位角偏差在1 °以內(nèi)。表1 統(tǒng)計(jì)了不同時(shí)刻仿真二和仿真三相對(duì)于仿真一的方位角偏差。在仿真二粗對(duì)準(zhǔn)結(jié)束時(shí)刻，仿真三的方位角偏差比仿真二小一個(gè)數(shù)量級(jí)。之后仿真二和仿真三均進(jìn)入卡爾曼濾波精對(duì)準(zhǔn)狀態(tài)。隨著卡爾曼濾波的進(jìn)行，仿真二和仿真三逐漸接近仿真一的方位角，在仿真結(jié)束時(shí)刻仿真三的方位角仍比仿真二小。

表1 不同時(shí)刻方位角對(duì)比Tab1.Comparison of azimuth at different times

圖4 對(duì)準(zhǔn)過(guò)程俯仰角曲線圖及部分放大圖Fig.4 The pitch angle curve and partial enlarged drawing during alignment

圖5 對(duì)準(zhǔn)過(guò)程滾動(dòng)角曲線圖及部分放大圖Fig.5 The roll angle curve and partial enlarged drawing during alignment

圖6 對(duì)準(zhǔn)過(guò)程航向角曲線圖及部分放大圖Fig.6 The heading angle curve and partial enlarged drawing during alignment

上面的仿真說(shuō)明，采用雙通道時(shí)變參數(shù)羅經(jīng)法，能使較大的方位失準(zhǔn)角快速收斂；在羅經(jīng)法進(jìn)入穩(wěn)態(tài)之后仍存在一定的方位誤差，此時(shí)切換到卡爾曼濾波精對(duì)準(zhǔn)狀態(tài)能進(jìn)一步提高對(duì)準(zhǔn)精度。實(shí)際上，對(duì)于凝固對(duì)準(zhǔn)，在船舶沒(méi)有外部參考信息的漂浮狀態(tài)下，外部環(huán)境引入的干擾加速度，都被積分到速度矢量當(dāng)中，從而導(dǎo)致對(duì)準(zhǔn)誤差的累積，但若縮短凝固對(duì)準(zhǔn)時(shí)間，又會(huì)因?yàn)椴煌瑫r(shí)刻速度矢量平行度增加，放大對(duì)準(zhǔn)誤差。而對(duì)于變參數(shù)羅經(jīng)方位對(duì)準(zhǔn)，控制回路具有一定的抗干擾性，能抑制誤差的放大。

另外要說(shuō)明的是，本文采用的方法中，將漂浮狀態(tài)隨機(jī)的加速度當(dāng)做等效加速度計(jì)的零偏來(lái)處理，盡管會(huì)降低方案三中的對(duì)準(zhǔn)精度，但對(duì)方案一、二中用于參考的對(duì)準(zhǔn)方案的影響是一致的。因此表1 中的對(duì)比偏差能說(shuō)明方法的快速性是有效的。

4 結(jié)論

本文提出了在粗對(duì)準(zhǔn)階段采用時(shí)變參數(shù)羅經(jīng)法、精對(duì)準(zhǔn)階段采用卡爾曼濾波精對(duì)準(zhǔn)的方案解決無(wú)外部參考信息的情況下船舶SINS 漂浮狀態(tài)捷聯(lián)慣導(dǎo)快速自對(duì)準(zhǔn)問(wèn)題；在對(duì)準(zhǔn)初期同時(shí)考慮北向通道和東向通道的速度誤差且采用時(shí)變參數(shù)，可使得在任意的大方位失準(zhǔn)角條件下都能快速收斂。相比較凝固對(duì)準(zhǔn)法，在船舶漂浮狀態(tài)下，本方案能更快地達(dá)到相同的對(duì)準(zhǔn)精度，從而更早地進(jìn)入卡爾曼濾波精對(duì)準(zhǔn)，節(jié)省對(duì)準(zhǔn)時(shí)間或者在相同時(shí)間內(nèi)提高卡爾曼濾波對(duì)準(zhǔn)精度。