基于虛擬圓球法向量的極區慣性導航算法

劉 潺，吳文啟，馮國虎，王茂松

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

隨著極區航線的開發，近年來極區導航已經成為了一個研究熱點問題。由于慣性導航擁有極好的自主性，并且不受極區地磁變化和太陽風暴的影響，因此慣性導航已經成為極區重要的導航手段。極區子午線密集匯聚引起經度退化，極點附近緯度的正切正割計算奇異，使得傳統的當地水平地理坐標系下的慣性導航解算誤差急劇增大。

目前極區慣性導航的主流解決方案有格網坐標系和橫坐標系導航算法。這兩種算法在圓球模型下會不可避免地產生原理性誤差，而采用橢球模型進行導航解算則會使計算過程變得復雜[1,2]。文獻[3]提出了虛擬圓球的概念，利用虛擬圓球模型簡化極區導航力學編排，其精度與橢球模型一致。

格網坐標系導航算法在赤道附近無法正常工作，橫坐標系算法會在赤道產生新的極點，因此二者都不具備全球導航能力。另外，格網坐標系與橫坐標系導航算法不能用統一的全球誤差模型分析其誤差特性，特別在穿越極區的應用場景中，都需要與傳統的地理坐標系導航算法進行切換，這樣會帶來積分過程的變化，對于阻尼算法和組合導航濾波算法，切換過程會影響內部算法的連續性與一致性，同時還極大地增加算法復雜度。間接混合導航算法[4,5]通過對不同坐標系導航算法輸出參數的結合實現全球導航，但是一致性問題仍然沒有完全解決，算法復雜度偏高。

文獻[6]強調要解決全球適用的問題，需要選擇一個完全獨立于地球形狀和系統位置的導航坐標系，例如地球坐標系。文獻[7]提出了一種非奇異的法向量（n-vector）水平位置表示方法。文獻[8]根據法向量方法實現了具有全球適用性的力學編排方案，但是該方法在法向量更新算法中仍需要引入北向和東向速度，導航結果轉換到其他坐標系比較復雜。

在此基礎上，本文提出了虛擬圓球模型下的法向量四元組位置表示方法，研究一種形式更簡潔、實現更簡便的適用于全球的慣性導航力學編排方案，分別從垂直通道和水平通道分析其誤差特性并基于實際航海導航數據進行了仿真驗證。

1 基本定義與基本原理

1.1 橫地理坐標系力學編排的局限

當載體處于極區時，通常采用橫經度λt和橫緯度tL表征水平位置，用橫坐標系進行導航解算。如圖1所示，橫地球坐標系的xte軸與原地球坐標系的ze軸重合，yte軸與xe軸重合，zte軸與ye軸重合。因此地球坐標系e與橫地球坐標系et的方向余弦表示為：

橫地理坐標系（nt系）北向與當地水平地理坐標系（n系，東北天）北向與之間的夾角為β，則由定義可得方向余弦矩陣表示為[1]：

其中

圖1 橫坐標系示意圖Fig.1 Sketch of transversal frame

橫地理坐標系相對于地球系的轉動角速度在橫地理坐標系下的投影，如果采用圓球模型進行導航解算會存在原理性誤差，而采用參考橢球模型時表示為[1]：

因此采用橫坐標系進行導航解算會遇到一些問題：一是涉及多個方向的曲率半徑，計算過程比較復雜；二是近極點處的sinβ、cosβ以及曲率半徑由于經線匯聚無法精確得到，極區姿態解算、速度解算存在誤差，當慣導系統本身已經存在位置誤差時，由此造成的誤差就會更加顯著。

1.2 格網坐標系力學編排的局限

格網坐標系Gn如圖2所示，σ為格網坐標系Gn北向與地理系n北向之間的夾角，記順時針方向為正。由定義可得方向余弦矩陣表示為：

其中

圖2 格網坐標系示意圖Fig.2 Sketch of grid frame

格網坐標系相對于地球系的轉動角速度在格網坐標系下的投影表示為[2]：

對比兩種坐標系下的計算過程，算法結構本質上是一致的，具有相同的天向坐標軸，只是格網北向與橫地理坐標系北向的定義不同。因此，格網坐標系下的計算存在和橫坐標系相同的問題。

1.3 虛擬圓球法向量模型

目前傳統的位置表示方法是使用經緯度（橫經度橫緯度）高程表征位置。然而由于所有經線在極點處交匯，因此極點處的經度沒有定義，經緯度位置表示方法存在奇異性，而當載體接近極點時，這種表示方法會導致極區導航算法的精度降低。此外，當載體位于180 °經線時，經度表示位置不連續，會影響計算的連續性。同理，橫經度橫緯度的表示方法會在赤道上產生新的極點，同樣不適用于全球導航算法。雖然地心直角系的位置表示方法具備全球適用性，但這種方法不能直接表示載體相對地球參考橢球表面的高度、橫向位置及其變化，得到的導航結果在需要進行坐標系轉化時比較復雜。

為了解決傳統位置表示方法的奇異性問題，本文在虛擬圓球模型的基礎上，提出用四維向量表示三維位置的方法，如圖3所示。

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

由載體對應參考橢球位置的卯酉圈構造一個虛擬圓球，其球心為對應卯酉圈的圓心，圓球半徑為卯酉圈半徑ER與大地高度h之和。

在新的虛擬圓球模型下，用包含法向量的四元組Pη=[ηxη yηzh]T表示載體位置。法向量η= [η xη yηz]T是虛擬圓球球心指向載體位置的單位矢量，實質上也就是當地水平面法向量在地球坐標系下的投影，用來表征載體的水平位置。大地高度h即載體位置相對地球橢球模型表面的高程，表征載體的垂直位置。由法向量定義可得：

四元組Pη與地球坐標系下載體到地心的位置矢量的轉換關系可以表示為：

采用地理坐標系n進行導航解算，將式(6)代入方向余弦矩陣可得：

由式（2）（8）可以推導出e系與nt系的方向余弦矩陣得：

由式（4）（8）可以推導出e系與nG系的方向余弦矩陣得：

由于虛擬圓球法向量模型在地球坐標系下的慣性導航力學編排只需要涉及參考橢球模型中的精確的子午圈和卯酉圈半徑計算，避免了復雜的沿其他方向的曲率和扭曲率半徑計算，避開了計算橫地理坐標系和格網坐標系相對地球的轉動角速度的復雜過程，有利于提高極區導航位置、速度姿態微分方程的解算精度。此外，虛擬圓球法向量的位置表示方法還能夠方便地得到地球坐標系與橫向、格網導航坐標系的方向余弦矩陣，便捷地將導航參數轉換為各種坐標系下的導航參數。

2 基于虛擬圓球法向量的力學編排

2.1 位置微分方程

基于虛擬圓球法向量的導航力學編排方案選取地球坐標系為導航坐標系，用四元組代替經緯高進行位置更新。

在垂直方向上，由于虛擬圓球法向量與載體垂直高度方向一致，因此載體垂直方向上的微分方程為：

在水平方向上，根據式（6）（7）可以推導出虛擬圓球法向量η的微分方程用子午圈半徑RN、卯酉圈半徑RE與大地高度h表示為：

即位置微分方程整理可得：

式中KR為參考橢球變換為虛擬圓球的尺度系數矩陣，kR為尺度變換系數，與文獻[3]一致，即：

觀察位置微分方程的物理意義可得，虛擬圓球的球心為對應卯酉圈的圓心，半徑為RE +h。

2.2 速度微分方程

地球坐標系下的速度微分方程為：

2.3 姿態微分方程

地球坐標系下的姿態微分方程為：

綜上，基于虛擬圓球法向量的慣性導航力學編排如圖4。

圖4 力學編排框圖Fig.4 Diagram of mechanization

綜上，基于虛擬圓球法向量的極區慣性導航力學編排的姿態微分方程、速度微分方程與地球坐標系下的解算方程形式基本一致，而位置微分方程則是四元組Pη的方程。由于η是當地水平面法線方向單位矢量在地球坐標系下的投影，無論在極區或非極區都不會出現退化問題，使新的力學編排具備了全球適用性。上述力學編排不需要涉及多個方向的曲率半徑的計算，而且在各個坐標系下的導航參數轉換方便，在保證精度的同時，復雜度相對較低。

3 基于虛擬圓球法向量的算法誤差特性分析

為了分析新模型下的導航性能，同時為以后的阻尼工作做準備，推導其誤差方程如下。

vh為當地垂直方向的速度，即

為水平方向速度在地球坐標系中的投影，即

由速度微分方程式(14)(17)(18)可得：

3.1 垂直方向誤差微分方程

觀察靜基座的高度通道，用ωs表示舒勒周期角頻率，由近似算法可得:

由式（17）（19）（20）可以得到：

3.2 水平方向誤差微分方程

同理，在靜基座的水平通道上，忽略小項后整理得速度誤差微分方程為：

另外，姿態誤差微分方程為：

水平位置誤差微分方程為：

整理式（26）（27）（28）得到水平通道的誤差微分方程式：

因此由式（20）（24）可以得到靜基座下高度通道的速度、位置誤差微分方程：

因此，可以得到狀態轉移矩陣的特征方程：

在配方過程中忽略和項，可以得到

觀察特征方程根的情況，除3 個零根外，有一對虛數根 ±jω ie，即地球周期振蕩對應的角頻率。另外由得到的兩對虛數根可以得到角頻率ω1與角頻率ω2。

角頻率ω1為：

角頻率ω2為：

由定義可得kR的取值范圍為[0.9933,1]，而，因此ω1≈ωs，也就是舒勒周期對應的角頻率；ω2≈ωiesinL，也就是傅科周期對應的角頻率，特別是當靠近極點時，傅科周期退化為地球周期。

4 仿真實驗

4.1 三種算法的靜態仿真實驗

考慮到極區導航實驗的特殊性，文獻[9]提出了一種“極區高度仿真”的數據轉換方法，將低緯度實際航行的導航數據轉換成高緯度仿真數據。為了與文獻[1]的橫坐標系算法、文獻[2]的格網坐標系算法做對比，采用這種轉換方法將80 h 靜態數據（200 Hz）轉換至北緯85°做極區靜態純慣導仿真，縱坐標用歸一化誤差表示，橫向位置誤差如圖5所示。

由圖5可以看出，格網坐標系與橫坐標系的導航解算結果重合，本文所提出的新方法的精度略有提高，同時避免了出入極區時的切換操作。另一方面，由于新模型不需要計算格網坐標系或橫坐標系在橢球模型下的多個方向的曲率半徑，因此計算耗時較少。如表1所示，以格網系的算法耗時為標準，橫坐標系的算法耗時與格網系相當，而新算法的耗時大約節省了20%。

圖5 橫向位置誤差Fig.5 Transversal position error

表1 三種方法的耗時Tab.1 Time spent on three algorithms

表2 多次仿真的結果對比Tab.2 Results of multiple simulations

為了驗證算法的有效性，分別將初始位置轉換到極區多個地方進行了仿真驗證，多次仿真結果如表2所示。實驗結果表明，轉換到極點時，新算法和格網坐標系、橫坐標系的解算結果重合，而轉換到北緯85°時，歸一化誤差大約減小了0.0025。根據式(3)(5)，在極點處傳統極區導航算法沒有曲率半徑的近似計算誤差，而隨著位置偏離極點，近似誤差逐漸增大，與實驗結果相一致。

4.2 三種算法的動態半實物仿真實驗

將起始位置（28.22° N,112.99° E）的15000 s 實際航行數據轉換至極區（85° N,0° E）即橫經度5° 橫緯度0°進行動態仿真實驗，基于虛擬圓球法向量的算法與極區傳統導航算法的軌跡與橫向位置誤差如圖6所示。

由圖6可以看出，本文所提出的新方法與格網坐標系、橫坐標系導航算法的軌跡基本重合，精度略優于格網坐標系、橫坐標系下的算法。圖中誤差曲線可以看到一些突起，這是由于參考真值（GPS）的跳變所導致的。

圖6 極區的航跡與橫向位置誤差Fig.6 Track and transversal position error in polar region

綜上，基于虛擬圓球法向量的極區慣性導航算法適用于穿越極區的場景，具有全球適用性。算法的性能與格網坐標系或橫坐標下的算法相比，實現方便，耗時較少，精度略優。

5 結 論

為了解決全球適用性問題，本文提出了虛擬圓球法向量的四元組載體位置表示方法。在此基礎上，實現了精度較高復雜度較低適用于全球的慣性導航力學編排方案，分析了誤差特性，為下一步的阻尼工作打下了基礎。通過靜態與動態半實物仿真實驗，新算法與傳統極區算法相比，形式簡潔容易實現，避免了出入極區的復雜切換，具有全球適用性。