定位精度優化驅動的水下無人平臺航路規劃方法

孫思博，秦莎莎，王 庸，明瑞和，王 勇

（1.哈爾濱工程大學 水聲工程學院，黑龍江 哈爾濱 150001；2.哈爾濱工業大學 電子與信息工程學院，黑龍江 哈爾濱 150001）

0 引言

隨著無人平臺的發展速度越來越快，相關研究也越來越廣泛，航路規劃作為無人平臺的關鍵技術之一，對無人平臺的自主航行起著至關重要的作用。航路規劃是指在具有障礙物的環境中，能夠規劃出一條從起始位置狀態到目標位置狀態無碰撞的最優路徑或次優路徑，并在滿足所有約束條件最優狀態下完成設定任務[1]。

1 水聲定位方法與航路規劃簡介

1.1 水聲定位

基本的聲學定位方法分為3類[2]。第1類是基于信號到達時間（Time of Arrival，TOA）的定位方法。TOA法觀察信號傳播時延，傳播時延與當前定位節點與目標間的距離成正比，其需要信號發送方與接收方建立時間同步。因此在非合作信號的水聲定位中不可用。

第2類是基于信號到達時間差（Time Difference of Arrival，TDOA）的定位方法。TDOA法不需要獲得信號具體發射時間，可以實現對非合作信號的定位。徐英杰[3]等人提出了一種基于牛頓迭代方法的三星時差定位融合算法，運用多組時差和高程信息融合，有效提高了定位精度。

第3類是基于信號到達方向（Direction of Arrival，DOA）的定位方法。DOA法利用目標的角度信息，通過傳感器陣列獲取目標的角度信息。傳統的目標源位置定位通過笛卡爾坐標系確定，陳嫻靜[4]等人提出了一種基于到達角的近遠場統一定位方法，使用修正后的極坐標表示定位目標的坐標，避免了目標位于遠場時數值問題的出現。

1.2 航路規劃

傳統的水下無人平臺航路規劃主要基于人工勢場法、蟻群法、遺傳算法等。M.Mucientes等[5]提出了一種采用遺傳算法的模糊控制器的自動化設計，該算法基于迭代規則學習（IRL）方法，定義了選擇規則數量參數，用于在機器人路徑規劃中繞過障礙物。該算法主要考慮航路規劃的避障問題，并沒有充分考慮到載荷任務的任務執行效率。王金龍[6]針對傳統人工勢場法可能會出現局部最小點和目標不可達問題，在人工勢場的引力場中增加了振蕩函數，當無人艇出現陷入局部最優解無法前進問題時，可以有選擇其他的備選路線。該方法也是基于航路規劃中的避障問題所提出的解決辦法，也沒有考慮提高航路規劃的任務執行效率。以上方法都沒有針對定位問題進行優化，導致定位精度下降，本文采用定位精度優化驅動的水下無人平臺航路規劃方法，在進行避障的同時，進一步提高定位精度。通過仿真實驗和湖試結果對該方法進行了驗證。

本文的其他部分如下：在第2節中，本文選取TDOA法對目標位置坐標進行定位解算，給出了TDOA法的定位模型，分析了 TDOA法的定位精度。第3節討論了傳統人工勢場法在執行定位任務時的局限性，提出了定位精度優化驅動的水下無人平臺航路規劃方法，并進行了理論分析。第4節針對本文提出的方法，進行了仿真分析，并在真實環境中對該方法進行了驗證。

2 TDOA定位法與精度分析

2.1 TDOA定位方法

本文使用的全球坐標系如圖1所示。X軸、Y軸、Z軸分別表示東、北和垂直向上的方向，圖1顯示了水下定位的幾何結構，所定位目標位于海底，坐標表示為[xs,ys,zs]，目標深度可由海圖測量。定位解算節點瞬時位置由GPS獲得，表示為[xi,yi,zi]，主要求解目標的水平坐標xs和ys。

圖1 水下定位過程中的幾何配置Fig.1 Geometrical configuration for the UWAL

令ts為信號發射周期，ta為信號初始發射時刻，第2個信號發射時刻為ta+ts，ta+nts為第n個信號的發送時刻。ti為第i個解算節點的信號接收時刻，ni為接收信號的周期號，c為聲速。di表示目標到第i個解算節點的直線距離，di=。TDOA法定位時選取2個解算節點，其時間差為

求解目標位置至少需要3個解算節點，基于TDOA法的數學模型為

式中：距離di是待求量xs和ys的非線性函數，因此 TDOA法的數學模型是非線性方程，可以采用牛頓迭代法進行求解。

2.2 TDOA法的定位精度分析

使用偏微分法分析 TDOA法的定位精度，將各變量的誤差看作相互獨立的且均值為0的服從高斯分布的隨機變量，引入HDOP（水平精度因子）表示定位精度，HDOP的數值越大，定位誤差越大，其定位精度越差[7]。式（3）中的E[ ]表示對變量求期望，HDOP可以表示為

式中：[xs,ys]表示目標的真實位置坐標；表示目標的解算估計坐標。對TDOA法定位表達式各變量求偏微分，可以得到

將各個偏導數寫成矩陣形式

改寫為矩陣形式

矩陣運算表達式為

式中：Dd，DINS，Dt，Dc和Md，MINS，Mt，Mc分別是深度測量誤差、解算節點坐標誤差、到達時間誤差、聲速測量誤差的協方差矩陣和其偏導數矩陣。各變量誤差協方差矩陣通過對角陣表示為

式中：diag()為對角矩陣，σd2，σINS2，σt2，σc2分別是深度測量、解算節點坐標、到達時間和聲速測量的均方誤差[8]。HDOP表示為HDOP=。

根據以上公式，對 TDOA法的性能進行仿真分析，仿真參數設置如表1所示。

表1 仿真參數和測量噪聲參數Tab.1 Simulation parameters and measurement noise parameters for the simulation

兩組定位精度仿真結果如圖2所示。

圖2 TDOA法的定位精度分布Fig.2 Positioning accuracy distribution for the TDOA algorithm

從圖中可以看出，TDOA法在解算節點呈等腰三角形分布時有較好的定位精度，其中1 km×1 km正方形區域內的平均定位精度為4.3 m。當定位節點從弧形軌跡上選取時，定位精度下降明顯，定位精度高的區域存在斷層，分別位于3個定位解算節點的上方和下方，區域內的平均定位精度為15.04 m。因此，定位節點的選擇對于TDOA法在進行定位解算時具有較大影響，選取節點時盡量選取一個圓形軌跡的內切等邊三角形。

進行目標位置定位時，選取不同的航跡，對TDOA法的定位精度進行分析。各變量誤差與表1相同，其中目標水平位置坐標為[0，0]，分別對圓形航跡和直線航跡進行仿真分析，從周期號為200的節點開始進行定位解算。航行軌跡如圖 3（a）所示。

圖3 不同航跡下TDOA法的定位精度Fig.3 Positioning accuracy for different navigation trajectories

從圖 3（b）中可以看出，直線航跡的定位誤差起伏大，定位精度不穩定，魯棒性差；圓形航跡的定位誤差起伏小，定位精度高，魯棒性好。因此，在定位過程中，航跡對 TDOA法的定位結果影響大，盡量選擇圍繞目標的圓形航跡。

3 航路規劃方法

3.1 傳統人工勢場法

人工勢場法航路規劃由 Khatib最先提出，是一種通過虛擬力來規劃航跡的算法。基本思想是將運動的機器人放在抽象的人為制造的力場中，目標點產生“引力”，障礙物產生“斥力”，各個力以矢量和形式疊加在機器人上，通過產生的合力來規劃航路。一般來說，在虛擬的人工勢場中，出發點勢能較大，障礙物等不可到達位置相當于高峰，而目標點相當于谷地的最低處，如同在勢場中出發點放置一個小球，小球在終點的引力與障礙物的斥力的共同作用下的軌跡就作為無人艇的運動軌跡，能夠自如地避開障礙物，到達終點，應用勢場法規劃出來的路徑一般是光順并確保能避開障礙物比較安全的[9]。

傳統人工勢場法基于目標點產生“引力”，障礙物產生“斥力”，各個力以矢量和形式疊加，通過產生的合力規劃航路。AUV進行航路規劃時，令引力來源為解算出的目標位置坐標，斥力來源為海圖中的障礙物或航禁區，AUV在引力和斥力的共同疊加下前行[10]。通常將斥力函數設為

式中：η為常數，為斥力尺度因子；c為斥力次數，通常設為-2；ρ為勢場中某點到障礙物中心點的距離；0ρ為障礙物的影響半徑，以障礙物中心點為原點，0ρ為半徑的圓內，其勢場值為Φ，Φ為一個極大的值；當勢場中某點到障礙物中心點的距離大于ρ0時，該點的勢場隨著ρ的增大而減小。

勢場中的斥力為斥力場的梯度，斥力是矢量，即Frep=(Fx,Fy)。

目標點產生的引力場采用類似重力與重力勢能的建模方式，如下式

式中：ξ為引力尺度因子；ρ為勢場中某點到目標點的距離。AUV在引力場中受到的引力為引力場的梯度，如下式

進行仿真建模：起點為圖4（a）左下角的紅點，終點為右上角的紅點，4個黑色的點為障礙物。斥力勢場的建模如圖 4（b）所示，4個不可到達點分別產生勢能很高的區域，每個障礙點的作用距離有限。引力勢場的建模如圖 4（c）所示，相對較為平整，作用于整個區域。引力勢場與斥力勢場的疊加勢場如圖4（d）所示，紅線為AUV從起點出發到達終點的軌跡，避開了障礙物，順利平滑地抵達了終點。

圖4 傳統人工勢場法示意圖Fig.4 Schematic diagram for the traditional artificial potential field method

傳統的人工勢場法只考慮了避障問題，并沒有充分考慮到定位精度問題，因此在執行定位任務時，往往會導致航行到定位誤差大的區域，使得任務執行效率低。因此，在航路規劃中，本文引入定位精度勢場，將定位精度勢場與人工勢場結合，在避障的同時，提高定位精度。

3.2 結合定位精度勢場的人工勢場

在傳統人工勢場的基礎上，疊加根據定位誤差所建立的定位精度勢場，避免AUV駛入定位誤差突然增大的區域。定位精度勢場的建立，首先對定位精度的分布進行分析。基于TDOA法，根據2.2節所提到的偏微分法求 HDOP的方法，某解算點的HDOP值的求解需要在誤差已知的情況下輸入 4個坐標，即黑匣子的位置坐標、2個歷史定位解算點坐標以及當前定位解算點的坐標。歷史解算節點的選取采取以下措施：目前航路中定位解算點的個數不足1 000時，選取出發點作為歷史解算點，以增大解算點之間的距離，有利于拉開解算尺度，減小誤差；當解算點的個數大于 1 000時，選取當前位置的前1 000個位置作為第1個歷史定位解算點，第500個位置作為第2個歷史定位解算節點，在每個解算點之間的距離得到保證的前提下，選取較為接近黑匣子的解算點，有利于減小由距離產生的解算誤差。

仿真的變量誤差與表1相同，分析定位精度勢場時，當前位置節點遍歷整片待計算的區域，所得定位精度分布如圖5（a）所示，第1解算點和第2解算點為歷史解算點，圖中可以看出當前解算節點不能位于圖中的黃色低定位精度區域。

圖5 定位精度勢場示意圖Fig.5 Schematic diagram for positioning accuracy potential field

以定位精度為基礎，將 HDOP看作勢能值，如圖 5（b）所示，黃色區域勢能較高，藍色區域勢能低，在定位精度勢場作用下產生的模擬力將會促使AUV駛向使目標定位精度更高的點。定位精度勢場產生的斥力表示為

式中：FHDOP,X(x,y)為誤差梯度力在X方向上的分量；FHDOP,Y(x,y)為誤差梯度力在Y方向上的分量；HDOP(x,y)為在位置(x,y)處的誤差值。

將引力、斥力、誤差梯度力做矢量疊加作為合力，引導AUV前進。引力場和斥力場是恒定的，如圖6（a）所示，定位精度勢能場隨著AUV的位置變動而發生變化。選取一個特定點作為第1解算點的疊加勢能場如圖 6（b）所示，特定點的位置即圖中標示出的“當前位置”，圖中紅線為模擬行駛的路徑。

圖6 疊加定位精度勢場后的勢場示意圖Fig.6 Schematic diagram for the total potential field after adding positioning accuracy potential field

由于實際AUV定位載荷（通常是安裝在腹鰭的水聽器陣列）有一定的指向性，因此，精確定位時期，加入核力勢場。精確定位階段的目的是精確測量目標位置，在精確定位階段指引AUV前進時，盡可能保證軌跡為一個圓，同時還要避開可能出現的障礙物，因此引入類似核力的力場建模。將核力函數設為

式中：η為核力尺度因子；ρ為勢場中某點到障礙物中心點的距離；0ρ為核力的穩定半徑，核力的分布如下

核力勢場與引力勢場的疊加勢場如圖 7（a）所示，在疊加了核力后的航路如圖7（b）所示。

圖7 疊加核力勢場后的總勢場示意圖Fig.7 Schematic diagram for the total potential field after adding nuclear force potential field

核力的建模非常適用于AUV航路規劃的精確定位階段，在設定好圓形航路半徑的情形下，AUV距目標的距離大于該半徑，指引AUV靠近目標；反之，指引AUV適當遠離目標，始終保持在圓形軌跡上。結合其他力場，在精確定位階段也能實現避障的基本操作。

4 實驗

4.1 仿真實驗

仿真過程中，對AUV 進行航路規劃時，在引力勢場和斥力勢場中加入定位精度勢場，以及在精確定位階段加入核力勢場，各個勢場力的矢量指引AUV行進。從圖8（a）中可以看出，規劃航跡后的AUV避開了所有障礙物。仿真實驗中，令目標實際坐標為[0，0，50] m，根據圖 8（a）中的航跡，對目標位置坐標進行定位解算，測量噪聲參數與表 1相同。為了有一定的對比性，同時根據只具備避障能力的AUV航跡對目標位置坐標進行定位解算。

圖8 仿真結果Fig.8 Simulation results

圖8（b）可以看出，規劃后的航跡比只具備避障能力的航跡定位結果更加精確。從圖 8（c）中可以看出，規劃后的航跡定位誤差小于只具備避障能力的航跡定位誤差，雖然規劃后的航跡因為受到實時航路的影響，前期定位結果誤差較大，但是在后期的精確定位階段，規劃后的航跡有很高的定位精度，可以更加精確的實現對目標位置坐標的定位。在精確定位階段，規劃后的航跡平均定位誤差為2.26 m，只具備避障能力的航跡定位誤差為4.29 m。

4.2 湖試實驗

為了進一步驗證該方法的有效性，在松花湖進行了實際實驗。在湖試中，用水面船代替AUV。目標固定在湖底，通過壓力傳感器測量目標的深度為62 m。水聽器陣列固定在船上，其位置可由GPS測量，聲速為1 486 m/s。圖9中綠色的點為障礙物，黑色的點為解算出的黑匣子位置坐標。

圖9 航路規劃及定位結果圖Fig.9 Result for path planning and underwater positioning

從船的航跡來看，在障礙物斥力的作用下，其避開了所有障礙點，并逐步定位到黑匣子所在位置，完成了對船進行航路規劃的要求。雖然在船航行前期定位誤差較大，但在精確定位階段，定位精度高，平均定位誤差在2.45 m，滿足水下定位任務對高定位精度的要求。實驗結果與理論分析吻合，證明了該方法的有效性。