嚴(yán)浙平，趙玉飛，陳濤，周佳加

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱150001)

0 引言

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

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

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

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

1 問(wèn)題分析

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

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

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

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

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

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

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

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

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

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

圖1 UUV 水平面運(yùn)動(dòng)模型Fig.1 UUV horizontal plane motion model

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

如圖3所示，UUV 進(jìn)入匹配區(qū)C 以前，導(dǎo)航是增大發(fā)散的，進(jìn)入匹配區(qū)C 后，導(dǎo)航誤差保持為ε0，離開(kāi)匹配區(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)移代價(jià)不簡(jiǎ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)的隨機(jī)位置采樣點(diǎn);D(Xk，i，Xk+1，i)表示兩采樣點(diǎn)的歐式距離。狀態(tài)轉(zhuǎn)移代價(jià)與路徑長(zhǎng)度不同，其大小與導(dǎo)航不確定狀態(tài)的概率分布緊密相關(guān)。為了便于計(jì)算，采用Monte Carlo 模擬(MCS)法計(jì)算狀態(tài)轉(zhuǎn)移代價(jià)，在狀態(tài)sk、sk+1的導(dǎo)航位置分布內(nèi)采樣N 個(gè)樣本點(diǎn)，如圖4所示，采樣集合Θ ={Xk，i，Xk+1，i}，狀態(tài)轉(zhuǎn)移代價(jià)為

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

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

通過(guò)計(jì)算CEP，對(duì)導(dǎo)航狀態(tài)進(jìn)行評(píng)估。

4.1 碰撞代價(jià)

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

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

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

導(dǎo)航誤差對(duì)路徑選擇有很大影響，增大的導(dǎo)航誤差除了在運(yùn)動(dòng)過(guò)程中會(huì)帶來(lái)碰撞代價(jià)，終點(diǎn)導(dǎo)航誤差的大小也是考慮的重要條件。

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

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

4.3 改進(jìn)A* 搜索算法

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

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

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

在路徑搜索過(guò)程中，由于節(jié)點(diǎn)會(huì)被重復(fù)搜索，很可能會(huì)導(dǎo)致在同一柵格位置出現(xiàn)兩個(gè)以上不同導(dǎo)航誤差的狀態(tài)，即可能存在不同的狀態(tài)使得si=(Xi，εi)，sj=(Xj，εj)，Xi=Xj，εj≠εi，重疊狀態(tài)會(huì)對(duì)狀態(tài)擴(kuò)展帶來(lái)混亂，不利于搜索。在此引入主導(dǎo)狀態(tài)法［16］來(lái)解決此問(wèn)題，根據(jù)主導(dǎo)狀態(tài)理論，相同空間位置的多個(gè)重疊狀態(tài)中，將某種代價(jià)最優(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，滿(mǎn)足Xi=Xj，CEPj＞CEPi，則非主導(dǎo)狀態(tài)sj在搜索圖中會(huì)被刪除，只對(duì)主導(dǎo)狀態(tài)si展開(kāi)后續(xù)搜索，通過(guò)獲取主導(dǎo)狀態(tài)可精簡(jiǎn)搜索空間，提高搜索效率。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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

References)

［1］ 李俊，徐德民，宋保維，等.自主式水下潛器導(dǎo)航技術(shù)現(xiàn)狀與展望［J］.中國(guó)造船，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ī)器人圓概率偏差的有效預(yù)報(bào)［J］.計(jì)算機(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］.武漢:武漢大學(xué)出版社，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ì)推斷的慣性定位精度評(píng)估方法對(duì)比［J］.計(jì)算機(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.