基于橫坐標系的捷聯慣性導航系統/多普勒速度儀極區組合導航算法

張福斌，馬朋，王智輝

（西北工業大學航海學院，陜西西安710072）

基于橫坐標系的捷聯慣性導航系統/多普勒速度儀極區組合導航算法

張福斌，馬朋，王智輝

（西北工業大學航海學院，陜西西安710072）

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

控制科學與技術；組合導航；極區；橫坐標系；捷聯慣性導航系統

0 引言

南極的淡水、蛋白質、礦產資源豐富，北極的能源、海洋生物資源豐富，同時南、北極還是地球上生物生存環境的重要調節器。自主水下航行器（AUV）作為海洋水文環境、海底地形地貌等探測的重要工具，在極地科學考察任務中扮演著重要角色，例如我國即將自主建造第一艘極地科學考察破冰船具有裝備AUV的支撐平臺。

在高緯度地區，由于水平磁力線變弱及南北半球均存在磁場異常區域，使得基于地磁導航的傳感器定向誤差大、自主性較差。由于地球自轉角速度矢量和重力矢量的夾角隨緯度的升高越來越小，使得電羅經的航向測量誤差越來越大。故捷聯慣性導航系統（SINS）成為AUV導航傳感器首選。但當前SINS普遍使用的指北方位、游動方位和自由方位力學編排均是相對經線建立航向，隨著緯度升高，經線逐漸收斂到極點，航向確定變得困難甚至失去了意義。

為了解決這一問題，文獻［1-3］提出了基于格網導航力學編排的慣性導航算法原理、SINS空中對準、慣性/天文組合導航算法；文獻［4-5］給出了橫坐標系的概念及定義方法，文獻［6-9］在此基礎上提出了SINS基于橫坐標系的極區導航方法。

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

1 基于橫坐標系的慣性導航

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

圖1 橫地球坐標系Fig.1 Transverse earth coordinate frame

1.1 橫地球坐標系

橫地球坐標系用Oxeyeze表示，由地球坐標系Oxeyeze繞Oye軸順時針旋轉90°得到，其Oye軸與原Oye軸重合，Oxe軸與原Oze軸重合，Oze軸為原Oxe軸的負向、過偽北極點；Oxeze平面為偽本初子午面，Oxeye平面為偽赤道平面，兩個平面的交線為過地球的南、北極點的直線。從地球坐標系Oxeyeze到橫地球坐標系Oxeyeze的坐標轉換矩陣為

1.2 橫地理坐標系

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

參照地球坐標系Oxeyeze到地理坐標系Oxgygzg的坐標轉換矩陣，直接寫出橫地球坐標系Oxeyeze到橫地理坐標系Oxgygzg的坐標轉換矩陣為

故由地球坐標系Oxeyeze到橫地理坐標系的坐標轉換矩陣可由下式求得

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

則由地球坐標系Oxeyeze到橫地理坐標系的坐標轉換矩陣為

1.3 基于橫坐標系的力學編排

以橫地理坐標系Oxgygzg作為導航坐標系Oxnynzn，其力學編排和誤差方程可借鑒指北方位慣性導航系統力學編排和誤差方程［10］形式得到，二者的主要區別體現在地球自轉角速度矢量ωie在橫地理坐標系Oxgygzg與在地理坐標系Oxgygzg各坐標軸投影分量的不同。

1.3.1 姿態微分方程

用四元數Q表示剛體旋轉運動學方程

1.3.2 速度微分方程

比力方程在橫坐標系下表示為

1.3.3 位置微分方程

橫緯度、橫經度、深度的微分方程分別為

1.3.4 姿態誤差方程

假設SINS中陀螺的刻度系數誤差和安裝誤差角已補償，姿態誤差方程的矢量形式為

1.3.5 速度誤差方程

假設SINS中加速度計的刻度系數誤差和安裝誤差角已經補償，速度誤差方程的矢量形式為

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

1.3.6 位置誤差方程

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

2 極區組合導航

假設AUV導航系統由SINS、DVL、全球衛星定位系統（GPS）、深度傳感器組成，組合導航系統的工作流程如下:

1）借助母船導航設備或岸基輔助導航設備完成SINS的初始對準；

2）組合導航系統切換到SINS/GPS以水平位置差值為量測量的組合導航狀態，AUV吊放入水；

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

4）AUV下潛進行水中航行，組合導航系統切換到SINS/DVL以三維速度差值為量測量的組合導航狀態；

5）待AUV執行任務結束，上浮到水面，組合導航系統切換到SINS/GPS/DVL以水平位置差值、三維速度差值為量測量的組合導航狀態，等待回收。

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

2.1 系統狀態方程

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

將（8）式、（9）式、（10）式去掉深度誤差有關項，整理可得系統狀態方程為

2.2 系統量測方程

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

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

利用SINS輸出的姿態角將DVL輸出的載體系下AUV的三維速度轉換到橫導航坐標系下，即

式中:I為單位陣。

將SINS輸出的三維速度減去轉換到橫坐標系下的DVL三維速度作為量測量，建立量測方程

將量測方程寫成矩陣形式

式中:

由于wD為零均值的高斯白噪聲，故V仍可認為零均值的高斯白噪聲，且強度不變。利用Kalman濾波算法，結合（12）式和（15）式對AUV航行姿態、速度及位置進行遞推估計。

3 仿真分析

3.1 仿真軌跡及仿真參數設置

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

SINS的誤差:定義i=x，y，z表示各坐標軸，陀螺的隨機常值誤差εbi=0.02°/h，零均值高斯白噪聲加速度計的隨機常值誤差，零均值高斯白噪聲；DVL的測速誤差:零均值高斯白噪聲wDi=0.01 m/s；導航初始誤差:姿態角誤差（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 姿態誤差Fig.2 Orientation errors

3.2 仿真結果

假設AUV航行過程中，DVL全程可獲得AUV相對海底的絕對速度，圖2～圖4為采用Matlab仿真得到SINS/DVL組合導航的姿態誤差、速度誤差、位置誤差。

圖3 速度誤差Fig.3 Velocity errors

圖4 位置誤差Fig.4 Position errors

從圖2可以看出，SINS/DVL組合導航系統的俯仰和橫滾角誤差均小于1′，航向角誤差的大小基本維持不變，這是因為，一方面AUV運動過程中水平加速度較小，另一方面在極區地球自轉角速度的北向分量很小。從圖3可以看出，水平速度誤差小于0.02 m/s，且基本保持不變，其數值主要受航向誤差角和AUV航速影響。從圖4可以看出，水平位置誤差隨時間變化，變化的快慢主要由航向誤差角和AUV航速的數值決定，當AUV初始航向誤差角較小時，SINS/DVL組合導航系統的自主導航定位精度可以滿足一般水下作業的任務需求。

4 結論

本文參考概念易于理解、物理意義明確的傳統指北方位慣性導航力學編排方案，提出了一種基于橫坐標系的SINS/DVL用于AUV執行極區水下探測任務時自主導航參數計算及濾波方法，并利用AUV作業時一條典型的運動軌跡對其導航性能進行了分析，結果表明:該算法能夠較好地滿足AUV水下自主導航定位的需求。

另外，本文假設SINS初始失準角較小，而在極區因重力矢量與地球自轉角速度矢量夾角小，僅靠SINS自對準很難獲得較高的對準精度。因此，研究極區橫坐標系下SINS的對準方法是下一步工作的重點。

（References）

［1］ 周琪，秦永元，付強文，等.極區飛行格網慣性導航算法原理［J］.西北工業大學學報，2013，31（2）:210-217. ZHOU Qi，QIN Yong-yuan，FU 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］.中國慣性技術學報，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］ 周琪，秦永元，嚴恭敏，等.大飛機極區慣性/天文組合導航算法［J］.系統工程與電子技術，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］ 徐曉蘇，豆嫚.基于橫向地理坐標系的極區慣性導航方法研究［J］.華中科技大學學報:自然科學版，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］ 李倩，孫楓，奔粵陽，等.基于橫坐標系的捷聯慣導系統極區導航方法［J］.中國慣性技術學報，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］ 李倩，孫楓，奔粵陽，等.橫坐標系捷聯慣導系統極區導航及阻尼設計［J］.系統工程與電子技術，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］ 姚逸卿，徐曉蘇，童金武.極區間接橫向慣性導航方法［J］.中國慣性技術學報，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］ 秦永元.慣性導航［M］.北京:科學出版社，2006. QIN Yong-yuan.Inertial navigation［M］.Beijing:Science Press，2006.（in Chinese）

［11］ 朱啟舉，秦永元，周琪.極區航空導航綜述［J］.測控技術，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

國家自然科學基金項目（61273333）

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