一種信標(biāo)水聲定位的改進擴展卡爾曼濾波算法

劉建波，韓樹平，徐景峰，楊 剛

(海軍潛艇學(xué)院，山東青島 266000)

0 引 言

信標(biāo)利用水聲信號對水下航行器提供導(dǎo)航定位服務(wù)，主要利用的是距離和方位信息。關(guān)于水聲純距離定位導(dǎo)航方面有較多的研究[1-4]；文獻[5]將水聲通信與定位導(dǎo)航結(jié)合，利用單程傳播時延實現(xiàn)距離估算和定位導(dǎo)航。水聲測距誤差受多種因素影響，一般并不滿足高斯噪聲假設(shè)，文獻[6]分析了影響水聲測距系統(tǒng)誤差的各種因素，文獻[7-8]提出了基于色噪聲條件下的改進卡爾曼濾波方法；文獻[9-10]等提出了基于乘性噪聲條件下的濾波定位算法；文獻[11-12]等討論了迭代初值對濾波的影響，提出了迭代初值的選取辦法。

針對水聲測距時噪聲模型失配的問題，本文提出了一種緩變測距偏差模型，將測距噪聲分為高斯分量和緩變非高斯分量，當(dāng)兩次觀測間隔較小時，假定緩變非高斯分量不變?；诖?，提出一種改進的擴展卡爾曼濾波(Improved Extended Kalman Filter, IEKF)算法，研究了IEKF算法的初值選取方法。仿真實驗和湖試結(jié)果表明，IEKF算法能夠?qū)y距偏差進行跟蹤補償，在復(fù)雜噪聲條件下定位精度明顯優(yōu)于常規(guī)擴展卡爾曼濾波(Extended Kalman Filter, EKF)算法。

1 基于緩變測距偏差的改進EKF

1.1 基于緩變測距偏差的改進EKF(IEKF)算法

水下純距離導(dǎo)航定位時，水聲測距誤差與多種因素相關(guān)。利用傳播時延進行距離估計，距離誤差dr/r=dt/t+dc/c，時延測量誤差dt與信號帶寬和信噪比有關(guān)，與距離無關(guān)，在遠(yuǎn)距離測量時對測距誤差影響較小。聲速誤差與實際水文環(huán)境有密切關(guān)系，即使利用聲速剖面對有效聲速進行修正，海底地形失配以及洋流等因素也會導(dǎo)致有效聲速在不同方向和距離上的變化，增大聲速誤差。在收發(fā)雙方距離較遠(yuǎn)時，聲速在水平方向上也有可能發(fā)生較大變化，增大聲速誤差。一般來說，水聲測距誤差與距離成正比，距離越遠(yuǎn)，誤差越大，但是在不同方向上，受洋流、地形等環(huán)境影響，聲速誤差規(guī)律可能存在不同。

假設(shè)測距噪聲ξk由兩部分組成：ξk=ηk+υk，其中ηk是零均值高斯噪聲，υk是緩變的測距偏差，由于實際物理環(huán)境具有連續(xù)性，當(dāng)前后兩次觀測時間間隔較短、距離較近時，認(rèn)為υk的變化較小，υk=υk?1。

根據(jù)以上假設(shè)，對EKF算法進行改進，將狀態(tài)向量進行擴展如下：Xk=[xkykυk]T，并將狀態(tài)方程和觀測方程修改為[13]

狀態(tài)向量先驗協(xié)方差陣P可初始化為單位陣。狀態(tài)噪聲協(xié)方差陣，其中主要取決于控制輸入量uk的精度，水下平臺一般采用航跡推算(Dead Reckoning, DR)或慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)獲取uk，與可根據(jù)經(jīng)驗確定；對于，在兩次觀測間隔距離較近時，vk的噪聲主要由狀態(tài)誤差與引起，，當(dāng)兩次觀測間隔較大時，可以適當(dāng)增大。

1.2 改進EKF濾波的初值確定方法

一般而言，二維純距離定位跟蹤系統(tǒng)滿足可觀測性條件[14-15]，但是機動路線和迭代初值等因素對解算收斂速度和精度有較大影響。IEKF算法與經(jīng)典EKF算法相比，已知信息量不變，而擴展了狀態(tài)向量，增加了未知信息υk，對算法收斂性能有不利影響，仿真顯示，IEKF算法對初值比較敏感。當(dāng)初值誤差較小時，跟蹤性能較好，系統(tǒng)很快進入穩(wěn)定跟蹤狀態(tài)，當(dāng)選取的初值誤差較大時，跟蹤性能較差，需要經(jīng)過長時間的震蕩才能趨于穩(wěn)定。

為了減小初值對IEKF跟蹤性能的影響，在進入迭代跟蹤之前，不僅需要對目標(biāo)進行預(yù)定位，而且需要對預(yù)定位結(jié)果進行評估，確保預(yù)定位結(jié)果具有足夠的精度，才能使后續(xù)濾波盡快收斂。

由于測距偏差υk的存在，前述估計結(jié)果仍有可能存在較大偏差，為了進一步提高預(yù)定位精度，并定量分析預(yù)定位誤差的大小，對觀測方程進行線性化近似，假設(shè)測距偏差在預(yù)定位期間不變，觀測方程可表示為

設(shè)水下平臺與浮標(biāo)的距離較遠(yuǎn)，dr/r?1，將式(4)泰勒展開并保留一階項可得：

假設(shè)從觀測伊始至完成預(yù)定位共獲得k組觀測數(shù)據(jù)ri,i=1,2,…k，利用[xkyk]T反推平臺歷史位置，獲得如下線性回歸方程：

其中：Y=[Y1Y2…Yk]T，X=[X1X2…Xk]T，η～N(0,σ2)，Xi、Yi與[xiyi]T以及ri的關(guān)系與式(6)形式相同。設(shè)式(7)中的線性回歸方程的最小二乘解為

式中，Var(?)表示求向量的協(xié)方差。根據(jù)文獻[16]中定理，預(yù)定位均方誤差可表示為

當(dāng)α?1時，預(yù)定位結(jié)果誤差較大，不足以采信，當(dāng)α≈1時，預(yù)定位誤差與測距噪聲誤差相近，可以作為后續(xù)濾波的初值。

2 仿真實驗與湖試結(jié)果分析

2.1 仿真結(jié)果與分析

仿真實驗中，水下平臺繞浮標(biāo)運動，每隔一定時間向靠近浮標(biāo)的方向轉(zhuǎn)向一次，轉(zhuǎn)向角度隨機，水下平臺一個典型的軌跡如圖1(a)所示。測距誤差ξk由高斯白噪聲ηk和緩變測距偏差vk組成。其中E(ηk)的均值為0、方差為5 m。υk分為兩部分，一是聲速誤差引起的與距離成正比的乘性偏差γrk，γ為聲速誤差，一般不超過 1%；二是緩變隨機偏差μk+ρ，μk為零均值隨機偏差，可以通過低通濾波器對高斯白噪聲濾波進行模擬，ρ為隨機偏差的均值，仿真測距誤差如圖1(b)所示。

預(yù)定位精度因子α隨觀測數(shù)據(jù)點數(shù)的變化如圖2所示。

圖1 水下平臺航跡與測距誤差Fig.1 Track and ranging error of underwater platform

在圖1所示軌跡下，預(yù)定位精度因子隨觀測點數(shù)據(jù)的增加迅速下降，在獲得約200組數(shù)據(jù)后降為1，此時平臺相對浮標(biāo)的方位變化約90°。

圖3顯示了初值偏差對常規(guī)EKF與IEKF定位跟蹤的影響，圖3(a)迭代初值與真值的距離誤差為150 m，圖3(b)迭代初值與真值的距離誤差為30 m。結(jié)果顯示，對于傳統(tǒng)EKF算法，定位誤差約100 m，受迭代初值誤差影響較小。對于IEKF算法，當(dāng)初值誤差150 m時，1 000組數(shù)據(jù)后定位誤差約40 m，測距偏差跟蹤結(jié)果震蕩，2 000組數(shù)據(jù)后漸趨穩(wěn)定，當(dāng)初值誤差30 m時，800組數(shù)據(jù)后定位跟蹤誤差收斂至20 m，測距偏差跟蹤穩(wěn)定。

圖2 預(yù)設(shè)定位精度隨觀測數(shù)據(jù)的變化關(guān)系Fig.2 Variation of pre-positioning precision with observed data points

圖3 預(yù)設(shè)定位精度對跟蹤算法的影響Fig.3 The influence of the pre-positioning precision on tracking algorithm

圖4顯示了經(jīng)過預(yù)定位后的跟蹤結(jié)果，在預(yù)定位精度因子α低于2時進行預(yù)定位。結(jié)果顯示，利用350組數(shù)據(jù)完成預(yù)定位后，定位精度約20 m，測距偏差跟蹤穩(wěn)定。與圖3相比可以看出，在缺乏精確的位置先驗信息時，預(yù)定位可以使跟蹤過程更快收斂。

圖5顯示了基于緩變測距偏差的IEKF算法和基于乘性噪聲的EKF算法[10]結(jié)果對比，可以看出兩者定位誤差約20 m，與常規(guī)EKF算法相比具有更高的精度。

圖4 預(yù)設(shè)定位處理后的IEKF算法的跟蹤結(jié)果Fig.4 The IEKF tracking result after pre-positioning processing

圖5 基于乘性噪聲的EKF算法和基于緩變測距偏差的IEKF算法的跟蹤結(jié)果對比Fig.5 Comparison of tracking results between the multiplicative noise based EKF and the slow variable ranging deviation based IEKF results

為了比較IEKF算法和基于乘性噪聲的EKF算法的計算結(jié)果，在測距誤差ξk總方差固定的條件下，為乘性偏差γrk和隨機偏差μk分配不同比例的方差，比較兩種算法穩(wěn)定跟蹤性能，不同條件下300次蒙特卡洛仿真結(jié)果如圖6所示。當(dāng)噪聲總方差固定時，IEKF算法性能比較穩(wěn)定，與γrk和μk的相對大小基本無關(guān)，而基于乘性噪聲的EKF算法性能與γrk和μk的相對大小有關(guān)，隨機偏差μk方差在ξk總方差中占比小于25%時，基于乘性噪聲的EKF算法表現(xiàn)優(yōu)于IEKF算法，反之則IEKF算法性能更優(yōu)。注意γrk方差與聲速誤差γ和距離rk的變化有關(guān)。在距離變化較小、能夠獲取實時聲速剖面、對聲線彎曲進行精確補償?shù)臈l件下，乘性偏差γrk較小，受海底地形、洋流等環(huán)境因素影響的隨機偏差μk較大，IEKF算法具有更好的性能。

圖6 噪聲總方差恒定時兩種算法跟蹤性能對比Fig.6 Comparison of tracking performance of the two algorithms for a certain total variance of noise

2.2 湖試結(jié)果與分析

為了檢驗IEKF算法的有效性，利用2018年某水域進行的測距定位試驗數(shù)據(jù)對算法進行了驗證，分別利用 EKF算法、IEKF算法和乘性噪聲 EKF算法進行定位。試驗中用兩條船分別搭載水聲信號發(fā)送和接收系統(tǒng)，并實時記錄收發(fā)船只全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System, GNSS)位置。試驗中，發(fā)射系統(tǒng)將自身GNSS數(shù)據(jù)利用水聲擴頻通信方法發(fā)送至接收系統(tǒng)。接收系統(tǒng)利用自身GNSS數(shù)據(jù)推算自身位移，模擬DR或INS系統(tǒng)的位移控制量ukr，利用發(fā)射系統(tǒng) GNSS數(shù)據(jù)推算發(fā)射系統(tǒng)位移信息uks，整個系統(tǒng)可等效為固定浮標(biāo)和移動平臺的定位跟蹤問題，位移控制輸入量uk=ukr?uks。

圖 7顯示了試驗期間收發(fā)船只位置和相對態(tài)勢。受試驗場地和條件所限，試驗期間，信號發(fā)射系統(tǒng)在四個方位上各發(fā)射一組信號，沒有進行長時間連續(xù)機動發(fā)射，其中第1、2組相對方位約50°，第3、4組相對方位約320°，方位差約90°。

圖7 收發(fā)船只的全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)相對位置Fig.7 GNSS relative positions of transmitting and receiving boats

圖8顯示了4組信號的GNSS距離和水聲測距結(jié)果。各組數(shù)據(jù)收發(fā)距離相近，測距噪聲各不相同，與距離的關(guān)系較小。

圖8 收發(fā)船只實際距離與估測距離Fig.8 The actual and estimated distances between the transmitting and receiving boats

試驗數(shù)據(jù)處理結(jié)果如圖9所示。圖9(a)顯示了不同算法定位結(jié)果，圖9(b)顯示了IEKF算法估算的υk與實際測距誤差的對比。

從圖9可以看出，IEKF算法經(jīng)過預(yù)定位后，定位誤差約10 m，EKF算法定位誤差收測距噪聲影響，在10～40 m范圍內(nèi)波動，IEKF算法性能明顯優(yōu)于EKF算法，能夠較好地跟蹤和補償測距緩變偏差。

圖9 湖試數(shù)據(jù)處理結(jié)果Fig.9 The processing results of lake trial data

3 結(jié) 論

本文提出了一種基于緩變測距偏差的改進EKF(IEKF)算法，將水聲測距誤差分為緩變偏差和零均值高斯噪聲兩部分，并通過對緩變偏差的跟蹤與補償，減小測距噪聲模型失配對EKF的影響。仿真實驗和湖試結(jié)果表明，本文提出的迭代初值選取方法能夠使IEKF算法實現(xiàn)對緩變測距偏差的跟蹤補償，當(dāng)緩變隨機偏差在總方差中的占比較大時，性能優(yōu)于基于乘性噪聲的EKF算法。