法向量位置模型下旋轉(zhuǎn)調(diào)制慣導(dǎo)極區(qū)綜合校正算法

劉 潺，吳文啟，馮國(guó)虎，王茂松

（國(guó)防科技大學(xué) 智能科學(xué)學(xué)院，長(zhǎng)沙 410073）

慣性導(dǎo)航具有連續(xù)性好、自主性高、隱蔽性強(qiáng)的特點(diǎn)，是極區(qū)航行最基本的導(dǎo)航手段。由于高緯度地區(qū)子午線匯聚，常用的指北方位慣性導(dǎo)航力學(xué)編排并不適用于極區(qū)[1]。另外當(dāng)運(yùn)載體出入極區(qū)時(shí)，需要將傳統(tǒng)導(dǎo)航算法與極區(qū)算法進(jìn)行切換，切換過程會(huì)影響算法內(nèi)部過程的連續(xù)性與一致性。在法向量位置模型下實(shí)現(xiàn)的力學(xué)編排具有全球適用性，是極區(qū)導(dǎo)航理想的力學(xué)編排[2]。

對(duì)于長(zhǎng)航時(shí)的慣性導(dǎo)航系統(tǒng)，慣性定位和航向角的主要誤差源是陀螺儀未被補(bǔ)償?shù)钠普`差[3]。陀螺漂移除了產(chǎn)生周期振蕩傳播的導(dǎo)航誤差外，還產(chǎn)生隨時(shí)間增長(zhǎng)而不斷累積的導(dǎo)航誤差。為抑制慣導(dǎo)系統(tǒng)隨時(shí)間積累的誤差，對(duì)于長(zhǎng)期水下航行的載體，可以通過偶爾上浮水面，采用衛(wèi)星導(dǎo)航、天文導(dǎo)航等外部參考信息校正慣導(dǎo)系統(tǒng)誤差。為避免頻繁上浮，根據(jù)稀疏的觀測(cè)信息對(duì)慣導(dǎo)系統(tǒng)進(jìn)行綜合校正具有重要的現(xiàn)實(shí)意義[4]。對(duì)于目前普遍使用的船用光學(xué)陀螺慣導(dǎo)系統(tǒng)，單軸旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)只需考慮方位陀螺常值漂移誤差[5]，而雙軸旋轉(zhuǎn)調(diào)制光學(xué)陀螺慣導(dǎo)系統(tǒng)的絕大部分方位陀螺常值漂移誤差雖然得到了抑制，仍會(huì)有殘存的等效方位陀螺常值漂移誤差。

平臺(tái)慣導(dǎo)系統(tǒng)的綜合校正方法已較為成熟，這些方法，大多是將陀螺常值漂移投影到qep 坐標(biāo)系下進(jìn)行估計(jì)[6]。文獻(xiàn)[7]介紹了基于傳統(tǒng)qep 坐標(biāo)系的綜合校正方法,推導(dǎo)一種慣性系下的最優(yōu)綜校方法。上述綜合校正方法中位置誤差與漂移角之間的關(guān)系方程是在傳統(tǒng)當(dāng)?shù)厮綄?dǎo)航坐標(biāo)系下實(shí)現(xiàn)的，不適用于高緯度地區(qū)。針對(duì)高緯度地區(qū)的捷聯(lián)慣性導(dǎo)航系統(tǒng)，文獻(xiàn)[8]提出了橫坐標(biāo)系慣性導(dǎo)航系統(tǒng)的綜合校正方案，文獻(xiàn)[9]在阻尼條件下設(shè)計(jì)了格網(wǎng)系慣性導(dǎo)航系統(tǒng)的兩點(diǎn)校和三點(diǎn)校方案，但是全球適用的問題并沒有得到解決，不能跨極區(qū)綜合校正。

針對(duì)傳統(tǒng)算法的問題，本文在法向量位置模型下設(shè)計(jì)實(shí)現(xiàn)了全球適用的綜合校正算法。基于北極實(shí)際導(dǎo)航數(shù)據(jù)的數(shù)據(jù)事后處理實(shí)驗(yàn)結(jié)果表明，本文提出的綜合校正算法能夠準(zhǔn)確估計(jì)等效方位陀螺漂移，能夠跨極區(qū)綜合校正，有效抑制了隨時(shí)間積累的導(dǎo)航誤差。

1 基本定義與基本原理

本文中涉及的坐標(biāo)系定義如下:

i系：地心慣性坐標(biāo)系；

e系：地心地固坐標(biāo)系；

n系：當(dāng)?shù)貙?dǎo)航坐標(biāo)系，本文中選用東北天（ENU）地理坐標(biāo)系；

b系：運(yùn)載體坐標(biāo)系；

c系：星敏感器坐標(biāo)系；

o系：qep 坐標(biāo)系。o系的坐標(biāo)原點(diǎn)位于載體所處位置，q 軸平行于赤道平面與本地子午面的交線,由地軸指向外，e 軸與等緯度圈相切指向東，p 軸平行于極軸構(gòu)成右手坐標(biāo)系。坐標(biāo)系o的特點(diǎn)是地球自轉(zhuǎn)角速度在q 軸和e 軸上的分量都等于零。o系與n系的關(guān)系如圖1所示。

圖1 o 系與n 系的示意圖Fig.1 Sketch of the o frame and n frame

1.1 傳統(tǒng)綜合校正方法局限性分析

傳統(tǒng)的綜合校正方法是把漂移角與陀螺漂移之間的關(guān)系方程投影到o系上。假設(shè)載體在地球表面位移運(yùn)動(dòng)的角速度遠(yuǎn)小于地球自轉(zhuǎn)角速度，同時(shí)假設(shè)陀螺漂移角速度誤差εo近似為常值，可以得到o系下漂移角與陀螺漂移之間關(guān)系方程的解析解為[6]：

其中

由于

由圖1 的坐標(biāo)系定義可得：

另一方面，緯度L和經(jīng)度λ的位置誤差向量與漂移角之間的關(guān)系方程為[6]：

由于此系數(shù)矩陣與緯度L的正切值相關(guān)，因此傳統(tǒng)綜合校正方法在高緯度地區(qū)的觀測(cè)方程系數(shù)矩陣是病態(tài)的，也就是說并不適用于極區(qū)。

由文獻(xiàn)[8]能夠推導(dǎo)出橫緯度Lt和橫經(jīng)度λt的位置誤差向量與漂移角之間的關(guān)系方程為：

其中

因此，當(dāng)Lt= 90°，即載體位于赤道（橫向地球坐標(biāo)系極點(diǎn)）時(shí)，在橫坐標(biāo)系下的極區(qū)校正方法同樣是不適用的，不具備全球適用性。另外，由圖1所示，o系的e 軸指向東，在極點(diǎn)附近e 軸指向?qū)l(fā)生較快變化，使得傳統(tǒng)綜合校正受到一定影響。

1.2 法向量位置模型的定義

地球上的水平位置通常采用適用于非極區(qū)的經(jīng)緯度或適用于極區(qū)的橫經(jīng)緯度來表示。為了解決極區(qū)經(jīng)線匯聚問題，需要在極區(qū)和非極區(qū)之間進(jìn)行算法切換。為了避免復(fù)雜的算法切換，采用全球適用的法向量位置表示方法表征載體的水平位置，便于實(shí)際的導(dǎo)航計(jì)算。文獻(xiàn)[2]對(duì)法向量位置模型的建立進(jìn)行了詳細(xì)的推導(dǎo)。如圖2所示，由載體對(duì)應(yīng)參考橢球位置的卯酉圈構(gòu)造一個(gè)虛擬圓球，其球心為對(duì)應(yīng)卯酉圈的圓心，圓球半徑為卯酉圈半徑RE與大地高度h之和。法向量是虛擬圓球球心指向載體位置的單位矢量，實(shí)質(zhì)上也就是當(dāng)?shù)厮矫娣ㄏ蛄吭诘厍蜃鴺?biāo)系下的投影。由法向量定義可得[2]：

圖2 虛擬圓球法向量示意圖Fig.2 Sketch of virtual sphere N-vector

由式(9)可得e系與n系的方向余弦矩陣為：

2 基于法向量位置模型的綜合校正方法

首先推導(dǎo)外水平阻尼條件下慣導(dǎo)系統(tǒng)的漂移角誤差、姿態(tài)角誤差與位置角誤差關(guān)系。由于

因此

其中ψe為e系下的漂移角誤差向量，?n為n系下的姿態(tài)角誤差向量，δθn為n系下的位置角誤差向量，[×]表示對(duì)應(yīng)的斜對(duì)稱矩陣。

忽略二階小量，整理式(12)可得：

因此得到：

對(duì)于長(zhǎng)航時(shí)高精度航海慣導(dǎo)系統(tǒng)，系統(tǒng)在水平阻尼狀態(tài)穩(wěn)定后水平姿態(tài)誤差角通?？梢院雎圆挥?jì)，即：

因此得到：

2.1 位置誤差向量與漂移角之間的數(shù)學(xué)模型

由式(9)(10)(16)可以推導(dǎo)出阻尼后的法向量位置誤差向量δη與e系下漂移角誤差向量的關(guān)系方程為：

由于此系數(shù)矩陣[η×]與緯度L或橫緯度Lt的正切值不再相關(guān)，因此本文的綜合校正方法是全球適用的。

2.2 e 系下的漂移角誤差傳播模型

根據(jù)文獻(xiàn)[2]可得e系下漂移角的誤差微分方程為：

由于旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)的水平方向陀螺漂移影響已得到較充分的抑制，因此本文只考慮殘存的等效方位陀螺漂移作為主要的誤差影響因素。由于法向量方向是當(dāng)?shù)卮咕€方向，因此：

其中，εU為等效方位陀螺漂移，wg為陀螺角度隨機(jī)游走誤差。

結(jié)合式(18)，將εU建模為緩慢變化的馬爾科夫過程，可以得到：

其中βε為馬爾科夫過程相關(guān)時(shí)間常數(shù)，wε為相關(guān)高斯白噪聲。

2.3 系統(tǒng)綜合校正方法

設(shè)綜合校正系統(tǒng)的濾波狀態(tài)為：

其中

如果選取慣性導(dǎo)航系統(tǒng)輸出法向量ηINS與衛(wèi)星導(dǎo)航參考位置法向量ηGNSS的差值z(mì)GNSS作為觀測(cè)量，則觀測(cè)方程可以表示為：

其中υGNSS為衛(wèi)星導(dǎo)航信息定位法向量的觀測(cè)高斯白噪聲。

因此觀測(cè)矩陣為：

選取第j顆確定星體相對(duì)于理想坐標(biāo)系的星光矢量理論計(jì)算值與星敏儀的實(shí)際測(cè)量值的差值作為觀測(cè)量，則觀測(cè)方程為：

因此觀測(cè)矩陣為：

為了不影響慣導(dǎo)系統(tǒng)的誤差傳播規(guī)律，本文采用輸出校正的方式，漂移角誤差的預(yù)測(cè)方程為：

位置法向量輸出校正量為：

2.4 可觀測(cè)性分析

由于噪聲不影響系統(tǒng)的可觀測(cè)性，因此分析可觀測(cè)性時(shí)忽略噪聲項(xiàng)。以衛(wèi)星導(dǎo)航信息作為參考位置為例，令表示獲得兩個(gè)外部參考位置的時(shí)刻，在第一個(gè)參考位置，由式(29)可得第一個(gè)關(guān)系式：

同樣，在第二個(gè)參考位置可得第二個(gè)關(guān)系式：

將式(22)代入式(36)可得：

其中

其中BΠ的各元素可以近似為：

根據(jù)式(35)(36)(37)可得：

其中：

此系數(shù)矩陣中的各個(gè)元素為：

觀察式(43)可得，系數(shù)矩陣M的第一、二、三行的行向量線性相關(guān)，第四、五、六行的行向量線性相關(guān)。當(dāng)與相等且為2π的整數(shù)倍時(shí)，系數(shù)矩陣M的秩為3，其他情況下經(jīng)MATLAB的符號(hào)運(yùn)算可以得到矩陣M的秩為4。因此式(43)的最小二乘解可以表示為：

由式(46)得到tm1時(shí)刻的漂移角和等效方位陀螺常值漂移的估計(jì)值后，根據(jù)式(37)就能夠得到tm2時(shí)刻相應(yīng)的估計(jì)值，進(jìn)而完成綜合校正。因此，本文提出的綜合校正算法至少需要兩個(gè)時(shí)刻的外部參考位置信息，才能滿足可觀測(cè)性要求。綜上所述，本文的綜合校正算法具有全球適用性，同時(shí)還避免了傳統(tǒng)方案對(duì)載體速度、等緯度航行和載體姿態(tài)的限制，具有較強(qiáng)的實(shí)用性。

3 極區(qū)航測(cè)試驗(yàn)數(shù)據(jù)處理及分析

采用北極科學(xué)考察的一段船載實(shí)際慣性導(dǎo)航數(shù)據(jù)和衛(wèi)星導(dǎo)航參考位置數(shù)據(jù)進(jìn)行法向量位置模型下的綜合校正事后處理實(shí)驗(yàn)。本段數(shù)據(jù)采用衛(wèi)星導(dǎo)航數(shù)據(jù)作為參考真值，船舶從起始位置（56.99 °N,174.1 ° E）開始，經(jīng)過 72 小時(shí)穿越 180 ° 經(jīng)線運(yùn)動(dòng)至（71.37 °N,169.5 °W）。本文中阻尼實(shí)驗(yàn)的結(jié)果根據(jù)文獻(xiàn)[10]的阻尼算法獲取。采用衛(wèi)星導(dǎo)航的位置數(shù)據(jù)作為外部參考信息進(jìn)行了3 次觀測(cè)，在第一次觀測(cè)(第10 小時(shí))處進(jìn)行了位置重調(diào)，然后分別在后兩次觀測(cè)(第30、50 小時(shí))處進(jìn)行了漂移角和方位陀螺常值漂移的估計(jì)與補(bǔ)償，進(jìn)而完成綜合校正。試驗(yàn)的軌跡對(duì)比曲線如圖3所示。

圖3 軌跡對(duì)比曲線Fig.3 Comparison on trajectories

由軌跡對(duì)比曲線圖3 可以看到本文的綜合校正算法適用于穿越極區(qū)、穿越對(duì)向子午線的場(chǎng)景，具有全球適用性。

位置誤差的結(jié)果對(duì)比如圖4所示，綠色曲線代表純慣導(dǎo)解算的結(jié)果，藍(lán)色曲線代表水平阻尼后的結(jié)果，紫紅色曲線代表傳統(tǒng)方法校正后的結(jié)果，紅色曲線代表本文方法校正后的結(jié)果，其中的位置誤差均做了歸一化處理。經(jīng)過第一次綜合校正后，歸一化定位誤差的對(duì)比如表1所示。

圖4 位置誤差對(duì)比曲線Fig.4 Comparison on position errors

表1 歸一化定位誤差對(duì)比Tab.1 Comparison on normalized error of localization

觀察圖4 和表1 可得，傳統(tǒng)校正方法的結(jié)果出現(xiàn)了發(fā)散現(xiàn)象，這是由于傳統(tǒng)方法的適用條件具有近似等緯度航行，載體的姿態(tài)保持不變等限制。相比于阻尼系統(tǒng)，本文提出的綜合校正算法能夠有效抑制長(zhǎng)航時(shí)積累的導(dǎo)航誤差，實(shí)驗(yàn)結(jié)果表明導(dǎo)航性能得到了提高。綜合校正后的歸一化定位誤差相比于阻尼后的結(jié)果，最大值減小了53.29%，平均值減小了62.56%，均方根減小了60.76%。綜上所述，采用所提綜合校正算法后的歸一化定位誤差相比于阻尼后的結(jié)果大約減小53%。

對(duì)該實(shí)際導(dǎo)航數(shù)據(jù)條件下的事后處理實(shí)驗(yàn)分別在不同時(shí)間點(diǎn)進(jìn)行了50 次校正測(cè)試，得到方位陀螺常值漂移估計(jì)的平均值及標(biāo)準(zhǔn)差如表2所示。

表2 估計(jì)的方位陀螺常值漂移平均值及標(biāo)準(zhǔn)差Tab.2 Mean and standard deviation of estimated azimuth gyro constant drift

由表2 可知，所設(shè)計(jì)的綜合校正算法能夠估計(jì)出方位陀螺常值漂移并用于誤差補(bǔ)償。

4 結(jié)論

本文以極區(qū)慣性導(dǎo)航為背景，在基于法向量位置模型的全球統(tǒng)一力學(xué)編排下設(shè)計(jì)實(shí)現(xiàn)了綜合校正算法?；诒睒O實(shí)際航行數(shù)據(jù)的數(shù)據(jù)事后處理實(shí)驗(yàn)結(jié)果表明，該綜合校正算法避免了出入極區(qū)時(shí)算法切換帶來的復(fù)雜性和不連續(xù)的問題，具有全球適用性；避免了對(duì)運(yùn)動(dòng)軌跡和運(yùn)行狀態(tài)的限制，具有較強(qiáng)的實(shí)用性；準(zhǔn)確估計(jì)了方位陀螺常值漂移，有效抑制了長(zhǎng)航時(shí)積累的導(dǎo)航誤差，提高了導(dǎo)航定位精度。