基于Dubins路徑的無人艇運動規劃算法

劉樂柱，肖長詩，文元橋

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

基于Dubins路徑的無人艇運動規劃算法

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

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

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

無人艇；路徑規劃；Dubins路徑；運動規劃

0 引言

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

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

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

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

1 問題描述

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

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

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

2 理論分析

2.1 轉向圓計算

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

(1)

(2)

圖1 無人艇的兩個轉向圓

2.2 公切線計算

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

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

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

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

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

(3)

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

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

(4)

(5)

(6)

(7)

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

(8)

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

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

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

(9)

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

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

(10)

(11)

(12)

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

(13)

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

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

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

1)分別計算起始狀態和目標狀態的轉向圓，各↑↓兩個；

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

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

根據以上分析得到的結論，可以設計下面的Dubins路徑計算算法。

3 算法設計

3.1 數據結構

由理論分析，根據問題特點，定義以下三種結構體數據類型：1)無人艇狀態數據類型；2)圓數據類型；3)路徑數據類型。

這樣算法中主要包含：起始狀態、目標狀態兩個狀態數據；兩個旋轉方向相反的起始圓和兩個旋轉方向相反的目標圓共4個圓數組數據；一個存放4條Dubins路徑點的路徑數據。

3.2 算法流程

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

圖4 本文算法流程

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

4 仿真實驗

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

4.1 Dubins路徑計算

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

表2 Dubins路徑計算算例

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

圖5 算例1計算結果

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

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

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

圖6 算例2計算結果

圖7 算例3計算結果

圖8 算例4計算結果

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

4.2 無人艇運動狀態調整

本節是仿真實驗中的第5個算例，使用Dubins路徑作無人艇運動狀態調整。無人艇運動響應模型為：

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

(14)

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

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

圖9 無人艇運動狀態調整

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

5 結語

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

References)

[1] 朱大奇,顏明重.移動機器人路徑規劃技術綜述[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] 孟偲,王田苗.一種移動機器人全局最優路徑規劃算法[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] 趙先章,常紅星,曾雋芳,等.一種基于粒子群算法的移動機器人路徑規劃方法[J].計算機應用研究,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].北京:清華大學出版社,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*算法的越野路徑規劃研究[J].計算機應用研究,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路徑的在線快速航路規劃方法[J].系統仿真學報,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。

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

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

1001- 9081(2017)07- 2114- 04

10.11772/j.issn.1001- 9081.2017.07.2114