歐陽明達， 楊元喜

(1.信息工程大學 地理空間信息學院，河南 鄭州 450052； 2.地理信息工程國家重點實驗室，陜西 西安 710054)

水下載體導航由于獨立性、自主性和隱蔽性要求，往往選擇慣性導航手段，但是，慣性導航系統(tǒng)由于累積誤差大，一般不適宜長時間、遠距離的水下航行導航。慣性導航與其他導航手段的組合或融合可以部分校正慣性系統(tǒng)產(chǎn)生的位置誤差積累[1-3]。依靠光學信號的天文導航或依賴無線電信號的全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)導航是開曠地區(qū)最常用的定位方式，但是它們只能在本身具備可用性的情況下才能與慣性系統(tǒng)進行組合導航[4]；水聲定位導航技術(shù)是一種通過測量聲波傳播的時間、相位、頻率等信息實現(xiàn)定位與導航的技術(shù)，受復雜海洋環(huán)境影響，水聲定位導航觀測數(shù)據(jù)受觀測有效性低、延遲性高、數(shù)據(jù)率低、觀測受水體環(huán)境影響大等特點的制約，水聲/慣性組合導航目前尚處于起步階段，以理論研究為主，實踐較少，暫時還未建成海底長期穩(wěn)定工作的水下大地基準網(wǎng)[5-7]；地磁/慣性組合導航方式由于缺乏地磁基礎(chǔ)資料，分辨率較低，且地磁變化較快，離實用化還存在很大差距[8]。

近年來重力匹配導航方式得到國內(nèi)外學者的廣泛關(guān)注，先后研制了重力輔助慣性導航系統(tǒng)和重力匹配導航算法[2,9]，經(jīng)典的算法有地形輪廓匹配(terrain contour matching,TERCOM)算法[10-12]、等值線最近點迭代(iterative closest contour point,ICCP)算法[13-15]、桑迪亞慣性地形輔助導航(sandia intertial terrain contour matching,SITAN)算法[16-17]，這些方法原理相對簡單、匹配效率高，早期多用于飛行器地形匹配導航。粒子濾波算法是采用大量隨機樣本完成貝葉斯濾波的遞推過程，通過概率計算，得到位置最優(yōu)解。與SITAN算法相比，粒子濾波算法無需地形隨機線性化過程，更適用于非線性/非高斯的動態(tài)系統(tǒng)，但也存在其自身問題，如重要性分布函數(shù)難以選取、重要性函數(shù)與似然函數(shù)不能完好匹配、粒子退化嚴重等。針對現(xiàn)實應(yīng)用問題，國內(nèi)外學者對粒子濾波進行了一系列改進，如：不敏卡爾曼粒子濾波[18-19]、輔助粒子濾波[20]、擴展卡爾曼粒子濾波[21]、高斯和粒子濾波[22]、高斯粒子濾波[23]、Rao-Blackwellized粒子濾波[24]、正則化粒子濾波[25]等，這些改進算法從不同側(cè)面提高了粒子濾波性能。在匹配導航算法實踐中，文獻[26]提出一種粒子濾波估計方法和軌跡相似變換相結(jié)合的二次匹配算法，選擇載體位置為濾波模型的狀態(tài)變量，利用慣導系統(tǒng)短期位移進行濾波模型參數(shù)估計，采用粒子濾波方法對載體真實位置進行最優(yōu)估計；文獻[27]針對貝葉斯濾波后驗概率密度函數(shù)的求解問題，用高斯混合密度函數(shù)近似狀態(tài)的后驗概率密度函數(shù)，提出了基于高斯和粒子濾波的水下地形匹配導航方法。

本文將粒子濾波算法用于水下重力匹配導航，引入重要性重采樣步驟，克服了濾波過程中的粒子退化問題。在詳細分析粒子濾波受不同初始偏差、不同蒙特卡羅采樣、不同重力異常觀測誤差影響的基礎(chǔ)上，為了控制初始偏差對匹配精度和計算時效的不利影響，引入粒子群優(yōu)化算法削弱初始配準誤差；通過算例分析驗證，聯(lián)合算法有助于進一步提高精度、提升效率、增強可靠性。

1 匹配算法原理

1.1 貝葉斯估計

非線性隨機系統(tǒng)可以用系統(tǒng)狀態(tài)方程表示為：

xk+1=fk(xk,wk)

(1)

系統(tǒng)觀測方程為：

zk=hk(xk,vk)

(2)

式中：xk為系統(tǒng)狀態(tài)向量；zk為k時刻的觀測向量；wk為過程噪聲；vk為觀測噪聲。

根據(jù)貝葉斯理論，給定后驗概率密度函數(shù)或后驗密度p(xk|z1:k)以及k時刻所有可用觀測向量z1:k={z1,z2,…,zk}估計xk出現(xiàn)的概率[28]，這個概率值即為p(xk|z1:k)。狀態(tài)向量的初始概率為p(x0|z0)，所求概率可以通過預(yù)測和更新2步法得到。預(yù)測階段使用公式為：

(3)

式中：p(xk|z1:k-1)為前一個后驗密度。給定測量值zk，更新階段通過貝葉斯準則得到后驗密度為：

(4)

歸一化常數(shù)：

(5)

從上述遞推過程可知，計算過程涉及大量積分運算，由于觀測方程式(2)一般為非線性方程，且觀測不一定服從正態(tài)分布，所以很難得到后驗概率積分值，通常采用蒙特卡羅采樣方法。

1.2 序貫重要性采樣算法

序貫重要性采樣(sequential important sampling,SIS)算法是一種蒙特卡羅(Monte Carlo,MC)方法，通過一組具有相關(guān)權(quán)重的隨機樣本表示后驗概率密度函數(shù)，并根據(jù)這些樣本和權(quán)重計算估計值[29]。當樣本數(shù)量變得非常大時，SIS濾波值接近最佳貝葉斯估計。

(6)

式中δ為狄拉克函數(shù)。

第i個粒子的歸一化權(quán)重為：

(7)

(8)

序貫貝葉斯估計的思路是，在每次迭代中，利用樣本可以構(gòu)成p(x0:k-1|z1:k-1)的近似值，并以一組新的觀測樣本得到p(x0:k|z1:k)的近似值。重要性密度表示為：

q(x0:k|z1:k)=q(xk|x0:k-1,z1:k)q(x0:k-1|z1:k-1)

(9)

(10)

根據(jù)式(9)和式(10)，權(quán)重更新方程為：

算法1：SIS粒子濾波

fori=1:N

采樣粒子：

end

1.3 重要性采樣(SIR)算法

采用SIS 方法進行遞歸貝葉斯估計時，會產(chǎn)生嚴重的粒子退化問題。粒子退化問題是指通過若干次遞歸計算，除了個別幾個粒子具有較大權(quán)值外，其余粒子的權(quán)值都很?。粯O端情況下，只有一個粒子具有趨近于 1 的較大權(quán)值，所有其他粒子的權(quán)值都趨近于0[30-32]。Gordon 等[33]在其提出的自舉濾波算法中引入重采樣(resampling)步驟，一定程度上解決了粒子退化問題。

重采樣的主要思想是，去除那些權(quán)值較小的粒子，保留并復制那些權(quán)值較大的粒子。重采樣的方法有很多，如分層采樣、殘差采樣、系統(tǒng)采樣等。一般使用有效粒子Neff來判斷粒子權(quán)重的退化程度：

2 不同工況條件下的結(jié)果討論

將某海域作為研究對象，重力異常數(shù)據(jù)采用美國斯克利普斯海洋研究所發(fā)布的SIO grav23.1模型，模型為1′分辨率，精度為2 mGal[34]。圖1示出了該海域重力異常。給出航跡模擬參數(shù)，令真實航跡起始點坐標為22.3°S,105.6°E，航向角45°，載體航行速度10 n mile/h，隨機誤差均方差為0.06 n mile，指示航跡線性誤差在經(jīng)度方向為0.3 n mile/h，緯度方向為0.5 n mile/h，隨機誤差均方差為0.12 n mile，航跡點重力異常均方根誤差為2 mGal，共有40個采樣點。

圖1 海域重力異常圖

采用粒子濾波算法進行重力匹配導航，設(shè)置如下部分固定參數(shù)：粒子分布半徑6 000 m，粒子數(shù)500個，重力觀測誤差標準差2 mGal。精度誤差評價指標采用均方根誤差(root mean square error,RMSE)表示為[35]：

本文設(shè)定A、B、C 3種不同工況，即不同初始誤差、不同蒙特卡羅采樣樣本量和不同觀測誤差標準差，分別討論不同工況的粒子濾波效果。

1)工況A：不同初始偏差

初始偏差是真實航跡相對慣導指示航跡在起始點的偏移量，由于無法提前預(yù)知其偏離程度，有必要對不同初始偏差條件下的濾波計算情況進行分析。設(shè)定蒙特卡羅采樣為10次，初始偏差分別為(1 n mile，0 n mile)、(3 n mile，0 n mile)、(5 n mile，0 n mile)、(9 n mile，0 n mile)，圖2示出了不同偏差的粒子濾波計算結(jié)果，圖3示出了航跡重力異常變化曲線，可以看出：

圖2 不同初始偏差的粒子濾波匹配結(jié)果

圖3 航跡重力異常變化

①航跡點重力異常變化特征明顯，適宜開展水下重力匹配導航，相鄰點間的重力異常差異較大，航跡點的重力異常觀測誤差不會淹沒相鄰2點間的重力異常數(shù)據(jù)變化；

②匹配航跡能夠較好吻合真實航跡，圖4示出了航跡的初始段差異值，可以看出，采用粒子濾波算法，初始偏移越大，初始段匹配的位置差異越大，倘若初始偏差超過粒子覆蓋范圍，很容易造成匹配失?。?/p>

圖4 不同初始偏差匹配結(jié)果精度

③匹配航跡逐漸向真實航跡靠攏，中后航段匹配精度高于初始航段。由此考慮，當對航跡整體平移后采用濾波計算，即若能通過算法將圖2(d)指示航跡平移變換為近似圖2(a)指示航跡，所得匹配結(jié)果的初始航段精度水平將會有較大改善。

2)工況B：不同次數(shù)蒙特卡羅采樣

采用多次蒙特卡羅采樣，粒子濾波算法在獲得高精度匹配結(jié)果的同時會犧牲實時性[36]。匹配精度與計算效率之間需要合理平衡，對采用1次、10次、30次、60次蒙特卡羅采樣的匹配結(jié)果進行分析，圖5示出了不同初始偏差的蒙特卡羅采樣后匹配結(jié)果，可以看出：

圖5 不同蒙特卡羅采樣匹配結(jié)果精度

①當偏差為(1 n mile，0 n mile)，進行1次蒙特卡羅采樣后的結(jié)果和多次采樣后結(jié)果未呈現(xiàn)顯著差異；當偏差為(9 n mile，0 n mile)時，進行1次蒙特卡羅采樣后的結(jié)果在初始段精度略低于多次采樣后結(jié)果；由此可見，初始偏差越小，1次蒙特卡羅采樣的穩(wěn)定性和可靠性更優(yōu)；

②多次采樣后的匹配結(jié)果，對精度改善和提升作用不大，甚至有個別點位在多次蒙特卡羅采樣后，精度低于較低次數(shù)的蒙特卡羅采樣結(jié)果。計算時效分別為1次，耗時約50 s；10次，耗時約5 min；30次，耗時約14 min；60次，耗時約28 min。進行1次蒙特卡羅采樣意味著能夠?qū)崟r性開展水下重力匹配導航，倘若對指示航跡整體平移以縮小初始偏差，則能夠得到更高效、更準確、更穩(wěn)妥的匹配結(jié)果。

3)工況C：不同觀測誤差標準差

設(shè)定蒙特卡羅采樣次數(shù)為10次，對航跡點重力異常分別加入2 mGal、5 mGal、9 mGal、13 mGal、20 mGal的隨機誤差。圖6示出了初始偏差為(1 n mile，0 n mile)和(3 n mile，0 n mile)的不同觀測誤差下匹配導航結(jié)果。圖6中，σ′為觀測誤差標準差?？梢钥闯觯?/p>

圖6 不同觀測誤差標準差的匹配結(jié)果精度

①當初始偏差為(3 n mile，0 n mile)時，匹配導航精度受σ′值變動影響較大，σ′在5 mGal以內(nèi)時，匹配航跡與真實航跡吻合程度較好，穩(wěn)定性和精確度較高；當σ′值過大，匹配結(jié)果呈現(xiàn)為發(fā)散狀態(tài)，偏離真實航跡越來越遠；當初始偏差為(5 n mile，0 n mile)、(9 n mile，0 n mile)時，σ′增大，匹配失敗。

②當初始偏差為(1 n mile，0 n mile)時，導航結(jié)果精度受σ′值影響程度減弱，當σ′較大時，匹配結(jié)果發(fā)散程度較偏差較大時的情況有所減緩，整體精度水平更優(yōu)。由此可見，縮小初始偏差，能夠在一定程度上克服實際重力異常觀測誤差較大帶來的負面影響。

3 聯(lián)合粒子群優(yōu)化的改進算法

綜上，減小初始偏差有助于提升匹配導航定位精度、提高計算效率，有必要對其進行預(yù)處理。引入粒子群優(yōu)化算法，該方法的優(yōu)點在于不拘泥格網(wǎng)分辨率的限制，通過設(shè)定適用性函數(shù)引導粒子向最優(yōu)位置逼近，得到初始段航跡最優(yōu)匹配解，其與指示航跡起始點間差異即為平移參數(shù)。程向紅等[37]給出了粒子群優(yōu)化算法的基本原理。

聯(lián)合粒子群優(yōu)化改進算法計算步驟如下：首先，利用粒子群優(yōu)化算法對初始段航跡進行匹配計算，實現(xiàn)初始點的精確定位；其次，對初始點匹配位置進行標記，得到與慣導指示航跡初始點的位置差異，設(shè)置為平移量，將慣導指示航跡整體平移；然后，將平移后航跡作為待匹配航跡，其上的重力異常值保持不變；最后，采用粒子濾波算法，得到匹配航跡，由于初始偏差得到改善，粒子濾波過程采用1次蒙特卡羅采樣計算，即可實現(xiàn)實時匹配導航，而后每隔一定航距進行多次蒙特卡羅計算驗證結(jié)果。

沿用上文算例，設(shè)定初始偏移量為(9 n mile，0 n mile)，蒙特卡羅采樣1次，重力異常觀測誤差標準差為2 mGal，采用粒子群優(yōu)化算法后，將慣導指示航跡整體平移，采用粒子濾波算法，得到匹配結(jié)果如圖7所示。圖8示出了粒子濾波算法和聯(lián)合算法的匹配航跡結(jié)果差異，可見，聯(lián)合算法對初始段航跡具有明顯改善作用；當重力異常觀測誤差標準差為5 mGal時，得到匹配結(jié)果如圖9所示，圖10示出了兩者間差異，可見，聯(lián)合算法能夠在一定程度上降低實測重力異常誤差對匹配導航結(jié)果精度的負面影響。

圖7 觀測誤差標準差2 mGal時，聯(lián)合方法匹配航跡結(jié)果

圖8 觀測誤差標準差2 mGal時匹配導航結(jié)果精度比較

圖9 觀測誤差標準差5 mGal時匹配航跡結(jié)果

圖10 觀測誤差標準差5 mGal匹配導航結(jié)果精度比較

4 結(jié)論

1)本文將粒子濾波算法應(yīng)用于水下重力匹配導航，首先分析了不同蒙特卡羅采樣樣本量、不同重力異常觀測誤差標準差、以及不同初始偏差3種工況對匹配導航精度的影響。

2)初始偏差較大時，對其進行改善有助于提升初始段匹配精度；初始偏差較小時，采用多次蒙特卡羅計算對提高精度的幫助作用不大，初始偏差較大時，單次蒙特卡羅采樣的計算精度會有所下降，多次蒙特卡羅采樣提升精度的同時降低了匹配導航工作效率。

3)實測重力異常觀測誤差標準差越小越好，然而，當誤差標準差增大，縮小初始偏差有助于減小其負面影響。

4)綜上可以看出，采用粒子群優(yōu)化和粒子濾波的聯(lián)合算法能夠?qū)Τ跏计钸M行改善，有助于進一步提高精度、提升效率、增強可靠性，通過算例，進一步驗證了該方法的可行性和有效性。