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

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

0 引言

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

1 水聲定位方法與航路規(guī)劃簡介

1.1 水聲定位

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

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

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

1.2 航路規(guī)劃

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

本文的其他部分如下：在第2節(jié)中，本文選取TDOA法對目標位置坐標進行定位解算，給出了TDOA法的定位模型，分析了 TDOA法的定位精度。第3節(jié)討論了傳統(tǒng)人工勢場法在執(zhí)行定位任務(wù)時的局限性，提出了定位精度優(yōu)化驅(qū)動的水下無人平臺航路規(guī)劃方法，并進行了理論分析。第4節(jié)針對本文提出的方法，進行了仿真分析，并在真實環(huán)境中對該方法進行了驗證。

2 TDOA定位法與精度分析

2.1 TDOA定位方法

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

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

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

求解目標位置至少需要3個解算節(jié)點，基于TDOA法的數(shù)學(xué)模型為

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

2.2 TDOA法的定位精度分析

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

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

將各個偏導(dǎo)數(shù)寫成矩陣形式

改寫為矩陣形式

矩陣運算表達式為

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

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

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

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

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

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

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

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

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

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

3 航路規(guī)劃方法

3.1 傳統(tǒng)人工勢場法

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

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

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

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

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

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

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

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

傳統(tǒng)的人工勢場法只考慮了避障問題，并沒有充分考慮到定位精度問題，因此在執(zhí)行定位任務(wù)時，往往會導(dǎo)致航行到定位誤差大的區(qū)域，使得任務(wù)執(zhí)行效率低。因此，在航路規(guī)劃中，本文引入定位精度勢場，將定位精度勢場與人工勢場結(jié)合，在避障的同時，提高定位精度。

3.2 結(jié)合定位精度勢場的人工勢場

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

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

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

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

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

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

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

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

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

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

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

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

4 實驗

4.1 仿真實驗

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

圖8 仿真結(jié)果Fig.8 Simulation results

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

4.2 湖試實驗

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

圖9 航路規(guī)劃及定位結(jié)果圖Fig.9 Result for path planning and underwater positioning

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