馮國君，單志龍,2，項(xiàng) 婉

（1.華南師范大學(xué)計(jì)算機(jī)學(xué)院，廣州 510631；2.華南師范大學(xué)網(wǎng)絡(luò)教育學(xué)院，廣州 510631）

海洋開發(fā)是國家重要發(fā)展戰(zhàn)略，水下定位技術(shù)是海洋開發(fā)的關(guān)鍵一環(huán)，廣泛應(yīng)用于環(huán)境監(jiān)測、災(zāi)害預(yù)報(bào)、輔助導(dǎo)航、資源勘探、國防軍事等領(lǐng)域[1]。水下定位系統(tǒng)受聲速變化及定位目標(biāo)移動等因素影響，實(shí)現(xiàn)水下移動目標(biāo)實(shí)時(shí)高精度定位相對困難[2]。

水下定位系統(tǒng)一般用聲吶通訊，但是聲速在水下環(huán)境中并非恒定，而是隨海水的溫度、鹽度、深度的變化而變化，聲波沿彎曲路徑傳播。由于自身機(jī)動或者受洋流等因素影響，水下節(jié)點(diǎn)大多時(shí)間都處于運(yùn)動狀態(tài)，導(dǎo)致其不能在同一時(shí)刻及位置接收不同信標(biāo)節(jié)點(diǎn)定位信號，造成的位置估計(jì)偏差較大[3]。圖1為移動節(jié)點(diǎn)定位示意圖。

針對上述問題，學(xué)者提出了多種解決方案。文獻(xiàn)[4]提出基于修正時(shí)延的粒子濾波算法，根據(jù)上一時(shí)刻目標(biāo)位置估計(jì)出當(dāng)前時(shí)刻接收不同定位信號的時(shí)間間隔，利用這一時(shí)間間隔對目標(biāo)狀態(tài)進(jìn)行粒子濾波或者卡爾曼濾波，得出當(dāng)前時(shí)刻的目標(biāo)位置估計(jì)。文獻(xiàn)[5]提出修正擴(kuò)展卡爾曼濾波方法（MEKF），在擴(kuò)展卡爾曼濾波算法基礎(chǔ)上，預(yù)測階段采用多點(diǎn)預(yù)測的方式，使預(yù)測時(shí)間點(diǎn)對應(yīng)信號到達(dá)目標(biāo)節(jié)點(diǎn)時(shí)刻。以上方法的定位精度雖然比原始濾波方法的有所提高，但是沒有考慮聲速變化及聲線彎曲的影響，造成定位誤差較大。文獻(xiàn)[6]同樣使用擴(kuò)展卡爾曼濾波方式對目標(biāo)進(jìn)行跟蹤，預(yù)測更新時(shí)間點(diǎn)加上信號傳播時(shí)間，同時(shí)引入平滑算法處理采樣周期不固定問題。文獻(xiàn)[7]則采用卡爾曼濾波“對齊”不同信標(biāo)節(jié)點(diǎn)到達(dá)時(shí)間戳，同時(shí)應(yīng)用聲線跟蹤算法修正彎曲聲線，凸優(yōu)化方法解算目標(biāo)位置，在深海試驗(yàn)中取得較高的定位精度。以上文獻(xiàn)都是采取一定的方法修正不同信號到達(dá)時(shí)間，雖然說水聲通訊延遲相對較大，在水下環(huán)境中不可忽視，但是相對濾波方法的觀測周期來說，不同信號到達(dá)目標(biāo)時(shí)間戳的差值還是相對較小，修正精度較高的時(shí)間戳相對比較困難。文獻(xiàn)[8]提出迭代逼近的方式修正彎曲聲線。但是由于移動節(jié)點(diǎn)測量不是在同一時(shí)刻獲得的，迭代逼近的方式只能初步修正彎曲聲線的影響，不能消除節(jié)點(diǎn)移動給觀測距離帶來的測量誤差，因此需要進(jìn)一步修正觀測值。

圖1 移動節(jié)點(diǎn)定位示意圖Fig.1 Mobile node positioning diagram

本文提出基于修正觀測的粒子濾波算法（Modified Observations Particle filter,MOPF），從觀測距離上減少聲線彎曲及節(jié)點(diǎn)移動給定位帶來的影響，實(shí)現(xiàn)對移動節(jié)點(diǎn)的高精度定位。

1 標(biāo)準(zhǔn)粒子濾波算法

在已知運(yùn)動參數(shù)的情況下，可以對節(jié)點(diǎn)運(yùn)動建模，采用序貫濾波的方式得出節(jié)點(diǎn)位置最佳估計(jì)。粒子濾波就是一種基于蒙特卡羅方法的隨機(jī)采樣序貫濾波方法[9]，其核心思想是通過在狀態(tài)空間傳播的隨機(jī)樣本近似后驗(yàn)概率密度，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)最小方差的過程。它擺脫了解決非線性濾波問題時(shí)隨機(jī)量必須滿足高斯分布的制約，對變量參數(shù)的非線性非高斯有更強(qiáng)的建模能力。粒子濾波算法先通過先驗(yàn)概率密度產(chǎn)生一組隨機(jī)粒子，通過測量向量不斷更新粒子權(quán)重，然后用帶有權(quán)重的粒子更新先驗(yàn)概率密度函數(shù)，得出狀態(tài)值的估量。

粒子濾波的算法流程為：

(1)粒子集初始化。

k=0 時(shí)刻，對于粒子i=1,2...N,由先驗(yàn)p(0x) 生成采樣粒子

(2)對于k=1,2...,循環(huán)執(zhí)行以下步驟：

① 重要性采樣：對于i=1,2...N,從重要性概率密度中生成采樣粒子計(jì)算粒子權(quán)值，并進(jìn)行歸一化，得到粒子集

③ 輸出：計(jì)算k時(shí)刻的狀態(tài)估計(jì)值：

2 MOPF 算法

粒子濾波對非線性非高斯隨機(jī)系統(tǒng)有很好的跟蹤效果。根據(jù)移動節(jié)點(diǎn)監(jiān)測自身運(yùn)動參數(shù)，對移動目標(biāo)運(yùn)動建模，運(yùn)用粒子濾波算法產(chǎn)生隨機(jī)粒子，根據(jù)水聲定位系統(tǒng)測量值確定粒子權(quán)重，組合粒子權(quán)重即可得出目標(biāo)方位。

對于隨機(jī)系統(tǒng)，觀測值的測量誤差往往決定了系統(tǒng)跟蹤的精度。要實(shí)現(xiàn)水下高精度定位，必然要考慮聲線彎曲對測距的影響。對于長基線等聲學(xué)定位系統(tǒng)，由于聲線入射角不可獲得，修正彎曲聲線相對困難?；诖耍疚奶岢龆涡拚郎y量值的MOPF 算法。算法原理及主要改進(jìn)流程如下：

(1)用迭代逼近的方式修正聲線彎曲的影響，得到信標(biāo)節(jié)點(diǎn)Aj到未知節(jié)點(diǎn)的觀測距離dj。假設(shè)此時(shí)在粒子濾波算法中，產(chǎn)生的隨機(jī)粒子所處方位與聲信標(biāo)Aj的距離為，則粒子權(quán)重表示為：

其中，d0為未知節(jié)點(diǎn)到錨節(jié)點(diǎn)的真實(shí)距離，φ測量噪聲，dm為由于節(jié)點(diǎn)運(yùn)動帶來的測量誤差。dm是關(guān)于移動節(jié)點(diǎn)所處方位、運(yùn)動速度的函數(shù)，當(dāng)節(jié)點(diǎn)的方位和運(yùn)動的速度確定時(shí)，dm是個(gè)確定值，相當(dāng)于移動節(jié)點(diǎn)在信標(biāo)節(jié)點(diǎn)方向上的運(yùn)動距離，可以通過移動節(jié)點(diǎn)在信標(biāo)方向的速度分量與信號到達(dá)時(shí)間差的乘積近似獲得。

如圖2所示，假設(shè)A、B、C、D 為信標(biāo)節(jié)點(diǎn)，運(yùn)動目標(biāo)分別在t1、t2、t3、t4時(shí)刻接收到不同錨節(jié)點(diǎn)定位信號，對應(yīng)傳播距離為r1、r2、r3、r4。當(dāng)運(yùn)動目標(biāo)接收到最晚到達(dá)信號時(shí)計(jì)算自身位置，此時(shí)目標(biāo)與錨節(jié)點(diǎn)距離應(yīng)該為r1'、r2'、r3'、r4。由圖2可知，節(jié)點(diǎn)移動導(dǎo)致定位精度隨位置變化的主要原因在于，用測得的信號時(shí)間計(jì)算的距離近似代替節(jié)點(diǎn)在接收到最后到達(dá)信號時(shí)所處的位置與聲信標(biāo)對應(yīng)的距離，即用rj近似rj的值。而rj與的差值可以看作節(jié)點(diǎn)在信'標(biāo)節(jié)點(diǎn)方向運(yùn)動的距離。用移動節(jié)點(diǎn)在信標(biāo)節(jié)點(diǎn)方向上的分速度，乘以信號到達(dá)時(shí)間差，可以得到的近似值。

圖2 第二次修正觀測值原理圖Fig.2 Second modified observation value diagram

其中，vx、vy為移動節(jié)點(diǎn)在當(dāng)前方位上x軸和y軸平均速度。由于節(jié)點(diǎn)運(yùn)動趨勢會發(fā)生改變，仿真過程使用前幾個(gè)歷史位置的平均值以及節(jié)點(diǎn)自身監(jiān)測的運(yùn)動參數(shù)近似獲得。α為目標(biāo)方位與信標(biāo)節(jié)點(diǎn)連線在垂直方向上的夾角。實(shí)驗(yàn)過程通過上一歷史位置與信標(biāo)節(jié)點(diǎn)的方位近似計(jì)算獲得。tj為第j個(gè)信標(biāo)節(jié)點(diǎn)定位信號到達(dá)時(shí)間，為最后定位信號到達(dá)時(shí)間。

在求得距離修正值后，粒子權(quán)重更新為:

MOPF 算法的完整流程如圖3所示。

圖3 修正觀測值粒子濾波算法流程圖Fig.3 Flow chart of particle filter algorithm for correction of observation value

3 仿真實(shí)驗(yàn)

如圖4所示，在水面布設(shè)4 個(gè)浮標(biāo)A，B，C，D呈正方形布局，間隔1600 m，仿真中浮標(biāo)位置在x,y,z坐標(biāo)軸上設(shè)定10 cm 定位誤差。設(shè)置移動節(jié)點(diǎn)在水下475 m 平面上，從信標(biāo)組成幾何中心出發(fā)，繞半徑為500 m 圓形軌跡，以確定的速度逆時(shí)針做勻速圓周運(yùn)動。假設(shè)移動節(jié)點(diǎn)配置高精度深度傳感器，各個(gè)信標(biāo)節(jié)點(diǎn)與移動節(jié)點(diǎn)經(jīng)過同步時(shí)鐘校準(zhǔn)。信標(biāo)節(jié)點(diǎn)每間隔5 s 同時(shí)向移動節(jié)點(diǎn)發(fā)送定位信號。仿真過程中使用南海某海域?qū)崪y聲速剖面精確計(jì)算聲線彎曲路徑及傳播時(shí)延[10]，測時(shí)誤差設(shè)定為50 μs。以上仿真環(huán)境參照文獻(xiàn)[11]。粒子濾波粒子數(shù)為1000。

圖4 移動節(jié)點(diǎn)運(yùn)動軌跡Fig.4 Mobile node trajectory

節(jié)點(diǎn)移動性對定位誤差造成的影響與目標(biāo)運(yùn)動的速度、目標(biāo)相對于信標(biāo)點(diǎn)的方位有關(guān)。如果忽略聲速多變及聲線彎曲的影響，用固定聲速計(jì)算觀測距離，平均定位誤差達(dá)數(shù)十米，與精確修正聲線的算法不具可比性。本文實(shí)驗(yàn)均在修正彎曲聲線的條件下進(jìn)行。

定義定位誤差為：

其中(x,y)代表某次定位中，接收到最后到達(dá)的定位信號時(shí)移動節(jié)點(diǎn)所處的位置。(x',y')代表算法解算目標(biāo)位置。

（1）定位精度對比

為了驗(yàn)證MOPF算法的優(yōu)勢，本節(jié)對MOPF算法、最小二乘算法、MEKF算法[5]的定位精度進(jìn)行對比。最小二乘法是水下定位慣用的解算方法。MEKF算法考慮節(jié)點(diǎn)移動性的影響，將擴(kuò)展卡爾曼濾波采用多點(diǎn)預(yù)測的方式進(jìn)行位置估算[5]。

如圖5所示，最小二乘法、MEKF算法和MOPF算法的平均定位誤差分別為2.0 m、1.43m 和0.72m。MOPF 算法的定位誤差明顯比最小二乘法和MEKF 算法低，并且在不同方位時(shí)定位精度都保持穩(wěn)定。定位精度相比傳統(tǒng)方法提高一倍。這是因?yàn)镸OPF算法首先采取聲線跟蹤的方式修正彎曲聲線，然后再根據(jù)節(jié)點(diǎn)的運(yùn)動特定與信標(biāo)節(jié)點(diǎn)的關(guān)系進(jìn)一步修正觀測距離，并且充分利用節(jié)點(diǎn)運(yùn)動模型以及歷史位置等信息得出最優(yōu)的位置估計(jì)，因而能夠最大限度消除聲線彎曲和節(jié)點(diǎn)移動性帶來的影響，實(shí)現(xiàn)了高精度定位。

圖5 定位精度對比Fig.5 Positioning accuracy comparison

（2）節(jié)點(diǎn)所處方位對定位精度的影響

當(dāng)節(jié)點(diǎn)處在不同方位時(shí)，定位信號到達(dá)移動節(jié)點(diǎn)的時(shí)間間隔差異較大，造成定位精度隨目標(biāo)方位變化而變化。

圖6為各個(gè)信標(biāo)節(jié)點(diǎn)定位信號到達(dá)深度為475 m平面坐標(biāo)點(diǎn)的最大時(shí)間差等時(shí)圖。由圖可以看出，信號到達(dá)最大的時(shí)間差由幾何中心點(diǎn)向四周方向逐漸擴(kuò)大，在信標(biāo)節(jié)點(diǎn)正下方最大時(shí)間差達(dá)已經(jīng)達(dá)到1.2s。相對于信標(biāo)節(jié)點(diǎn)位置，在相對對稱的方位時(shí)間差相對較小，等時(shí)線呈菱形分布。該圖表明，即使是目標(biāo)靜止不動，在不同方位接收到各錨節(jié)點(diǎn)的信號時(shí)刻差別也比較大，在偏離中心點(diǎn)位尤為明顯，目標(biāo)節(jié)點(diǎn)偏離信標(biāo)節(jié)點(diǎn)幾何中心是導(dǎo)致信號不能同時(shí)到達(dá)的主要因素。

圖6 定位信號到達(dá)475m深度平面最大時(shí)間差等時(shí)圖Fig.6 Isochronousmapof the maximum timedifference when positioning signal reaches 475m depthplane

如圖7所示，當(dāng)目標(biāo)節(jié)點(diǎn)以5m/s的速度做勻速圓周運(yùn)動時(shí)，第一次修正觀測值后，MOPF算法平均定位誤差為2.1m，定位精度及變化趨勢與最小二乘法大體相當(dāng)，誤差隨著目標(biāo)位置變化而變化。這是因?yàn)榈谝淮涡拚^測值只是修正了彎曲聲線帶來的影響，并沒有修正目標(biāo)的移動性帶來的誤差。二次修正觀測值后，MOPF算法平均定位誤差為0.72 m，定位誤差明顯下降，目標(biāo)運(yùn)動造成的誤差得到補(bǔ)償，體現(xiàn)了改進(jìn)算法的良好性能。如對最小二乘法也使用兩次修正的方法，則其平均誤差為1.2m，定位精度也會有所提高。

圖7 節(jié)點(diǎn)所處方位對定位精度的影響Fig.7 Influence of theorientation of thenodeon the positioning accuracy

（3）節(jié)點(diǎn)運(yùn)動速度對定位精度的影響

節(jié)點(diǎn)運(yùn)動速度決定定位信號相繼到達(dá)的時(shí)間間隔內(nèi)節(jié)點(diǎn)移動的距離。假設(shè)目標(biāo)節(jié)點(diǎn)以同樣的軌跡做勻速圓周運(yùn)動。如圖8所示，以2m/s速度運(yùn)動的節(jié)點(diǎn)平均定位誤差為0.58m，與5m/s速度運(yùn)動節(jié)點(diǎn)的平均定位誤差0.72 m 相比，定位誤差減小，但變化幅度不大。這是因?yàn)樗俣茸冃?，在接收到信?biāo)節(jié)點(diǎn)全部定位信號時(shí)移動節(jié)點(diǎn)位移相對較小，因而定位誤差相對減小。但對于低速運(yùn)動的水下節(jié)點(diǎn)，在相對較短的時(shí)間內(nèi)，節(jié)點(diǎn)的位移不會太大，況且MOPF算法會根據(jù)節(jié)點(diǎn)的運(yùn)動特性修正觀測距離，因此速度變化并不會對定位精度產(chǎn)生較大的影響。對于低速移動的運(yùn)動節(jié)點(diǎn)，造成定位精度不穩(wěn)定的主要原因是節(jié)點(diǎn)位置偏離信標(biāo)節(jié)點(diǎn)幾何中心，定位信號不是同一時(shí)間到達(dá)，到達(dá)的前后的時(shí)間差是造成誤差偏大的主要因素。

圖8 運(yùn)動速度對定位誤差影響Fig.8 Influenceof velocity on positioning error

4 結(jié)論

針對水下定位系統(tǒng)中節(jié)點(diǎn)運(yùn)動及聲線彎曲造成測距誤差大的問題，本文提出了MOPF算法。首先根據(jù)聲線跟蹤原理，采用迭代逼近的方式初步修正觀測距離，補(bǔ)償聲線彎曲造成的測量誤差；然后利用目標(biāo)運(yùn)動速度在信標(biāo)節(jié)點(diǎn)方向上的投影與信號到達(dá)時(shí)間差的乘積，進(jìn)一步修正觀測距離。最后修正粒子濾波算法的粒子權(quán)重，通過不同權(quán)重的粒子計(jì)算目標(biāo)方位。仿真實(shí)驗(yàn)表明，移動節(jié)點(diǎn)以5 m/s的速度移動時(shí)，MOPF算法的平均定位誤差為0.72m。定位精度較擴(kuò)展卡爾曼濾波提升了50%。MOPF算法能夠有效地克服聲線彎曲和目標(biāo)運(yùn)動對測量距離造成的影響，在提高定位精度的同時(shí)確保了定位系統(tǒng)穩(wěn)定性。