一種基于導航誤差空間的無人水下航行器路徑規劃方法

嚴浙平，趙玉飛，陳濤，周佳加

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱150001)

0 引言

無人水下航行器(UUV)具有工作時間長、環境復雜、信息源少、隱蔽性高等特點［1］。傳感器精度、控制誤差和非精確環境模型會對航行器的導航和運動帶來諸多不確定性因素。現有的傳統規劃方法，常忽視不斷累積的導航誤差，使得規劃的路徑不安全或不可行。因此在規劃層面，考慮導航誤差變化對路徑的影響，對于規劃路徑的安全性和可行性有重要意義，基于導航不確定性的規劃(PNU)獲得越來越多的重視。

PNU 問題在機器人領域研究較多，部分可觀馬尓可夫決策過程(POMDP)是不確定性路徑規劃的常用方法［2］，將環境狀態的概率分布視為信度狀態，尋找最大獎勵代價下的最優行為策略。文獻［3］綜合考慮運動、感知和環境不確定性，提出基于采樣的POMDP 全局規劃方法，但POMDP 需要較大的計算量，且在高維度的環境中容易出現維數災難問題。基于采樣法的概率路徑圖法(PRM)［4］和快速搜索隨機樹(RRT)法［5］能有效解決高維決策問題，但對連續狀態空間不適用。滾動時域控制(RHC)可在線對每一小段路徑進行有限時域約束優化，能有效處理不確定信息［6］。其他方法還有動態A*［7］、貝葉斯［8］等。上述方法，由于處理速度和存儲空間有限，一般只應用于小范圍空間。

為滿足大范圍路徑規劃，文獻［9 -10］提出在誤差擴展空間上進行全局路徑搜索，但建立的誤差傳遞模型一般較簡單，如文獻［9］中采用單變量高斯誤差模型，誤差按路徑長度線性增長，在處理速度上有較大優勢，卻不符合實際導航系統的特性。

針對UUV 的導航特點和長航程任務需要，為減少導航誤差對路徑選擇造成的不利影響，基于拓展規劃空間的基本思想，提出一種基于導航誤差空間的全局路徑規劃方法，將非精確導航信息轉化為環境模型中的未知環境信息，融合到路徑規劃之中，綜合考慮路徑長度、導航不確定性等約束條件，實現安全性和適航性最優。

1 問題分析

針對水下長航程隱蔽航行任務，由常用的捷聯慣導/多普勒測速儀(SINS/DVL)艦位推算為主要導航信息，以地形匹配輔助導航，在規劃中需考慮如下約束:

1)航程約束。由于存在能量或航行時間限制，航路長度會有限制，最大航程為各航路長度和，取決于UUV 的物理限制。

2)導航誤差的影響。隨著航行時間增加，導航誤差的累積使UUV 與障礙之間的安全區域逐漸減少，使預先選擇的自由路徑不可行或執行困難。導航誤差越大，UUV 與障礙物發生碰撞的概率越大，帶來的安全問題不容忽視。

3)隱蔽性要求。全程水下作業，不能出水進行GPS 校正，采用地形匹配輔助導航。

4)終點誤差要求。為方便安全回航與出水回收，終點導航誤差應小于某容許范圍。

因此，PNU 不再以精確導航信息為假設前提，在環境模型中充分考慮導航誤差的影響，規劃中，盡可能減少導航誤差對路徑選擇造成的不利影響。

2 基于導航誤差空間的環境模型

首先對UUV 艦位推算系統誤差源進行分析。導航誤差是指航行器推算位置與實際位置之間的偏差，主要來自DVL 測速誤差和航向角誤差，可視為高斯白噪聲。

2.1 UUV 導航誤差建模

在固定坐標系OXeYe下，UUV 的水平面位姿狀態表示為q =(X，θ)=(x，y，θ)∈R3，包括UUV 的位置X 和航向角θ.控制輸入U=(v，ω)∈R2，分別代表UUV 的前向速度和航向角速度，水平面運動模型如圖1所示，運動學方程可表示為

圖1 UUV 水平面運動模型Fig.1 UUV horizontal plane motion model

由于傳感器存在測量噪聲，根據傳感器信息輸入只能獲得的UUV 下一拍位姿qk+1的預測值。在qk+1臨近區域，對(1)式采用泰勒級數展開，取一階展開式得近似線性方程［12］

式中:σ2v是前向速度誤差的方差;σ2w是航向誤差的方差。

而Σqk決定了位姿狀態矢量落在概率橢球體內的可能性。根據等概率密度曲面方程(qk-)=m2，得位姿誤差概率橢球，其中m 取值1，2，3，…，表示標準方差σ 的倍數。文中，取m=3，即狀態誤差分布區域Γk可視為基于Σqk的3σ 誤差概率橢球體。

Σqk代表整個位姿協方差矩陣，但是計算導航誤差只需要OXeYe位置協方差矩陣即可。為了得到相應的位置協方差矩陣，Σqk被拆分為相應的子矩陣，取對應的位置協方差子矩陣，簡寫為Σk.位置誤差分布區域取橢圓球體在OXeYe平面上的投影，等概率線構成位置誤差橢圓的輪廓線。

2.2 導航誤差空間

描述信息的不確定性常采用非定常模型和概率模型。非定常模型是一種比較保守的處理方法，它將導航誤差視為一個包含不確定性的幾何區域，但區域的屬性參數復雜，且出于安全考慮，路徑搜尋只考慮最壞情況，相應會增加路徑長度。對于概率模型，所有當前位置狀態以條件概率出現在狀態空間上，容易獲得狀態轉移關系，將導航誤差作為一種概率事件，處理起來也相對簡單。

傳統規劃空間常采用由自由空間與障礙空間構成的二元結構環境。作為一種概率事件，導航誤差不能簡單地歸入某一類空間，二元空間不適用于描述導航誤差這一約束條件，因此采用導航誤差空間(NES)，既反映UUV 的位置，也反映導航信息的不確定性。由上述分析構建NES:

此空間中的每個狀態包括兩部分信息:X 是UUV 在OXeYe平面上的導航位置;ε 是表征位置誤差的參數變量，在下面會給出具體形式。Ω 空間實際上也是二維平面加導航誤差的擴維空間。給定初始條件，在激勵輸入u∈U 下，導航誤差ε 隨著UUV移動而改變，當存在可量化的函數Xk+1=fx(Xk，εk，uk)和εk+1=fε(Xk，εk，uk)來描述導航誤差與導航位置的變化關系時，則從位置Xk到Xk+1的狀態轉移代價是可度量的［9］。

2.3 導航誤差遞推

設UUV 的規劃速度為常速，時刻k 的導航位置是Xk=(xk，yk)，隨機干擾因素均服從高斯分布

式中:μk=(，)是UUV 期望導航位置。導航位置分布概率密度函數取

為了方便計算，位置協方差矩陣可表示為

則k 時的導航誤差取εk=(σxk，σyk)，NES 中的導航不確定狀態可寫成sk=(μk，εk).為了獲得導航誤差與導航位置的變化關系，需計算任意時刻的Σk，Σk由Σqk得到，而Σqk隨著預測時間按(3)式進行遞推，初始狀態協方差矩陣為

式中:σx0、σy0、σθ0取極小值，則初始誤差為ε0=(σx0，σy0).

根據上述分析設計仿真案例，給定初始協方差矩陣，誤差遞推按(3)式進行，取v=3 m/s，Δt=1 s，σv=0.06 m/s，σw=0.1°/s.圖2是某圓形閉合航路導航誤差隨路徑變化圖，散點是采樣點，黑實線表示3σ 置信橢圓。可以從圖中看出，導航誤差不斷增大，符合UUV 實際導航特點。

圖2 導航誤差的發散Fig.2 Increase in navigation error

3 導航誤差校正

在水下環境中，單一采用SINS/DVL 艦位推算，其精度和可靠性很難滿足要求，需定期上浮到水面獲取GPS 信號進行校正，這不利于隱蔽性作業要求。近年來，地形匹配等導航技術獲得越來越多的關注，地形匹配導航根據實測地形序列與背景地形的匹配來實施定位，每一次定位都是孤立的，不存在誤差積累，對艦位推算定期修正，可有效消除累積誤差［13-14］。多個備選匹配區的地形數據作為背景圖預先存入UUV 載體中，當UUV 航行至匹配區，根據實測圖與背景圖匹配獲得實時位置。

為了計算方便，此處將匹配區C 簡化成半徑Rd的圓形區域(見圖3)，并假設匹配精度較高，在匹配區內可將導航誤差修正為初始誤差，如

如圖3所示，UUV 進入匹配區C 以前，導航是增大發散的，進入匹配區C 后，導航誤差保持為ε0，離開匹配區C 后，導航誤差重新發散。

圖3 地形導航校正SINS/DVL 導航誤差Fig.3 Error of SINS/DVL corrected by terrain navigation

4 基于導航誤差空間的路徑規劃

在NES 上，從狀態sk到狀態sk+1的轉移代價不簡單是μk與μk+1之間的歐式距離，而是取sk內任意可能導航位置與sk+1內任意導航位置之間的歐式距離的期望值，如下式:

式中:Xk，i、Xk+1，i分別是導航不確定狀態內的隨機位置采樣點;D(Xk，i，Xk+1，i)表示兩采樣點的歐式距離。狀態轉移代價與路徑長度不同，其大小與導航不確定狀態的概率分布緊密相關。為了便于計算，采用Monte Carlo 模擬(MCS)法計算狀態轉移代價，在狀態sk、sk+1的導航位置分布內采樣N 個樣本點，如圖4所示，采樣集合Θ ={Xk，i，Xk+1，i}，狀態轉移代價為

評估導航精度的指標很多，其中圓概率偏差(CEP)是評定運載體導航精度的主要指標。如果二維水平定位上誤差在半徑為r 的概率為50%，這個半徑r 稱為CEP，按工程經驗公式［15］得

圖4 從sk 到sk+1的狀態轉移Fig.4 State transition from sk to sk+1

通過計算CEP，對導航狀態進行評估。

4.1 碰撞代價

一般將障礙物和危險區域視為碰撞區，UUV 禁止進入碰撞區。在PNU 中，導航誤差使得UUV 與障礙物的位置關系變得不確定，是否會發生碰撞與當前導航誤差分布緊密有關。UUV 期望位置與障礙物位置一定的前提下，隨著導航誤差的增大，UUV與障礙之間的安全區域勢必減少，進入碰撞區的概率會增大。此處引入碰撞概率來衡量UUV 不確定的導航位置落入碰撞區的概率，碰撞概率由導航位置概率密度函數在碰撞區進行積分獲得:

導航誤差概率密度函數由(6)式獲得，B 定義為碰撞區外形輪廓的外包絡，為了簡化計算，將碰撞區設為圓形或矩形規則區域。

4.2 路徑優化目標

導航誤差對路徑選擇有很大影響，增大的導航誤差除了在運動過程中會帶來碰撞代價，終點導航誤差的大小也是考慮的重要條件。

PNU 規劃問題歸結為滿足最大容許導航誤差的前提下，在NES 上，尋找從初始狀態s0∶=(X0，ε0)到目標狀態sg∶= (Xg，εg)的路徑代價最小問題。從起點到目標的路徑由連續的路徑點組成，在NES上就構成一個連續的狀態序列，{s0，s1，…，sk，…，sM}，其中sM=sg.規劃目標J 需使得狀態序列的狀態轉移代價最小:

式中:約束條件(12)式規定了目標狀態的誤差上限，CEPmax為終點最大允許圓概率偏差;約束條件(13)式規定了每個狀態點的安全條件，Ps是安全閾值;約束條件(14)式規定了路徑長度上限，Dmax是最大航程限制，取決于攜帶能量上限。

4.3 改進A* 搜索算法

路徑搜索可采用A*、RRT、波前法等，這里基于已建立的NES，采用啟發式A*算法搜尋最優路徑。二維空間經過柵格離散化后的柵格加上導航誤差擴展成狀態格sk，sk視為A*搜索路圖中的節點。

針對A*算法特點，建立如下數據結構:OpenList記錄已計算但未擴展的節點;ClosedList 記錄已擴展的節點。每個節點結構包括如下信息:導航不確定狀態si=(xi，yi，εi);節點歷經代價g(i)，啟發代價h*(i)，以及航行總代價f(i)=g(i)+h*(i);父節點。

路徑搜索的關鍵是設計一個搜索回報函數，對每條可行路徑的回報代價進行評估，當前狀態sk的總代價為Lg(s0，sk)是初始狀態到當前狀態的最優代價，由(9)式計算。L*h (sk，sg)是當前狀態到目標狀態的啟發代價，此處取兩狀態的期望位置之間的歐式距離，即L*h(sk，sg)=D(μk，μg).

在路徑搜索過程中，由于節點會被重復搜索，很可能會導致在同一柵格位置出現兩個以上不同導航誤差的狀態，即可能存在不同的狀態使得si=(Xi，εi)，sj=(Xj，εj)，Xi=Xj，εj≠εi，重疊狀態會對狀態擴展帶來混亂，不利于搜索。在此引入主導狀態法［16］來解決此問題，根據主導狀態理論，相同空間位置的多個重疊狀態中，將某種代價最優的狀態視為主導狀態，其他狀態視為非主導狀態，主導狀態替代或覆蓋非主導狀態。此處將導航精度最高即CEP最小的狀態視為主導狀態。如果存在重疊狀態si和sj，滿足Xi=Xj，CEPj＞CEPi，則非主導狀態sj在搜索圖中會被刪除，只對主導狀態si展開后續搜索，通過獲取主導狀態可精簡搜索空間，提高搜索效率。

當前節點sk向其相鄰的8 節點擴展，sk的相鄰節點si可被擴展到OpenList 表的準則是:si未被擴展，且滿足約束(12)式～(14)式。存在重疊狀態的情況下，si必須是主導狀態。

有地形導航的情況下，匹配區內的節點一旦被擴展，由(7)式，其先前累積的導航誤差會被校正為初始值，從而使經過匹配區的路徑代價減小，匹配區對搜索路徑起到某種程度的啟發作用。同樣，在最大誤差的約束下，最優路徑也會選擇經過匹配區修正導航誤差，而考慮到航程限制，毋需經過所有備選匹配區。

當在某搜索節點的碰撞概率大于安全閾值Ps，可認為UUV 與障礙物發生碰撞的幾率很大，應當停止向碰撞區移動，Lf(sk)會增加一個很大的懲罰代價來停止對碰撞區的搜索。

為滿足終點誤差要求，在搜尋過程中，如果某節點的CEP 大于CEPmax，則停止對該節點的搜索。算法終止的條件是:成功搜索到目標或OpenList 為空。

具體規劃步驟如下:

1)初始化，OpenList 置空，s0放入OpenList.

2)如果OpenList 為空，搜索失敗，退出，否則繼續。

3)從OpenList 中選擇代價最小的節點作為當前節點，放入ClosedList 中。如果當前節點是目標點sg，搜索成功，退出。

4)對相鄰8 子節點進行擴展，擴展的子節點按(3)式或(7)式計算導航誤差，按(10)式計算CEP，按(11)式計算碰撞概率。

5)如果子節點的CEP 大于CEPmax，或其碰撞概率大于Ps，或實際路徑長度大于Dmax，則刪除此子節點，否則視為待擴展節點。

6)按(15)式計算待擴展子節點的總代價。將可擴展子節點插入到OpenList 中。如果子節點既不在OpenList 中又不在ClosedList 中，則將子節點插入到OpenList 中;如果子節點已在OpenList 中，且子節點的當前代價值小于該子節點在OpenList 中的原代價，則將其父節點指針指向當前節點;如果子節點已在ClosedList 中，則跳過它，擴展其他子節點。

7)返回步驟2，繼續循環。

5 仿真驗證與結果分析

為了驗證算法的可行性，由數字海洋環境模擬真實復雜海洋環境，在數字海洋地圖上選取80 km×80 km 某海區，分布有多個海島障礙，海島之間存在狹水道供UUV 通過。柵格密度200×200，Monte Carlo 采樣次數為100，起點和終點分別選取(70 km，10 km)和(10 km，75 km)，規劃速度v =3 m/s，測量誤差方差取σv=0.06 m/s，σω=0.02°/s，最大航程Dmax=150 km.

案例1:未考慮導航誤差約束的路徑規劃。未考慮導航誤差約束下，按傳統A*規劃方法設計，采用NES 作為規劃空間。需要說明的是，采用NES 是為了方便計算規劃后的路徑碰撞概率，在規劃中NES 只起著普通柵格空間的作用，未考慮碰撞概率約束和終點誤差約束，只考慮最大航程約束，以路徑長度最短為優化目標。圖5是仿真結果，深色區域是海島，黑色圓表示導航誤差3σ 置信區間，最短路徑的長度為92.62 km，運行時間1.7 s，由于未考慮導航誤差的影響，部分路徑與海島相距較近，導航誤差使得后半段靠近海島的路徑變得不安全，整條路徑的最大碰撞概率為0.37.

圖5 未考慮導航誤差的路徑規劃Fig.5 Path planning without considering navigation error

案例2:無地形匹配導航下的PNU 仿真。考慮導航誤差約束的改進A*方法，導航方式是艦位推算，采用NES 作為規劃空間，規劃約束取CEPmax=3 km，Ps=0.1.獲得的最優路徑如圖6(a)所示，總代價342.94，終點CEP 為2.62 km，路徑長度96.77 km，運行時間4.8 s.最優路徑的導航誤差變化曲線如圖6(b)所示，導航誤差整體趨勢增大，使圖6(a)中的路徑以一定的安全距離適當遠離，此外，通過海島之間的狹水道會遭遇碰撞風險，故繞行到開闊海域，盡管路徑長度比圖5中路徑增加了4.15 km，但碰撞概率減到0.1 以下，提高了安全性。

圖6 無地形匹配導航的PNUFig.6 PNU without terrain-matching navigation

案例3:地形匹配導航下的PNU 仿真。導航方式是地形匹配輔助艦位推算。規劃約束如案例2，選取2 個備選匹配區，位置分別在(30 km，20 km)，(25 km，45 km)，作用范圍2 km.圖7(a)是規劃的最優路徑，總代價151.01，終點CEP 為0.55 km，路徑實際長度95.58 km，路徑經過(25 km，45 km)處的匹配區進行導航校正，誤差校正為初始誤差，最優路徑的導航誤差變化曲線如圖7(b)所示。誤差減小后的路徑會接近海島，與圖6(a)的路徑相比，終點導航誤差也減小了。案例證明了NES 也適用于有導航校準的情況，在工程應用上有一定的參考意義。

6 結論

基于導航誤差空間的路徑規劃是一種離線全局規劃方法。考慮隨時間變化的導航誤差，建立了綜合位置和誤差信息的規劃空間。根據實際導航系統特點，建立導航誤差模型;運用CEP 標準評估導航精度，采用主導狀態解決狀態重疊問題;轉移代價通過Monte Carlo 采樣計算，根據誤差分布計算碰撞概率;目標代價函數同時考慮了路徑長度和航行安全性雙重因素，由改進A*算法搜索最優路徑。仿真結果表明:提出的規劃算法簡捷、快速，相比于未考慮導航誤差的傳統規劃方法，盡管會增加路徑長度，但可有效降低航行中的碰撞風險;該算法也適用于地形匹配輔助艦位推算等情況，在航程約束下，最優路徑經過匹配區，有效減小了導航誤差;增加了規劃的成功率，提高了航行安全性和可靠性，滿足水下長航程任務需要。

References)

［1］ 李俊，徐德民，宋保維，等.自主式水下潛器導航技術現狀與展望［J］.中國造船，2004，45(3):70 -77.LI Jun，XU De-min，SONG Bao-wei，et al.Development and prospect of AUV navigation technology［J］.Shipbuilding of China，2004，45(3):70 -77.(in Chinese)

［2］ Berg J V D，Patil S，Alterovitz R.Motion planning under uncertainty using iterative local optimization in belief space［J］.The International Journal of Robotics Research，2012，31(11):1263 -1278.

［3］ Kurniawati H，Bandyopadhyay T，Patrikalakis N M.Global motion planning under uncertain motion，sensing，and environment map［J］.Auton Robot，2012，33:255 -272.

［4］ Chakravorty S，Kumar S.Generalized sampling-based motion planners［J］.IEEE Transactions on Systems，Man，and Cybernetics-Part B:Cybernetics，2011，41(3):856 -866.

［5］ Bry A，Roy N.Rapidly-exploring random belief trees for motion planning under uncertainty［C］∥IEEE International Conference on Robotics and Automation.Shanghai:IEEE，2011:723 -730.

［6］ Du T N E，Burdick J W.Robot motion planning in dynamic，uncertain environments［J］.IEEE Transactions on Robotics，2012，28(1):101 -115.

［7］ González-Sieira A，Mucientes M，Bugarin A.A state lattice approach for motion planning under control and sensor uncertainty［C］∥ROBOT2013:First Iberian Robotics Conference.Madrid，Spain:Springer，2014:247 -260.

［8］ Censi A，Calisi D，Luca A D，et al.A Bayesian framework for optimal motion planning with uncertainty［C］∥2008 IEEE International Conference on Robotics and Automation.Pasadena，US:IEEE，2008:1798 -1805.

［9］ Gonzalez J P.Planning with uncertainty in position using high-resolution maps［D］.Pittsburgh:Carnegie Mellon University，2008:36 -42.

［10］ Lambert A，Gruyer D.Safe path planning in an uncertain-configuration space［C］∥2003 IEEE International Conference on Robotics and Automation.Taipei:IEEE，2003:4185 -4190.

［11］ 冀大雄，劉健.水下機器人圓概率偏差的有效預報［J］.計算機應用，2012，32(10):2960 -2962.JI Da-xiong，LIU Jian.Effective forecast of circular error probability in underwater robots［J］.Journal of Computer Application，2012，32(10):2960 -2962.(in Chinese)

［12］ Moon I，Miura J，Yoshiaki S.On-line viewpoint and motion planning for efficient visual navigation under uncertainty［J］.Robotics and Autonomous Systems，1999，28(2/3):237 -248.

［13］ Hagen O K，Anonsen K B，Mandt M.The HUGIN real-time terrain navigation system［C］∥Oceans 2010.Kjeller，Norway:MTS/IEEE，2010:1 -7.

［14］ 張紅梅，趙建虎，楊鯤，等.水下導航定位技術［M］.武漢:武漢大學出版社，2010:1 -3.ZHANG Hong-mei，ZHAO Jian-hu，YANG Kun，et al.Underwater navigation and positioning technology［M］.Wuhan:Wuhan University Press，2010:1 -3.(in Chinese)

［15］ 王艷永，賈興榮，高暉，等.基于統計推斷的慣性定位精度評估方法對比［J］.計算機應用，2012，32(增刊2):276 -279.WANG Yan-yong，JIA xing-rong，GAO Hui，et al.Comparison of inertial positioning accuracy evaluation approaches based on statistical inference［J］.Journal of Computer Applications，2012，32(S2):276 -279.(in Chinese)

［16］ Gonzalez J P，Dornbush A，Likhachev M.Using state dominance for path planning in dynamic environments with moving obstacles［C］∥2012 IEEE International Conference on Robotics and Automation.Saint Paul，US:IEEE，2012:4009 -4015.