基于非線性最小二乘估計(jì)的eLoran/INS/磁傳感器組合導(dǎo)航方法

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

（1.中國(guó)人民解放軍 92578 部隊(duì)，北京 100089；2.中國(guó)船舶集團(tuán)有限公司 低頻電磁通信技術(shù)實(shí)驗(yàn)室，武漢市 430205；3.武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心,武漢 430072）

近年來(lái)由于全球衛(wèi)星導(dǎo)航系統(tǒng)（GNSS）的普及，在現(xiàn)代定位、導(dǎo)航和授時(shí)（PNT）體系內(nèi)GNSS 占據(jù)了重要地位，在覆蓋范圍、精度和使用成本上相對(duì)其它導(dǎo)航系統(tǒng)具有明顯的綜合優(yōu)勢(shì)。但由于其脆弱性和易受干擾的缺點(diǎn)，過(guò)度依賴GNSS 的PNT 體系在社會(huì)的經(jīng)濟(jì)安全、生產(chǎn)安全和國(guó)防安全方面均存在巨大隱患。因此，世界各國(guó)均在尋求建立新一代的PNT 體系。

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

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

因此，為保證羅蘭導(dǎo)航系統(tǒng)的精度，必須設(shè)法修正這些誤差。其中，空間因素導(dǎo)致的傳播延遲可以通過(guò)構(gòu)造ASF 地圖[6]來(lái)校準(zhǔn)，緩慢變化的時(shí)變誤差可以通過(guò)建設(shè)差分站（differential station）來(lái)實(shí)現(xiàn)抑制，但由于受限于羅蘭數(shù)據(jù)通道的速率僅40 bps，對(duì)于站點(diǎn)的ASF 更新速度往往有一定的滯后性，故在實(shí)際使用中由于遠(yuǎn)處的雷電等大氣噪聲或其它隨機(jī)瞬時(shí)因素導(dǎo)致的快速變化的噪聲需要通過(guò)其它方法抑制。

在導(dǎo)航領(lǐng)域，組合導(dǎo)航通常通過(guò)融合幾種不同類型的傳感器來(lái)實(shí)現(xiàn)，這些傳感器通常具有完全不同的特性，通過(guò)傳感器相互之間取長(zhǎng)補(bǔ)短實(shí)現(xiàn)導(dǎo)航精度的提升。在以往的研究中[7]，大多數(shù)組合導(dǎo)航系統(tǒng)通常采用GNSS 系統(tǒng)與其它導(dǎo)航方式實(shí)現(xiàn)，因此，在建立獨(dú)立于GNSS 的PNT 系統(tǒng)的目標(biāo)指引下，新的更加泛在、智能、融合的不依賴GNSS 的組合導(dǎo)航系統(tǒng)在理論和實(shí)踐中都具有重要意義。

1 eLoran/INS/磁傳感器組合導(dǎo)航系統(tǒng)算法基本原理

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

本文采用eLoran/INS/磁傳感器三個(gè)分系統(tǒng)組成整個(gè)導(dǎo)航系統(tǒng)，其中 eLoran 系統(tǒng)可以提供位置，其差分信息還可以提供地理坐標(biāo)系中航向φ和速度信息；INS 系統(tǒng)可以提供載體坐標(biāo)系中角速度和加速度信息，下標(biāo)s代表載體坐標(biāo)系；磁傳感器可提供載體坐標(biāo)系內(nèi)測(cè)量到的磁場(chǎng)的信息。

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

首先，利用陀螺儀測(cè)量載體坐標(biāo)系中載體旋轉(zhuǎn)的角速度。由于直接利用該角速度計(jì)算的姿態(tài)角隨時(shí)間發(fā)散，因此，需要增加其它參考矢量來(lái)克服此問(wèn)題。故利用加速度計(jì)測(cè)量在載體坐標(biāo)系內(nèi)的重力矢量，該方位對(duì)應(yīng)于載體的橫滾角和俯仰角，再利用磁傳感器測(cè)量地磁場(chǎng)在載體坐標(biāo)系內(nèi)的方位，該方位對(duì)應(yīng)于載體的航向角，由這兩個(gè)傳感器的測(cè)量結(jié)果與傳感器當(dāng)前估計(jì)姿態(tài)反推的重力矢量與磁場(chǎng)矢量之間的最小二乘解，計(jì)算出姿態(tài)角的修正量，這樣就可以克服姿態(tài)角發(fā)散的問(wèn)題。同時(shí)磁傳感器和加速度傳感器噪聲較大，因此反過(guò)來(lái)也需要利用陀螺儀數(shù)據(jù)與它們?nèi)诤希梢杂行Ы档瓦@兩個(gè)傳感器的噪聲對(duì)姿態(tài)角的影響，有效提高姿態(tài)角解算精度。

然后，利用載體當(dāng)前姿態(tài)和重力矢量，分離加速度計(jì)中運(yùn)動(dòng)產(chǎn)生的附加加速度，并對(duì)該加速度進(jìn)行積分，得到在載體坐標(biāo)系內(nèi)速度的改變量。根據(jù)載體當(dāng)前速度和姿態(tài)，可以得到載體位置的改變量。但是由于加速度計(jì)的噪聲、計(jì)算誤差等原因?qū)е碌乃俣日`差會(huì)隨時(shí)間累積，這導(dǎo)致位置的誤差會(huì)隨時(shí)間發(fā)散，因此，需要通過(guò)eLoran 導(dǎo)航系統(tǒng)提供的位置和估計(jì)的位置之間的最小二乘解，來(lái)對(duì)計(jì)算得到的速度和位置進(jìn)行修正。

最后，為了提高組合導(dǎo)航系統(tǒng)的實(shí)時(shí)解算的精度，需要計(jì)算速度和角速度的零偏誤差，因?yàn)樵撜`差既產(chǎn)生長(zhǎng)期的累積誤差，又會(huì)對(duì)單次計(jì)算產(chǎn)生影響。由于慣性器件的特性，該誤差相比高頻噪聲，是隨機(jī)且緩慢變化的低頻誤差，在計(jì)算中表現(xiàn)為姿態(tài)和位置修正量的低頻分量，因此，通過(guò)對(duì)修正量進(jìn)行低通濾波可以得到速度和角速度的零偏。

2 導(dǎo)航算法與數(shù)學(xué)推導(dǎo)

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

式中，下標(biāo)ω代表該估計(jì)量來(lái)自于陀螺儀的輸出。再利用加速度傳感器數(shù)據(jù)計(jì)算的速度可以得到位置的估計(jì)量：

式中，下標(biāo)a代表該量來(lái)自于加速度計(jì)的輸出，載體狀態(tài)的增量和估計(jì)量分別為：

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

其雅克比行列式為：

其雅克比行列式為：

整體的誤差函數(shù)可以構(gòu)造為：

其雅克比行列式為：

根據(jù)文獻(xiàn)[8]，基于高斯-牛頓法計(jì)算這個(gè)誤差的最小二乘解的載體狀態(tài)?s遞推公式中的遞推量為：

式中，下標(biāo)n代表歸一化，完整的遞推公式為：

其中，μ是收斂步長(zhǎng)因子，該參數(shù)需要根據(jù)傳感器的性能進(jìn)行調(diào)整[8]。

在實(shí)際情況中，磁傳感器和INS 器件都存在誤差。磁傳感器的軟磁和硬磁誤差可通過(guò)文獻(xiàn)[10]中提到的方法進(jìn)行校準(zhǔn)。首先，通過(guò)測(cè)量各個(gè)方向上的磁場(chǎng)，如圖1所示的三維球面是一組在載體上的磁傳感器實(shí)測(cè)的結(jié)果。然后，利用式(17)中磁傳感器誤差模型，求解軟磁和硬磁校準(zhǔn)參數(shù)的最小二乘解來(lái)校準(zhǔn)磁傳感器：

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

圖2 校準(zhǔn)后磁傳感器三維數(shù)據(jù)Fig.2 3D data of magnetic sensor after calibration

加速度計(jì)和陀螺儀的對(duì)載體狀態(tài)的計(jì)算結(jié)果存在累積誤差，因此，需要通過(guò)位置和姿態(tài)數(shù)據(jù)進(jìn)行補(bǔ)償，由于陀螺儀的零偏誤差為一個(gè)緩變量，因此可以通過(guò)對(duì)估計(jì)姿態(tài)與測(cè)量姿態(tài)之間的誤差低通濾波得到：

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

因此，校準(zhǔn)后的角速度和速度分別為：

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

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

3 仿真試驗(yàn)結(jié)果及分析

為驗(yàn)證上述算法，我們采用了文獻(xiàn)[1]所提供的eLoran 誤差模型，為了更貼合實(shí)際使用情況，選擇距離eLoran 臺(tái)站較遠(yuǎn)的區(qū)域，環(huán)境中隨機(jī)干擾較大，定位誤差達(dá)到100 m（2σ），遠(yuǎn)大于GNSS 系統(tǒng)的定位精度。

INS模塊仿真采用了湖北三江紅峰公司的HZ-1 型光纖慣性測(cè)量系統(tǒng)的參數(shù)，產(chǎn)品指標(biāo)如表1所示。磁傳感器采用PNI 公司的RM3100 三軸磁場(chǎng)傳感器，其噪聲RMS 大小為15 nT。

表1 HZ-1 光纖慣性測(cè)量產(chǎn)品指標(biāo)Tab.1 HZ-1 Optical Fiber Inertial Measurement Index

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

圖4 仿真航線、eLoran 導(dǎo)航定位與組合導(dǎo)航結(jié)果對(duì)比Fig.4 Comparison of simulation route,eLoran navigation and positioning and integrated navigation results

可見(jiàn)組合導(dǎo)航系統(tǒng)相對(duì)單純的eLoran 導(dǎo)航系統(tǒng)與仿真航線重合度高，相對(duì)慣性導(dǎo)航其誤差不隨時(shí)間發(fā)散，可以在連續(xù)機(jī)動(dòng)的條件下保持定位精度。其誤差分布如圖5所示，其定位誤差（2 σ）從eLoran 的100 m降低至6.1 m。

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

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

圖6 速度零偏曲線和速度零偏補(bǔ)償曲線對(duì)比Fig.6 Comparison of velocity offset curve and velocity offset compensation curve

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

4 結(jié)論

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