一種基于導(dǎo)航誤差空間的無人水下航行器路徑規(guī)劃方法

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

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

0 引言

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

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

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

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

1 問題分析

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

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

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

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

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

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

2 基于導(dǎo)航誤差空間的環(huán)境模型

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

2.1 UUV 導(dǎo)航誤差建模

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

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

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

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

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

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

2.2 導(dǎo)航誤差空間

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

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

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

2.3 導(dǎo)航誤差遞推

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

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

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

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

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

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

圖2 導(dǎo)航誤差的發(fā)散Fig.2 Increase in navigation error

3 導(dǎo)航誤差校正

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

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

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

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

4 基于導(dǎo)航誤差空間的路徑規(guī)劃

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

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

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

圖4 從sk 到sk+1的狀態(tài)轉(zhuǎn)移Fig.4 State transition from sk to sk+1

通過計算CEP，對導(dǎo)航狀態(tài)進行評估。

4.1 碰撞代價

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

導(dǎo)航誤差概率密度函數(shù)由(6)式獲得，B 定義為碰撞區(qū)外形輪廓的外包絡(luò)，為了簡化計算，將碰撞區(qū)設(shè)為圓形或矩形規(guī)則區(qū)域。

4.2 路徑優(yōu)化目標

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

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

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

4.3 改進A* 搜索算法

路徑搜索可采用A*、RRT、波前法等，這里基于已建立的NES，采用啟發(fā)式A*算法搜尋最優(yōu)路徑。二維空間經(jīng)過柵格離散化后的柵格加上導(dǎo)航誤差擴展成狀態(tài)格sk，sk視為A*搜索路圖中的節(jié)點。

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

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

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

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

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

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

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

具體規(guī)劃步驟如下:

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

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

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

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

5)如果子節(jié)點的CEP 大于CEPmax，或其碰撞概率大于Ps，或?qū)嶋H路徑長度大于Dmax，則刪除此子節(jié)點，否則視為待擴展節(jié)點。

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

7)返回步驟2，繼續(xù)循環(huán)。

5 仿真驗證與結(jié)果分析

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

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

圖5 未考慮導(dǎo)航誤差的路徑規(guī)劃Fig.5 Path planning without considering navigation error

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

圖6 無地形匹配導(dǎo)航的PNUFig.6 PNU without terrain-matching navigation

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

6 結(jié)論

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

References)

［1］ 李俊，徐德民，宋保維，等.自主式水下潛器導(dǎo)航技術(shù)現(xiàn)狀與展望［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］ 冀大雄，劉健.水下機器人圓概率偏差的有效預(yù)報［J］.計算機應(yīng)用，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］ 張紅梅，趙建虎，楊鯤，等.水下導(dǎo)航定位技術(shù)［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］ 王艷永，賈興榮，高暉，等.基于統(tǒng)計推斷的慣性定位精度評估方法對比［J］.計算機應(yīng)用，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.