胡 薔1，高立娥1，2，劉衛(wèi)東1，2，李澤宇1

（1.西北工業(yè)大學(xué)航海學(xué)院，西安 710072；2.水下信息與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710072）

基于Dubins曲線和改進(jìn)A*算法的AUV路徑規(guī)劃方法

胡 薔1，高立娥1，2，劉衛(wèi)東1，2，李澤宇1

（1.西北工業(yè)大學(xué)航海學(xué)院，西安 710072；2.水下信息與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710072）

將Dubins曲線和具有角度約束的改進(jìn)A*搜索算法結(jié)合應(yīng)用于路徑規(guī)劃中，能解決路徑長(zhǎng)度最短和安全性的問(wèn)題。這樣規(guī)劃出來(lái)的路徑由兩段滿足AUV最小轉(zhuǎn)彎半徑的圓弧和一段同時(shí)與兩弧相切的直線構(gòu)成；圓弧段由產(chǎn)生Dubins路徑的方法產(chǎn)生，直線段由改進(jìn)A*搜索算法擴(kuò)展產(chǎn)生；首先通過(guò)判斷Dubins路徑存在條件，解算Dubins曲線參數(shù)，從而確定此路徑中兩圓弧的起始點(diǎn)、終止點(diǎn)坐標(biāo)；再通過(guò)這些圓弧坐標(biāo)可得到直線與圓弧的切入點(diǎn)、切出點(diǎn)，此兩點(diǎn)就是改進(jìn)A*搜索算法擴(kuò)展路徑的起始點(diǎn)和終止點(diǎn)；以Matlab為工具進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證了此方法能產(chǎn)生規(guī)避障礙物的可行的最短路徑。

Dubins曲線；改進(jìn)A*搜索算法；路徑規(guī)劃；Matlab

0 引言

自主式水下航行器（autonomous underwater vehicle，AUV），具有活動(dòng)范圍大、機(jī)動(dòng)性好、安全并且隱蔽性好等優(yōu)點(diǎn)，從60年代中期起，工業(yè)界和軍方開(kāi)始對(duì)其發(fā)生巨大興趣。AUV是利用自身傳感器，搜集環(huán)境中的信息，采用一定的算法建立周圍環(huán)境的模型，控制其在環(huán)境中按照預(yù)定的路徑航行到目標(biāo)位置［1-3］。AUV研究領(lǐng)域的范圍很廣，但是路徑規(guī)劃是最基礎(chǔ)也是最必要的部分，在復(fù)雜的海洋環(huán)境中，它保證AUV的有效行駛并完成給定的任務(wù)要求。

AUV路徑規(guī)劃方法多種多樣，目前大都是從地面機(jī)器人系統(tǒng)借鑒來(lái)的，后來(lái)人們將室內(nèi)地面機(jī)器人技術(shù)加以提升和改進(jìn)，應(yīng)用到空中和水下［4 5］。Voronoi圖法，就是Chandler等最先采用的，在已知環(huán)境內(nèi)，產(chǎn)生的單架無(wú)人機(jī)路徑的方法［6］。概率法中，Eagle和Yee將路徑規(guī)劃看做是分割單元內(nèi)的搜索問(wèn)題［7］。對(duì)于單元分解法，Kambhampari和Davis提出了一種減小內(nèi)存使用率的估計(jì)法，成為四叉樹(shù)或八叉樹(shù)表示法［8］。但這些使用了全局搜索和計(jì)算方法的Voronoi圖法、隨機(jī)概率圖法和單元分解法都沒(méi)有考慮路徑的運(yùn)動(dòng)學(xué)約束，而AUV航行每一時(shí)刻存在速度和方向，所以這些路徑規(guī)劃方法都是有缺陷的。

1975年，Dubins在研究中指出，平面內(nèi)滿足最小轉(zhuǎn)彎半徑約束的兩個(gè)矢量間的最短路徑是由直線和圓弧段組成的復(fù)合路徑。Balkcom和Mason使用Pontryagin最大化原理證實(shí)一般的移動(dòng)機(jī)器人在平面內(nèi)的運(yùn)動(dòng)是直線與弧線的組合［9］。所以本文設(shè)計(jì)一種將Dubins路徑和改進(jìn)的A*算法結(jié)合的路徑規(guī)劃方法，產(chǎn)生一種圓弧和直線相結(jié)合的路徑，在路徑始末兩端引入了圓弧，有效解決了AUV航行機(jī)動(dòng)性和終端約束的問(wèn)題，圓弧之外的路徑用改進(jìn)的A*算法擴(kuò)展產(chǎn)生。在無(wú)障礙物的情況下，路徑總長(zhǎng)度是滿足最小轉(zhuǎn)彎半徑的兩個(gè)圓弧和它們之間的切線之和，是長(zhǎng)度最短的路徑。在有障礙物的情況下，圓弧部分通過(guò)擴(kuò)大圓弧半徑方法規(guī)避障礙，直線部分用改進(jìn)A*算法的節(jié)點(diǎn)評(píng)價(jià)函數(shù)自主產(chǎn)生規(guī)避障礙的路徑，這樣產(chǎn)生的的路徑仍然是滿足安全性約束的最短路徑。

1 Dubins曲線

由于AUV每時(shí)每刻都有速度和方向，有必要在帶有方向的初始位置（起始位姿點(diǎn)）和終止位置（終止位姿點(diǎn)）間尋找最短路徑。

兩個(gè)位姿點(diǎn)間的最短路徑就是Dubins路徑。Dubins路徑可以簡(jiǎn)單地被定義為，在最大曲率限制下，平面內(nèi)兩個(gè)有方向的點(diǎn)間的最短可行路徑是CLC路徑或CCC路徑，或是他們的子集，其中C表示圓弧段，L表示與C相切的直線段。

1.1 Dubins存在的條件

設(shè)計(jì)Dubins路徑時(shí)，需要尋找直線段與兩圓弧的切點(diǎn)，如果找不到切點(diǎn)，那么Dubins路徑就不存在。

用公式表示Dubins存在的條件［2］，式（1）表示存在外切

式中，rt表示終止圓弧半徑，rs表示起始圓弧半徑，c表示兩圓弧圓心連線的長(zhǎng)度。

1.2 參數(shù)計(jì)算

對(duì)于已知起始位姿點(diǎn)、終止位姿點(diǎn)的路徑規(guī)劃問(wèn)題，一旦經(jīng)過(guò)式（1）和（2）驗(yàn)證存在Dubins路徑，那么就存在本文所設(shè)計(jì)改進(jìn)A*算法和Dubins曲線結(jié)合的路徑，求解此路徑只需求解Dubins路徑參數(shù)。

以二維平面為例，已知某AUV速度25 m／s，路徑起始點(diǎn)S坐標(biāo)為（0，0），單位m，起始航向角α為90°，終止點(diǎn)T坐標(biāo)（700，700），單位m，終止航向角β為135°，最小轉(zhuǎn)彎半徑rmin為100 m，對(duì)于起始圓弧、終止圓弧都是最小轉(zhuǎn)彎圓的Dubins路徑，可以判斷出同時(shí)存在具有內(nèi)切線和外切線的兩種Dubins路徑，顯然AUV只能向前運(yùn)動(dòng)，且根據(jù)起始點(diǎn)終止點(diǎn)角度約束，只有如圖1所示路徑SPsPtT滿足要求。

圖1 兩圓弧半徑均為100 m時(shí)的Dubins路徑曲線

用微分向量方法求解路徑參數(shù)方法解此路徑的參數(shù)［10-13］。起始點(diǎn)為S，目標(biāo)點(diǎn)為T，位置變換向量P有如下可得：

解θ2即可。再用解析幾何的方法可以求解兩圓弧圓心O1、O2，和直線切圓弧的切入切出點(diǎn)Ps、Pt。θ1、θ2即為A*算法和Dubins曲線結(jié)合產(chǎn)生的路徑的起始圓弧圓心角和終止圓弧圓心角。Ps、Pt即為上述路徑直線部分的起始點(diǎn)與終止點(diǎn)。已知這些路徑參數(shù)就可以得到完整的路徑曲線圖。

2 A*搜索算法

傳統(tǒng)A*算法將規(guī)劃空間劃分為網(wǎng)格的形式，通過(guò)起始節(jié)點(diǎn)所在網(wǎng)格，通過(guò)節(jié)點(diǎn)評(píng)價(jià)函數(shù)，選擇相鄰節(jié)點(diǎn)所在網(wǎng)格不斷向目標(biāo)節(jié)點(diǎn)所在網(wǎng)格進(jìn)行擴(kuò)展。算法具有搜索速度快，搜索結(jié)果優(yōu)的特點(diǎn)。但基于網(wǎng)格的節(jié)點(diǎn)擴(kuò)展方式，將網(wǎng)格中心點(diǎn)連接而成的路徑，難以適合具有一定運(yùn)動(dòng)方向和運(yùn)動(dòng)性能限制的運(yùn)動(dòng)，路徑必須進(jìn)行平滑修正。

2.1 改進(jìn)的A*算法

對(duì)傳統(tǒng)A*算法進(jìn)行缺陷分析，明確了傳統(tǒng)A*算法沒(méi)有考慮到路徑平滑性要求，因而通過(guò)在傳統(tǒng)A*算法中加入角度信息，而引入一種改進(jìn)的A*搜索算法。

AUV在水下運(yùn)動(dòng)時(shí)由于受過(guò)載和最小轉(zhuǎn)彎半徑的限制，在Δt時(shí)間內(nèi)可轉(zhuǎn)的角度Δψ一定（Δψ＜Maxψ，Maxψ為AUV在一定速度、時(shí)間范圍內(nèi)可轉(zhuǎn)過(guò)的最大角度），所以在搜索待擴(kuò)展節(jié)點(diǎn)時(shí)，搜索范圍是以當(dāng)前速度矢量為中心的扇形區(qū)域內(nèi)，扇形半徑即為搜索步長(zhǎng)。

以二維平面為例，用改進(jìn)的A*搜索算法擴(kuò)展節(jié)點(diǎn)如圖2所示。建立水中某一深度下的平面坐標(biāo)系OXY，設(shè)魚(yú)雷起始點(diǎn)S（xs，ys），速度方向?yàn)镺S所指方向，初始航向角為ψ0＝∠SOX，搜索步長(zhǎng)為L(zhǎng)，待擴(kuò)展節(jié)點(diǎn)P1i（P1i＝［p11，p12，p13.....］，SP1i＝L）可能存在的范圍是以2maxψ為圓心角，以O(shè)S為角平分線的扇形區(qū)域的圓弧上，扇形的半徑為L(zhǎng)。

2.2 障礙規(guī)避

對(duì)于如圖2所示的路徑節(jié)點(diǎn)擴(kuò)展方式，在沒(méi)有障礙物的情況下，Dubins路徑的非圓弧段能擴(kuò)展成一段切于兩圓弧的直線，但是在障礙物環(huán)境中接下就不一定是直線。在具有障礙物的環(huán)境中從起始點(diǎn)S準(zhǔn)確擴(kuò)展路徑節(jié)點(diǎn)直到目標(biāo)點(diǎn)T，就要依據(jù)節(jié)點(diǎn)評(píng)價(jià)函數(shù)。

擴(kuò)展節(jié)點(diǎn)的評(píng)價(jià)函數(shù)如下定義：

圖2 改進(jìn)的A*算法擴(kuò)展節(jié)點(diǎn)圖

式中，g（p）表示初始路徑節(jié)點(diǎn)S到當(dāng)前節(jié)點(diǎn)P的最小路徑代價(jià)的估計(jì)值，h（p）表示從當(dāng)前節(jié)點(diǎn)P到目標(biāo)節(jié)點(diǎn)T的最小路徑代價(jià)的估計(jì)值，a和b表示真實(shí)代價(jià)和預(yù)計(jì)代價(jià)的加權(quán)，F(xiàn)（p）表示從初始節(jié)點(diǎn)S經(jīng)節(jié)點(diǎn)P到目標(biāo)節(jié)點(diǎn)T的最小路徑代價(jià)的估計(jì)。

定義g（p）為：

表示當(dāng)前節(jié)點(diǎn)與初始節(jié)點(diǎn)間距離。

定義h（p）為：

式中，D（p）表示當(dāng)前節(jié)點(diǎn)P到目標(biāo)節(jié)點(diǎn)T的路徑代價(jià)，T（p）表示當(dāng)前節(jié)點(diǎn)P到目標(biāo)節(jié)點(diǎn)T的威脅代價(jià)，n和m分別表示路徑代價(jià)和威脅代價(jià)的權(quán)值。n，m不同的權(quán)值組合反映了AUV更側(cè)重于選擇較短的路徑來(lái)減少損耗或是更側(cè)重于遠(yuǎn)離威脅使路徑更安全。

定義P點(diǎn)的路徑代價(jià)D（p）為：

表示當(dāng)前點(diǎn)P到目標(biāo)點(diǎn)T的距離。

威脅代價(jià)T（p）的估計(jì)函數(shù)具有多種形式，不同形式的估計(jì)函數(shù)對(duì)節(jié)點(diǎn)的威脅評(píng)估不一定相同，而且在不同估計(jì)函數(shù)下，路徑規(guī)劃耗時(shí)也不一定相同。給出了一用常用的威脅代價(jià)評(píng)估函數(shù)［14－16］如式（21）所示：

式中，T（Sj）表示Sj點(diǎn)到各威脅的代價(jià)，Ri表示Sj點(diǎn)到第個(gè)i威脅的距離，ri表示第i個(gè)威脅的威脅半徑，（xsj，ysj）表示Sj點(diǎn)的位置坐標(biāo)，（xthreteni，ythretani）表示第i個(gè)威脅在空間中的位置坐標(biāo)。

威脅代價(jià)T（p）不僅對(duì)當(dāng)前節(jié)點(diǎn)威脅代價(jià)做出評(píng)估，還對(duì)選取該節(jié)點(diǎn)后可能采用路徑所面臨的威脅代價(jià)做出評(píng)估，有利于算法向更可行的區(qū)域經(jīng)行擴(kuò)展與搜索。

3 仿真實(shí)驗(yàn)

根據(jù)2.2例子中給定的AUV基本信息，對(duì)本文所設(shè)計(jì)的路徑規(guī)劃方法進(jìn)行仿真驗(yàn)證。圖3是無(wú)障礙物情況下，假設(shè)起始轉(zhuǎn)彎半徑與終止轉(zhuǎn)彎半徑相等，AUV轉(zhuǎn)彎半徑逐漸增加的路徑仿真曲線，它們的半徑依次為100 m，150 m，200 m， 276.5 m。曲線的存在滿足了AUV航行路徑平滑性要求，直線由改進(jìn)A*算法擴(kuò)展而來(lái)，由于無(wú)障礙物，非圓弧段擴(kuò)展成直線，曲線與直線相切，滿足路徑連續(xù)性要求。半徑為100 m時(shí)，路徑最短，路徑損耗最小。

圖3 圓弧半徑遞增的路徑曲線

當(dāng)存在障礙物，并且存在于非圓弧段時(shí)，原來(lái)的直線部分根據(jù)節(jié)點(diǎn)評(píng)價(jià)函數(shù)重新擴(kuò)展可行的安全路徑，此時(shí)就不是擴(kuò)展成直線了。改進(jìn)A*搜索算法規(guī)避障礙物通過(guò)在式（19）中選取不同的n，m組合來(lái)確定選擇較短的路徑還是遠(yuǎn)離威脅來(lái)規(guī)劃路徑。這里，n＝0.168，m＝5.05表明此時(shí)規(guī)劃路徑側(cè)重于規(guī)避障礙。此時(shí)路徑長(zhǎng)度雖大于無(wú)障礙時(shí)擴(kuò)展的直線長(zhǎng)度，但卻是滿足安全性約束的最短路徑。路徑參數(shù)見(jiàn)表1。規(guī)劃的路徑曲線如圖4所示。

表1 路徑參數(shù)表半徑

圖4 非圓弧段路徑存在障礙物的路徑曲線

當(dāng)障礙與存在于圓弧段并且存在于目標(biāo)點(diǎn)所在的圓弧段時(shí)，根據(jù)2.2節(jié)計(jì)算出路徑參數(shù)見(jiàn)表2。通過(guò)調(diào)整終止圓弧的半徑長(zhǎng)度可以達(dá)到規(guī)避障礙的目的。規(guī)劃的路徑如圖5所示。

表2 路徑參數(shù)表半徑

4 結(jié)束語(yǔ)

路徑規(guī)劃的最基本也是最重要的兩個(gè)約束就是路徑的可行性及安全性，即路徑需要滿足切線連續(xù)性和規(guī)避障礙物性能。通過(guò)以上的算法分析，實(shí)驗(yàn)仿真，驗(yàn)證了本文所設(shè)計(jì)的路徑規(guī)劃算法的有效性和高效性，即通過(guò)引入Dubins路徑的圓弧段解決了大多數(shù)路徑規(guī)劃算法不滿足運(yùn)動(dòng)學(xué)約束的缺陷，通過(guò)在直線段運(yùn)用最好優(yōu)先搜索的改進(jìn)A*搜索算法，使AUV在此部分自主規(guī)避障礙物，且用此方法規(guī)避完障礙物的路徑長(zhǎng)度也是滿足約束條件下最短最優(yōu)的路徑。

圖5 目標(biāo)點(diǎn)所在圓弧段存在障礙物的路徑曲線

［1］徐德民.魚(yú)雷的自動(dòng)控制系統(tǒng)［M］.西安：西北工業(yè)大學(xué)出版社，2001.

［2］嚴(yán)衛(wèi)生.魚(yú)雷航行力學(xué)［M］.西安：西北工業(yè)大學(xué)出版社，2005.

［3］高 劍，劉富檣，嚴(yán)衛(wèi)生.自主水下航行器回塢路徑規(guī)劃與跟蹤控制［J］.機(jī)械科學(xué)與技術(shù)，2012，31（5）：810-813.

［4］沈林成，多無(wú)人機(jī)自主協(xié)同控制理論與方法［M］.北京：國(guó)防工業(yè)出版社，2013.

［5］Tsourdos A，White B，Shanmugavel M.Cooperative path planning of unmanned aerial vehicles［M］.祝小平，等譯.北京：國(guó)防工業(yè)出版社，2103.

［6］Chadler P，Rasmussen S，Pachter M.UAV cooperative path planning［J］.AIAAGuidance，Navigation，andControl Sciences，2000，：167-194.

［7］Eagle J，Yee J.An optimal branch－and－bound procedure for the constrained path，moving target search problem［J］.Operations Research，1990，28：110-114.

［8］Kambhampari，L S，Davie L S.Multi-resolution path planning for mobile robots［J］.IEEE Journal of Robotics andAutomation，1986，2（3）：135-145.

［9］Balkcom D J，Mason M T.2000.Extremal trajectories for bounded velocity differential drive robots［A］.Proc.IEEE Int.Conf.onRobotics andAutomation［C］.SanFrancosco，pp.2479 -2484.

［10］梁 勇，張友安，雷軍委.一種基于Dubins路徑的在線快速航路規(guī)劃方法［J］.系統(tǒng)仿真學(xué)報(bào)，2013，25：291-296.

［11］張友根，張友安.基于雙圓弧原理的多導(dǎo)彈協(xié)同制導(dǎo)律研究［J］.海軍航空工程學(xué)報(bào)，2009，24（5）：537-541.

［12］張友根，張友安，鄔寅生.基于Dubins路徑的多飛行器協(xié)同在線航路規(guī)劃的研究［A］.第六屆博士學(xué)術(shù)年會(huì)［C］.重慶：中國(guó)科學(xué)技術(shù)出版社，2008.

［13］吳友謙，裴海龍.基于Dubins曲線的直升機(jī)曲線規(guī)劃［J］.計(jì)算機(jī)工程與技術(shù)，2011，34（4）：1426-1429.

［14］周 良，王耀南，印 峰，等.基于類三維地圖的無(wú)人機(jī)路徑規(guī)劃［J］.計(jì)算機(jī)測(cè)量與控制，2011，19（11）：2788 2790，2794.

［15］鄭昌文.飛行器航跡規(guī)劃方法研究［D］.武漢：華中科技大學(xué)，2003.

AUV Path Planning Method Based on Improved A* Search and Dubins Curve

Hu Qiang1，Gao Lie1，2，LiuWeidong1，2，Li Zeyu1

（1.School of Marine Science and Technology，Northwestern Polytechnical University，Xi′an 710072，China；2.State Key Laboratory of Underwater Information and Control，Xi′an 710072，China）

The application of the Dubins curve and the improved A*searching algorithm with angle constraint in the path planning，can solve the problem of the shortest path and route security.The path generated by this method，is composed of two circular arcs and a straight line which tangents to them.The circular arc which has the minimum turning radius，was produced by the Dubins curves，while the straight line by the A* searching algorithm.First，by judging the existing condition to calculate the parameters of Dubins’curve，then the coordinates of the starting point and terminal point can be calculated.With the coordinates，we can calculate the entry point and cut－in point of the straight line and arcs.And these two points will be the starting and terminal points which improve the A*searching algorithms’propagating path.The Matlab simulation experiment verified that the path produced in this way is the shortest one to avoid the obstacles.

Dubins curve；improved A*searching algorithm；path planning；Matlab

1671-4598（2016）08-0259-04

10.16526／j.cnki.11－4762／tp.2016.08.071

：TP391.9

：A

2016-02-29；

：2016-03-26。

國(guó)家自然基金項(xiàng)目（61473224）；水下信息與控制重點(diǎn)實(shí)驗(yàn)室基金項(xiàng)目（9140C230202150C23001）。

胡 薔（1989-），女，陜西西安人，碩士研究生，主要從事計(jì)算機(jī)控制與系統(tǒng)仿真方向的研究。