單機(jī)動平臺時(shí)差定位航路規(guī)劃方法

郭惠功，曲 龍，龐佳蕙，商志剛，*，周 鋒

（1. 哈爾濱工程大學(xué) 水聲技術(shù)全國重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001；2. 海洋信息獲取與安全工信部重點(diǎn)實(shí)驗(yàn)室（哈爾濱工程大學(xué)）工業(yè)和信息化部，黑龍江 哈爾濱 150001；3. 哈爾濱工程大學(xué) 水聲工程學(xué)院，黑龍江 哈爾濱 150001；4. 中國船舶集團(tuán)有限公司第七〇三研究所，黑龍江 哈爾濱 150001）

0 引言

根據(jù)水聲定位所依據(jù)信息的不同，現(xiàn)有的水聲無源定位方法可以分為2 類。第1 類是基于信號到達(dá)方向（Direction of Arrival，DOA）的定位方法，第2 類是基于信號到達(dá)時(shí)間（Time of Arrival，TOA）的定位方法，TOA 定位方法要求接收機(jī)和發(fā)射機(jī)精確的時(shí)間同步；若信號收發(fā)機(jī)時(shí)間不同步，此時(shí)可使用基于信號到達(dá)時(shí)間差（Time Difference of Arrival，TDOA）的定位方法，基站間需要進(jìn)行精確的時(shí)間同步。根據(jù)觀測站數(shù)量的不同，無源定位可分為單站無源定位和多站無源定位[1]。在單站無源定位和多站無源定位中，基站的位置分布極大地影響了定位精度[2]。因此，如何規(guī)劃機(jī)動觀測平臺的路徑，實(shí)現(xiàn)定位節(jié)點(diǎn)的合理分布，對于提高目標(biāo)定位精度至關(guān)重要。

無源定位中常用的衡量定位精度的指標(biāo)有圓概率誤差[3]、費(fèi)歇爾信息矩陣（Fisher Information matrix，F(xiàn)IM）[4]、克拉美羅下界（Cramer-Rao Lower Bound，CRLB）和幾何精度因子（Geometric Dilution of Precision，GDOP）等。眾多研究采用以上指標(biāo)對觀測平臺的軌跡進(jìn)行了優(yōu)化，文獻(xiàn)[5]采用最小化廣義克拉美羅下界的跡為優(yōu)化準(zhǔn)則對AOA 協(xié)同定位下多無人機(jī)的路徑規(guī)劃進(jìn)行優(yōu)化；文獻(xiàn)[6]采用FIM 行列式的最大值作為目標(biāo)函數(shù)，優(yōu)化了TDOA 定位無人機(jī)群的軌跡；文獻(xiàn)[7]采用最小化GDOP 為準(zhǔn)則，優(yōu)化了單測向觀測器的運(yùn)動軌跡；上述研究均通過選取合適的指標(biāo)和優(yōu)化準(zhǔn)則，對觀測器的軌跡進(jìn)行了優(yōu)化并提高了定位精度。

然而，在針對如何提升定位精度，優(yōu)化觀測器軌跡的同時(shí)，也應(yīng)考慮來自障礙物或其他威脅等約束條件的影響。路徑規(guī)劃算法具有在復(fù)雜環(huán)境中為觀測器規(guī)劃出一條高效避障且威脅小的最優(yōu)路徑或次優(yōu)路徑的優(yōu)點(diǎn)，常見的路徑規(guī)劃算法有：人工勢場法[8]、動態(tài)窗口法（Dynamic Window Approach，DWA）[9]、A*算法、蟻群算法、遺傳算法[10]和粒子群算法[11]等。路徑規(guī)劃算法之間不僅可以相互融合，而且還能與實(shí)際的任務(wù)場景相結(jié)合。文獻(xiàn)[12]考慮了UAV 導(dǎo)航和雙基地合成孔徑雷達(dá)圖像分辨率2 方面問題，提出了一種基于約束自適應(yīng)多目標(biāo)差分進(jìn)化的路徑規(guī)劃算法。路徑規(guī)劃算法中DWA 算法不僅實(shí)現(xiàn)相對簡單，而且可以通過改變采樣策略、評價(jià)函數(shù)以及權(quán)重系數(shù)對算法進(jìn)行調(diào)整和擴(kuò)展，以適應(yīng)不同的應(yīng)用場景；文獻(xiàn)[13]將障礙物數(shù)量因子與方向角變化因子引入評價(jià)函數(shù)，提高了機(jī)器人障礙物效果的同時(shí)保障了安全性；文獻(xiàn)[14]提出了基于障礙物信息的速度權(quán)重動態(tài)調(diào)整機(jī)制，增強(qiáng)了無人機(jī)穿越密集障礙區(qū)域能力。

本文首先介紹了單觀測平臺基于TDOA 定位方法定位水下目標(biāo)的基本原理；分析了不同航跡下的定位精度；針對定位精度受航跡影響的問題，提出了一種基于改進(jìn)DWA 算法的航路規(guī)劃方法。引入了以HDOP 最小化為準(zhǔn)則構(gòu)造的定位精度評價(jià)子函數(shù)，并使定位精度和航向角評價(jià)函數(shù)的權(quán)重系數(shù)可動態(tài)調(diào)整；以期規(guī)劃出無碰撞且定位精度較高的航路。

1 定位態(tài)勢

水下靜止目標(biāo)的定位態(tài)勢如圖1 所示。坐標(biāo)系滿足右手定則，X軸、Y軸和Z軸分別指向正東、正北和垂直向上；被定位目標(biāo)位于海底，坐標(biāo)為；單艘觀測船按規(guī)劃航跡在海面上航行，接收由聲信標(biāo)發(fā)射的周期聲信號。設(shè)被定位目標(biāo)的聲脈沖的初始發(fā)射時(shí)刻為t0，周期為ts，則信號的到達(dá)時(shí)間可以表示為

圖1 定位態(tài)勢Fig. 1 Localization situation

式中：n為周期號；tn為第n個(gè)周期聲信號的到達(dá)時(shí)間；dn為tn時(shí)刻觀測船到目標(biāo)的距離，為tn時(shí)刻觀測船的坐標(biāo)，由GPS 測得；c為水中聲速。

2 TDOA 定位方法與精度分析

2.1 TDOA 定位方法基本原理

觀測船對目標(biāo)的TDOA 觀測方程可寫為

如圖2 所示，觀測船在定位過程中，會接收若干個(gè)周期聲信號，因此在已駛航路上會存在若干個(gè)定位節(jié)點(diǎn)，引入N× 1矩陣（集合Γ包含N個(gè)可用的定位節(jié)點(diǎn)）

圖2 單機(jī)動平臺TDOA 定位示意圖Fig. 2 Diagram of single motive station using TDOA localization

此時(shí)，基于TDOA 定位方法的數(shù)學(xué)模型寫為

基于TDOA 的定位問題實(shí)際上就是對式（3）方程組的求解問題。如圖2 所示，不利用冗余定位節(jié)點(diǎn)，至少需要在可用定位節(jié)點(diǎn)集合中選取出3 個(gè)定位解算節(jié)點(diǎn)，構(gòu)成雙曲線交匯模型才可以解算出目標(biāo)的水平坐標(biāo)[15]，此時(shí)對目標(biāo)的定位轉(zhuǎn)化為對以下方程組的求解

2.2 TDOA 定位精度分析

引入水平精度因子HDOP 作為評價(jià)定位精度的指標(biāo)，HDOP 的值越小，定位精度越高[16]。HDOP的表達(dá)式為

式中：Δr=[Δxs, Δys]T表示定位解算結(jié)果與目標(biāo)真實(shí)位置的偏差。

設(shè)Dx＝E( ΔrΔrT)為目標(biāo)水平坐標(biāo)測量誤差的協(xié)方差矩陣，Dx的計(jì)算公式為

式中：Dc、Dt、Dz和DGPS分別是聲速測量誤差、到達(dá)時(shí)間測量誤差、目標(biāo)深度測量誤差和觀測船水平坐標(biāo)測量誤差的協(xié)方差矩陣；Mx、Mc、Mt、Mz和MGPS分別是F＝ [f1,f2]T關(guān)于變量 [xs,ys]、c、[t1,t2,t3]、zs和 [x1,y1,x2,y2,x3,y3]的雅可比矩陣。假設(shè)各測量誤差均是相互獨(dú)立且服從零均值高斯分布的隨機(jī)變量，則上述協(xié)方差矩陣和雅可比矩陣的表達(dá)式為

式中：diag( ?) 表示對角矩陣；σc、σt、σz、σGPS分別表示聲速、信號到達(dá)時(shí)間、目標(biāo)深度和觀測船水平坐標(biāo)測量的均方根誤差。

對2 種航路下TDOA 法的定位精度進(jìn)行分析，參數(shù)設(shè)置如下：聲速為1 500 m/s；目標(biāo)位于以原點(diǎn)為中心的400 m× 400 m 區(qū)域內(nèi)，深度為50 m；σc、σt、σz和σGPS分別為1.5 m/s、1 ms、1 m 和2 m。2種航路下的定位精度分布情況如圖3 所示，其中灰線為觀測船的航路，紅點(diǎn)為在航路上選取的定位解算節(jié)點(diǎn)，定位解算節(jié)點(diǎn)從兩段距離相同的航路上被等長選取。由圖3 可知，航路1 對目標(biāo)區(qū)域的平均定位精度（3.16 m）比航路2 的平均定位精度（6.95 m）高；但是航路1 的盲區(qū)（黃色區(qū)域所示）比航路2要大。根據(jù)2 條航路下定位精度分布的特點(diǎn)，航路1和航路2 分別對應(yīng)已經(jīng)確定目標(biāo)大致區(qū)域后的精確定位階段和未確定目標(biāo)大致區(qū)域時(shí)的粗略定位階段。定位節(jié)點(diǎn)包圍目標(biāo)并呈等角分布是TDOA 定位的最優(yōu)定位節(jié)點(diǎn)分布形式[17]。如圖3（a）所示，航路1 的圓形航路下，更容易選取包圍目標(biāo)且接近等角分布的定位節(jié)點(diǎn)。因此，在精確定位階段，觀測船應(yīng)盡可能地航行在環(huán)繞目標(biāo)的圓形航路下。

圖3 TDOA 法的定位精度分布Fig. 3 Localization precision distribution for TDOA algorithm

由定位精度分析可知，定位精度不僅受測量誤差的影響，而且與定位節(jié)點(diǎn)的位置分布密切相關(guān)。由圖2 可知，觀測船的航路在很大程度上影響了定位節(jié)點(diǎn)的分布。因此，為提升定位精度，有必要對觀測船的航路進(jìn)行規(guī)劃。

3 航路規(guī)劃方法

3.1 傳統(tǒng)DWA 算法原理

DWA 算法包含了3 部分：1）根據(jù)自身動力學(xué)性能和環(huán)境障礙物的限制生成動態(tài)窗口并進(jìn)行速度采樣；2）基于運(yùn)動學(xué)模型和采樣的速度進(jìn)行軌跡預(yù)測；3）根據(jù)評價(jià)函數(shù)對預(yù)測軌跡進(jìn)行評估。

觀測船在水面上執(zhí)行定位任務(wù)時(shí)，速度采樣空間所受的限制如下。

1）觀測船最小最大航速、轉(zhuǎn)向速度限制。

式中：vmin和vmax為觀測船的最小航行速度和最大航行速度；ωmin和ωmax為觀測船的最小轉(zhuǎn)向速度和最大轉(zhuǎn)向速度。

2）觀測船最小最大航行加速度、轉(zhuǎn)向加速度限制。

式中：va和ωa分別為觀測船當(dāng)前航行速度和轉(zhuǎn)向速度；和分別是觀測船的最大航行加速度和最大轉(zhuǎn)向加速度。

3）環(huán)境障礙物限制。

式中：distance(v,ω)為預(yù)測航跡到最近障礙物的距離，保證了當(dāng)前速度下觀測船可在碰到障礙物前停止。

動態(tài)窗口的速度采樣空間Vr即為上述3 個(gè)集合的交集，滿足

在每個(gè)離散時(shí)間間隔Δt內(nèi)，對觀測船的運(yùn)動軌跡做近似化處理，看作是直線；則觀測船的運(yùn)動學(xué)模型可寫為

式中：xk，yk和θk分別為觀測船k時(shí)刻的X軸坐標(biāo)、Y軸坐標(biāo)和航向角；vk和ωk為觀測船k時(shí)刻的航行速度大小和轉(zhuǎn)向速度。

DWA 算法在速度采樣空間Vr中離散化地選擇可行的航行速度和轉(zhuǎn)向速度，基于觀測船的運(yùn)動學(xué)模型，就可向前預(yù)測出一段時(shí)間T后的航跡。航跡的預(yù)測如圖4 所示，綠色實(shí)線表示預(yù)測航跡。

圖4 航跡預(yù)測圖Fig. 4 Trajectory prediction diagram

對于每一條預(yù)測航跡，通過評價(jià)函數(shù)來評估其優(yōu)劣程度。最終，將最優(yōu)預(yù)測航跡對應(yīng)的航行速度和轉(zhuǎn)向速度作為控制量，控制觀測船航行并執(zhí)行任務(wù)。傳統(tǒng)DWA 算法的評價(jià)函數(shù)為

式中： heading(v,ω)為航向角評價(jià)函數(shù)，heading(v,ω) ＝π -θ，θ為觀測船在預(yù)測航跡末端時(shí)的航向和期望航向之間的角度偏差，期望航向?yàn)橛深A(yù)測航跡末端指向目標(biāo)點(diǎn)的方向； dist(v,ω)為避障評價(jià)函數(shù)， dist(v,ω) ＝distance(v,ω)，評估預(yù)測航跡到最近障礙物的距離，當(dāng)觀測船與障礙物之間的距離超過一定值時(shí)，設(shè)置為常數(shù)；velocity(v,ω)為速度評價(jià)函數(shù)， velocity(v,ω) ＝v，評估預(yù)測航跡對應(yīng)的速度大?。沪?、β、γ分別為各評價(jià)子函數(shù)的權(quán)重系數(shù)；σ表示對各評價(jià)子函數(shù)進(jìn)行歸一化。

3.2 改進(jìn)DWA 算法

傳統(tǒng)DWA 算法只考慮了如何在躲避障礙物的同時(shí)快速地抵達(dá)目標(biāo)，并未考慮定位精度問題。因此，本文在傳統(tǒng)DWA 算法的評價(jià)函數(shù)的基礎(chǔ)上，引入定位精度評價(jià)子函數(shù)，以提升對目標(biāo)的定位精度。

如圖5 所示，通常情況下，從歷史航跡中選取1號定位解算節(jié)點(diǎn)和2 號定位解算節(jié)點(diǎn)，當(dāng)前時(shí)刻選取為3 號定位解算節(jié)點(diǎn)，通過式(1)對目標(biāo)進(jìn)行實(shí)時(shí)定位解算，設(shè)1 號和2 號定位解算節(jié)點(diǎn)的坐標(biāo)分別為Xh,1和Xh,2。由式(11)向前預(yù)測某條航跡的同時(shí)，可根據(jù)聲信號發(fā)射周期ts，推算出觀測船在該條預(yù)測航跡上接收到未來M個(gè)聲信號的大致位置（M×ts≤T），并將此M個(gè)點(diǎn)作為預(yù)測定位解算節(jié)點(diǎn)（如圖5 中紅色圓點(diǎn)所示），設(shè)該條預(yù)測航跡上第j個(gè)預(yù)測定位解算節(jié)點(diǎn)的坐標(biāo)為Xp,j，j? [1,M]，則該條預(yù)測航跡上的預(yù)測定位解算節(jié)點(diǎn)坐標(biāo)集為{Xp,1,Xp,2,…,Xp,M}。

圖5 定位節(jié)點(diǎn)的選取示意圖Fig. 5 Diagram of localization nodes selection

本文以HDOP 最小化為優(yōu)化準(zhǔn)則，對預(yù)測航跡進(jìn)行針對定位精度的評價(jià)。由HDOP 的計(jì)算式可知，在測量誤差已知或已假設(shè)的條件下，HDOP 是只與目標(biāo)和3 個(gè)定位解算節(jié)點(diǎn)的坐標(biāo)有關(guān)的函數(shù)，但是在定位問題中目標(biāo)的真實(shí)坐標(biāo)Xs始終未知，因此計(jì)算HDOP 時(shí)，由定位解算結(jié)果替代。HDOP 值越大，定位精度越低，定位精度評價(jià)函數(shù)值與HDOP值呈負(fù)相關(guān)。因此，定義該條預(yù)測航跡的定位精度評價(jià)函數(shù)為

整個(gè)過程采用邊定位目標(biāo)邊規(guī)劃航路的策略，在定位初期階段，觀測船距離目標(biāo)位置較遠(yuǎn)，定位誤差較大，定位解算結(jié)果不準(zhǔn)確， hdop(v,ω)的可靠性差，觀測船應(yīng)盡量快地抵達(dá)目標(biāo)所在區(qū)域。隨著觀測船與目標(biāo)間距離的減小，信噪比增大，時(shí)延估計(jì)標(biāo)準(zhǔn)差σt反比于信噪比[18]，聲吶對信號到達(dá)時(shí)間tn的測量精度提高，定位結(jié)果逐漸趨于真值Xs，計(jì)算出的hdop(v,ω)逐漸可靠。因此，在觀測船靠近目標(biāo)的定位過程中，隨著觀測船與目標(biāo)距離的減小，定位精度評價(jià)函數(shù)的權(quán)重應(yīng)該逐漸增加，航向角評價(jià)函數(shù)的權(quán)重逐漸降低。

如圖6 所示，考慮到水下定位所使用的聲吶垂直開角范圍有限[19]，為避免觀測船過于靠近目標(biāo)，導(dǎo)致目標(biāo)進(jìn)入盲區(qū)而丟失信號，需設(shè)計(jì)一個(gè)最小水平距離rm。綜上所述，改進(jìn)后的評價(jià)函數(shù)為

圖6 聲吶垂直開角示意圖Fig. 6 Vertical directivity of sonar

式中：λ為定位精度評價(jià)函數(shù)的權(quán)重系數(shù)；p為觀測船到目標(biāo)位置的水平距離。

由式（15）可知，在遠(yuǎn)距離情況下（p?rm），ε的值較小，此時(shí)heading(v,ω)對評價(jià)函數(shù)的影響較大，觀測船快速地向目標(biāo)區(qū)域靠近。觀測船在接近目標(biāo)的過程中，ε的值趨于1，此時(shí)heading(v,ω)對評價(jià)函數(shù)的影響降低，hdop(v,ω)對評價(jià)函數(shù)的影響增大，航路的規(guī)劃更多地取決于hdop(v,ω)。

4 仿真驗(yàn)證

通過仿真實(shí)驗(yàn)對本文所提算法的有效性進(jìn)行驗(yàn)證。水下目標(biāo)的水平坐標(biāo)設(shè)為[–400，–400] m，深度為70 m，發(fā)射周期為1 s；聲速為1500 m/s；最小水平距離rm為180 m；觀測船起點(diǎn)為[–700，800] m；環(huán)境中設(shè)置8 個(gè)障礙物，其位置坐標(biāo)分別為[–200，0] m，[–200，200] m，[–200，–300] m，[170，–200] m，[70，–200] m，[–400，–700] m，[100，100] m，[200，200] m，半徑分別為30 m，20 m，30 m，30 m，20 m，20 m，30 m，40 m；測量誤差的參數(shù)設(shè)置同定位精度分析時(shí)一致，仿真參數(shù)見表1，其中動力學(xué)性能參數(shù)參考自文獻(xiàn)[20]。本文進(jìn)行了3 種航路下的仿真實(shí)驗(yàn)，分別是航路1：常規(guī)勻速直線航路；航路2：基于傳統(tǒng)DWA 算法規(guī)劃出的直達(dá)目標(biāo)區(qū)域的航路；航路3：基于改進(jìn)DWA 算法規(guī)劃出的航路。

表1 仿真參數(shù)Table 1 Parameters

1）航路1。

規(guī)劃的航路如圖7（a）所示，其中藍(lán)色為規(guī)劃的航路，黑色為障礙物，紅色星號為目標(biāo)，藍(lán)色圓點(diǎn)為所選取的3 個(gè)定位節(jié)點(diǎn)。為簡化處理，仿真中所用的節(jié)點(diǎn)選取策略為：當(dāng)前位置為3 號定位節(jié)點(diǎn)，并往前每隔100 個(gè)周期號選取一個(gè)定位節(jié)點(diǎn)；定位結(jié)果如圖7（b）所示；整個(gè)定位過程中定位誤差如圖7（c）所示。此常規(guī)勻速直線航路為基于傳統(tǒng)DWA算法向預(yù)設(shè)終點(diǎn)引導(dǎo)而規(guī)劃出的，預(yù)設(shè)終點(diǎn)的位置為[800，–100] m。

圖7 航路1 的仿真結(jié)果Fig. 7 Simulation results of Path 1

2）航路2 和航路3。

圖8（a）和圖8（d）中前一段藍(lán)色航路與圖7（a）中一致，同為基于傳統(tǒng)DWA 算法向預(yù)設(shè)終點(diǎn)引導(dǎo)而規(guī)劃出的直線航路。此段藍(lán)色航路期間TDOA 定位的基線長度隨著船的航行而不斷增長，定位精度隨基線的增長而提升，待基線增長至一定長度，定位解算結(jié)果較可靠之后，可得出目標(biāo)所在大致區(qū)域，然后再基于傳統(tǒng)DWA 算法和改進(jìn)DWA算法規(guī)劃航路（如圖8（a）和圖8（d）中紅色航路所示）。

圖8 航路2 和航路3 的仿真結(jié)果Fig. 8 Simulation results of Path 2 and Path 3

由圖8（c）可知，當(dāng)觀測船直接向目標(biāo)抵近時(shí)，定位誤差很大，甚至無法進(jìn)行定位解算。這是因?yàn)門DOA 定位方法在基線方向上的定位精度很低，尤其當(dāng)目標(biāo)處于定位節(jié)點(diǎn)包圍區(qū)域外并和3 個(gè)定位節(jié)點(diǎn)的任何2 個(gè)趨于共線時(shí)，定位精度變得極差，無法對聲源進(jìn)行有效定位。因此，當(dāng)使用單觀測平臺基于TDOA 定位方法對目標(biāo)進(jìn)行定位時(shí)，不能使觀測船直接向目標(biāo)所在區(qū)域抵近。對比圖7 和圖8 可以看出，改進(jìn)DWA 算法規(guī)劃航路下的定位結(jié)果更接近于目標(biāo)真實(shí)位置，定位誤差更小，并且觀測船在抵達(dá)最小水平距離rm后，在定位精度評價(jià)函數(shù)的作用下自動選擇了定位精度更高的圓形航路繞目標(biāo)航行。傳統(tǒng)DWA 算法規(guī)劃的航路1 在整個(gè)定位過程中定位精度穩(wěn)定，均方根誤差為55.9 m；改進(jìn)DWA 算法規(guī)劃的航路 3 在整個(gè)定位過程中的均方根誤差為31.2 m，后半段的均方根誤差為5.3 m；航路2 因其無法有效定位，故不討論其誤差。因此，改進(jìn)DWA算法規(guī)劃的航路不僅有效避開了障礙物，而且由于定位精度評價(jià)函數(shù)的引入，航路對應(yīng)的定位精度得以提高。

5 結(jié)束語

針對單艘觀測船TDOA 定位水底靜止目標(biāo)的問題，本文提出了一種基于改進(jìn)DWA 算法的航路規(guī)劃方法。首先，結(jié)合DWA 算法向前預(yù)測的特點(diǎn)，設(shè)計(jì)了一種定位精度評價(jià)子函數(shù)，提升了定位精度。其次，根據(jù)觀測船與目標(biāo)的距離關(guān)系，平衡了航向角評價(jià)子函數(shù)和定位精度評價(jià)子函數(shù)的權(quán)重系數(shù)，使航路規(guī)劃方法更加貼合定位任務(wù)。最后，通過仿真實(shí)驗(yàn)對比了傳統(tǒng)DWA 算法和改進(jìn)DWA算法，證明了基于改進(jìn)DWA 算法所規(guī)劃出的航路相比傳統(tǒng)算法的定位精度有顯著提高。