單機動平臺時差定位航路規劃方法

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

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

0 引言

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

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

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

本文首先介紹了單觀測平臺基于TDOA 定位方法定位水下目標的基本原理；分析了不同航跡下的定位精度；針對定位精度受航跡影響的問題，提出了一種基于改進DWA 算法的航路規劃方法。引入了以HDOP 最小化為準則構造的定位精度評價子函數，并使定位精度和航向角評價函數的權重系數可動態調整；以期規劃出無碰撞且定位精度較高的航路。

1 定位態勢

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

圖1 定位態勢Fig. 1 Localization situation

式中：n為周期號；tn為第n個周期聲信號的到達時間；dn為tn時刻觀測船到目標的距離，為tn時刻觀測船的坐標，由GPS 測得；c為水中聲速。

2 TDOA 定位方法與精度分析

2.1 TDOA 定位方法基本原理

觀測船對目標的TDOA 觀測方程可寫為

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

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

此時，基于TDOA 定位方法的數學模型寫為

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

2.2 TDOA 定位精度分析

引入水平精度因子HDOP 作為評價定位精度的指標，HDOP 的值越小，定位精度越高[16]。HDOP的表達式為

式中：Δr=[Δxs, Δys]T表示定位解算結果與目標真實位置的偏差。

設Dx＝E( ΔrΔrT)為目標水平坐標測量誤差的協方差矩陣，Dx的計算公式為

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

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

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

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

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

3 航路規劃方法

3.1 傳統DWA 算法原理

DWA 算法包含了3 部分：1）根據自身動力學性能和環境障礙物的限制生成動態窗口并進行速度采樣；2）基于運動學模型和采樣的速度進行軌跡預測；3）根據評價函數對預測軌跡進行評估。

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

1）觀測船最小最大航速、轉向速度限制。

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

2）觀測船最小最大航行加速度、轉向加速度限制。

式中：va和ωa分別為觀測船當前航行速度和轉向速度；和分別是觀測船的最大航行加速度和最大轉向加速度。

3）環境障礙物限制。

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

動態窗口的速度采樣空間Vr即為上述3 個集合的交集，滿足

在每個離散時間間隔Δt內，對觀測船的運動軌跡做近似化處理，看作是直線；則觀測船的運動學模型可寫為

式中：xk，yk和θk分別為觀測船k時刻的X軸坐標、Y軸坐標和航向角；vk和ωk為觀測船k時刻的航行速度大小和轉向速度。

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

圖4 航跡預測圖Fig. 4 Trajectory prediction diagram

對于每一條預測航跡，通過評價函數來評估其優劣程度。最終，將最優預測航跡對應的航行速度和轉向速度作為控制量，控制觀測船航行并執行任務。傳統DWA 算法的評價函數為

式中： heading(v,ω)為航向角評價函數，heading(v,ω) ＝π -θ，θ為觀測船在預測航跡末端時的航向和期望航向之間的角度偏差，期望航向為由預測航跡末端指向目標點的方向； dist(v,ω)為避障評價函數， dist(v,ω) ＝distance(v,ω)，評估預測航跡到最近障礙物的距離，當觀測船與障礙物之間的距離超過一定值時，設置為常數；velocity(v,ω)為速度評價函數， velocity(v,ω) ＝v，評估預測航跡對應的速度大??；α、β、γ分別為各評價子函數的權重系數；σ表示對各評價子函數進行歸一化。

3.2 改進DWA 算法

傳統DWA 算法只考慮了如何在躲避障礙物的同時快速地抵達目標，并未考慮定位精度問題。因此，本文在傳統DWA 算法的評價函數的基礎上，引入定位精度評價子函數，以提升對目標的定位精度。

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

圖5 定位節點的選取示意圖Fig. 5 Diagram of localization nodes selection

本文以HDOP 最小化為優化準則，對預測航跡進行針對定位精度的評價。由HDOP 的計算式可知，在測量誤差已知或已假設的條件下，HDOP 是只與目標和3 個定位解算節點的坐標有關的函數，但是在定位問題中目標的真實坐標Xs始終未知，因此計算HDOP 時，由定位解算結果替代。HDOP 值越大，定位精度越低，定位精度評價函數值與HDOP值呈負相關。因此，定義該條預測航跡的定位精度評價函數為

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

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

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

式中：λ為定位精度評價函數的權重系數；p為觀測船到目標位置的水平距離。

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

4 仿真驗證

通過仿真實驗對本文所提算法的有效性進行驗證。水下目標的水平坐標設為[–400，–400] m，深度為70 m，發射周期為1 s；聲速為1500 m/s；最小水平距離rm為180 m；觀測船起點為[–700，800] m；環境中設置8 個障礙物，其位置坐標分別為[–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；測量誤差的參數設置同定位精度分析時一致，仿真參數見表1，其中動力學性能參數參考自文獻[20]。本文進行了3 種航路下的仿真實驗，分別是航路1：常規勻速直線航路；航路2：基于傳統DWA 算法規劃出的直達目標區域的航路；航路3：基于改進DWA 算法規劃出的航路。

表1 仿真參數Table 1 Parameters

1）航路1。

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

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

2）航路2 和航路3。

圖8（a）和圖8（d）中前一段藍色航路與圖7（a）中一致，同為基于傳統DWA 算法向預設終點引導而規劃出的直線航路。此段藍色航路期間TDOA 定位的基線長度隨著船的航行而不斷增長，定位精度隨基線的增長而提升，待基線增長至一定長度，定位解算結果較可靠之后，可得出目標所在大致區域，然后再基于傳統DWA 算法和改進DWA算法規劃航路（如圖8（a）和圖8（d）中紅色航路所示）。

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

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

5 結束語

針對單艘觀測船TDOA 定位水底靜止目標的問題，本文提出了一種基于改進DWA 算法的航路規劃方法。首先，結合DWA 算法向前預測的特點，設計了一種定位精度評價子函數，提升了定位精度。其次，根據觀測船與目標的距離關系，平衡了航向角評價子函數和定位精度評價子函數的權重系數，使航路規劃方法更加貼合定位任務。最后，通過仿真實驗對比了傳統DWA 算法和改進DWA算法，證明了基于改進DWA 算法所規劃出的航路相比傳統算法的定位精度有顯著提高。