購(gòu)買設(shè)計(jì)請(qǐng)充值后下載，，資源目錄下的文件所見即所得，都可以點(diǎn)開預(yù)覽，，資料完整，充值下載可得到資源目錄里的所有文件。。?！咀ⅰ浚篸wg后綴為CAD圖紙，doc,docx為WORD文檔，原稿無水印，可編輯。。。具體請(qǐng)見文件預(yù)覽，有不明白之處，可咨詢QQ:12401814
文章編號(hào)：
基于Dubins路徑的智能車輛路徑規(guī)劃算法研究
宋國(guó)浩，黃晉英，蘭艷亭
（中北大學(xué)機(jī)械與動(dòng)力工程學(xué)院 太原，030051）
摘要：路徑規(guī)劃是車輛智能化的核心問題之一，而所有路徑均可分解為簡(jiǎn)單的Dubins路徑。在Dubins路徑的思想下對(duì)智能車輛的行駛路徑進(jìn)行分段研究，并利用經(jīng)典PID控制對(duì)該算法的執(zhí)行性能進(jìn)行檢驗(yàn)。研究表明：算法能計(jì)算出車輛行駛的最短路徑，減少了車輛行駛的路徑長(zhǎng)度，縮短了行駛時(shí)間，減少了控制系統(tǒng)的計(jì)算量，提高了車輛執(zhí)行系統(tǒng)的執(zhí)行力度，降低了執(zhí)行誤差，對(duì)最優(yōu)路徑具有較好的選擇性。
關(guān)鍵詞：智能車；路徑規(guī)劃；Dubins路徑；最短路徑
中圖分類號(hào)：TP273+.1 文獻(xiàn)標(biāo)志碼：A
Intelligent Vehicles’ path planning algorithm based on the Dubins Path
Song Guo-hao,Huang Jin-ying,Lan Yan-ting
(School of Mechanical and Power Engineering ,North University of China Taiyuan,030051)
Abstract: The path planning is one of the core issues of intelligent vehicles. All paths can be decomposed into Dubins path. This paper did sectional research into the intelligent vehicles’ travel path under the idea?of?Dubins path and carried out tests on the execution?performance of the algorithm using PID control strategy. Researches showed that this algorithm could calculate the vehicles’ shortest path, reduced the vehicles’ path length, shortened the time of driving, reduced the computation?amount of the control system, improved the?enforcement?of the vehicle?execution system, reduced the execution error, had a good selectivity?of the optimal path.
Keyword: Intelligent Vehicles; Path planning; Dubins Path; the shortest path
1 引言
路徑規(guī)劃應(yīng)用在很多領(lǐng)域，例如：軍事無人機(jī)，航天探測(cè)機(jī)器人，智能車輛，以及監(jiān)視和偵察等工作[1-3]。路徑規(guī)劃在現(xiàn)代汽車領(lǐng)域中是一個(gè)研究熱門領(lǐng)域，需要考慮多方面的因素，如：汽車自身約束條件，車輛行駛環(huán)境的約束以及其他的行駛問題。在路徑規(guī)劃中，首先應(yīng)考慮車輛的可行駛性，在對(duì)車輛行駛路線進(jìn)行規(guī)劃時(shí)，應(yīng)保證其安全行駛的前提下，盡可能大地規(guī)劃出車輛行使范圍。在保證車輛安全行駛的問題中，需要使車輛自主地繞開其它影響車輛行駛的物體，使車輛避免與障礙物相撞。路徑規(guī)劃算法應(yīng)具有精確性，占有較小的內(nèi)存，并滿足實(shí)時(shí)性的要求，在執(zhí)行過程中沒有明顯的延時(shí)問題[4-5]。此外，為了使行駛路徑達(dá)到最優(yōu)，提高行駛效率，還應(yīng)縮短車輛行駛長(zhǎng)度。
目前，在有關(guān)路徑規(guī)劃中的研究中，如張明環(huán)等[6]提出的觸須算法，此算法是在車輛行駛前，首先對(duì)車輛將要行駛的路線進(jìn)行規(guī)劃，讓車輛按照規(guī)劃好的16*81條可使用的路徑行駛，這樣可以使車輛節(jié)省大量的反應(yīng)時(shí)間，但卻不能夠處理突變情況，研究背景過于理想化；王凱等[7]提出了改進(jìn)的人工勢(shì)場(chǎng)法，將此算法應(yīng)用在智能車路徑規(guī)劃中的避障環(huán)節(jié)，解決了傳統(tǒng)人工勢(shì)場(chǎng)法在路徑規(guī)劃中易陷入局部極小值的問題，具有一定的實(shí)時(shí)性，但其受限于所用傳感器性能的影響，其作用范圍較小，且易受外界環(huán)境影響；Jiaojie Li 等[8]提出了協(xié)調(diào)避障算法，在車輛行駛過程中，應(yīng)用一階運(yùn)動(dòng)學(xué)、二階動(dòng)力學(xué)，對(duì)障礙物進(jìn)行無速度監(jiān)測(cè)并繞過障礙物，但其將所有障礙物作為靜態(tài)處理，不具有靈活性，可能存在兩車同時(shí)避讓卻無處可避的局面。
本文主要根據(jù)Dubins路徑規(guī)劃算法對(duì)車輛的行駛路線進(jìn)行規(guī)劃控制，此種算法能夠很好地決策出車輛在行駛過程中的最優(yōu)路徑，并可以解決多障礙物間的避障問題，具有良好的實(shí)時(shí)性，延時(shí)性較小。
2 路徑的選擇
路徑規(guī)劃的主要目的是尋求一條安全、快捷的行駛路線，使車輛完成行駛目的。一般情況下，車輛行駛在已知或者部分已知的區(qū)域內(nèi)，即已知一些靜態(tài)障礙物?，F(xiàn)用P（x,y,θ,v,a)來表示車輛行駛狀態(tài)，（x,y)表示車輛行駛位置，（θ,v,a)中參數(shù)分別表示車輛行駛偏向角、行駛速度和行駛加速度。若車輛從起點(diǎn)P0行駛到終點(diǎn)P2的路徑為R(q)，則可以近似表示為：
(1)
式中R(q)為產(chǎn)生的路徑，q為路徑參數(shù)，表示路徑的長(zhǎng)度變量（0≤q≤s)或路徑中的角度變量（0≤q≤θ),q的具體值取決于行駛中的路況。
對(duì)上式進(jìn)行詳細(xì)描述如下：
(2)
當(dāng)車輛行駛過程中，遇到障礙物時(shí)，可通過控制系統(tǒng)及時(shí)改變參數(shù)（θ,v,a)來使車輛繞開障礙物。
當(dāng)然，車輛在行駛過程中，不只有已知的障礙物，還有其它的約束條件，如：在正常行駛條件下，最小時(shí)間、最小路徑等作為其約束條件。用ψ表示約束條件，則路徑可表示為
(3)
其控制原理示意圖如下所示：
圖1 路徑規(guī)劃示意圖
路徑規(guī)劃中的運(yùn)動(dòng)學(xué)模型，在兩自由度下的運(yùn)動(dòng)學(xué)特性和當(dāng)前狀態(tài)可表示為：
(4)
式中，v是車輛行駛速度，θ為水平角，為車輛轉(zhuǎn)動(dòng)角速度。
當(dāng)車輛行駛時(shí)，路徑約束條件是必須考慮的重要因素之一。車輛路徑規(guī)劃的兩個(gè)重要約束條件是可行性和安全性。車輛在行駛過程中避免相撞的問題可以表示為,。、為車輛本身的位置，、為車輛所監(jiān)測(cè)到的障礙物的位置，、分別為橫向、縱向安全距離。則路徑規(guī)劃中的約束問題可表示為
(5)
我們期望車輛在行駛過程中能繞過障礙物且最終回到原行駛軌道上。則車輛行駛路徑可以簡(jiǎn)化為Dubins路徑：一個(gè)圓?。–路徑)或兩個(gè)相切圓?。–C路徑）或兩個(gè)圓弧通過一個(gè)共同的相切直線連接（CLC路徑）。這是Dubins證明的兩個(gè)位置點(diǎn)的最短路徑[9]，C表示圓弧段，L表示與圓弧段相切的直線段?？梢钥闯鲎詈笠环N路徑包含前兩種路徑，本文對(duì)CLC路徑進(jìn)行研究，即車輛行駛路徑為起始轉(zhuǎn)彎、直線行駛、終止轉(zhuǎn)彎。分別對(duì)CLC路徑中的三段Dubins路徑建立對(duì)應(yīng)的坐標(biāo)系，坐標(biāo)系分別為、、。每個(gè)基本坐標(biāo)定義為，是與速度矢量平行的單位切矢量，是與垂直的單位法矢量。圖2為CLC路徑幾何圖：
圖2 Dubins路徑
矢量和的模表示為初始與終止的轉(zhuǎn)彎半徑，其正負(fù)代表車輛是向左轉(zhuǎn)彎還是向右轉(zhuǎn)彎，同時(shí)也確定了運(yùn)動(dòng)曲率Q的正負(fù)，每個(gè)矢量定義如下：
， (6)
， (7)
， (8)
式中：是初始轉(zhuǎn)彎曲率，是終止轉(zhuǎn)彎曲率，是直線行駛的長(zhǎng)度。
則車輛行駛時(shí)的速度矢量在各坐標(biāo)系下的變換關(guān)系為：
（9）
式中：是從起始坐標(biāo)系變換到終止坐標(biāo)系的旋轉(zhuǎn)矩陣。
所以總的旋轉(zhuǎn)角θ可表示為：
（10）
又因?yàn)檫B接矢量與、垂直。
（11）
是基矢量；是起始圓弧對(duì)應(yīng)的角度。
系統(tǒng)在不同坐標(biāo)下對(duì)車輛行駛的位置進(jìn)行定義時(shí)，在起始坐標(biāo)系下定義起點(diǎn)與終點(diǎn)的相對(duì)位置，如下式表示：
, （12）
在起始坐標(biāo)系內(nèi)用矢量的和表示這個(gè)位置矢量：
（13）
等式左邊表示從起始圓弧中心到終止圓弧中心之間的矢量，所以：
（14）
等式中d表示兩圓弧中心之間的距離。
為了減少系統(tǒng)所處理數(shù)據(jù)的工作量，使系統(tǒng)更加高效的完成車輛在行駛時(shí)路徑選擇減少計(jì)算的誤差率，希望整個(gè)過程能夠在同一坐標(biāo)系下進(jìn)行運(yùn)算，其基礎(chǔ)就是將連接各坐標(biāo)系的連接矢量、、表示在同一坐標(biāo)系下，則三者表示在起始坐標(biāo)系中如下:
（15）
則，等式（14）在轉(zhuǎn)換坐標(biāo)下，可以表示為：
（16）
由于為旋轉(zhuǎn)矩陣，所以：
（17）
即：
（18）
顯然，若路徑可行，則
（19）
由等式（17）和式（18）可得：
（20）
又因轉(zhuǎn)換矩陣為：
（21）
可以得到關(guān)于角度的等式如下：
（22）
其中：
（23）
由已知等式（10）求得終止角度如下：
（24）
所以，車輛經(jīng)過一個(gè)CLC路徑行駛的總長(zhǎng)度為:
（25）
車輛從所求得的可行性的行駛路徑中，比較選擇最短路徑來作為車輛的行駛路徑，即min(L)。
而車輛從坐標(biāo)系行駛到坐標(biāo)系后，其在兩個(gè)坐標(biāo)系中存在的關(guān)系為：
（26）
根據(jù)等式（26）可知，車輛在行駛過程中，通過坐標(biāo)轉(zhuǎn)換可以使用同一坐標(biāo)進(jìn)行表示，等式中矩陣B為齊次變換坐標(biāo)矩陣。
3 避障系統(tǒng)控制
由圖3可知車輛在行駛過程中，可能存在同時(shí)與多個(gè)障礙物相遇的問題，在解決此類問題中，首先根據(jù)Dubins最短路徑算法確定車輛與障礙物間的最短距離以及相對(duì)速度。相對(duì)速度的表示則可由下式給出：
（27）
表示車輛行駛的的速度，表示第i個(gè)障礙物的行駛速度，表示第i個(gè)障礙物相對(duì)于車輛的行駛速度。
則第i個(gè)障礙物相對(duì)于車輛的最短相遇距離矢量如下式所示：
（28）
表示車輛與第i個(gè)障礙物之間的直線距離，表示車輛與第i個(gè)障礙物行駛的速度方向之間的夾角。表示車輛與第i個(gè)障礙物相遇時(shí)的最短距離。
圖3 躲障礙物相遇示意圖
因此，若要保證車輛在行駛中的安全性與可行駛，需要根據(jù)監(jiān)測(cè)的情況對(duì)車輛行駛速度的方向進(jìn)行調(diào)整，使得大于車輛的安全距離、。
在求解與多個(gè)障礙物相遇的問題中，通常需要構(gòu)造Lyapunov函數(shù)來確定使用函數(shù)的穩(wěn)定性[10]:
（29）
其中，是車輛行駛速度方向與系統(tǒng)期望車輛行駛速度方向之間的角度差，即車輛行駛速度方向角度誤差；、分別為車輛由行駛速度方向向左或向右旋轉(zhuǎn)到避開障礙物時(shí)的安全行駛速度方向的角度。
則車輛與多個(gè)障礙物相遇的狀況下，車輛的行駛速度方向的角速度可表示為：
（30）
其中：，
上式也可表述為控制車輛避障系統(tǒng)的算法。
由此可知，
（31）
其中：
（32）
車輛在行駛過程中，控制系統(tǒng)根據(jù)監(jiān)測(cè)裝置傳遞的信息進(jìn)行控制，監(jiān)測(cè)值用表示，其輸出為高電平1時(shí)，表示前方有障礙物；輸出為低電平0時(shí)，表示前方無障礙物。而分別表示車輛左、中、右三個(gè)方向的監(jiān)測(cè)值。則車輛行駛決策表如下：
表1 車輛行駛決策表
有無障礙物
決策結(jié)果
0
0
0
無
直行
0
0
1
右側(cè)有障礙物
向左轉(zhuǎn)度
0
1
0
中間有障礙物
向右轉(zhuǎn)度
0
1
1
左側(cè)無障礙物
向左轉(zhuǎn)度
1
0
0
左側(cè)有障礙物
向右轉(zhuǎn)度
1
0
1
中間無障礙物車輛可通過
直行
1
0
1
中間無障礙物車輛不可通過
停止或右轉(zhuǎn)度
1
1
0
右側(cè)無障礙物
向右轉(zhuǎn)度
1
1
1
前方有障礙物
停止或右轉(zhuǎn)度
4 實(shí)驗(yàn)仿真
(1)仿真系統(tǒng)搭建 車輛控制系統(tǒng)根據(jù)Dubins路徑進(jìn)行路徑規(guī)劃，并將此算法應(yīng)用Matlab軟件進(jìn)行仿真。首先對(duì)道路進(jìn)行設(shè)計(jì)，為了使道路狀況盡可能的接近現(xiàn)實(shí)路況，設(shè)計(jì)道路為15m*15m的矩形場(chǎng)地，并與改進(jìn)的人工勢(shì)場(chǎng)法[7]進(jìn)行對(duì)比；然后利用經(jīng)典PID控制對(duì)系統(tǒng)的跟隨情況進(jìn)行檢驗(yàn)。
將車輛的舵機(jī)作為被控對(duì)象，圖4為車輛對(duì)輸入信號(hào)的跟隨響應(yīng)控制的方框圖：
圖4 車輛跟隨響應(yīng)控制方框圖
根據(jù)車輛的控制方案圖可得方塊圖如下：
圖5 車輛跟隨響應(yīng)控制方塊圖
(2)仿真結(jié)果及分析 車輛行駛過程中，首先確定目標(biāo)方向，通過轉(zhuǎn)向、直行等過程繞過監(jiān)測(cè)到的障礙物，并在整個(gè)過程中實(shí)時(shí)更新車輛所在位置的坐標(biāo)。仿真結(jié)果如下，其中：黑色為Dubins算法行駛路徑，藍(lán)色為對(duì)比算法行駛路徑。
設(shè)計(jì)如圖6所示典型路況，場(chǎng)地內(nèi)設(shè)有各種典型矩形障礙物，并包括“一”字形障礙物和“U”形障礙物，檢驗(yàn)車輛在典型路況下的行駛路徑，驗(yàn)證Dubins路徑規(guī)劃算法的可行性。圖7中設(shè)置有隨機(jī)障礙物，檢驗(yàn)車輛在復(fù)雜路況下的行駛路徑。從兩圖中可以看出車輛在最短路徑下對(duì)車輛行駛方位進(jìn)行調(diào)整，在保證車輛行駛安全的條件下，能對(duì)路徑進(jìn)行最優(yōu)選擇。
圖6 典型路況下車輛行駛路線仿真圖
圖7 復(fù)雜路況下車輛行駛路線仿真圖
表2 典型路況仿真結(jié)果
Dubins路徑規(guī)劃算法
對(duì)比算法
行駛時(shí)間(s)
2.9
3.5
行駛路徑長(zhǎng)度(m)
18.6
21.7
表3 復(fù)雜路況仿真結(jié)果
Dubins路徑規(guī)劃算法
對(duì)比算法
行駛時(shí)間(s)
4.4
5.4
行駛路徑長(zhǎng)度(m)
28.4
35.7
通過典型路況和復(fù)雜路況仿真結(jié)果表明，Dubins路徑規(guī)劃算法能選擇最短路徑，縮短了車輛行駛時(shí)間，能為車輛提供更為合適的路徑，并避免了車輛在行駛時(shí)陷入局部最優(yōu)的傳統(tǒng)缺陷，車輛在繞過障礙物后可以回到與初始位置相對(duì)應(yīng)的路徑上，與其它算法對(duì)比可得，Dubins路徑規(guī)劃算法能更好的完成實(shí)驗(yàn)?zāi)康摹?
圖8 車輛跟蹤曲線與參考曲線對(duì)比
圖9 車輛跟蹤曲線誤差
圖8、圖9反映了車輛實(shí)際行駛路線對(duì)控制器設(shè)計(jì)路線的跟隨情況，圖8可以看出車輛對(duì)系統(tǒng)設(shè)計(jì)的路線具有很好的執(zhí)行性，其執(zhí)行偏差在0.5米以內(nèi)；圖9可看出，車輛的執(zhí)行誤差在4%以內(nèi)，而隨著車輛行駛狀況的逐漸穩(wěn)定，其執(zhí)行誤差會(huì)繼續(xù)降低，具有較高的可靠性。
5 結(jié)論
如今在車輛智能化的趨勢(shì)下，路徑規(guī)劃已經(jīng)被越來越多的人們研究。本文在Dubins路徑思想下對(duì)車輛行駛路徑進(jìn)行規(guī)劃，完成了該算法下的仿真實(shí)驗(yàn)，并于其他算法進(jìn)行了對(duì)比，算法簡(jiǎn)潔可行；在
此算法理論下，對(duì)車輛行駛路徑設(shè)計(jì)了兩種不同復(fù)雜程度的路況，滿足復(fù)雜路況下的行駛要求；對(duì)比結(jié)果表明，Dubins路徑規(guī)劃算法能計(jì)算出最短路徑，對(duì)最優(yōu)路徑具有良好的選擇性；車輛對(duì)此算法具有較高的執(zhí)行性，并具有較低的誤差率。
參考文獻(xiàn)
[1]吳友謙,裴海龍.基于Dubins曲線的無人直升機(jī)軌跡規(guī)劃[J].計(jì)算機(jī)工程與設(shè)計(jì),2011,32(4):1426-1429.
[2]朱大奇,顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述[J].控制與決策,2010,25(7):961-967.
[3]高強(qiáng),宋雨,呂東澔等.多移動(dòng)機(jī)器人路徑規(guī)劃仿真研究[J].計(jì)算機(jī)仿真,2014,31(7):325-329.
[4] Giordano P R, Vendittelli M. Shortest paths to obstacles for a polygonal Dubins car[J]. Robotics, IEEE Transactions on, 2009, 25(5): 1184-1191.
[5] Savla K, Frazzoli E, Bullo F. Traveling salesperson problems for the Dubins vehicle[J]. Automatic Control, IEEE Transactions on, 2008, 53(6): 1378-1391.
[6]張明環(huán),張科.智能車避障觸須算法中的障礙物探測(cè)研究[J].西北工業(yè)大學(xué)學(xué)報(bào),2012,30(5):763-767.
[7]王凱,宋星秀,張一聞.利用改進(jìn)人工勢(shì)場(chǎng)法的智能車避障路徑規(guī)劃[J].遼寧工程技術(shù)大學(xué)學(xué)報(bào):自然科學(xué)版,2014,33(9):1236-1239.
[8] Li J, Zhang W, Su H, et al. Coordinated obstacle avoidance with reduced interaction[J]. Neurocomputing, 2014,139: 233-245.
[9]Anderson R P, Milutinovic D. A Stochastic Approach to Dubins Vehicle Tracking Problems[J]. Automatic Control, IEEE Transactions on, 2014,10:2081-2086.
[10][英]楚拉多斯,[英]懷特,[英]尚穆加韋爾.無人機(jī)協(xié)同路徑規(guī)劃[M].北京：國(guó)防工業(yè)出版社，2013.
6
> REPLACE THIS LINE WITH YOUR PAPER IDENTIFICATION NUMBER (DOUBLE-CLICK HERE TO EDIT) <
This paragraph of the first footnote will contain the date on which you submitted your paper for review. It will also contain support information, including sponsor and financial support acknowledgment. For example, “This work was supported in part by the U.S. Department of Commerce under Grant BS123456”.
The next few paragraphs should contain the authors’ current affiliations, including current address and e-mail. For example, F. A. Author is with the National Institute of Standards and Technology, Boulder, CO 80305 USA (e-mail: author@ boulder.nist.gov).
S. B. Author, Jr., was with Rice University, Houston, TX 77005 USA. He is now with the Department of Physics, Colorado State University, Fort Collins, CO 80523 USA (e-mail: author@lamar.colostate.edu).
T. C. Author is with the Electrical Engineering Department, University of Colorado, Boulder, CO 80309 USA, on leave from the National Research Institute for Metals, Tsukuba, Japan (e-mail: author@nrim.go.jp).
Song Guo-Hao,Huang Jin-Ying,Lan Yan-Ting
(School of Mechanical and Power Engineering, North University of China Taiyuan,030051)
Abstract—The path planning is one of the core issues of intelligent vehicles. All paths can be decomposed into Dubins path. This paper did sectional research into the intelligent vehicles’ travel path under the idea?of?Dubins path and carried out tests on the execution?performance of the algorithm using PID control strategy. Researches showed that this algorithm could calculate the vehicles’ shortest path, reduced the vehicles’ path length, shortened the driving time, reduced the computation?amount of the control system, improved the?enforcement?of the vehicle?execution system, reduced the execution error, had a good selectivity?of the optimal path.
Index Terms—Intelligent Vehicles, path planning,Dubins path, the shortest path.
I. INTRODUCTION
P
ATH planning is used in many fields, such as: military unmanned aircraft, space exploration robot, intelligent vehicle, surveillance and reconnaissance and so on[1-3]. Path planning is a hot area of research in the field of modern vehicle, which needs to consider many factors, such as: constraints from vehicle itself, constraints of driving environment and other issues. In the planning of driving route, we should plan out of the scope of vehicle as far as possible under the premise of safe driving and make the vehicle bypass obstacles autonomously. Path planning algorithm should have precision, occupy less memory, meet the requirements of real-time, and have no obvious delay problems during the implementation[4-5]. In addition, in order to make the driving path optimal and improve the driving efficiency, it is necessary to shorten the driving length of the vehicle.
There are many related research of path planning. Such as the Tentacle Algorithm proposed by Zhang Minghuan, et al[6]. This algorithm planned the route that the vehicle will driving at first, let the vehicle driving according to the planned 16 * 81 usable routes. In this way, the vehicle could save a lot of reaction time, but it was not able to handle mutation, and the research background was too idealistic. Wang Kai, et. al[7]. proposed the improved-artificial potential field method. This algorithm could be applied to obstacle avoidance stages in intelligent car path planning, solved the problem about the vehicle easily trapped in local minimum in the traditional artificial potential field method in path planning, had certain real-time performance. But it is limited by the influence of the sensor performance and its function scope is small, and it is easily affected by the external environment. Jiaojie Li et al[8]. proposed the coordinated obstacle avoidance algorithm. This algorithm applied the first-order kinetics and second order kinetics in the process of driving to conduct no-speed monitoring and bypass the obstacles. But it handled all obstacles as static treatment, and do not have flexibility. It may happens that two cars avoid obstacles at the same time but nowhere to avoid.
This paper did research into the intelligent vehicle’ travel path under the idea?of?Dubins path. This algorithm can well decide out of the optimal path when driving on the road and can solve the problem of the obstacle avoidance between many obstacles. This algorithm has good real-time performance and small delay.
II. The Choice of Paths
The main purpose of path planning is to seek a safe and fast driving route, and make the vehicle driving to the end. Generally speaking, vehicles driving in the area of the known or partially known areas, which means areas with some static obstacles. Now, we use P(x,y,θ,v,a) as driving states,(x,y) as driving position. Parameters in (θ,v,a) respectively represent driving deviation angle, speed and acceleration. If we put the path of the vehicles driven from the starting point P0 to the ending point P2 as R (q), which can be approximately shown as follows:
(1)
In it, R (q) is the produced path, q is one of the parameters of path, which indicates the length variable in the path (0≤q≤s)) or angle variable in the path (0≤q≤θ).The specific value of q depends on the driving condition.
The detailed description is as follows:
(2)
When the vehicle meeting with obstacles in the driving process, we can make the vehicle bypass obstacles by changing parameters of control system(θ, v, a) timely.
The vehicle is constrained by other constraint condition in the driving process, except to the known obstacles, such as: the minimum time, the minimum path. Use ψ as constraint condition, the path can be expressed as:
(3)
The control principle diagram is shown below:
Fig.1. The control principle diagram
The kinematics characteristics and the current state of the kinematics model in the path planning under the two degrees of freedom can be expressed as
(4)
In the above formulas, v is the vehicle speed, θ is the horizontal angle, ω is the vehicle rotation angular velocity.
The path constraint condition is one of the important factors that must be considered when driving. The two important constraint conditions of vehicle path planning is the feasibility and safety. The problem of avoiding car collision in the process of driving can be expressed as ,. , is the position of the vehicle itself. , is the position of the obstacles the vehicle monitoring. , is the safe distance of the horizontal and vertical, respectively. The constraint issues of the path planning can be expressed as
(5)
What we hope is the vehicle can steer around obstacles and eventually return to the original orbit. The path of the vehicle can be simplified as Dubins path: a circular path (C path) or two tangent arc path (CC path) or two arc through a common tangent line connection path (CLC). This is the shortest path between the two points Dubins prove[9], C is arc section, L is the line segment tangent to the arc segment. It can be seen that the last path contains the first two path. This paper studies the CLC path, namely the vehicle path to start turning, go straight, termination turning. To build the corresponding coordinate system of the CLC three Dubins path, coordinate system is ,,, respectively. Each basic coordinates are defined as .In it, t is the unit tangent vector parallel to the velocity vector, and n is the unit normal vector vertical to t. Fig.2 is the CLC path geometry figure:
Fig.2. the CLC path geometry figure
The length of the vector and is expressed as the initial and terminal turning radius, and the plus or minus of it not only represents vehicles turning to the left or right, but also determines the plus or minus of the movement curvature Q . Each vector are defined as follows:
, (6)
, (7)
, (8)
In the above formulas, is initial turning curvature, is terminal turning curvature, and is the length of the straight line driving.
The transformation of relations of the vehicle velocity vector in the coordinates is:
(9)
In it, R(θ) is the rotation matrix from the initial coordinate system transforming to the terminate coordinate system.
Thus, the total rotation angle can be expressed as
(10)
And because the connect vector is vertical with ,, then:
(11)
is ’s base vector. is the angle corresponding to the starting arc.
System define the position of the vehicle under different coordinate system. Define the relative position of starting point and end point under the starting coordinates. Defined of the path , expressed as the following type:
, (12)
Express?the position vector using the sum of vectors under the starting coordinates:
(13)
Equation on the left means vector between the start arc center to terminate arc center, thus:
(14)
d is the distance between the two arc center.
In order to reduce the workload of the system when processing data, to make the system complete the choice of the driving path efficiently and reduce the error rate calculation, to make the whole operation processed under the same coordinate system, expressing the connection vector ,, of each coordinate system under the same coordinate system is necessary. It can be expressed as follows:
(15)
Then equation (14) in conversion coordinates can be represented as:
(16)
Due to the rotation matrix , thus:
(17)
namely：
(18)
Obviously, if the path is feasible, then
(19)
By equation (17) and equation (18), we can get:
(20)
And because of the transformation matrix :
(21)
We can get:
(22)
In it:
(23)
The end angle is obtained by the known equation (10), expressed as follows:
(24)
So, the total length of a vehicle’s CLC routes is as follows:
(25)
Vehicles choose the shortest path, means min(L) for the vehicle driving route from the calculated path with feasibility through the comparison.
And when vehicle driving from the coordinate system to the coordinate system , the relations that exist in the coordinate system is:
(26)
According to the equation (26), the vehicle can be expressed using the same coordinate in the driving process through the coordinate transformation. In it, matrix B is homogeneous transformation coordinate matrix.
III. Obstacle avoidance control system
From fig.3,we can illustrates that the vehicle may come across multiple obstacles in the process of driving. To solve these problems, the first need to do is determine the shortest distance between the vehicle and the obstacles, as well as the vehicle’s relative speed based on the Dubins shortest path algorithm. The representation of relative speed can be given by the following?formula:
(27)
In it,is the speed of the vehicle, is the speed of the ith obstacle, is the relative speed of the ith obstacle relative to the vehicle’s speed .
Then, the ith obstacle’s shortest distance vector relative to the vehicle is as follows:
(28)
In it,is the linear distance between vehicle and the ith obstacle, is the angle between the direction of the speed of the vehicle and that of the ith obstacle, and is the shortest distance between the vehicle and the ith obstacle when they met.
Therefore, to ensure the safety and driving of driving vehicles, it is needed to adjust the direction of the vehicle’s speed according to the monitoring situation, which can make greater than the vehicle’s safe distance ,.
Fig.3. encounter more obstacles schematic diagram
When solving multiple obstacles meeting problem, we usually need to construct the Lyapunov function to determine the stability of the function used[10]:
(29)
In it,is the angle difference between the direction of vehicle’s driving speed and the expect driving speed, that is, the angle difference between the direction of vehicle’s speed; ,are the vehicle’s safe driving speed direction angle produced by the vehicle turning left or right to avoid obstacles, respectively.
Then under the condition of the vehicle meeting with multiple obstacles, the vehicle's angular speed in the driving speed direction can be expressed as:
(30)
In it,,.
The formula can also be expressed as the algorithm of vehicle’s obstacle avoidance system.We can get,
(31)
In it,
(32)
In the driving process, the control system take control according to the message from the monitoring device, and the monitoring value is expressed by .Its output is the high level 1 indicates that there is obstacle ahead; Its output is low level 0 means no obstacle ahead. And ,,indicates the monitoring values from the vehicle’s left, middle and right three directions respectively. Then the vehicle decision table is as follows:TABLE I
Vehicle decision table
Presence of obstacles
The decision results
0
0
0
None
Go straight
0
0
1
Obstacles on the right
degrees to the left
0
1
0
Obstacles in the middle
degrees to the right
0
1
1
No obstacle on the left
degrees to the left
1
0
0
Obstacles on the left
degrees to the right
1
0
1
No obstacle in the middle and the vehicle can pass
Go straight
1
0
1
No obstacle in the middle and the vehicle cannot pass
Stop or degrees to the right
1
1
0
No obstacle on the right
degrees to the right
1
1
1
Obstacles ahead
Stop or degrees to the right
IV. The experimental simulation
A. Setting Up the Simulation System
Vehicle control system implement path planning according to the Dubins path and the algorithm uses Matlab software for simulation. The first to do is design road. In order to make the road conditions similar to real road conditions as close as possible, we take 15m * 15m rectangular area, and compared with the improved artificial potential field method[7]; Then test the system’s following situation using the classical PID control.
Taking the vehicle's steering gear as the controlled object, fig.4 is the response control block diagram to the input signal:
Fig.4. vehicle following response control diagram
According to the control scheme figure, we can get the block diagram as follows:
Fig.5. vehicle following response control block diagram
B. The Results of Simulation And Analysis
In the process of driving, the vehicle determine the target direction firstly, then bypass the monitoring obstacles through turning or going straight, and update the vehicle’s location coordinates in real time in the whole process. Simulation results are as follows. In it: black indicates the driving path using Dubins algorithm, while blue for the comparison algorithm driving path.
We design the typical road conditions shown in fig.6, which field with various typical rectangular obstacles including "-" glyph and "U" shape obstacles, to test vehicle’s driving path under typical road condition and to validate the feasibility of Dubins path planning algorithm. There are random obstacles set in fig.7, which is designed to inspect vehicle’s driving path under complex road condition. It can be seen from the two diagrams that the vehicle can adjust position effectively and can make optimal path choice under the condition of safety.
Fig.6. the vehicle’s driving path simulation diagram under typical road condition
Fig.7. the vehicle’s driving path simulation diagram under complex road condition
TABLE II
The simulation results of typical road conditions
Dubins path planning algorithm
Compared algorithm
Driving time(s)
2.9
3.5
The length of driving(m)
18.6
21.7
The simulation results show that the Dubins path planning algorithm can choose the shortest path, shorten the driving time, and avoid traditional defect of driving into local optimum. Vehicles can return to the corresponding initial path after bypassing obstacles. Compared with other algorithms, Dubins path planning algorithm can finish the experiment better.TABLE III
The simulation results of complex road conditions
Dubins path planning algorithm
Compared algorithm
Driving time(s)
4.4
5.4
The length of driving(m)
28.4
35.7
(a)
(b)
Fig.8. Contrast figure about vehicle’s real driving route and planning route
Fig.8 reflects the following situation that actual vehicle driving route follow controller-designed route. It can be seen from figure(a) that the vehicle has a good execution to the controller-designed route, and the executive deviation is within 0.5 meters; Figure (b) tell us that the execution error is within 4%, and will continue to reduce with the driving conditions becoming stable gradually. It has high reliability.
V. Conclusion
Now under the trend of intelligent vehicle, path planning has been studied by more and more people. This paper planes the vehicle path under the Dubins path thought, completes the simulation experiment. And compared with other algorithms, this algorithm is simple and feasible. Under the theory of the algorithm, we have designed the vehicle path of two kinds of different complex road conditions, meeting the driving requirements of the complex road conditions. Comparison results show that the Dubins path planning algorithm can calculate the shortest path, and have good selectivity of the optimal path. Vehicle with this algorithm has higher execution and lower error rate.
References
[1] WU You-qian, PEI Hai-long.Trajectory planning for unmanned helicopter based on Dubins curves[J]. Computer Engineering and Design,2011,04:1423-1429+1448.
[2] ZHU Da-qi, YAN Ming-zhong.Survey on technology of mobile robot path planning[J]. Control and Decision, 2010,07:961-967.
[3] GAO Qiang，SONG Yu，LV Dong－h(huán)ao，CHEN Sha－sha. Simulation On Path Planning for Multiple Mobile Ｒobot[J]. Computer Simulation, 2014,07:325-329.
[4] Giordano, Paolo Robuffo; Vendittelli, Marilena.Shortest Paths to Obstacles for a Polygonal Dubins Car[J]. IEEE TRANSACTIONS ON ROBOTICS,2009,10:1184-1191.
[5] Ketan Savla, , Emilio Frazzoli, Francesco Bullo. Traveling Salesperson Problems for the Dubins Vehicle[J]. IEEE TRANSACTIONS ON AUTOMATIC CONTROL,2008,08:1378-1391.
[6] Zhang Minghuan，Zhang Ke. A Better Obstacle Detection Method Based on Tentacle Algorithm of Obstacle Avoidance for Intelligent Vehicle[J]. Journal of Northwestern Polytechnical University, 2012,05:763-767.
[7] WANG Kai, SONG Xingxiu, ZHANG Yiwen. Path planning for avoiding obstacles of autonomous vehicle using improved-artificial potential field[J]. Journal of Liaoning Technical University（Natural Science）, 2014,09:1236-1239.
[8] Jiaojie Li, Wei Zhang, Housheng Su. Coordinated obstacle avoidance with reduced interaction[J]. Neurocomputing,2014,09:233-245.
[9] Ross P. Anderson, Dejan Milutinovi′c. A Stochastic Approach to Dubins Vehicle Tracking Problems[J]. IEEE TRANSACTIONS ON AUTOMATIC CONTROL,2014,10:2081-2086.
[10] Antonios Tsourdos,Brian White,Madhavan Shanmugavel. Cooperative Path Planning of Unmanned Aerial Vehicles [M].BeiJing: National Defend Industry Press,2013.