劉樂柱，肖長詩，文元橋

(武漢理工大學(xué) 航運學(xué)院，武漢430000) (*通信作者電子郵箱cs_xiao@hotmail.com)

基于Dubins路徑的無人艇運動規(guī)劃算法

劉樂柱，肖長詩*，文元橋

(武漢理工大學(xué) 航運學(xué)院，武漢430000) (*通信作者電子郵箱cs_xiao@hotmail.com)

針對無人艇運動規(guī)劃問題，通過Dubins路徑的理論分析，提出一種利用純粹幾何方法的Dubins路徑計算方法。該方法中沒有出現(xiàn)解方程組的運算，而是首先根據(jù)無人艇運動狀態(tài)計算轉(zhuǎn)向圓，然后利用幾何方法計算轉(zhuǎn)向圓間的公切線，最后通過公切線連接得到Dubins路徑。通過5組仿真實驗驗證了所提方法的有效性。前4組仿真實驗分別設(shè)計了計算Dubins路徑過程中可能出現(xiàn)的各種情形，以驗證算法適用于多種情況的Dubins路徑計算。最后一組仿真實驗用于無人艇的路徑規(guī)劃及運動狀態(tài)調(diào)整，仿真結(jié)果表明，基于Dubins路徑的無人艇運動規(guī)劃算法是可行的。

無人艇；路徑規(guī)劃；Dubins路徑；運動規(guī)劃

0 引言

路徑規(guī)劃是無人艇自主導(dǎo)航的關(guān)鍵技術(shù)之一，路徑規(guī)劃已在移動機器人技術(shù)中得到廣泛研究。路徑規(guī)劃是指在有障礙物的工作環(huán)境中，利用一定的算法或某些優(yōu)化準(zhǔn)則使移動機器人能從起始點繞開障礙物到達(dá)目標(biāo)點，在此過程中盡量優(yōu)化機器人運動軌跡[1-3]。

路徑規(guī)劃算法已得到廣泛關(guān)注和研究，由于真實環(huán)境的復(fù)雜性，及路徑規(guī)劃的自身困難，至今路徑規(guī)劃仍然是移動機器人自主導(dǎo)航的研究熱點之一?，F(xiàn)已提出多種路徑規(guī)劃算法。A*算法是一種典型的啟發(fā)式搜索算法[4]，最早由Nilsson等[5]提出。A*算法在路徑規(guī)劃研究中得到廣泛應(yīng)用和發(fā)展，考慮真實環(huán)境中各種因素的影響，在評價函數(shù)中加入描述真實環(huán)境的各種參量。例如吳天羿等[6]考慮坡度及路面粗糙度等因素的影響對越野車路徑規(guī)劃研究；Guinness等[7]研究了浮冰水域船舶路徑規(guī)劃問題。

考慮動力學(xué)和運動學(xué)約束時，路徑規(guī)劃將朝向運動規(guī)劃發(fā)展。運動規(guī)劃的最簡單形式是一個純幾何問題，給定機器人形狀和障礙物分布，計算其無碰路徑[8]。運動規(guī)劃中需要考慮無人艇的運動學(xué)約束，如旋回半徑、無人艇的操縱方程等。運動規(guī)劃已經(jīng)不僅僅是在位置坐標(biāo)(地圖)上進行路徑規(guī)劃，而是在包含運動狀態(tài)的位姿空間中搜索合適的運動軌跡及其運動狀態(tài)。運動規(guī)劃常用的算法是基于隨機采樣的路圖或者樹狀圖算法，如隨機路圖法(Probabilistic Roadmap Method, PRM)[8-9]、快速搜索隨機樹(Rapidly-exploring Random Tree, RRT)[10-11]。但這些基于隨機采樣的方法已被證明是概率完備但結(jié)果不最優(yōu)的。Dubins路徑采用圓弧—直線—圓弧的形式構(gòu)造軌跡路線，其中圓弧部分可以作為無人艇轉(zhuǎn)向形成的旋回圈，直線部分可以作為無人艇直行的軌跡，因此本文選取Dubins路徑作為無人艇的基本路徑，進行路徑規(guī)劃。

基于Dubins路徑的路徑規(guī)劃方法的理論依據(jù)是已經(jīng)證明了滿足曲率限制的最短曲線是圓弧與直線構(gòu)成的[12]。在此基礎(chǔ)上，研究者嘗試通過設(shè)計各種曲線連接構(gòu)成滿足運動約束的規(guī)劃路徑，如文獻[12]中設(shè)計的回旋曲線。Dubins路徑的計算常用的方法是通過建立方程組進行求解，如文獻[13]中通過角度和位置變化等約束建立方程組進行計算。本文設(shè)計一種利用純幾何的方法進行Dubins路徑的計算，該方法直接易行，沒有出現(xiàn)解方程組的運算。

1 問題描述

無人艇狀態(tài)矢量s=(x,y,cosθ,sinθ,v)T，其中包含無人艇位置坐標(biāo)P(x,y)和速度方向向量vdire=(cosθ,sinθ)T及速度大小v。無人艇運動規(guī)劃可以描述成計算由起始狀態(tài)矢量sstart至目標(biāo)狀態(tài)矢量starget的滿足無人艇運動約束的無碰最短路徑。

無人艇以一定速度轉(zhuǎn)向時，轉(zhuǎn)向半徑需要大于在此速度下的極小半徑。基于Dubins路徑的路徑規(guī)劃方法可以有效解決此問題。Dubins路徑是由起始轉(zhuǎn)向圓弧與目標(biāo)轉(zhuǎn)向圓弧通過公共切線相連構(gòu)成，其中，起始轉(zhuǎn)向圓弧半徑Rvstart和目標(biāo)轉(zhuǎn)向圓弧半徑Rvtarget需要滿足無人艇運動約束，因此，在獲取無人艇起始狀態(tài)矢量sstart和目標(biāo)狀態(tài)矢量starget后，計算Dubins路徑是路徑規(guī)劃的一個核心問題。

下面通過理論分析設(shè)計一種完全利用幾何方法計算Dubins路徑的運動規(guī)劃算法。

2 理論分析

2.1 轉(zhuǎn)向圓計算

無人艇狀態(tài)矢量s=(x,y,cosθ,sinθ,v)T以半徑Rv轉(zhuǎn)向，其轉(zhuǎn)向圓可以有兩種選擇。按右手法則，逆時針轉(zhuǎn)向標(biāo)記為↑，順時針轉(zhuǎn)向記為↓。

(1)

(2)

圖1 無人艇的兩個轉(zhuǎn)向圓

2.2 公切線計算

兩個外離圓之間的公切線有4條，分別為兩條外公切線和兩條內(nèi)公切線，但可以按方向連接兩個外離有向圓的公切線僅有1條，且若兩有向圓的轉(zhuǎn)向相同，連接兩圓的公切線是一條外公切線；若兩有向圓的轉(zhuǎn)向相反，連接兩圓的公切線是一條內(nèi)公切線。此結(jié)論可利用下面定理證明。

外離兩圓的Dubins路徑計算方法如下：

1)兩圓同向，兩圓通過外公切線相連。

如圖2所示，連心線C1C2與外切線的夾角α按式(3)求得：

α=arcsin(|r1-r2|/|C1C2|)

(3)

圖 2 同向圓的Dubins路徑中的公切線

若圓C1與圓C2的轉(zhuǎn)向方向為↑,則切線為圖2中灰色直線所示，兩切點按下面公式計算：

(4)

(5)

(6)

(7)

若圓C1與圓C2的轉(zhuǎn)向方向為↓則切線為圖2中黑色直線所示，由式(8) 計算得到eR后，兩切點仍按式(5)～(6) 計算。

(8)

2)兩圓異向，兩圓通過內(nèi)公切線相連。

如圖3所示，連心線C1C2與內(nèi)切線的夾角α按式(9)求得：

α=arcsin((r1+r2)/|C1C2|)

(9)

圖 3 反向圓Dubins路徑中的公切線

若圓C1的轉(zhuǎn)向為↑而圓C2的轉(zhuǎn)向方向為↓,則切線為圖3中灰色直線所示，兩切點按下面公式計算:

(10)

(11)

(12)

若圓C1的轉(zhuǎn)向為↓而圓C2的轉(zhuǎn)向方向為↑，則切線為圖3中黑色直線所示，由式(13) 計算得到eR后，兩切點仍按式(11)～(12) 計算。

(13)

兩圓的位置關(guān)系分為外離、外切、相交、內(nèi)切、內(nèi)含五種類型。外切和相交時，外公切線依然存有兩條，與外離時一樣，此時若兩圓的轉(zhuǎn)向相同，Dubins路徑計算方法與外離時一致。內(nèi)切時，兩條外公切線退化成一條，同向圓間的Dubins路徑中公切線部分退化成兩圓的切點。內(nèi)含時，已不存在外公切線。對于內(nèi)公切線，兩圓外切時已經(jīng)退化成一條，反向兩圓間的Dubins路徑中內(nèi)公切線部分退化成兩圓的切點。相交、內(nèi)切、內(nèi)含已不存在內(nèi)公切線。上述結(jié)論可總結(jié)為表1。

表1 Dubins路徑中的公切線計算方法

由以上分析可得，Dubins路徑的計算步驟可歸納為以下3步：

1)分別計算起始狀態(tài)和目標(biāo)狀態(tài)的轉(zhuǎn)向圓，各↑↓兩個；

2)分別選擇起始轉(zhuǎn)向圓與目標(biāo)轉(zhuǎn)向圓，按上面計算方法依次計算連接兩轉(zhuǎn)向圓的公切線。接著計算路徑長度，由起始點按方向經(jīng)起始轉(zhuǎn)向圓至公切線切點，然后沿著公切線至目標(biāo)轉(zhuǎn)向圓切點，按方向經(jīng)目標(biāo)轉(zhuǎn)向圓至目標(biāo)點的路徑長度。

3)在計算得到的Dubins路徑(共4條或少于4條)中，選擇最短路徑并輸出。

根據(jù)以上分析得到的結(jié)論，可以設(shè)計下面的Dubins路徑計算算法。

3 算法設(shè)計

3.1 數(shù)據(jù)結(jié)構(gòu)

由理論分析，根據(jù)問題特點，定義以下三種結(jié)構(gòu)體數(shù)據(jù)類型：1)無人艇狀態(tài)數(shù)據(jù)類型；2)圓數(shù)據(jù)類型；3)路徑數(shù)據(jù)類型。

這樣算法中主要包含：起始狀態(tài)、目標(biāo)狀態(tài)兩個狀態(tài)數(shù)據(jù)；兩個旋轉(zhuǎn)方向相反的起始圓和兩個旋轉(zhuǎn)方向相反的目標(biāo)圓共4個圓數(shù)組數(shù)據(jù)；一個存放4條Dubins路徑點的路徑數(shù)據(jù)。

3.2 算法流程

本文提出的基于Dubins路徑的路徑規(guī)劃算法流程如圖4所示。

圖4 本文算法流程

本文提出的計算Dubins路徑的算法完全使用幾何方法求解，算法簡易實用。下面通過仿真實驗驗證此算法的有效性，仿真實驗中設(shè)計5個算例，其中4個分別驗證外離、外切、內(nèi)切、內(nèi)含四種圓的位置關(guān)系的Dubins路徑計算，最后給出一個無人艇運動狀態(tài)調(diào)整的仿真實驗。

4 仿真實驗

仿真實驗分為兩部分：一部分是驗證本文所提算法可以計算兩圓各種位置關(guān)系下的Dubins路徑；另一部分是使用Dubins路徑完成無人艇運動狀態(tài)調(diào)整。

4.1 Dubins路徑計算

本節(jié)使用4個算例，如表2所示，分別驗證外離、外切、內(nèi)切、內(nèi)含四種圓的位置關(guān)系的Dubins路徑計算。

表2 Dubins路徑計算算例

算例1數(shù)值實驗結(jié)果如圖5所示，此算例中起始轉(zhuǎn)向圓與目標(biāo)轉(zhuǎn)向圓的位置關(guān)系均是外離，由起始狀態(tài)至目標(biāo)狀態(tài)的Dubins路徑存有4條。圖5(a)給出由起始轉(zhuǎn)向圓連接至目標(biāo)轉(zhuǎn)向圓間的公切線，圖5(b)給出4條Dubins路徑，其中灰色曲線長度為s=121.55，是最短的一條路徑，即最終輸出的規(guī)劃路徑。

圖5 算例1計算結(jié)果

算例2數(shù)值實驗結(jié)果如圖6所示，此算例中起始轉(zhuǎn)向圓與目標(biāo)轉(zhuǎn)向圓的位置關(guān)系有兩種：外離和外切。圓CS↑與圓CT↓之間的位置關(guān)系是外切，Dubins路徑中內(nèi)公切線退化成切點，這樣由起始狀態(tài)至目標(biāo)狀態(tài)的Dubins路徑仍存有4條，如圖6(b)所示，其中灰色曲線長度s=109.96，為最短的一條，即最終輸出的規(guī)劃路徑。

算例3數(shù)值實驗結(jié)果如圖7所示，此算例中起始轉(zhuǎn)向圓與目標(biāo)轉(zhuǎn)向圓的位置關(guān)系有三種：外離、外切及內(nèi)切。圓CS↑與圓CT↓之間的位置關(guān)系是內(nèi)切，圓CS↑與圓CT↓之間不存在內(nèi)公切線，即此兩圓之間不存有Dubins路徑。圓CS↑與圓CT↑之間的位置關(guān)系是外切，兩圓通過外公切線連接成Dubins路徑，如圖7(a)所示。這樣由起始狀態(tài)至目標(biāo)狀態(tài)的Dubins路徑僅存有3條，如圖7(b)所示，其中灰色曲線長度s=124.66，為最短的一條，即最終輸出的規(guī)劃路徑。

算例4數(shù)值實驗結(jié)果如圖8所示，此算例中起始轉(zhuǎn)向圓與目標(biāo)轉(zhuǎn)向圓的位置關(guān)系有三種：外離、內(nèi)切及內(nèi)含。圓CS↑與圓CT↓之間的位置關(guān)系是內(nèi)含，圓CS↑與圓CT↓不存有Dubins路徑。圓CS↑與圓CT↑之間的位置關(guān)系是內(nèi)切，兩圓之間的外公切線退化成切點，但仍存有Dubins路徑。這樣由起始狀態(tài)至目標(biāo)狀態(tài)的Dubins路徑僅存有3條，如圖8(b)所示，其中灰色曲線長度s=109.96，為最短的一條，即最終輸出的規(guī)劃路徑。

圖6 算例2計算結(jié)果

圖7 算例3計算結(jié)果

圖8 算例4計算結(jié)果

通過4組算例的數(shù)值實驗可以看出，本文算法對兩圓各種位置關(guān)系的Dubins路徑計算均能得到正確的結(jié)果，這充分驗證了該算法的有效性。該算法中完全使用幾何方法進行計算，計算量較小，適合應(yīng)用于在線路徑規(guī)劃。

4.2 無人艇運動狀態(tài)調(diào)整

本節(jié)是仿真實驗中的第5個算例，使用Dubins路徑作無人艇運動狀態(tài)調(diào)整。無人艇運動響應(yīng)模型為：

ψ″+ψ′/T=Kδ/T

(14)

其中：ψ為船艏向;δ為舵角。數(shù)值實驗中取K=0.285，T=0.275。

航跡跟蹤使用比例-積分-微分(Proportion-Integration-Differentiation, PID)控制，定義偏差e(t)為船當(dāng)前位置與預(yù)設(shè)航跡之間的距離。仿真結(jié)果如圖9所示，圖中分別給出無人船航跡與預(yù)設(shè)航跡對比圖及航向角、偏差函數(shù)e(t)的變化曲線。圖10給出仿真實驗中的舵角操縱過程。仿真結(jié)果表明，無人艇按照預(yù)設(shè)航跡進行航行，航向角的變化較平滑沒有出現(xiàn)過多的超調(diào)。航跡偏差維持在2.5 m范圍內(nèi)，舵角的操縱在左右30°舵以內(nèi)，操舵過程也較合理。

圖9 無人艇運動狀態(tài)調(diào)整

圖10 仿真實驗中舵角操縱過程

5 結(jié)語

本文提出一種基于純粹幾何理論的Dubins路徑計算方法。對于Dubins路徑計算可能出現(xiàn)的各種情況，該方法均能夠進行計算。仿真結(jié)果表明所提方法的Dubins路徑計算直接有效，基于Dubins路徑的無人艇運動規(guī)劃算法用于無人艇的運動狀態(tài)調(diào)整是可行的。接下來將利用文中所提算法對無人艇自主路徑規(guī)劃和自主靠泊等方向作進一步的研究。

References)

[1] 朱大奇,顏明重.移動機器人路徑規(guī)劃技術(shù)綜述[J].控制與決策,2010,25(7):961-967.(ZHU D Q, YAN M Z. Survey on technology of mobile robot path planning [J]. Control and Decision, 2010,25(7): 961-967.)

[2] 孟偲,王田苗.一種移動機器人全局最優(yōu)路徑規(guī)劃算法[J].機器人,2008,30(3):217-222.(MENG C, WANG T M. A global optimal path planning algorithm for mobile robot [J]. Robot, 2008, 30(3): 217-222.)[3] 趙先章,常紅星,曾雋芳,等.一種基于粒子群算法的移動機器人路徑規(guī)劃方法[J].計算機應(yīng)用研究,2007,24(3):181-183,186.(ZHAO X Z, CHANG H X, ZENG J F, et al. Path planning method for mobile robot based on particle swarm algorithm [J]. Application Research of Computers, 2007, 24(3): 181-183, 186.)

[4] 馬少平,朱小燕.人工智能[M].北京:清華大學(xué)出版社,2004:21-45.(MA S P, ZHU X Y. Artificial Intelligence [M]. Beijing: Tsinghua University Press, 2004: 21-45.)

[5] HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths [J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[6] 吳天羿,許繼恒,劉建永,等.基于改進A*算法的越野路徑規(guī)劃研究[J].計算機應(yīng)用研究,2013,30(6):1724-1726.(WU T Y, XU J H, LIU J Y, et al. Research of cross-country path planning based on improved A*algorithm [J]. Application Research of Computers, 2013, 30(6): 1724-1726.)

[7] GUINNESS R E, SAARIMAKI J, RUOTSALAINEN L, et al. A method for ice-aware maritime route optimization [C]// PLANS 2014: Proceedings of the 2014 IEEE/ION Position, Location and Navigation Symposium. Piscataway, NJ: IEEE, 2014: 1371-1378.

[8] HSU D, KINDEL R, LATOMBE J C, et al. Randomized kinodynamic motion planning with moving obstacles [J]. International Journal of Robotics Research, 2002, 21(3): 233-255.

[9] KAVRAKI L E,VESTKA P, LATOMBE J C, et al. Probabilistic roadmaps for path planning in high dimensional configuration space [J]. IEEE Transactions on Robotics and Automation, 1996, 12(4): 566-580.

[10] LAVALLE S M. Rapidly-exploring random trees: a new tool for path planning, TR 98- 11 [R]. Ames, Iowa: Iowa State University, 1998.

[11] LAVALLE S M, KUFFNER J J. Randomized kinodynamic planning [J]. International Journal of Robotics Research, 2001, 20(5): 378-400.

[12] DUBINS L E. On plane curves with curvature [J]. Pacific Journal of Mathematics, 1961, 11(2): 471-481.

[13] 梁勇,張友安,雷軍委.一種基于Dubins路徑的在線快速航路規(guī)劃方法[J].系統(tǒng)仿真學(xué)報,25(S1):291-296.(LIANG Y, ZHANG Y A, LEI J W. New method of online fast path planning based Dubins path [J]. Journal of System Simulation, 25(S1): 291-296.)

This work is partially supported by the National Natural Science Foundation of China (51579204, 51679180), the Self-determined and Innovative Research Fund of Wuhan University of Technology (2016IVA064).

LIULezhu, born in 1984, Ph. D. candidate. His research interests include path planning, attitude control.

XIAOChangshi, born in 1974, Ph. D., professor. His research interests include machine vision, unmanned vehicle.

WENYuanqiao, born in 1975, Ph. D., professor. His research interests include maritime traffic safety and environment.

MotionplanningalgorithmforunmannedsurfacevehiclebasedonDubinspath

LIU Lezhu, XIAO Changshi*, WEN Yuanqiao

(SchoolofNavigation,WuhanUniversityofTechnology,WuhanHubei430000,China)

Aiming at the problem of motion planning for unmanned surface vehicle, a new method of calculating Dubins path by using pure geometric method was proposed by the theoretical analysis of Dubins path. The operation of solving equations was not used by the proposed method. Firstly, the steering circle was calculated according to the motion state of the unmanned surface vehicle. Then, the geometric method was used to compute the common tangent of the steering circles. Finally, the Dubins path was obtained by the common tangent connection. The effectiveness of the proposed method was verified through five groups of simulation experiments. All kinds of situations in the process of calculating the Dubins path were designed in the first four simulation experiments, the proposed algorithm was verified to be applicable to a variety of Dubins path calculation. The last experiment of simulations was used for path planning and motion state adjustment of unmanned surface vehicle. The simulation results show that the motion planning algorithm based on Dubins path is feasible.

unmanned surface vehicle; path planning; Dubins path; motion planning

TP391.9; TP18

:A

2016- 11- 16;

:2016- 12- 23。

國家自然科學(xué)基金資助項目(51579204，51679180)；武漢理工大學(xué)自主創(chuàng)新基金資助項目(2016IVA064)。

劉樂柱(1984—)，男，安徽淮北人，博士研究生，主要研究方向：路徑規(guī)劃、姿態(tài)控制； 肖長詩(1974—)，男，湖北松滋人，教授，博士，主要研究方向：機器視覺、無人航行器； 文元橋(1975—)，男，湖北松滋人，教授，博士，主要研究方向：水上交通安全與環(huán)境。

1001- 9081(2017)07- 2114- 04

10.11772/j.issn.1001- 9081.2017.07.2114