基于非線性最小二乘估計的eLoran/INS/磁傳感器組合導航方法

閆泓衫，曹 宜，張楊勇，曾歌明，江 鵬

（1.中國人民解放軍 92578 部隊，北京 100089；2.中國船舶集團有限公司 低頻電磁通信技術實驗室，武漢市 430205；3.武漢大學 衛星導航定位技術研究中心,武漢 430072）

近年來由于全球衛星導航系統（GNSS）的普及，在現代定位、導航和授時（PNT）體系內GNSS 占據了重要地位，在覆蓋范圍、精度和使用成本上相對其它導航系統具有明顯的綜合優勢。但由于其脆弱性和易受干擾的缺點，過度依賴GNSS 的PNT 體系在社會的經濟安全、生產安全和國防安全方面均存在巨大隱患。因此，世界各國均在尋求建立新一代的PNT 體系。

羅蘭導航系統是一種起源于二次世界大戰期間的雙曲無線導航系統，其有效范圍可達2000 km 以上，可穿透一定深度的水體和山體，解決了GNSS 系統無法在室內和水下提供導航的問題，提供了比GNSS 系統更廣泛的應用范圍。其后續升級的增強型羅蘭(eLoran)無線電導航系統普遍被認為是GPS 的最佳代替[1]。英國在歐洲西北部通過布設差分站，建立了eLoran 導航系統[1]并通過測試得出該系統的定位精度最高可達5 m，授時精度可達幾個ns[3]，為了應對GPS干擾，韓國也計劃建設高精度的羅蘭導航系統[4],[5]。

羅蘭導航系統的誤差主要來源于傳播路徑上各種因素導致的到達時間（TOA）的延遲誤差，可分為時變延遲和空間延遲兩大類，其主要成因是由于傳播路徑上電導率的瞬時變化（ASF 時變）和空間變化（ASF空間）導致的二次附加因子（ASF）的變動，如式(1)所示，若不進行校準，這些誤差可導致羅蘭接收機解算的位置與實際地點偏離達到數公里。

因此，為保證羅蘭導航系統的精度，必須設法修正這些誤差。其中，空間因素導致的傳播延遲可以通過構造ASF 地圖[6]來校準，緩慢變化的時變誤差可以通過建設差分站（differential station）來實現抑制，但由于受限于羅蘭數據通道的速率僅40 bps，對于站點的ASF 更新速度往往有一定的滯后性，故在實際使用中由于遠處的雷電等大氣噪聲或其它隨機瞬時因素導致的快速變化的噪聲需要通過其它方法抑制。

在導航領域，組合導航通常通過融合幾種不同類型的傳感器來實現，這些傳感器通常具有完全不同的特性，通過傳感器相互之間取長補短實現導航精度的提升。在以往的研究中[7]，大多數組合導航系統通常采用GNSS 系統與其它導航方式實現，因此，在建立獨立于GNSS 的PNT 系統的目標指引下，新的更加泛在、智能、融合的不依賴GNSS 的組合導航系統在理論和實踐中都具有重要意義。

1 eLoran/INS/磁傳感器組合導航系統算法基本原理

傳統的多傳感器信息融合導航算法通常采用卡爾曼濾波算法及其各種改進方法，該方法利用預測-測量模型可以有效融合不同傳感器的信息從而提高系統的準確度和適應性，但這些算法在傳感器數量較多時，運算量大、占用資源多，不利于設備的低功耗與小型化。因此，針對這個問題Madgwick 等人[8],[9]提出了基于梯度下降算法的組合姿態估計算法，該方法具有運算量小，占用資源少，解算精度高的特點。該算法本質上是利用非線性最小二乘法尋找預測姿態與傳感器測量姿態之間殘差最小的解，但由于簡單的梯度下降算法收斂慢，導致初始姿態對準慢，快速運動時誤差增大的問題。因此，采用收斂速度更高的高斯-牛頓算法，可以在保持梯度下降算法優點的前提下提高系統在各種環境下的適應能力。

本文采用eLoran/INS/磁傳感器三個分系統組成整個導航系統，其中 eLoran 系統可以提供位置，其差分信息還可以提供地理坐標系中航向φ和速度信息；INS 系統可以提供載體坐標系中角速度和加速度信息，下標s代表載體坐標系；磁傳感器可提供載體坐標系內測量到的磁場的信息。

本文所述算法的主要原理是，利用多個傳感器對載體的姿態、位置或與它們相關的物理量進行測量，由于各個傳感器存在噪聲和誤差，因此，在實際狀況下，載體的姿態和最可能的位置，可以認為是那個使得傳感器測量到的物理量與利用載體姿態反推的物理量之間誤差最小的非線性最小二乘解。

首先，利用陀螺儀測量載體坐標系中載體旋轉的角速度。由于直接利用該角速度計算的姿態角隨時間發散，因此，需要增加其它參考矢量來克服此問題。故利用加速度計測量在載體坐標系內的重力矢量，該方位對應于載體的橫滾角和俯仰角，再利用磁傳感器測量地磁場在載體坐標系內的方位，該方位對應于載體的航向角，由這兩個傳感器的測量結果與傳感器當前估計姿態反推的重力矢量與磁場矢量之間的最小二乘解，計算出姿態角的修正量，這樣就可以克服姿態角發散的問題。同時磁傳感器和加速度傳感器噪聲較大，因此反過來也需要利用陀螺儀數據與它們融合，可以有效降低這兩個傳感器的噪聲對姿態角的影響，有效提高姿態角解算精度。

然后，利用載體當前姿態和重力矢量，分離加速度計中運動產生的附加加速度，并對該加速度進行積分，得到在載體坐標系內速度的改變量。根據載體當前速度和姿態，可以得到載體位置的改變量。但是由于加速度計的噪聲、計算誤差等原因導致的速度誤差會隨時間累積，這導致位置的誤差會隨時間發散，因此，需要通過eLoran 導航系統提供的位置和估計的位置之間的最小二乘解，來對計算得到的速度和位置進行修正。

最后，為了提高組合導航系統的實時解算的精度，需要計算速度和角速度的零偏誤差，因為該誤差既產生長期的累積誤差，又會對單次計算產生影響。由于慣性器件的特性，該誤差相比高頻噪聲，是隨機且緩慢變化的低頻誤差，在計算中表現為姿態和位置修正量的低頻分量，因此，通過對修正量進行低通濾波可以得到速度和角速度的零偏。

2 導航算法與數學推導

首先，定義載體在時刻t的狀態矢量為，位置矢量，姿態四元數，可表示為位置矢量和姿態四元數的函數：

式中，下標ω代表該估計量來自于陀螺儀的輸出。再利用加速度傳感器數據計算的速度可以得到位置的估計量：

式中，下標a代表該量來自于加速度計的輸出，載體狀態的增量和估計量分別為：

注意，式中由于eLoran 系統通常不提供高度信息，因此，位置的增量需要將高度方向上的數值置零，與四元數進行運算時需要將該矢量填充為四維矢量，在式(6)計算完成后將第一項和第四項，置零并去掉，即可得到：速度的增量也需要用相同的方法進行處理。

其雅克比行列式為：

其雅克比行列式為：

整體的誤差函數可以構造為：

其雅克比行列式為：

根據文獻[8]，基于高斯-牛頓法計算這個誤差的最小二乘解的載體狀態?s遞推公式中的遞推量為：

式中，下標n代表歸一化，完整的遞推公式為：

其中，μ是收斂步長因子，該參數需要根據傳感器的性能進行調整[8]。

在實際情況中，磁傳感器和INS 器件都存在誤差。磁傳感器的軟磁和硬磁誤差可通過文獻[10]中提到的方法進行校準。首先，通過測量各個方向上的磁場，如圖1所示的三維球面是一組在載體上的磁傳感器實測的結果。然后，利用式(17)中磁傳感器誤差模型，求解軟磁和硬磁校準參數的最小二乘解來校準磁傳感器：

圖1 磁傳感器原始三維數據Fig.1 Raw 3D data of magnetic sensor

圖2 校準后磁傳感器三維數據Fig.2 3D data of magnetic sensor after calibration

加速度計和陀螺儀的對載體狀態的計算結果存在累積誤差，因此，需要通過位置和姿態數據進行補償，由于陀螺儀的零偏誤差為一個緩變量，因此可以通過對估計姿態與測量姿態之間的誤差低通濾波得到：

同理，速度的偏置在計算中體現為對位置估計的一個緩變的誤差量，可以利用估計位置與eLoran 導航系統給出的位置之差，即式(15)中位置的修正量，再除以時間間隔，并進行低通濾波得到，式(19)中為載體坐標系中速度的修正量，為陀螺儀零偏，δ為常數，控制低通濾波器的帶寬。

因此，校準后的角速度和速度分別為：

綜上所述，該算法完整的流程圖如圖3所示。

圖3 基于非線性最小二乘估計的組合導航算法流程圖Fig.3 Flow chart of integrated navigation algorithm based on nonlinear least squares estimation

3 仿真試驗結果及分析

為驗證上述算法，我們采用了文獻[1]所提供的eLoran 誤差模型，為了更貼合實際使用情況，選擇距離eLoran 臺站較遠的區域，環境中隨機干擾較大，定位誤差達到100 m（2σ），遠大于GNSS 系統的定位精度。

INS模塊仿真采用了湖北三江紅峰公司的HZ-1 型光纖慣性測量系統的參數，產品指標如表1所示。磁傳感器采用PNI 公司的RM3100 三軸磁場傳感器，其噪聲RMS 大小為15 nT。

表1 HZ-1 光纖慣性測量產品指標Tab.1 HZ-1 Optical Fiber Inertial Measurement Index

仿真的航線為在海平面內的大角度機動和直線行駛的組合，如圖4所示，所有仿真數據均基于該航行軌跡，再結合傳感器噪聲模型得到，仿真的載體狀態的初始值的位置和姿態誤差為：偏東100 m，偏北100 m，航向角偏北45 °。圖中紅色的曲線為仿真航線，綠色的圓點代表eLoran 導航定位數據，橙色的曲線為組合導航系統輸出的航行曲線，黑色曲線為慣性導航的輸出結果。

圖4 仿真航線、eLoran 導航定位與組合導航結果對比Fig.4 Comparison of simulation route,eLoran navigation and positioning and integrated navigation results

可見組合導航系統相對單純的eLoran 導航系統與仿真航線重合度高，相對慣性導航其誤差不隨時間發散，可以在連續機動的條件下保持定位精度。其誤差分布如圖5所示，其定位誤差（2 σ）從eLoran 的100 m降低至6.1 m。

圖5 eLoran/INS/磁傳感器組合導航定位誤差與eLoran 定位誤差對比Fig.5 Comparison of eLoran/INS/magnetic sensor integrated navigation positioning error and eLoran positioning error

根據式(18)(19)估計的角速度和速度零點偏移的補償值曲線與實際零點偏移曲線的對比如圖6所示。對二維平面運動影響最大的Z 軸角速度的零點偏移的補償值曲線與陀螺零點偏移曲線如圖7所示，從圖中可以看出，本算法可以有效地補償由于慣性器件的特性導致的角度和速度的零點偏移，從而提高組合導航系統的定位精度。

圖6 速度零偏曲線和速度零偏補償曲線對比Fig.6 Comparison of velocity offset curve and velocity offset compensation curve

圖7 陀螺儀輸出z 軸零偏曲線和z 軸角速度零偏補償曲線對比Fig.7 Comparison of gyroscope's z-bias curve and z-angular velocity compensation curve

4 結論

基于非線性最小二乘估計的eLoran/INS/磁傳感器組合導航與單獨的eLoran 導航定位相比，通過多種傳感器數據融合，提高了信息的冗余度，通過陀螺儀、加速度計和磁傳感器給出的姿態和速度信息對當前位置的變化量做出估計，由于INS/磁傳感器系統短時的穩定性好，因而可以有效抑制eLoran 導航系統由于噪聲干擾導致的定位誤差，證明了eLoran 導航定位系統在與INS/磁傳感器進行組合后可在eLoran 導航系統有效的作用范圍內代替GNSS 定位系統成為可靠的高精度導航定位手段，其穿透力強，可在水下、掩體內、密林中使用，不易受人為蓄意干擾的特點，使其對我國軍事和社會經濟領域均具備重要的現實意義。