基于虛擬圓球法向量的極區(qū)慣性導(dǎo)航算法

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

（國防科技大學(xué) 智能科學(xué)學(xué)院，長沙 410073）

隨著極區(qū)航線的開發(fā)，近年來極區(qū)導(dǎo)航已經(jīng)成為了一個研究熱點問題。由于慣性導(dǎo)航擁有極好的自主性，并且不受極區(qū)地磁變化和太陽風(fēng)暴的影響，因此慣性導(dǎo)航已經(jīng)成為極區(qū)重要的導(dǎo)航手段。極區(qū)子午線密集匯聚引起經(jīng)度退化，極點附近緯度的正切正割計算奇異，使得傳統(tǒng)的當(dāng)?shù)厮降乩碜鴺?biāo)系下的慣性導(dǎo)航解算誤差急劇增大。

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

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

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

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

1 基本定義與基本原理

1.1 橫地理坐標(biāo)系力學(xué)編排的局限

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

橫地理坐標(biāo)系（nt系）北向與當(dāng)?shù)厮降乩碜鴺?biāo)系（n系，東北天）北向與之間的夾角為β，則由定義可得方向余弦矩陣表示為[1]：

其中

圖1 橫坐標(biāo)系示意圖Fig.1 Sketch of transversal frame

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

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

1.2 格網(wǎng)坐標(biāo)系力學(xué)編排的局限

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

其中

圖2 格網(wǎng)坐標(biāo)系示意圖Fig.2 Sketch of grid frame

格網(wǎng)坐標(biāo)系相對于地球系的轉(zhuǎn)動角速度在格網(wǎng)坐標(biāo)系下的投影表示為[2]：

對比兩種坐標(biāo)系下的計算過程，算法結(jié)構(gòu)本質(zhì)上是一致的，具有相同的天向坐標(biāo)軸，只是格網(wǎng)北向與橫地理坐標(biāo)系北向的定義不同。因此，格網(wǎng)坐標(biāo)系下的計算存在和橫坐標(biāo)系相同的問題。

1.3 虛擬圓球法向量模型

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

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

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

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

在新的虛擬圓球模型下，用包含法向量的四元組Pη=[ηxη yηzh]T表示載體位置。法向量η= [η xη yηz]T是虛擬圓球球心指向載體位置的單位矢量，實質(zhì)上也就是當(dāng)?shù)厮矫娣ㄏ蛄吭诘厍蜃鴺?biāo)系下的投影，用來表征載體的水平位置。大地高度h即載體位置相對地球橢球模型表面的高程，表征載體的垂直位置。由法向量定義可得：

四元組Pη與地球坐標(biāo)系下載體到地心的位置矢量的轉(zhuǎn)換關(guān)系可以表示為：

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

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

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

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

2 基于虛擬圓球法向量的力學(xué)編排

2.1 位置微分方程

基于虛擬圓球法向量的導(dǎo)航力學(xué)編排方案選取地球坐標(biāo)系為導(dǎo)航坐標(biāo)系，用四元組代替經(jīng)緯高進行位置更新。

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

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

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

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

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

2.2 速度微分方程

地球坐標(biāo)系下的速度微分方程為：

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

地球坐標(biāo)系下的姿態(tài)微分方程為：

綜上，基于虛擬圓球法向量的慣性導(dǎo)航力學(xué)編排如圖4。

圖4 力學(xué)編排框圖Fig.4 Diagram of mechanization

綜上，基于虛擬圓球法向量的極區(qū)慣性導(dǎo)航力學(xué)編排的姿態(tài)微分方程、速度微分方程與地球坐標(biāo)系下的解算方程形式基本一致，而位置微分方程則是四元組Pη的方程。由于η是當(dāng)?shù)厮矫娣ň€方向單位矢量在地球坐標(biāo)系下的投影，無論在極區(qū)或非極區(qū)都不會出現(xiàn)退化問題，使新的力學(xué)編排具備了全球適用性。上述力學(xué)編排不需要涉及多個方向的曲率半徑的計算，而且在各個坐標(biāo)系下的導(dǎo)航參數(shù)轉(zhuǎn)換方便，在保證精度的同時，復(fù)雜度相對較低。

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

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

vh為當(dāng)?shù)卮怪狈较虻乃俣?，?/p>

為水平方向速度在地球坐標(biāo)系中的投影，即

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

3.1 垂直方向誤差微分方程

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

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

3.2 水平方向誤差微分方程

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

另外，姿態(tài)誤差微分方程為：

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

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

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

因此，可以得到狀態(tài)轉(zhuǎn)移矩陣的特征方程：

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

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

角頻率ω1為：

角頻率ω2為：

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

4 仿真實驗

4.1 三種算法的靜態(tài)仿真實驗

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

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

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

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

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

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

4.2 三種算法的動態(tài)半實物仿真實驗

將起始位置（28.22° N,112.99° E）的15000 s 實際航行數(shù)據(jù)轉(zhuǎn)換至極區(qū)（85° N,0° E）即橫經(jīng)度5° 橫緯度0°進行動態(tài)仿真實驗，基于虛擬圓球法向量的算法與極區(qū)傳統(tǒng)導(dǎo)航算法的軌跡與橫向位置誤差如圖6所示。

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

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

綜上，基于虛擬圓球法向量的極區(qū)慣性導(dǎo)航算法適用于穿越極區(qū)的場景，具有全球適用性。算法的性能與格網(wǎng)坐標(biāo)系或橫坐標(biāo)下的算法相比，實現(xiàn)方便，耗時較少，精度略優(yōu)。

5 結(jié) 論

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