一種游動傅科機械編排的捷聯慣性間接極區導航算法

孫新雷，尹浩凌，富立，王玲玲

（1.北京航空航天大學機械與控制工程國家級（虛擬仿真）實驗教學中心，北京 100191；2.莫斯科羅蒙諾索夫國立大學力學數學系，莫斯科 119991）

0 引言

極地地區在資源開采、科研探索、國家安全等方面都具有獨特的地位［1-3］。因此，在空間和資源競爭日益激烈的國際形勢下，各國逐鹿兩極的態勢早已形成，大量勘測隊伍相繼被派往極區開展地形測繪和資源勘探。極區飛行器在執行勘測、警戒、偵察、搜索和救援等任務時具備重要且獨特的功能，其必要性和不可替代性顯著。然而，由于極區的特殊的地理環境和極端氣候條件，極區飛行器執行任務時面臨著許多挑戰。為了應對極區安全飛行的挑戰，需要有高精度導航定位技術作為支撐。眾所周知，極區附近存在經線收斂迅速、電磁干擾嚴重、太陽磁暴頻發等現象，導致極區導航面臨方向基準丟失、磁極位置漂移、無線通信受擾等困難。因此，目前的單一導航方式如衛星導航、磁羅經導航、天文導航和圖像匹配等都難以直接用于極區導航［4-6］。

為了確保導航系統能夠在極區附近長時間、高精度地工作，目前主要采用組合導航方法來彌補單一導航方式的不足。文獻［7-8］研究了基于偏振光輔助的組合導航方法，其中偏振光導航可用于提供航向和姿態信息。在文獻［9］中，提出了一種基于多普勒測速儀輔助的組合導航方法。該方法利用多普勒測速信息來校正慣性導航系統的方位角，從而提高其在極區的導航精度。文獻［10-11］研究了衛星/慣性組合導航在極區的工作性能，文中利用衛星導航系統提供的位置信息校正慣性導航誤差。可見，無論上述哪種方式，都是以慣性導航為中心開展的組合導航。這是因為慣性導航系統具有自主性好、環境適用性高、抗干擾能力強等優點，且能夠為運載體提供姿態角、速度以及位置等自主導航信息，是目前最適用于極區附近的導航方式［12］。因此，提高慣性導航系統的精度是保證高精度極區導航的關鍵所在。

已有研究表明，慣性導航系統中常用的指北、游移方位等機械編排方式在極點附近存在計算溢出和導航誤差發散等問題［13-14］。為了解決這些問題，學者們提出了地心地固（Earth-centred earth-fixed，ECEF）機械力學編排作為一種有效的解決方案［15-16］。該方法將地球坐標系作為導航坐標系，且所有的導航信息都基于該坐標系定義。當需要獲取橫滾、俯仰和偏航角信息時，可利用局部位置信息間接得到機體坐標系與地理坐標系之間的姿態角。然而，在兩極地區，由于ECEF 坐標系的z軸與地理坐標系平行，偏航角將不再是唯一確定的。網格慣導機械力學編排和橫向慣導機械力學編排也是常用的極區導航方法［17-20］。它們通過引入網格基準線和橫向坐標系重新定義方向基準，避免了極區附近北向方向基準不可用的問題。然而，這兩種方法間斷式的導航坐標系切換對于平臺慣導系統來說是無法實現的，只能用于捷聯慣性導航系統。此外，導航坐標系切換過程中的計算誤差可能導致導航解算精度下降。為了避免導航坐標系間的切換，一些學者設計并提出了基于偽地球坐標系和凝固地理坐標系的極區慣性導航方法［21-22］，實現了導航算法在全局區域的內在統一性和導航坐標系間的平滑切換。然而，上述文獻中的偽地球坐標系和凝固地理坐標系的定義高度依賴于經度和緯度信息，因此位置誤差會對導航系統的精度產生很大影響。近年來還出現了基于地球坐標系的法向量慣性導航力學編排方法［23］，該方法用法向量代替經緯度來進行位置更新，可適用于全球范圍。然而，地球曲率半徑的變化會對法向量慣性導航方法的計算精度產生不利影響。

綜上所述，極區導航需要一種能夠平滑轉換坐標系、穩定且簡單直觀的機械編排方式。游動傅科機械力學編排方式為上述問題提供了一種全新的解決方案［24］。該編排方式既適用于平臺和捷聯慣性導航系統，又能夠實現導航誤差的部分解耦。在此基礎上，本文首先推導并驗證了基于游動傅科機械力學編排方式的捷聯慣性導航算法。其次，通過引入橫向導航信息進行間接導航解算，成功解決了游動傅科機械力學編排方式在極點處導航信息無法分離的問題，使得基于游動傅科機械力學編排的慣性導航系統能夠在全區域（包括極區附近）正常工作。

1 游動傅科機械力學編排的捷聯慣性導航算法

1.1 坐標系定義

本文涉及到的坐標系定義如圖1所示。其中用下標簡稱各坐標系：e為地心地固坐標系，b為載體坐標系，g為地理坐標系，eˉ為橫向地心地固坐標系，gˉ為橫向地理坐標系，其具體定義可以參考文獻［25］。

圖1 慣性導航相關坐標系Fig.1 Inertial navigation related coordinate systems

f為局部水平游動傅科坐標系［24］（簡稱傅科坐標系）。游動傅科坐標系以載體所在的真實地理位置為原點，y軸初始時指向北極，z軸沿局部重力矢量方向，構成右手標準正交坐標系。游動傅科坐標系相對慣性坐標系在z軸上的旋轉分量為零（即=0）。此外，游動傅科坐標系與游移方位坐標系［13］有許多相似之處，二者均為局部水平坐標系，且z軸始終與局部重力矢量方向保持平行，并指向向上的方向。游動傅科坐標系與游移方位坐標系的區別在于傅科坐標系的z軸無地球自轉分量（該分量即為傅科振蕩）。因此，游動傅科坐標系相對游移方位坐標系以(360°/24 h) · sin(L)的角速度旋轉。

1.2 游動傅科坐標系下慣性導航方程

基于以上游動傅科坐標系的定義，容易得到游動傅科機械力學編排的導航方程

式中：f b為加速度計比力測量矢量在載體坐標系下的投影為載體對地速度矢量在傅科坐標系中的投影為重力加速度矢量和為牽連角速度矢量；符號(·)×表示反對稱矩陣運算。一般地，對于三維矢量a=[a1a2a3]T，可定義(a)×為：

式中：α為游動角，即游動傅科坐標系y軸與北向之間的夾角為載體坐標系到地理坐標系的方向余弦矩陣。此外，游動傅科機械編排方式的位置矩陣與游移方位機械編排具有類似的形式［26］。式（1）中的牽連角速度可由載體對地速度在游動傅科坐標系中的投影求得：

1.3 游動傅科機械編排慣性導航誤差分析

忽略二階小量的影響，游動傅科坐標系下的姿態誤差方程可以表示為

式中：φf表示計算得到的傅科坐標系與理想傅科坐標系之間的偏差角，即平臺誤差角+表示傅科坐標系相對慣性坐標系的角速度；εf為陀螺在傅科坐標系內的等效漂移。根據式（5）可以得到平臺誤差角沿傅科坐標系x，y，z軸分量的具體形式：

式中：ωie為地球自轉角速度，δv為速度誤差，δα為游動角誤差，δh為高度誤差，δλ和δL分別為經度和緯度誤差。

游動傅科坐標系下的速度誤差滿足如下方程：

式中：f f表示比力在傅科坐標系中的投影，?f為加速度計的等效偏置誤差。根據式（8）可以得到速度誤差沿傅科坐標系x，y，z軸分量的具體形式：

進一步，可以得到位置誤差方程：

式中：vE和vN分別表示東向對地速度和北向對地速度，可以由式（14）計算得到：

航向角誤差為：

式中：φz為平臺誤差角沿傅科坐標系z軸的分量，δα為游動角誤差。

1.4 游動傅科機械編排慣性導航誤差特性

在常用的地理導航坐標系下，導航誤差模型可以被近似為耦合的非線性時變系統。但航向誤差中存在緯度的正切項，當載體接近極點時，該正切項將趨于無窮大，從而導致航向角誤差指數級增長，呈現發散狀態。另一方面，在地理導航坐標系中，載體航向角與緯度之間存在的非線性關系會隨著載體位置的變化而改變，也會增大航向角誤差。為解決以上問題，最直接的方法就是采用游移方位坐標系作為導航坐標系，以便消除航向角誤差方程中的正切項。此時，相對于導航坐標系而言，姿態角誤差的微分不會發散［28］。然而，緯度的增加仍然會降低導航誤差的估計精度，這種影響從游移方位坐標系轉換到地理坐標系的代數運算過程中，即它被轉移至游動角誤差中。也就是說，引入游移方位坐標系仍無法解決導航誤差高度耦合的問題。鑒于游動傅科坐標系的特殊定義，其具備將導航誤差的水平分量在慣性坐標系和傅科坐標系之間實現部分解耦的能力，即解耦水平通道位置誤差。為了更加便捷地分析和表示位置、速度和加速度誤差之間的關系，考慮以下形式的導航誤差方程：

式中：qf為載體在游動傅科坐標系中的位置，Δqf為位置誤差是慣性坐標系到傅科坐標系的正交變換矩陣。根據游動傅科坐標系的定義易知，游動傅科坐標系相對慣性坐標系的角速度為：

式中：vf為傅科坐標系下的慣性速度表示由地球曲率引起的傅科坐標系相對于慣性空間的旋轉矢量，R是地球半徑。由于在傅科坐標系z軸上的分量為零，使得式（16）所示的誤差方程水平分量對于位置矢量的解耦成為可能。對式（16）進一步變形可得：

式中：Δf b表示比力測量誤差，φf表示平臺誤差角（即實際傅科坐標系與理想傅科坐標系不重合引起的誤差），ΔGf表示局部重力加速度估計誤差。

由式（18）可以得到誤差方程沿x軸和y軸分量的具體形式：

式中：舒勒反饋環節的特征頻率ωs由載體受到的重力加速度和離心力共同決定。由式（20）可知，誤差方程中的和之間實現了解耦，從而可以更好估計并補償慣性導航系統的誤差。由此可見，傅科坐標系在導航誤差分析過程中具有優勢。

2 極區導航算法

雖然游動傅科捷聯慣性導航系統在高緯度地區有較好的工作性能，且具有水平誤差分量可解耦的優點，但該系統在極點附近工作時仍然面臨和游移方位導航系統同樣的困境，即緯度L趨近于90°時有：

此時，位置矩陣可以簡化為：

由式（22）可知，此時經度λ和游動角α無法分離，這將導致航向角計算失效［27］。為避免上述問題，本文采用橫向導航信息來進行間接極區導航，從而拓寬游動傅科導航系統的適用范圍。

2.1 傳統導航信息與橫向導航信息的轉換

橫向坐標系的提出最早是為了解決慣性導航系統在極區附近存在物理奇點的問題［28-29］。如圖1所示，定義90°E經線與赤道線的交點為橫向北極點，地理坐標系（g系）與橫向地理坐標系（gˉ系）之間的夾角為p。分別記經度和緯度為λ和L，橫向經度和緯度為和，地球半徑為R，載體所在位置的水平高度為h。那么地球上任意一點P的位置可以表示為：

由式（23）可以得到傳統位置信息與橫向位置信息之間的轉換關系：

此外，地理坐標系（g系）與橫向地理坐標系系）之間的正交變換矩陣可以表示為：

式中：夾角p的值域為(-180°，180°]，可根據反正切和反余弦函數的值來確定。

2.2 游動傅科捷聯慣性間接極區導航

游動傅科坐標系與地理坐標系和橫向地理坐標系具有如圖2 所示的角度關系。其中，α為傳統游動角，β為橫向游動角，p為地理坐標系與橫向地理坐標系之間的夾角，Ng和Eg分別為地理北向和東向和分別為橫向地理的北向和東向。

圖2 傅科坐標系和地理坐標系Fig.2 Foucault coordinate and geographic coordinate

由圖2可以看出傳統游動角和橫向游動角之間具有如下關系：

當載體進入極點附近區域時，仍采用游動傅科坐標系作為導航坐標系，但需要將導航信息解算到橫向坐標系中再間接求出導航信息。設載體進入極點附近區域時的游動角為α0，地理坐標系與橫向地理坐標系的夾角為p0，則初始橫向游動角為

由于導航坐標系沒有發生改變，基于橫向導航方式的游動傅科機械編排慣性導航系統的導航方程仍然為式（1）。但需要將坐標變換矩陣和中各項元素替換成橫向坐標系下的位置和姿態信息，即將式（1）和式（2）中的λ，L，α分別替換成，，β。由于地理坐標系與橫向地理坐標系均為當地水平坐標系，則兩種導航方式下姿態角之間的關系可以表示為：

式中：δψ為地理北向與橫地理北向之間的夾角，可以下式計算得到：

在極點附近區域使用式（1）求解載體速度和位置時，需要注意的是地球自轉角速度在橫向地理坐標系中的投影為

2.3 游動傅科捷聯慣性間接極區導航的實現

由2.2 節中的推導可知，當載體運動到極點附近區域時，仍然可以采用游動傅科機械編排方式，但需要將導航信息轉換到橫向坐標系中。此外，載體在該區域邊界運動時，可能會出現頻繁切換導航方式的情況，因此實際應用時應選取不同緯度L2和L1(L2＞L1)作為“進入”和“退出”捷聯慣性間接極區導航方式的閾值。游動傅科捷聯慣性間接極區導航算法的實現流程如圖3所示。

圖3 極區導航算法流程圖Fig.3 Flowchart of polar navigation algorithm

在游動傅科捷聯慣性間接極區導航方法中，首先根據載體當前位置和游動角信息由式（30）計算出橫向初始游動角β0，并通過式（25）將當前導航信息轉換為橫向導航信息。由于轉換前后導航方程都是投影到游動傅科坐標系中，故轉換前后不需要進行導航算法的改變。但需要注意，采用游動傅科捷聯慣性間接極區導航方式時，傅科坐標系的牽連角速度是相對橫向坐標系而言的。

3 仿真校驗

3.1 游動傅科慣性導航系統誤差分析

極區導航有兩點需要注意：重力加速度矢量與地球自轉角速度矢量幾乎平行，經線快速收斂導致較小的位置變化可能引起很大的經度變化。正因如此，目前絕大多數機械力學編排方式都無法勝任極區導航工作［30］。為了驗證游動傅科機械編排方式在高緯度地區的工作性能，采用一條沿經線飛行的軌跡進行導航誤差仿真。

假設載體沿經線方向向北航行，在行駛過程中載體坐標系相對當地地理坐標系始終保持重合，即載體的姿態角不發生改變。具體仿真條件設置為：初始經度λ0=120°E，初始緯度L0=82°N；東向速度vE=5 m/s，北向速度vN=50 m/s，天向速度vU=0 m/s，飛行高度h=6 000 m，這是大多數長航時無人機較為合適的飛行速度和高度；初始姿態角為0°，初始游動角α0=0°。陀螺漂移誤差εx=εy=εz=0.01°/h，加速度計偏置誤差為?x=?y=?z=5 × 10-5g，不考慮傳感器的標度因數誤差。仿真飛行總時長設置為3 h，結束時載體所在位置緯度為Lend=86.8°。

為了更清晰、更明確地分析游動傅科導航系統的誤差特性，實驗過程中將其與一些常用慣性導航系統的誤差進行了對比。圖4、圖5、圖6 分別示出了不同機械編排方式下的捷聯慣性導航姿態誤差、速度誤差和位置誤差。由此可見，采用游動傅科機械編排方式進行導航解算后，與指北方位機械編排方式和游移方位機械編排方式相比較，三者的俯仰角和滾轉角誤差、東向和北向速度誤差、經度和緯度誤差結果基本相同。這一性質稱為誤差模型的等價性，即不管慣性導航系統選擇何種平臺坐標系和導航解算坐標系，它們的系統誤差模型變換到同一個坐標系中，都具有相同的方程、相同的自由振蕩和相同的誤差傳播特性［31］。對于捷聯慣性導航系統，以上三種不同機械編排導航方式的誤差源和傳播特性相同，因此表現出相同的位置、姿態和速度誤差特性，仿真結果也驗證了這一點。然而，由于三種機械編排方式下航向角的定義方式不同，航向角誤差存在明顯的差異，但這并不會影響到位置和速度誤差的特性。

圖4 不同編排方式下姿態誤差對比圖Fig.4 Comparison of attitude errors under different mechanization

圖5 不同編排方式下速度誤差對比圖Fig.5 Comparison of velocity errors under different mechanization

圖6 不同編排方式下位置誤差對比圖Fig.6 Comparison of position errors under different mechanization

此外，由圖4（c）和圖6（a）容易看出，以上三種機械力學編排方式的航向角誤差和經度誤差都會隨著緯度的升高逐漸發散。也就是說單純的游動傅科機械力學編排方式仍然無法解決航向角誤差和經度誤差在極點附近發散的問題。

3.2 導航方式切換驗證

本文提出的游動傅科間接極區慣性導航算法旨在確保慣性導航系統全局區域內的一致性，并實現不同導航方式的平滑切換。本節通過仿真示例對比并分析游動傅科捷聯慣性間接極區導航方法、基于橫向地理的慣性導航方法在導航方式切換時的情況。考慮到實際極區導航數據難以獲取；純數學仿真方法只能用于算法可行性的驗證，但難以反映真實環境對于導航方式切換的影響；目前多數研究采用半物理模擬的方法生成極區導航數據。通常將慣性測量組件放置在三軸轉臺上，通過旋轉轉臺模擬載體的角運動，通過數字輸入比力模擬載體的線運動［32］。另一方面，考慮到極區導航數據的主要特征是地球自轉角速度分量幾乎與陀螺儀的天向軸重合，且較短時間內地球自轉角速度的影響很小。因此，在短時間內可以忽略地球自轉角速度的影響，用實驗室內獲得的陀螺數據模擬極區附近的載體角運動。按照上述半物理仿真方法，本文使用某高精度光纖陀螺慣性組件短時間內的靜態實測數據（采樣頻率100 Hz）對導航方式切換過程中的導航性能進行分析。著重比較了游動傅科坐標系和橫向地理坐標系作為導航坐標系時，捷聯慣性導航系統在傳統導航方式和橫向導航方式切換時的姿態解算結果。具體對比結果如圖7所示。

圖7 導航方式切換時的姿態角誤差對比圖Fig.7 Comparison of attitude errors for navigation mode switching

本實驗使用了時長為600 s 的實測數據來評估導航方式切換時的姿態解算結果。圖7展示了游動傅科機械編排（方式1）和指北方位機械編排（方式2）兩種不同的導航方式下的姿態解算誤差。實線部分代表方式1，其全程采用游動傅科坐標系作為導航坐標系。在前300 s內，使用傳統導航信息進行姿態解算；而在后300 s 改為使用橫向導航信息進行姿態解算。虛線部分代表方式2，在前300 s 內采用地理坐標系作為導航坐標系，并使用傳統導航信息進行姿態解算；而在后300 s，導航坐標系切換為橫向地理坐標系，并利用橫向導航信息進行姿態解算。

圖7 結果表明，在前300 s內，兩種方法的姿態解算誤差基本相同，且均能以較高的精度實現姿態解算。然而，當切換到橫向導航方式時，游動傅科慣性導航系統的性能幾乎沒有發生變化，其姿態解算誤差仍然保持在接近0°的水平，而橫向地理慣性導航系統的姿態解算結果則在300 s 內產生了0.005°的累積誤差。造成這種情況的原因在于地理坐標系和橫向地理坐標系之間的切換是不連續的，也就是說它們并不完全重合。這種不連續性導致重新計算姿態矩陣和位置矩陣時會引入誤差，從而影響了姿態解算結果的準確性。本文提出的游動傅科間接極區慣性導航方法能夠在不切換導航坐標系的情況下實現橫向導航解算，避免了上述問題，說明該方法在導航方式切換時具有明顯的優越性。

3.3 極地導航仿真驗證

為驗證本文提出的游動傅科捷聯慣性間接極區導航算法在高緯地區，特別是載體通過極點地區的可行性，利用數值解的形式構造了一條沿經線穿過北極點的飛行軌跡，為所提出的極區間接導航算法進行實驗驗證提供仿真數據。

假設載體的航行軌跡為沿120°E經線向北航行，東向速度和天向速度設為vE=vU=0 m/s，北向速度設為vN=200 m/s，飛行高度h=10 km。航行時間為10 min，初始緯度為L0=89.5°，那么載體會在航行5 min 左右后穿越北極極點。航行過程中始終保持俯仰角、橫滾角、航向角為00。當載體穿越北極點后將朝著遠離極點的方向航行，此時經度λ將從120°E變為60°W，航向角ψ將從0°（靠近極點）變為180°（遠離極點）。則游動傅科捷聯慣性間接極區導航方法的姿態、速度和位置解算結果分別如圖8、圖9、圖10所示。

圖8 游動傅科捷聯慣性間接極區導航的姿態解算結果Fig.8 Attitude solving results of Foucault strapdown inertial indirect polar navigation

圖9 游動傅科捷聯慣性間接極區導航的速度解算結果Fig.9 Velocity solving results of Foucault strapdown inertial indirect polar navigation

圖10 游動傅科捷聯慣性間接極區導航的位置解算結果Fig.10 Position solving results of Foucault strapdown inertial indirect polar navigation

由圖8（a）和圖10（a）可以看出，采用傳統游動傅科捷聯慣性導航算法進行導航解算時，載體穿越北極點前后航向角和經度的解算結果未發生改變（這與實際不符），也就是說傳統游動傅科捷聯慣性導航解算在穿越極點時失效。主要原因在于極點附近的位置矩陣退化，無法分離游動角和經度的值，進一步導致游動角和經度的符號無法確定，影響到航向角和經度的求解。此外，由圖8、圖9、圖10可知，在不改變機械編排方式的前提下，借助橫向導航信息進行導航解算，可間接求出正確的導航信息，并能夠避免在極點處因L→90°出現位置矩陣退化的問題，從而解決了游動傅科機械力學編排方式在極點附近不適用的問題。可見，游動傅科捷聯慣性間接極區導航算法能夠滿足載體高緯度飛行以及穿越極點飛行的導航需求。

由圖8 可以看出，在上述傳統游動傅科慣性導航方式和游動傅科慣性間接極區導航方式下，隨著飛行時間的增加，俯仰角和滾轉角的解算結果變化緩慢，而橫滾角的變化明顯大于俯仰角的變化（俯仰角誤差的量級為10-3，橫滾角誤差的量級為10-2）。這是因為在捷聯慣性導航解算過程中，計算精度和參數初始化誤差會影響到數字平臺計算的準確性，從而導致姿態解算誤差。對于較短時間內的慣性導航解算，這樣的誤差是可以接受的。但對于長期的極區慣性導航，則需要GPS 等外部輔助信息對游動傅科坐標系進行重新對準，從而消除捷聯慣性導航系統數字平臺計算過程中的累積誤差。

4 結論

為了解決傳統慣性導航在極點附近出現的奇異性問題，本文首先分析了游動傅科機械編排慣性導航系統的誤差特性。然而，分析表明該系統存在無法分離位置信息和航向角誤差發散的缺陷。在此基礎上，本文基于游動傅科坐標系和橫向導航方法，設計了一種游動傅科捷聯編排的間接極區導航算法。研究結果表明，該算法在載體接近極點時保持機械編排方式不變，可將導航信息映射到橫向坐標系中，能夠準確計算導航信息，并有效解決了緯度趨近于90°時位置矩陣元素無法分離的問題。由此可見，本文所提方法實現了傳統導航方式和橫向導航方式導航方程的統一，可為高緯度地區以及極點附近的安全航行提供高精度導航信息。