基于橫坐標(biāo)系的捷聯(lián)慣性導(dǎo)航系統(tǒng)/多普勒速度儀極區(qū)組合導(dǎo)航算法

張福斌，馬朋，王智輝

（西北工業(yè)大學(xué)航海學(xué)院，陜西西安710072）

基于橫坐標(biāo)系的捷聯(lián)慣性導(dǎo)航系統(tǒng)/多普勒速度儀極區(qū)組合導(dǎo)航算法

張福斌，馬朋，王智輝

（西北工業(yè)大學(xué)航海學(xué)院，陜西西安710072）

針對(duì)傳統(tǒng)指北方位的慣性導(dǎo)航力學(xué)編排在高緯度地區(qū)因地理經(jīng)線快速收斂，建立相對(duì)于經(jīng)線的航向越來(lái)越困難，以及在地理極點(diǎn)存在奇異值等問(wèn)題，結(jié)合自主水下航行器在導(dǎo)航過(guò)程中對(duì)自主性、導(dǎo)航精度等需要，提出了基于橫坐標(biāo)系的捷聯(lián)慣性導(dǎo)航系統(tǒng)/多普勒速度儀極區(qū)組合導(dǎo)航方案。給出了橫地球坐標(biāo)系、橫地理坐標(biāo)系等定義以及與常規(guī)導(dǎo)航坐標(biāo)系的轉(zhuǎn)換關(guān)系；通過(guò)類比常用的指北方位慣性導(dǎo)航力學(xué)編排推導(dǎo)了基于橫坐標(biāo)系的慣性導(dǎo)航力學(xué)編排；設(shè)計(jì)了適用于極區(qū)的捷聯(lián)慣性導(dǎo)航系統(tǒng)/多普勒速度儀組合導(dǎo)航卡爾曼濾波算法；對(duì)設(shè)計(jì)的組合導(dǎo)航系統(tǒng)進(jìn)行了仿真分析。結(jié)果表明，該導(dǎo)航方案能有效抑制方位失準(zhǔn)角的增長(zhǎng)，導(dǎo)航定位精度可滿足自主水下航行器極區(qū)導(dǎo)航的要求。

控制科學(xué)與技術(shù)；組合導(dǎo)航；極區(qū)；橫坐標(biāo)系；捷聯(lián)慣性導(dǎo)航系統(tǒng)

0 引言

南極的淡水、蛋白質(zhì)、礦產(chǎn)資源豐富，北極的能源、海洋生物資源豐富，同時(shí)南、北極還是地球上生物生存環(huán)境的重要調(diào)節(jié)器。自主水下航行器（AUV）作為海洋水文環(huán)境、海底地形地貌等探測(cè)的重要工具，在極地科學(xué)考察任務(wù)中扮演著重要角色，例如我國(guó)即將自主建造第一艘極地科學(xué)考察破冰船具有裝備AUV的支撐平臺(tái)。

在高緯度地區(qū)，由于水平磁力線變?nèi)跫澳媳卑肭蚓嬖诖艌?chǎng)異常區(qū)域，使得基于地磁導(dǎo)航的傳感器定向誤差大、自主性較差。由于地球自轉(zhuǎn)角速度矢量和重力矢量的夾角隨緯度的升高越來(lái)越小，使得電羅經(jīng)的航向測(cè)量誤差越來(lái)越大。故捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）成為AUV導(dǎo)航傳感器首選。但當(dāng)前SINS普遍使用的指北方位、游動(dòng)方位和自由方位力學(xué)編排均是相對(duì)經(jīng)線建立航向，隨著緯度升高，經(jīng)線逐漸收斂到極點(diǎn)，航向確定變得困難甚至失去了意義。

為了解決這一問(wèn)題，文獻(xiàn)［1-3］提出了基于格網(wǎng)導(dǎo)航力學(xué)編排的慣性導(dǎo)航算法原理、SINS空中對(duì)準(zhǔn)、慣性/天文組合導(dǎo)航算法；文獻(xiàn)［4-5］給出了橫坐標(biāo)系的概念及定義方法，文獻(xiàn)［6-9］在此基礎(chǔ)上提出了SINS基于橫坐標(biāo)系的極區(qū)導(dǎo)航方法。

目前，SINS/多普勒速度儀（DVL）組合導(dǎo)航方案在中、低緯度地區(qū)被廣泛用于AUV水下高精度自主導(dǎo)航，本文在已有成果的基礎(chǔ)上，提出了一種適用于AUV極區(qū)水下導(dǎo)航的SINS/DVL組合導(dǎo)航算法，并通過(guò)AUV的典型運(yùn)動(dòng)軌跡對(duì)該方案的導(dǎo)航性能進(jìn)行了分析、驗(yàn)證。

1 基于橫坐標(biāo)系的慣性導(dǎo)航

橫坐標(biāo)系是橫地球坐標(biāo)系和橫地理坐標(biāo)系的統(tǒng)稱，其沿用傳統(tǒng)導(dǎo)航坐標(biāo)系關(guān)于經(jīng)度、緯度、航向等的定義模式［10］，以180°經(jīng)線與赤道平面的交點(diǎn)為偽北極點(diǎn)，以0°經(jīng)線與赤道平面的交點(diǎn)為偽南極點(diǎn)，以90°E/90°W經(jīng)線圈作為偽赤道［11］，如圖1所示。在橫坐標(biāo)系中，地理的南極點(diǎn)、北極點(diǎn)不再是經(jīng)線的匯集點(diǎn)，從而避免了AUV在極區(qū)相對(duì)傳統(tǒng)經(jīng)線定向的困難。

圖1 橫地球坐標(biāo)系Fig.1 Transverse earth coordinate frame

1.1 橫地球坐標(biāo)系

橫地球坐標(biāo)系用Oxeyeze表示，由地球坐標(biāo)系Oxeyeze繞Oye軸順時(shí)針旋轉(zhuǎn)90°得到，其Oye軸與原Oye軸重合，Oxe軸與原Oze軸重合，Oze軸為原Oxe軸的負(fù)向、過(guò)偽北極點(diǎn)；Oxeze平面為偽本初子午面，Oxeye平面為偽赤道平面，兩個(gè)平面的交線為過(guò)地球的南、北極點(diǎn)的直線。從地球坐標(biāo)系Oxeyeze到橫地球坐標(biāo)系Oxeyeze的坐標(biāo)轉(zhuǎn)換矩陣為

1.2 橫地理坐標(biāo)系

用橫經(jīng)度λ、橫緯度L表示點(diǎn)P在橫地球坐標(biāo)系中的位置，橫經(jīng)度λ是通過(guò)點(diǎn)P的偽子午面與偽本初子午面之間的夾角，橫緯度L是點(diǎn)P與地心連線與偽赤道平面之間的夾角。以點(diǎn)P為原點(diǎn)的橫地理坐標(biāo)系Oxgygzg與傳統(tǒng)地理坐標(biāo)系Oxgygzg的定義方法相同，平面Oxgyg為過(guò)點(diǎn)P的水平面，Oyg軸指向偽北極，Ozg軸指向天向，Oxg軸、Oyg軸、Ozg軸構(gòu)成右手坐標(biāo)系。

參照地球坐標(biāo)系Oxeyeze到地理坐標(biāo)系Oxgygzg的坐標(biāo)轉(zhuǎn)換矩陣，直接寫(xiě)出橫地球坐標(biāo)系Oxeyeze到橫地理坐標(biāo)系Oxgygzg的坐標(biāo)轉(zhuǎn)換矩陣為

故由地球坐標(biāo)系Oxeyeze到橫地理坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣可由下式求得

設(shè)偽北向與真北向之間的夾角為?，即除極點(diǎn)外，指向北極點(diǎn)的軸Oyg繞ozg軸（或Ozg軸）順時(shí)針旋轉(zhuǎn)?角（北偏東為正）后與軸Oyg重合，故從地理坐標(biāo)系Oxgygzg到橫地理坐標(biāo)系Oxgygzg的坐標(biāo)轉(zhuǎn)換矩陣為

則由地球坐標(biāo)系Oxeyeze到橫地理坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣為

1.3 基于橫坐標(biāo)系的力學(xué)編排

以橫地理坐標(biāo)系Oxgygzg作為導(dǎo)航坐標(biāo)系Oxnynzn，其力學(xué)編排和誤差方程可借鑒指北方位慣性導(dǎo)航系統(tǒng)力學(xué)編排和誤差方程［10］形式得到，二者的主要區(qū)別體現(xiàn)在地球自轉(zhuǎn)角速度矢量ωie在橫地理坐標(biāo)系Oxgygzg與在地理坐標(biāo)系Oxgygzg各坐標(biāo)軸投影分量的不同。

1.3.1 姿態(tài)微分方程

用四元數(shù)Q表示剛體旋轉(zhuǎn)運(yùn)動(dòng)學(xué)方程

1.3.2 速度微分方程

比力方程在橫坐標(biāo)系下表示為

1.3.3 位置微分方程

橫緯度、橫經(jīng)度、深度的微分方程分別為

1.3.4 姿態(tài)誤差方程

假設(shè)SINS中陀螺的刻度系數(shù)誤差和安裝誤差角已補(bǔ)償，姿態(tài)誤差方程的矢量形式為

1.3.5 速度誤差方程

假設(shè)SINS中加速度計(jì)的刻度系數(shù)誤差和安裝誤差角已經(jīng)補(bǔ)償，速度誤差方程的矢量形式為

設(shè)加速度Δ計(jì)的誤差δfb由隨機(jī)常值和零均值高斯白噪聲w兩部分組成，即

1.3.6 位置誤差方程

位置誤差方程的分量形式為

2 極區(qū)組合導(dǎo)航

假設(shè)AUV導(dǎo)航系統(tǒng)由SINS、DVL、全球衛(wèi)星定位系統(tǒng)（GPS）、深度傳感器組成，組合導(dǎo)航系統(tǒng)的工作流程如下:

1）借助母船導(dǎo)航設(shè)備或岸基輔助導(dǎo)航設(shè)備完成SINS的初始對(duì)準(zhǔn)；

2）組合導(dǎo)航系統(tǒng)切換到SINS/GPS以水平位置差值為量測(cè)量的組合導(dǎo)航狀態(tài)，AUV吊放入水；

3）AUV處于水面漂浮狀態(tài)，啟動(dòng)DVL測(cè)量AUV相對(duì)于海底或其下方一定深度水層的三維速度，組合導(dǎo)航系統(tǒng)切換到SINS/GPS/DVL以水平位置差值、三維速度差值為量測(cè)量的組合導(dǎo)航狀態(tài)，由于深度測(cè)量精度較高，一般可達(dá)到深度傳感器滿量程的萬(wàn)分之一，為了簡(jiǎn)化，組合導(dǎo)航過(guò)程中AUV的航行深度采用深度傳感器測(cè)量值，并忽略深度測(cè)量誤差的影響；

4）AUV下潛進(jìn)行水中航行，組合導(dǎo)航系統(tǒng)切換到SINS/DVL以三維速度差值為量測(cè)量的組合導(dǎo)航狀態(tài)；

5）待AUV執(zhí)行任務(wù)結(jié)束，上浮到水面，組合導(dǎo)航系統(tǒng)切換到SINS/GPS/DVL以水平位置差值、三維速度差值為量測(cè)量的組合導(dǎo)航狀態(tài)，等待回收。

考慮到關(guān)于極區(qū)SINS初始對(duì)準(zhǔn)已有研究［1-2］，AUV吊放入水和水面漂浮時(shí)間一般較短，引起的導(dǎo)航誤差變化通常較小。而AUV水下航行過(guò)程中導(dǎo)航精度對(duì)其完成任務(wù)影響很大，且未見(jiàn)基于橫坐標(biāo)系的SINS/DVL組合導(dǎo)航算法，故這里僅研究基于橫坐標(biāo)系的SINS/DVL組合導(dǎo)航Kalman濾波算法。

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

基于AUV的航行深度已利用深度傳感器獲得較為精確的測(cè)量，故不考慮SINS的深度誤差，選取SINS的姿態(tài)誤差、速度誤差、水平位置誤差Δ和，陀螺和加速度計(jì)的隨機(jī)常值零位偏移εb和b作為狀態(tài)變量，即

將（8）式、（9）式、（10）式去掉深度誤差有關(guān)項(xiàng)，整理可得系統(tǒng)狀態(tài)方程為

2.2 系統(tǒng)量測(cè)方程

DVL輸出測(cè)量值為其儀表坐標(biāo)系下的三維速度，假定SINS與DVL之間的安裝偏差已充分補(bǔ)償，即可認(rèn)為SINS的載體坐標(biāo)系與DVL的儀表坐標(biāo)系重合。以美國(guó)RDI公司工作頻率300 kHz的DVL為例，當(dāng)AUV距海底高程小于200 m時(shí)，DVL既可測(cè)得AUV相對(duì)于海底的絕對(duì)速度，也可測(cè)得AUV相對(duì)于其下方某一水層的速度。當(dāng)AUV距海底高程大于200 m時(shí)，DVL僅可測(cè)得AUV相對(duì)于其下方某一水層的速度。

假設(shè)AUV距海底高程小于200 m，DVL的測(cè)量誤差為零均值的高斯白噪聲wD，其測(cè)量輸出為其載體系坐標(biāo)系下的三維速度，即式中:vbD為載體系下AUV的實(shí)際速度。

利用SINS輸出的姿態(tài)角將DVL輸出的載體系下AUV的三維速度轉(zhuǎn)換到橫導(dǎo)航坐標(biāo)系下，即

式中:I為單位陣。

將SINS輸出的三維速度減去轉(zhuǎn)換到橫坐標(biāo)系下的DVL三維速度作為量測(cè)量，建立量測(cè)方程

將量測(cè)方程寫(xiě)成矩陣形式

式中:

由于wD為零均值的高斯白噪聲，故V仍可認(rèn)為零均值的高斯白噪聲，且強(qiáng)度不變。利用Kalman濾波算法，結(jié)合（12）式和（15）式對(duì)AUV航行姿態(tài)、速度及位置進(jìn)行遞推估計(jì)。

3 仿真分析

3.1 仿真軌跡及仿真參數(shù)設(shè)置

仿真軌跡:AUV航行起點(diǎn)為橫地理坐標(biāo)系的（5°N，0°E，0 m），對(duì)應(yīng)的地理坐標(biāo)系坐標(biāo)為（85°N，30°E，0 m），初始航向30°、橫滾和俯仰角均為0°、深度0 m，速度0 m/s；航行過(guò)程分為13個(gè)階段:1）水面靜止階段，持續(xù)時(shí)間5 s；2）水面加速階段，航行器縱軸方向加速度0.4 m/s2，持續(xù)時(shí)間10 s；3）水面勻速直航階段，持續(xù)時(shí)間30 s；4）進(jìn)入下潛階段，俯仰角速度-0.6°/s，持續(xù)時(shí)間10 s；5）定角下潛階段，持續(xù)時(shí)間50 s；6）改平階段，俯仰角速度0.6°/s，持續(xù)時(shí)間10 s；7）定深直航階段，深度-25 m，持續(xù)時(shí)間7 035 s；8）進(jìn)入上爬階段，俯仰角速度0.6°/s，持續(xù)時(shí)間10 s；9）定角上爬階段，持續(xù)時(shí)間50 s；10）改平階段，俯仰角速度-0.6°/s，持續(xù)時(shí)間10 s；11）水面勻速直航階段，持續(xù)時(shí)間20 s；12）水面減速階段，航行器縱軸方向加速度-0.4 m/s2，持續(xù)時(shí)間10 s；13）水面靜止，持續(xù)時(shí)間5 s.整個(gè)過(guò)程共7 255 s.

SINS的誤差:定義i=x，y，z表示各坐標(biāo)軸，陀螺的隨機(jī)常值誤差εbi=0.02°/h，零均值高斯白噪聲加速度計(jì)的隨機(jī)常值誤差，零均值高斯白噪聲；DVL的測(cè)速誤差:零均值高斯白噪聲wDi=0.01 m/s；導(dǎo)航初始誤差:姿態(tài)角誤差（0.2°，0.2°，0.2°），位置誤差（10 m，10 m，0.2 m），速度誤差（0.1 m/s，0.1 m/s，0.1 m/s）；濾波周期為1 s.

圖2 姿態(tài)誤差Fig.2 Orientation errors

3.2 仿真結(jié)果

假設(shè)AUV航行過(guò)程中，DVL全程可獲得AUV相對(duì)海底的絕對(duì)速度，圖2～圖4為采用Matlab仿真得到SINS/DVL組合導(dǎo)航的姿態(tài)誤差、速度誤差、位置誤差。

圖3 速度誤差Fig.3 Velocity errors

圖4 位置誤差Fig.4 Position errors

從圖2可以看出，SINS/DVL組合導(dǎo)航系統(tǒng)的俯仰和橫滾角誤差均小于1′，航向角誤差的大小基本維持不變，這是因?yàn)椋环矫鍭UV運(yùn)動(dòng)過(guò)程中水平加速度較小，另一方面在極區(qū)地球自轉(zhuǎn)角速度的北向分量很小。從圖3可以看出，水平速度誤差小于0.02 m/s，且基本保持不變，其數(shù)值主要受航向誤差角和AUV航速影響。從圖4可以看出，水平位置誤差隨時(shí)間變化，變化的快慢主要由航向誤差角和AUV航速的數(shù)值決定，當(dāng)AUV初始航向誤差角較小時(shí)，SINS/DVL組合導(dǎo)航系統(tǒng)的自主導(dǎo)航定位精度可以滿足一般水下作業(yè)的任務(wù)需求。

4 結(jié)論

本文參考概念易于理解、物理意義明確的傳統(tǒng)指北方位慣性導(dǎo)航力學(xué)編排方案，提出了一種基于橫坐標(biāo)系的SINS/DVL用于AUV執(zhí)行極區(qū)水下探測(cè)任務(wù)時(shí)自主導(dǎo)航參數(shù)計(jì)算及濾波方法，并利用AUV作業(yè)時(shí)一條典型的運(yùn)動(dòng)軌跡對(duì)其導(dǎo)航性能進(jìn)行了分析，結(jié)果表明:該算法能夠較好地滿足AUV水下自主導(dǎo)航定位的需求。

另外，本文假設(shè)SINS初始失準(zhǔn)角較小，而在極區(qū)因重力矢量與地球自轉(zhuǎn)角速度矢量夾角小，僅靠SINS自對(duì)準(zhǔn)很難獲得較高的對(duì)準(zhǔn)精度。因此，研究極區(qū)橫坐標(biāo)系下SINS的對(duì)準(zhǔn)方法是下一步工作的重點(diǎn)。

（References）

［1］ 周琪，秦永元，付強(qiáng)文，等.極區(qū)飛行格網(wǎng)慣性導(dǎo)航算法原理［J］.西北工業(yè)大學(xué)學(xué)報(bào)，2013，31（2）:210-217. ZHOU Qi，QIN Yong-yuan，F(xiàn)U Qiang-wen，et al.Grid mechanization in inertial navigation system for transpolar aircraft［J］.Journal of Northwestern Polytechnical University，2013，31（2）: 210-217.（in Chinese）

［2］ 吳楓，秦永元，周琪.機(jī)載武器極區(qū)傳遞對(duì)準(zhǔn)算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2013，21（2）:141-146. WU Feng，QIN Yong-yuan，ZHOU Qi.Airborne weapon transfer alignment algorithm in polar regions［J］.Journal of Chinese Inertial Technology，2013，21（2）:141-146.（in Chinese）

［3］ 周琪，秦永元，嚴(yán)恭敏，等.大飛機(jī)極區(qū)慣性/天文組合導(dǎo)航算法［J］.系統(tǒng)工程與電子技術(shù)，2013，35（12）:2559-2565. ZHOU Qi，QIN Yong-yuan，YAN Gong-min，et al.Stellar-inertialnavigation for transpolar large aircraft［J］.Systems Engineering and Electronics，2013，35（12）:2559-2565.（in Chinese）

［4］ Fox W A W.Transverse navigation:an alternative to the grid system［J］.Journal of Navigation，1949，2（1）:25-35.

［5］ Dyer G C.Polar navigation—a new transverse mercator technique［J］.Journal of Navigation，1971，24（4）:484-495.

［6］ 徐曉蘇，豆嫚.基于橫向地理坐標(biāo)系的極區(qū)慣性導(dǎo)航方法研究［J］.華中科技大學(xué)學(xué)報(bào):自然科學(xué)版，2014，42（12）: 116-121. XU Xiao-su，DOU Man.Inertial navigation algorithm transverse geographic in polar regions based on coordinate system［J］.Journal of Huazhong University of Science and Technology:Natural Science Edition，2014，42（12）:116-121.（in Chinese）

［7］ 李倩，孫楓，奔粵陽(yáng)，等.基于橫坐標(biāo)系的捷聯(lián)慣導(dǎo)系統(tǒng)極區(qū)導(dǎo)航方法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2014，22（3）:288-295. LI Qian，SUN Feng，BEN Yue-yang，et al.Polar navigation of strapdown inertial navigation system based on transversal frame in polar region［J］.Journal of Chinese Inertial Technology，2014，22（3）:288-295.（in Chinese）

［8］ 李倩，孫楓，奔粵陽(yáng)，等.橫坐標(biāo)系捷聯(lián)慣導(dǎo)系統(tǒng)極區(qū)導(dǎo)航及阻尼設(shè)計(jì)［J］.系統(tǒng)工程與電子技術(shù)，2014，36（12）:2496-2503. LI Qian，SUN Feng，BEN Yue-yang，et al.Transversal strapdown INS and damping design in polar region［J］.Systems Engineering and Electronics，2014，36（12）:2496-2503.（in Chinese）

［9］ 姚逸卿，徐曉蘇，童金武.極區(qū)間接橫向慣性導(dǎo)航方法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2015，23（1）:29-34. YAO Yi-qing，XU Xiao-su，TONG Jin-wu.Indirect transverse inertial navigation algorithm in polar region［J］.Journal of Chinese Inertial Technology，2015，23（1）:29-34.（in Chinese）

［10］ 秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006. QIN Yong-yuan.Inertial navigation［M］.Beijing:Science Press，2006.（in Chinese）

［11］ 朱啟舉，秦永元，周琪.極區(qū)航空導(dǎo)航綜述［J］.測(cè)控技術(shù)，2014，33（10）:5-8. ZHU Qi-ju，QIN Yong-yuan，ZHOU Qi.Summary of polar air navigation［J］.Measurement and Control Technology，2014，33（10）:5-8.（in Chinese）

SINS/DVL Integrated Navigation Algorithm Based on Transversal Coordinate Frame in Polar Region

ZHANG Fu-bin，MA Peng，WANG Zhi-hui
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi'an 710072，Shaanxi，China）

The traditional north-pointing inertial navigation mechanics arrangement of inertial navigation system is not suitable for using in the polar region due to the rapid convergence of geographical longitude at high latitudes so the establishment of the heading relative to the longitude is more and more difficult，and the singular value in geographic poles exists.Focusing on the above problem and the autonomy and navigation accuracy requirements of autonomous underwater vehicle（AUV）during navigation，a SINS/ DVL integrated navigation scheme based on transversal frame in polar region is proposed.The definition of the transverse earth/geographic coordinate frame and its conversion with conventional navigation frame are presented.The mechanizations of inertial navigation based on transversal frame in polar region are derived by analogy with the traditional north-pointing navigation mechanizations.A SINS/DVL integrated navigation algorithm based on Kalman filter in polar region is designed.The simulation is carried out to analyze the proposed integrated navigation system.The results indicate that the navigation method can restrain effectively the increase in azimuth misalignment angle，and the navigation accuracy can satisfy the navigation requirement of AUV in polar region.

control science and technology；integrated navigation；polar region；transversal frame；strapdown inertial navigation system

U666.1

A

1000-1093（2016）07-1229-07

10.3969/j.issn.1000-1093.2016.07.010

2015-10-14

國(guó)家自然科學(xué)基金項(xiàng)目（61273333）

張福斌（1972—），男，副教授。E-mail:zhangfb@nwpu.edu.cn