一種基于非線性濾波的重力匹配定位算法?

岳 燦官 斌

（1.武警湖北省總隊 武漢 430070）（2.海裝駐武漢地區第七軍事代表室 武漢 430223）

1 引言

慣性導航系統以陀螺儀和加速度計為核心敏感器件，可以在保證自主性的前提下獲取導航信息，具有數據更新率高、導航信息完整可靠等突出優點［1］。自20世紀70年代起，立足于制造技術和計算機技術的發展成果，慣性導航系統已在軍事打擊、軍事偵察、地質勘探等領域得到大規模應用。同時，隨著高性能慣性器件技術和高可靠系統技術的越發成熟，慣性導航系統必將為生命健康、信息技術、能源、材料等前沿科技熱點的發展提供重要支撐，具備廣闊應用前景［2］。

慣性導航系統需要通過積分運算來實現定位解算，其必定面臨定位誤差長期累積的限制，不利于水下潛航、遠洋導航等對長航時自主定位精度要求較高的領域。針對上述限制，為不破壞系統的自主性，主要包括以下幾種解決方案：陳劍從慣性器件本身出發［3］，通過研究提高陀螺儀基本性能的技術，直接改進慣性器件性能來提高慣導系統精度，從源頭抑制慣導系統誤差發散趨勢，但其面臨性能提升瓶頸、研發周期長等限制；Zhi-Liang Deng將經典控制理論引入慣導系統的姿態更新過程［4］，采用系統誤差阻尼策略，可以有效改善系統振蕩誤差特性，實現周期振蕩相關誤差的有效抑制，但無法解決陀螺漂移引起的累積性位置誤差；劉生攀根據慣導系統誤差特性［5］，將旋轉調制技術引入慣性導航系統，通過特殊的調制轉位設計可以抵消器件的對稱性誤差，能夠在不提高慣性器件性能的前提下大幅提高整體定位精度，是提升系統精度的有效手段，但無法從根本上解決位置誤差漂移問題；YF Chen將實測重力相關信息作為觀測量［6］，利用重力匹配技術來實現系統誤差的修正，從而有效重調系統初始位置，能夠從根本上解決定位誤差長期累積問題，具有良好的研究價值和應用價值。

綜上所述，針對長航時慣性導航系統面臨的位置誤差發散問題，本文以重力匹配為核心思想，提出一種基于非線性濾波的重力匹配定位算法。該算法基于慣導系統誤差模型，將重力信息作為觀測量，并結合非線性濾波技術和量測噪聲自適應技術，能夠實時估計系統的定位誤差。半物理仿真結果表明，本文提出的重力匹配定位方法可以實現0.61海里/20天的定位精度（3σ）。

2 重力匹配架構

重力匹配導航是以重力測量為基礎發展起來的無源導航，這類導航方式首先需要制備離線重力數據庫，然后利用搭載在運載體上的重力敏感儀器實測重力場，再采用有效的匹配算法獲取基于實測重力場對應的位置信息，最終修正載體位置誤差，如圖1所示。可以看出，研制高精度重力測量設備、構建高分辨率重力數據庫以及設計有效重力匹配算法是重力匹配導航的三大關鍵技術［7～9］，其中有效重力匹配算法作為實現重力匹配定位的核心，其性能將直接影響總體精度。

圖1 重力匹配流程圖

3 一種基于非線性濾波的重力匹配定位算法

3.1 濾波模型構建

3.1.1 系統狀態模型

根據慣導系統誤差傳播特性［10］，選取13維狀態變量來描述系統特征：

式中，狀態變量按先后順序為經度誤差、緯度誤差、東向速度誤差、北向速度誤差、東向姿態誤差角、北向姿態誤差角、天向姿態誤差角、x方向加速度計偏置、y方向加速度計偏置、z方向加速度計偏置、x方向陀螺儀偏置、y方向陀螺儀偏置，z方向陀螺儀偏置。根據上述狀態變量，選取狀態模型為

式（2）中，w為取決于器件性能的系統噪聲；f(X，t)為狀態模型解析式，其具體形式為

3.1.2 非線性觀測模型構建

重力匹配過程中，需要利用觀測模型來表示實測重力測量值和載體位置誤差之間的關系，觀測模型通用形式如下：

式中，Y(t)為觀測量，h(X，t)為觀測模型，r(t)為觀測噪聲。首先令觀測量Y(t)的物理含義為

式中，λINS，LINS分別是源自慣導的經度和緯度；gmap(，)表示重力數據庫輸出函數（本質上是某種插值或擬合方法）；gmap(λINS，LINS)表示按照經度為λINS且緯度為LINS，由重力數據庫和重力數據庫輸出函數獲取的重力信息；gmeasure是載體上重力測量設備獲取的重力信息。所以，為配合Y(t)的物理含義，式（12）中h(X，t)的解析形式為

將式（12）、式（13）帶入式（11），得到量測方程的最終形式：

基于式（14）可知，量測方程取決于重力數據庫輸出函數gmap(λINS-δλ，LINS-δL)和量測噪聲r(t)。

3.2 擴展卡爾曼濾波

擴展卡爾曼濾波是卡爾曼濾波的次優估計形式，適用于濾波模型中存在非線性的情況［11］。已知某系統的非線性狀態方程和觀測方程為

式中，X(t)為系統狀態量，Z(t)為觀測量，f[X(t)，t]為非線性系統模型，h[X(t)，t]為系統觀測模型，G(t)為噪聲驅動矩陣，w(t)為過程噪聲，v(t)為觀測噪聲。根據泰勒展開，對式（15）進行線性化近似，得到狀態變化的系統模型：

再根據狀態變化的離散系統模型，得到狀態估計的濾波基本方程：

3.3 重力數據庫輸出函數構建

由3.1.2節可知，重力數據庫輸出函數gmap(，)是構造非線性觀測模型的核心。有學者［12］將高程信息用多項式曲面擬合，得到了較好的應效果。考慮到重力數據和高程信息具有一定的相關性，本文采用3次多項式曲面來重構重力數據庫，從而完成重力數據庫輸出函數gmap(，)的構建，數據庫輸出函數模型為

式中，z為重力信息，a，b，c，d，e，f，g，h，i，j為帶擬合系數，x為目標位置的經度，y為目標位置的緯度。

4 半物理仿真

4.1 仿真條件設置

4.1.1 慣性數據設置

為驗證本文提出算法的有效性，本文采用某慣導設備的動態海測數據進行半物理仿真，數據詳細信息如表1所示。仿真過程中，利用海測數據進行初始對準及無阻尼純慣性導航解算，為重力匹配定位算法提供基本的導航參數，同時，基于衛星導航位置信息，評價算法的匹配定位精度。

表1 仿真數據說明

4.1.2 重力數據設置

重力數據設置包括數據庫和實測重力數據生成兩方面。本文選用加利福尼亞大學于2014年發布的海洋重力數據庫進行仿真，數據庫具有1′的分辨率，由CryoSat-2衛星和Jason-1衛星的測高數據以及已有重力數據融合而成，具有相對更高的精度。同時，加利福尼亞大學給出了該數據庫詳細下載地址，利于實現重力匹配仿真驗證。而且，為模擬獲取用于重力匹配的實測重力信息，本文采用衛星導航位置信息和3.3節所述的重力數據庫輸出函數來仿真生成實測重力信息，并在仿真生成的實測重力信息上疊加標準差為1mGal的噪聲來模擬真實數據。

4.2 仿真結果

進行數據時長約為20天的半物理仿真，得到匹配算法定位誤差和純慣性定位誤差的對比圖，如圖2和圖3所示，且各誤差的統計結果如表2所示。

圖2 經度誤差仿真結果

圖3 緯度誤差仿真結果

表2 仿真結果統計

由表2可以看出，本文所提出的重力匹配算法具有0.48海里/20天（3σ）的緯度誤差和0.38海里/天的經度誤差（3σ），優于純慣導定位，且無明顯發散跡象。

5 結語

重力匹配作為一種可以在不破壞慣導自主性條件下重調慣導系統位置的有效手段，已經逐步成為了長航時自主導航的發展熱點。為此，本文瞄準重力匹配的應用發展方向，提出了一種基于非線性濾波的重力匹配定位算法，該算法依據重力信息具備的空間相關性，構造系統狀態模型和非線性量測方程，并結合擴展卡爾曼濾波技術和三次曲面擬合技術，完成對慣導系統位置誤差的有效估計。半物理仿真結果表明，本文提出的重力匹配定位算法可以實現0.61海里/20天（3σ）的定位精度，且無明顯發散跡象。