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

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

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

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

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

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

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

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

0 引言

自主式水下航行器（autonomous underwater vehicle，AUV），具有活動范圍大、機動性好、安全并且隱蔽性好等優(yōu)點，從60年代中期起，工業(yè)界和軍方開始對其發(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ī)劃方法多種多樣，目前大都是從地面機器人系統(tǒng)借鑒來的，后來人們將室內(nèi)地面機器人技術(shù)加以提升和改進(jìn)，應(yīng)用到空中和水下［4 5］。Voronoi圖法，就是Chandler等最先采用的，在已知環(huán)境內(nèi)，產(chǎn)生的單架無人機路徑的方法［6］。概率法中，Eagle和Yee將路徑規(guī)劃看做是分割單元內(nèi)的搜索問題［7］。對于單元分解法，Kambhampari和Davis提出了一種減小內(nèi)存使用率的估計法，成為四叉樹或八叉樹表示法［8］。但這些使用了全局搜索和計算方法的Voronoi圖法、隨機概率圖法和單元分解法都沒有考慮路徑的運動學(xué)約束，而AUV航行每一時刻存在速度和方向，所以這些路徑規(guī)劃方法都是有缺陷的。

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

1 Dubins曲線

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

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

1.1 Dubins存在的條件

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

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

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

1.2 參數(shù)計算

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

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

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

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

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

2 A*搜索算法

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

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

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

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

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

2.2 障礙規(guī)避

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

擴展節(jié)點的評價函數(shù)如下定義：

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

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

定義g（p）為：

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

定義h（p）為：

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

定義P點的路徑代價D（p）為：

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

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

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

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

3 仿真實驗

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

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

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

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

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

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

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

4 結(jié)束語

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

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

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

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

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

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

［5］Tsourdos A，White B，Shanmugavel M.Cooperative path planning of unmanned aerial vehicles［M］.祝小平，等譯.北京：國防工業(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é)報，2013，25：291-296.

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

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

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

［14］周 良，王耀南，印 峰，等.基于類三維地圖的無人機路徑規(guī)劃［J］.計算機測量與控制，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。

國家自然基金項目（61473224）；水下信息與控制重點實驗室基金項目（9140C230202150C23001）。

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