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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 邊緣Cubature 粒子濾波

2.1 Rao-Blackwell理論

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

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

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

2.2 線性/非線性模型

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

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

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

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

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

① 計(jì)算Cubature點(diǎn)：

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

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

⑤ 近似卡爾曼增益：

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

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

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

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

表1 陀螺和加速度計(jì)主要性能參數(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

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

3.1 濾波計(jì)算

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

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

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

其中，

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

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

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

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

濾波時(shí)設(shè)引入重力梯度輔助定位時(shí)的位置誤差已達(dá)2000 m，仿真粒子數(shù) N= 100，進(jìn)行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)緯度誤差估計(jì)穩(wěn)定且始終收斂，其精度高于APO-PF算法。同時(shí)MCPF單次運(yùn)行時(shí)間為0.0113 s，高于APO-PF單次的0.418 s。

3.2 結(jié)果分析

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

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

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

圖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對(duì)1E2和10E2噪聲下位置誤差進(jìn)行了比較計(jì)算。

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

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

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

3.2.2 定性分析

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

可見，航向失準(zhǔn)角γ不可觀，因?yàn)棣盟鶎?duì)應(yīng)列為零。

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

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

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

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

4 結(jié) 論

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

（References）：

[1] 楊勇，王可東，吳鎮(zhèn)，等. 不同參數(shù)對(duì)地形等值線匹配算法精度影響的評(píng)估分析[J]. 航空學(xué)報(bào)，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]. 測(cè)繪學(xué)報(bào)，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] 袁贛南，張紅偉，袁克非，吳簡(jiǎn)彤. 基于重力梯度輔助定位的概率神經(jīng)網(wǎng)絡(luò)改進(jìn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，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ù)費(fèi)資助；國(guó)家自然科學(xué)基金項(xiàng)目（61001154）

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

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