邊緣CPF算法及在重力梯度輔助導(dǎo)航中應(yīng)用

（1. 哈爾濱工程大學(xué) 理學(xué)院，哈爾濱 150001；2. 哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001）

（1. 哈爾濱工程大學(xué) 理學(xué)院，哈爾濱 150001；2. 哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001）

慣導(dǎo)固有原因使得載體長時間航行累積大量誤差?？赏ㄟ^重力梯度量測與慣導(dǎo)組合導(dǎo)航方法來修正導(dǎo)航誤差。先對重力梯度儀與慣導(dǎo)組合導(dǎo)航原理進行闡述，提出重力梯度儀輔助 INS (GAINS)的系統(tǒng)框架圖，對導(dǎo)航用重力梯度圖和重力梯度儀進行分析，設(shè)定組合量測方程。然后根據(jù)狀態(tài)空間方程的特點，提出使用邊緣Cubature粒子濾波(CPF)進行融合估值。通過理論方法證明其對方差的減小，同時給出算法流程。相同條件下與已有APO-PF算法仿真進行經(jīng)緯度RMSE結(jié)果對比，表明該算法估值精度更高；并用CEP對導(dǎo)航誤差研究，得到在性能較低的慣導(dǎo)條件下、在梯度儀1E2和10 E2噪聲下4h的CEP數(shù)值分別為0.044 n mile和0.072 n mile的結(jié)果。最后對狀態(tài)方程簡化，定性分析出其余狀態(tài)量的估值效果。

重力梯度儀輔助慣性導(dǎo)航系統(tǒng)；邊緣濾波；狀態(tài)分解；Cubature粒子濾波；圓誤差概率

精確可靠的導(dǎo)航系統(tǒng)是運載器航行關(guān)鍵技術(shù)。潛艇長時間水下航行會累積導(dǎo)航誤差，而常用的矯正手段GPS信號在水下無法接收，所以需要增加其它的輔助手段?，F(xiàn)在有重力/慣性、地形/慣性組合均為研究熱點。但理想導(dǎo)航系統(tǒng)工作將是無源的、自動的，不需要發(fā)射和接收信號，其中，重力儀和重力梯度儀量測完全是無源的，不易受到外界干擾源干擾，且重力場空間分布特征比較穩(wěn)定。因此重力和重力梯度輔助導(dǎo)航是水下無源輔助導(dǎo)航的重要方法。

重力輔助導(dǎo)航目前廣泛采用的是基于最近等值線點匹配的(ICCP)方法，然而這種方法是在航行一段時間后對狀態(tài)調(diào)整的方法，并且在較大初始誤差下就發(fā)散[1]，即便不發(fā)散也常會收斂到局部極小。重力的濾波方法由于重力圖在重力變化平緩區(qū)域的相似性，經(jīng)常造成誤匹配而誤差較大。與重力異常相比，重力梯度能反映重力場(重力加速度矢量)全空間變化率，有5個相互獨立的張量。從技術(shù)發(fā)展角度看，重力量測很難再獲得更高分辨率的重力場特征，重力梯度測量則可進一步提高重力場測量的精度和空間分辨率，量測高精度必然使組合導(dǎo)航精度進一步提高。高精度重力梯度圖的實現(xiàn)目前在海洋測量領(lǐng)域越來越成熟，逼近程度也越來越精細。

已有重力梯度融合研究較少。非線性濾波方法主要有基于EKF的SITAN算法[2]，但該算法在遠離真值的位置進行線性化時經(jīng)常發(fā)散；還有使用UKF直接進行非線性傳遞[3]，但UKF使用Sigma樣本點數(shù)量有限，并不能總是包含真正位置點。近期人工物理優(yōu)化濾波方法被提出，它采用粒子濾波方法，通過調(diào)整在重采樣階段粒子間作用力，改變狀態(tài)后驗分布，克服粒子退化問題，取得了較穩(wěn)定效果，位置定位誤差在140 m左右[4]。雖然誤差仍然較大，但說明非線性粒子濾波的可行性。

1 重力梯度輔助導(dǎo)航

重力梯度是重力加速度g的全空間導(dǎo)數(shù)，也是重力勢的二階導(dǎo)數(shù)[5]，單位為厄特弗斯，用E或EU表示，1 E= 1 mGal/10 km。由于重力梯度具有對稱性和泊松方程的存在，所以實際上有五個獨立的張量Γij(i=x,y,z;j=x,y,z)，分別對應(yīng)不同方向上重力梯度分量。相比于重力異常，它包含了重力場高階分量，“區(qū)別”和“特征”也更明顯。已有重力異常濾波輔助導(dǎo)航定位精度不高，在300～500m之間，就是重力異常包含信息量少的緣故。

忽略垂直通道誤差，GAINS導(dǎo)航原理說明如下：INS加速度計量測載體想要的加速度里含有重力分量，其中法向分量能夠計算補償，但異常分量無法獲取，所以這時的加速度由于加速度計與陀螺儀誤差使誤差隨時間的增長而發(fā)散。

重力梯度儀獨立于運載器加速度而量測重力場，不含重力儀的厄特弗斯效應(yīng)，所以其值可用來糾正慣導(dǎo)加速度計輸出中異常重力分量。重力梯度儀結(jié)合 INS的導(dǎo)航性能相比于傳統(tǒng)純慣導(dǎo)情形有較大幅度提高。但由于慣導(dǎo)和重力梯度儀誤差，無圖情況下GAINS的東向位置誤差仍然會有不斷增長的情況。

重力梯度圖獲得條件下，實現(xiàn)慣導(dǎo)與重力梯度圖匹配模式，這種情況下能將實時采集的重力場值和組合系統(tǒng)提供的指示位置在圖上得到的計算值做差比較，通過相應(yīng)濾波算法給出載體實時位置估計，以來限定位置和速度誤差。所以圖匹配模式是被廣泛研究的組合方式[2-4]。

圖模式間接反饋的系統(tǒng)框架圖如圖1所示。

圖1 慣導(dǎo)/重力梯度儀導(dǎo)航結(jié)構(gòu)圖Fig.1 Configuration for INS/gravity gradiometer navigation

可見 GAINS系統(tǒng)三個重要組成部分為重力梯度數(shù)據(jù)庫、重力梯度儀和數(shù)據(jù)融合算法。

導(dǎo)航重力梯度庫圖需事先存儲裝配到載體上，一般采用離散網(wǎng)格化的形式存儲，量值與位置坐標(biāo)一一對應(yīng)。當(dāng)前輔助定位基準(zhǔn)梯度圖采用實際量測完成的較少，主要是通過理論方法逼近重力梯度。數(shù)字高程模型(DEM)正演重力梯度是理論方法逼近重力梯度一種重要形式。DEM主要下載自美國國家地球物理數(shù)據(jù)中心，它通過假定地面密度常值獲得重力場。另外一種是通過自由空間重力異常轉(zhuǎn)化。由于自由空間重力異常包含了周圍地形和地下介質(zhì)共同作用引起的重力梯度，特征更明顯。若自由空間重力異常數(shù)據(jù)分辨率較低時，單獨構(gòu)圖反映不了梯度高頻特征，這時可與DEM聯(lián)合構(gòu)圖能取得精細效果[6]。本文所用導(dǎo)航梯度圖示例如圖3所示。

GAINS的重力梯度儀當(dāng)前主要使用的是貝爾公司的一種旋轉(zhuǎn)加速度計重力梯度儀。它安裝在慣導(dǎo)組件平臺上一個緩慢旋轉(zhuǎn)的固定平臺裝置上，每一個重力梯度儀包含4～8個加速度計，加速度計垂直于旋轉(zhuǎn)軸，其值通過對提取加速度計信號放大處理獲取。為了量測5個獨立重力梯度分量，需要3個重力梯度儀，單重力梯度儀只能量測重力梯度中的兩個分量。

目前國外重力梯度儀有較快的發(fā)展和較高的精度。與國外相比，國內(nèi)部分科研院所從2000年開始對國外重力梯度儀及相關(guān)技術(shù)跟蹤論證和實驗研究。但由于技術(shù)復(fù)雜，從目前已有文獻看，國內(nèi)還不具備制造能實際使用有較高精度重力梯度儀的能力。

融合就是濾波部分。融合算法本文仍然以粒子濾波為基礎(chǔ)，根據(jù)重力梯度導(dǎo)航系統(tǒng)狀態(tài)方程線性、量測方程是狀態(tài)量非線性特點，提出采用邊緣Cubature粒子濾波進行狀態(tài)估計。CKF算法與粒子濾波結(jié)合在導(dǎo)航上能使粒子群更快向高似然區(qū)移動。

2 邊緣Cubature 粒子濾波

2.1 Rao-Blackwell理論

邊緣濾波又稱Rao-Blackwellised濾波，它是根據(jù)統(tǒng)計上的Rao-Blackwell理論[7]。該理論說明統(tǒng)計量在某個函數(shù)條件下，通過計算條件均值能提高其估值。它的核心是表示統(tǒng)計量條件和非條件方差之間的關(guān)系。具體如下：

對兩個變量 U,Y的估計量τ ( U,Y)，非條件方差可以表示為條件方差的和：

此公式的理論依據(jù)是條件均值性質(zhì)：

2.2 線性/非線性模型

包括重力梯度導(dǎo)航用一類線性非線性模型為：

2.3 狀態(tài)估計過程推導(dǎo)

粒子濾波雖然可以全空間采樣而且計算快捷，但會出現(xiàn)樣本貧化問題。這里將CKF引入時間更新的重要性采樣函數(shù)中。CKF是根據(jù)Spherical-Radial Cubature準(zhǔn)則，通過一組具有相同權(quán)重的點結(jié)合最新量測，經(jīng)非線性系統(tǒng)轉(zhuǎn)換后產(chǎn)生新的點給出下一時刻系統(tǒng)狀態(tài)的預(yù)測，其精度可達到三階[9]。該濾波方法與粒子濾波結(jié)合使粒子更快向高似然區(qū)移動，在導(dǎo)航應(yīng)用中得到很高的精度。下面對兩種濾波的結(jié)合進行推導(dǎo)。

線性狀態(tài)估值在時間更新階段為額外的噪聲，時間更新分布是：

在時間更新階段引入CKF，CKF作用的模型為：

① 計算Cubature點：

③ 自相關(guān)協(xié)方差陣：

④ 互相關(guān)協(xié)方差陣：

⑤ 近似卡爾曼增益：

⑥ 后驗均值和協(xié)方差：

狀態(tài)整體后驗分布為：

3 邊緣CPF重力梯度輔助導(dǎo)航仿真實例

此處慣性導(dǎo)航系統(tǒng)為平臺慣導(dǎo)，所用數(shù)據(jù)為船舶航行實際數(shù)據(jù)，潛艇潛航一般在24 h以上。這里進行仿真驗證，數(shù)據(jù)時長取7 h，數(shù)據(jù)采樣周期為1 s，并假設(shè)梯度圖制圖精準(zhǔn)，不包含圖誤差。導(dǎo)航器件性能較低，如表1所示。載體軌跡、速度如圖2所示。

表1 陀螺和加速度計主要性能參數(shù)Tab.1 Parameter of gyro and accelerometer

圖2 載體軌跡速度圖Fig2 Vehicle track and velocity

基準(zhǔn)梯度圖采用自由空間重力異常正演的重力梯度，分辨率30′。圖3列出北向值沿北向分解的 Γxx和東向值沿北向分解的 Γxy兩幅。

圖3 導(dǎo)航用重力梯度圖 Γ xx和 ΓxyFig.3 Gravity gradient map Γ xxand Γxy

重力梯度儀量測值這里采用真實位置的梯度值加上白噪聲模擬。

3.1 濾波計算

此處采用平臺慣導(dǎo)系統(tǒng)。狀態(tài)是間接量緯度經(jīng)度誤差(δφ, δλ)、東向北向速度誤差(δVx,δVy)、水平方位姿態(tài)誤差(α,β,γ)、加速度計零位偏差(Δ Ax,Δ Ay)和陀螺常值漂移(εx,εy,εz)，系統(tǒng)方程為：

式中，X表示12維狀態(tài)，狀態(tài)矩陣A和噪聲矩陣G表達式參見文獻[4]。系統(tǒng)噪聲W為

式中， wax、 way、 wgx、 wgy和 wgz均為零均值的高斯白噪聲，分別對應(yīng)加速度計誤差和陀螺隨機漂移。設(shè)濾波周期 Tf=10 s，將狀態(tài)方程離散化，離散化后表達式：

其中，

由式(28)知，量測方程是狀態(tài)δL和δλ的非線性函數(shù)，而且如果位置誤差越小，則梯度之間的差值越小。經(jīng)緯度誤差用粒子集表示，如果粒子能代表位置的真實后驗分布，則其對應(yīng)權(quán)值就較大，反之較小。粒子濾波通過遞推和更新，可不斷估計慣導(dǎo)的位置誤差，從而校正系統(tǒng)位置輸出。

由于重力梯度數(shù)據(jù)庫以網(wǎng)格形式給出，對于不在網(wǎng)格上坐標(biāo)梯度值可通過雙線性插值獲得。海洋中能利用高精度重力梯度儀，噪聲可達1Eotvos或更高精度，仿真中設(shè)置分量的量測噪聲為ij= 10E2。

經(jīng)緯度誤差在量測方程中屬于非線性狀態(tài)，余下誤差為線性狀態(tài)，將狀態(tài)方程 Ak按照式(3)進行分解：

將狀態(tài)方程噪聲矩陣分解：

濾波時設(shè)引入重力梯度輔助定位時的位置誤差已達2000 m，仿真粒子數(shù) N= 100，進行10次Monte Carlo仿真，相同條件下濾波穩(wěn)定后位置均方根誤差(RMSE)與APO-PF算法比較結(jié)果如圖4所示。

圖4 緯度經(jīng)度誤差比較Fig.4 Latitude and longitude RMSE comparison

由圖4的經(jīng)緯度RMSE可以看出，MCPF算法經(jīng)緯度誤差估計穩(wěn)定且始終收斂，其精度高于APO-PF算法。同時MCPF單次運行時間為0.0113 s，高于APO-PF單次的0.418 s。

3.2 結(jié)果分析

3.2.1 圓概率誤差定位精度分析

常用圓概率誤差(CEP)評定導(dǎo)航定位誤差，定位誤差用到規(guī)定點(真實位置或均值)的徑向距離表示，這一誤差稱為圓誤差。根據(jù)國軍標(biāo)GJB729—1989《慣性導(dǎo)航系統(tǒng)精度評定辦法》，船用慣導(dǎo)圓概率誤差(CEP)評定試驗數(shù)據(jù)通常以50%置信水平選取概率半徑，即R50。它的意義是以真實位置為圓心，包圍全部定位誤差點一半的圓的半徑為 R50，可通過在評定時間內(nèi)各時刻采樣點上位置誤差的圓概率誤差半徑 Rpj中的最大值 Rp來衡量，即 Rp=max Rpj(1 ≤ j≤ m)。若令：

式中，Δ λij、Δ φij表示第i次仿真中第j個時間點的經(jīng)緯度誤差值， xij、 yij、 rij記作水平、垂直、徑向位置誤差，其中仿真次數(shù) i= 5， j∈[2.7 h, 7 h]，每隔10 s進行一次采樣。

圖5 MCPF 10E2噪聲的CEP曲線圖Fig.5 CEP curve graph of MCPF under noise of 10E2

圖6 MCPF 1E2噪聲的CEP曲線圖Fig.6 CEP curve graph of MCPF under noise of 1E2

10 E2和1E2梯度儀噪聲下4 h航跡CEP曲線如圖5和圖6所示。表2對1E2和10E2噪聲下位置誤差進行了比較計算。

表2 算法結(jié)果數(shù)值Tab.2 Value of algorithm

從表2可見， MCPF算法對所用數(shù)據(jù)梯度儀1E2噪聲比10E2噪聲CEP（單位：n mile）[12]及經(jīng)緯度RMSE都有較大降低。由于使用慣導(dǎo)性能較低，所以當(dāng)采更高精度的導(dǎo)航儀時，CEP定位精度將會進一步提高。

經(jīng)緯度以外狀態(tài)量的估計可通過 MCPF中的Kalman濾波給出。

3.2.2 定性分析

其余狀態(tài)量估計效果也可通過狀態(tài)陣簡化進行簡略定性分析。從圖2速度圖像可見，載體近似勻速運動，所以東向和北向加速度計值近似為0，即 ▽Ax≈0，▽Ay≈ 0。為明確可觀測問題，把短時間內(nèi)(1～2 min)可忽略不計的元素舍棄以簡化系統(tǒng)方程，同時還忽略短時間內(nèi)地球旋轉(zhuǎn)和地球表面彎曲，≈03×1，≈03×1。簡化后，狀態(tài)轉(zhuǎn)移陣為：

可見，航向失準(zhǔn)角γ不可觀，因為γ所對應(yīng)列為零。

載體近似直線航行，因此 - gβ與加速度計偏差▽Ax的和以及gα與 ▽Ay的和是可觀測的，即：但可觀測性較低，通過載體速度變化可將兩者估計出來。水平失準(zhǔn)角與水平陀螺漂移耦合，通過轉(zhuǎn)彎，水平失準(zhǔn)角與水平陀螺漂移可觀測。

陀螺漂移及水平失準(zhǔn)角估計圖像如圖7和圖8所示。

圖7 陀螺漂移RMSE比較Fig.7 Gyro drifts RMSE comparison

圖8 水平失準(zhǔn)角RMSE比較Fig.8 Level misalignment angel RMSE comparison

4 結(jié) 論

本文使用能更充分利用地球物理場信息的重力梯度輔助慣性導(dǎo)航，對重力梯度輔助導(dǎo)航的基本原理及各部分組成進行了論述。設(shè)計了在圖匹配下的框架圖，并使用新的邊緣CPF算法作為融合算法。從理論上證明了邊緣粒子濾波能夠降低線性狀態(tài)估計方差。推導(dǎo)了CKF算法與MPF算法結(jié)合的MCPF算法流程，給出系統(tǒng)狀態(tài)方程和量測方程，根據(jù)量測是經(jīng)緯度誤差非線性函數(shù)特點，狀態(tài)方程分解。在較大初始誤差下，計算表明，MCPF在梯度儀1E2和10E2噪聲下，4 h CEP限制在80 m和133 m內(nèi)。然后通過狀態(tài)方程簡化，對其余狀態(tài)量估計結(jié)果進行了說明。

（References）：

[1] 楊勇，王可東，吳鎮(zhèn)，等. 不同參數(shù)對地形等值線匹配算法精度影響的評估分析[J]. 航空學(xué)報，2010，31(5)：996-1003.

YANG Yong, WANG Ke-dong, WU Zhen, et al. Evaluation of performance of ICCP algorithm with different parameters[J]. Acta Aeronautica et Astronautica Sinica, 2010, 31(5): 996-1003.

[2] Richeson J A. Gravity gradiometer aided inertial navigation within non-GNSS environments[D]. Baltimore: University of Maryland, 2008: 20-32

[3] 王艷東，胡華峰，楊少帥. 重力梯度數(shù)據(jù)在導(dǎo)航系統(tǒng)中應(yīng)用[J]. 電光與控制，2013，20(11)：11-15.

WANG Yan-dong, HU Hua-feng, YANG Shao-shuai. The application of gravity gradient data to navigation system[J]. Electro Optics and Control., 2013, 20(11): 11-15.

[4] 劉繁明，錢東，劉超華. 一種人工物理優(yōu)化的粒子濾波算法[J]. 控制與決策，2012，27(8)：1145-1156.

LIU Fan-ming, QIAN Dong, LIU Chao-hua. An artificial physics optimized particle filter[J]. Control and Decision, 2012, 27(8): 1145-1156.

[5] Brown D, Mauser L, Young B, et al. Atom interferometric gravity sensor system[C]//2012 IEEE Position Location and Navigation Symposium. 2012: 30-37.

[6] 錢東，劉繁明，李艷，等. 導(dǎo)航用重力梯度基準(zhǔn)圖構(gòu)建方法的比較研究[J]. 測繪學(xué)報，2011，40(6)：736-744.

QIAN Dong, LIU Fan-ming, LI Yan, et al. Comparison of gravity gradient reference map composition for navigation[J]. Acta Geodaetica et cartographica Sinica, 2011, 40(6): 736-744.

[7] Casella G. Rao-Blackwellisation of sampling schemes[J]. Biometrika, 1996, 83(1): 81-94.

[8] Wang Zong-yuan, Sun Feng. Decentralized estimation of nonlinear target tracking based on nonlinear filter[C] //Proceedings of the 2ndInternational Conference on Mechatronic Science, Electric Engineering and Computer. 2013: 22-26.

[9] Arasaratnam I, Haykin S. Cubature Kalman filter[J]. IEEE Transactions on Automatic Control, 2009, 54: 1254-1269.

[10] Affleck C A, Jircitano A. Passive gravity gradiometer navigation system[C]//1990 IEEE Position Location and Navigation Symposium. 1990: 60-66.

[11] Jircitano A, White J, Dosch D. Gravity based navigation of AUV’s. Proceedings of the symposium on autonomous underwater vehicle technology. Washington, 1990: 177-180

[12] 袁贛南，張紅偉，袁克非，吳簡彤. 基于重力梯度輔助定位的概率神經(jīng)網(wǎng)絡(luò)改進方法[J]. 中國慣性技術(shù)學(xué)報，2013，21(3)：369-374.

YUAN Gan-nan, ZHANG Hong-wei, YUAN Ke-fei, WU Jian-tong. Improved probabilistic neural network method based on gravity gradient aided location[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 369-374.

邊緣CPF算法及在重力梯度輔助導(dǎo)航中應(yīng)用

王宗原1,2，孫 楓2

Marginal cubature particle filter and its application in gravity gradient aided navigation

WANG Zong-yuan1,2, SUN Feng2
(1. College of Science, Harbin Engineering University, Harbin 150001, China; 2. College of Automation, Harbin Engineering University, Harbin 150001, China)

INS has large accumulated errors after long journey owing to its inherent reason, which can be decreased by integrating INS with gravity gradient measurement. In this paper, the theory of gravity gradiometer aided INS is described, and its framework chart is presented. Then the gravity map and gradiometer is analyzed, and its measurement equation is set. Based on the feature of state-apace function, a marginal cubature particle filter is proposed to provide estimation in information fusion, and then its variance is proved to be reduced by statistical theory. In addition, the flowchart of algorithm is given, and the simulation is performed. Under the same condition, the position accuracy of the proposed algorithm is higher than that of the existing APO-PF algorithm based on the comparison of their attitude and longitude’s RMSE results. The CEP is used for assessing navigation errors, and it is shown that the gradiometer’s 4 h CEPs under 1E2and 10E2noises are 0.044 n mile and 0.072 n mile, respectively, in low-accuracy inertial navigation system. Finally, the state equation is simplified and used to qualitatively analyze the estimation effect of other state variables.

gradiometer aided inertial navigation system; marginal filter; state decomposition; cubature particle filter; CEP

1005-6734(2014)06-0734-07

10.13695/j.cnki.12-1222/o3.2014.06.007

U666.1

A

2014-07-14；

2014-11-21

中央高?；究蒲袠I(yè)務(wù)費資助；國家自然科學(xué)基金項目（61001154）

王宗原（1977—），男，博士研究生，從事組合導(dǎo)航、非線性濾波研究。E-mail：wangzongyuan@hrbeu.edu.cn

聯(lián) 系 人：孫楓（1944—），男，教授，博士生導(dǎo)師。E-mail：w0164@sina.com