購(gòu)買設(shè)計(jì)請(qǐng)充值后下載，，資源目錄下的文件所見即所得，都可以點(diǎn)開預(yù)覽，，資料完整，充值下載可得到資源目錄里的所有文件。。。【注】：dwg后綴為CAD圖紙，doc,docx為WORD文檔，原稿無(wú)水印，可編輯。。。具體請(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)域，例如：軍事無(wú)人機(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)行無(wú)速度監(jiān)測(cè)并繞過障礙物，但其將所有障礙物作為靜態(tài)處理，不具有靈活性，可能存在兩車同時(shí)避讓卻無(wú)處可避的局面。
本文主要根據(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)來(lái)表示車輛行駛狀態(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)來(lái)使車輛繞開障礙物。
當(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）
車輛從所求得的可行性的行駛路徑中，比較選擇最短路徑來(lái)作為車輛的行駛路徑，即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ù)來(lái)確定使用函數(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í)，表示前方無(wú)障礙物。而分別表示車輛左、中、右三個(gè)方向的監(jiān)測(cè)值。則車輛行駛決策表如下：
表1 車輛行駛決策表
有無(wú)障礙物
決策結(jié)果
0
0
0
無(wú)
直行
0
0
1
右側(cè)有障礙物
向左轉(zhuǎn)度
0
1
0
中間有障礙物
向右轉(zhuǎn)度
0
1
1
左側(cè)無(wú)障礙物
向左轉(zhuǎn)度
1
0
0
左側(cè)有障礙物
向右轉(zhuǎn)度
1
0
1
中間無(wú)障礙物車輛可通過
直行
1
0
1
中間無(wú)障礙物車輛不可通過
停止或右轉(zhuǎn)度
1
1
0
右側(cè)無(wú)障礙物
向右轉(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)被越來(lái)越多的人們研究。本文在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曲線的無(wú)人直升機(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][英]楚拉多斯,[英]懷特,[英]尚穆加韋爾.無(wú)人機(jī)協(xié)同路徑規(guī)劃[M].北京：國(guó)防工業(yè)出版社，2013.