基于方向引導(dǎo)的智能車VFH+路徑規(guī)劃算法研究
2023-12-31 00:00:00朱茂飛賀晨辰張春鵬吳瓊朱守力
計算機(jī)應(yīng)用研究 2023年7期
摘 要：針對矢量場直方圖（VFH+）算法在路徑規(guī)劃過程中容易陷入環(huán)境死區(qū)，生成的路徑不能滿足車輛運動學(xué)限制的問題，提出方向引導(dǎo)的VFH+路徑規(guī)劃算法。首先在雙向快速隨機(jī)樹（Bi-RRT）節(jié)點擴(kuò)展中引入車輛的運動學(xué)約束，在去除路徑冗余節(jié)點的基礎(chǔ)上，使用三次B樣條曲線得到平滑引導(dǎo)路徑。其次，在VFH+算法中引入車輛的最大轉(zhuǎn)角約束與引導(dǎo)路徑的離散點方向，來限制VFH+的候選方向范圍，并修改代價函數(shù)獲取合適的前進(jìn)方向。最后，在MATLAB軟件上進(jìn)行算法的仿真對比以及基于ROS平臺的實驗驗證。結(jié)果表明，改進(jìn)后的VFH+算法能夠在滿足車輛運動學(xué)約束的情況下，生成一條避開環(huán)境死區(qū)的有效路徑。
關(guān)鍵詞：路徑規(guī)劃； 雙向快速擴(kuò)展隨機(jī)樹； 矢量場直方圖； 轉(zhuǎn)角約束； 方向引導(dǎo)； 智能車輛
中圖分類號：TP242.6
文獻(xiàn)標(biāo)志碼：A
文章編號：1001-3695（2023）07-025-2090-06
doi：10.19734/j.issn.1001-3695.2022.10.0552
Vehicle path planning research based on VFH+ algorithm under direction guidance
Zhu Maofei1，2， He Chenchen1， Zhang Chunpeng1， Wu Qiong3， Zhu Shouli4
（1.College of Advanced Manufacturing Engineering， Hefei University， Hefei 230601， China; 2.Anhui Intelligent Vehicle Control amp; Integrated Design Technology Engineering Research Center， Hefei 230601， China; 3.Anhui Jianghuai Automotive Group， Hefei 230601， China; 4.Hefei ChangAn Automobile Co.， Ltd.， Hefei 230001， China）
Abstract：Aiming at the problem that the vector field histogram （VFH+） algorithm is easy to fall into the environment dead zone in the path planning process， and the generated path cannot satisfy the vehicle kinematics constraints， this paper proposed a direction guided VFH+path planning algorithm. Firstly， it introduced the kinematic constraints of vehicles into the node expansion of Bi-RRT. On the basis of removing redundant nodes on the path， it obtained a smooth guidance path by using cubic B-spline curves. Secondly， in the VFH+algorithm， it introduced the maximum angle constraint of the vehicle and the discrete point direction of the guidance path to limit the range of candidate directions of VFH+， and modified the cost function to obtain the appropriate forward direction. Finally， it performed the simulation comparison and experimental verification on MATLAB software and the robot operating system（ROS） platform. The results show that the improved VFH+algorithm can gene-rate an effective path to avoid the environment dead zone under the condition of meeting the vehicle kinematics constraints.
Key words：path planning; bi-directional fast extended random tree; vector field histogram; corner constraint; direction guidance; intelligent vehicle
0 引言
近年來，路徑規(guī)劃是智能車領(lǐng)域技術(shù)研究的熱點問題。路徑規(guī)劃方法主要分為全局路徑規(guī)劃與局部路徑規(guī)劃。
對于全局路徑規(guī)劃而言，以A*為代表的基于圖搜索算法［1］能夠保證在柵格環(huán)境內(nèi)找到全局最優(yōu)解，但搜索得到的路徑不夠平滑、高分辨率環(huán)境下搜索時間長以及無法做到實時應(yīng)對環(huán)境變化［2～4］。以RRT（rapidly-exploring random tree）算法［5］為代表的基于采樣搜索的算法具有規(guī)劃效率高、對環(huán)境建模要求低的優(yōu)點，被廣泛用于移動機(jī)器人和智能車領(lǐng)域，但RRT算法生成的路徑存在無法保證全局最優(yōu)、隨機(jī)采樣的特點導(dǎo)致算法搜索的目標(biāo)性不強(qiáng)以及采樣節(jié)點過多等問題［6］。為此，文獻(xiàn)[7]在RRT*算法的擴(kuò)展節(jié)點中引用非完整約束車輛模型，使用B樣條曲線對不斷迭代得到的粗路徑進(jìn)行平滑，確保規(guī)劃的軌跡能滿足車輛約束。Jiang等人[8]在RRT*的基礎(chǔ)上進(jìn)行改進(jìn)，對初始路徑進(jìn)行橢圓區(qū)域的限制，使用具有線性插值獲取障礙物頂點，減小橢圓面積來提高算法的收斂速度，提高算法效率。文獻(xiàn)［9］通過在RRT搜索過程中引入同心圓采樣策略以及改進(jìn)鄰近點選擇方法來加快算法搜索效率，得到滿足車輛運動學(xué)約束的有效路徑。文獻(xiàn)［10］提出將A*算法中的啟發(fā)式功能與RRT相結(jié)合，用來提供隨機(jī)樹節(jié)點擴(kuò)展方向上的引導(dǎo)以及減少擴(kuò)展的節(jié)點數(shù)量和搜索時間。文獻(xiàn)[11]提出改進(jìn)的雙向RRT*算法，利用人工勢場法（APF）［12］的作用力來確定搜索方向，采用分離軸定理進(jìn)行碰撞檢測，加快采樣搜索的速度。
對于局部路徑規(guī)劃則要考慮到車輛自身的運動學(xué)與環(huán)境的碰撞因素，對規(guī)劃過程的實時性要求較高，得到的路徑能夠滿足局部最優(yōu)，但也存在規(guī)劃時間慢、對環(huán)境整體的信息不敏感的問題。在眾多局部路徑規(guī)劃方法中，矢量直方圖算法（VFH）［13］將環(huán)境信息轉(zhuǎn)換為可測量的閾值，通過統(tǒng)計直方圖的表示方式，適合多傳感器數(shù)據(jù)的融合，具有響應(yīng)快、路徑平滑等優(yōu)點，但該算法存在運動方向抖動以及陷入環(huán)境死區(qū)的問題。針對這些問題，Ulrich等人［14］在對VFH算法作出改進(jìn)的基礎(chǔ)上提出VFH+算法，充分考慮了不同機(jī)器人的外形尺寸，并通過設(shè)計閾值遲滯以及候選方向的代價函數(shù)來獲取下一步前進(jìn)方向以提升軌跡的平滑度。Babinec等人提出了VFH*TDT方法［15］，將VFH+與VFH*算法［16］相結(jié)合，在保障路徑平滑的條件下，處理復(fù)雜的環(huán)境和動態(tài)障礙物。文獻(xiàn)[17]提出VFH+D算法，解決算法在封閉區(qū)域內(nèi)的方向振蕩，并通過閾值的衰減策略來處理動態(tài)障礙物環(huán)境下的算法避障問題。在針對車輛的路徑規(guī)劃上，屈盼讓等人［18］提出融合車輛運動學(xué)約束的VFH算法，充分考慮了車輛的運動學(xué)限制，通過設(shè)計車輛的活動區(qū)域，保證規(guī)劃的路徑適合車輛行駛，并采用B樣條曲線的平滑方式，使得路徑更平滑。Yan等人［19］提出VFH#算法，消除狹窄區(qū)域下VFH算法對閾值太敏感導(dǎo)致汽車無法通過的問題。文獻(xiàn)［20］結(jié)合VFH與貝塞爾曲線方式，在結(jié)構(gòu)化道路上獲取滿足車輛運動學(xué)約束的路徑。
矢量直方圖算法通過依靠活動窗口檢測到的車輛周邊障礙物與設(shè)置的閾值大小來選取車輛當(dāng)前位置的運動方向，但無法對活動窗口范圍外障礙物有較好的預(yù)知，容易陷入局部死區(qū)。本文提出建立車輛轉(zhuǎn)角約束模型，獲得車輛的前輪轉(zhuǎn)角范圍，通過在雙向RRT算法［21］中引入擴(kuò)展節(jié)點的角度限制，生成一條連接車輛當(dāng)前位置與目標(biāo)位置的全局路徑作為VFH+算法的方向引導(dǎo)路徑。在VFH+算法中根據(jù)車輛的轉(zhuǎn)角限制來篩選候選方向的取值，修改VFH+算法的代價函數(shù)并提高引導(dǎo)路徑方向的權(quán)重，保證生成的路徑避開死區(qū)位置的同時，能夠避開環(huán)境中的未知障礙物。
1 車輛模型
在考慮車輛的路徑規(guī)劃時，生成的路徑首先要滿足車輛的運動學(xué)約束。通過將車輛的兩個前輪看做擁有相同轉(zhuǎn)角的單個車輪，把整個車輛運動學(xué)模型等效為二輪自行車模型，建立如圖1所示的車輛轉(zhuǎn)向模型。
圖中的L為車輛的軸距;R為車輛最小轉(zhuǎn)彎半徑;O點為轉(zhuǎn)彎圓的圓心;σ為允許的前輪最大轉(zhuǎn)角。以此得到車輛軸距、轉(zhuǎn)彎半徑與最大前輪轉(zhuǎn)角以及曲率κ的關(guān)系式為
在VFH+算法中，生成的相鄰路徑點之間的距離與設(shè)置的步長大小有關(guān)。所以，在智能車的應(yīng)用上還需要考慮在最小轉(zhuǎn)彎半徑下，當(dāng)前位置與下一個路徑點的車輛姿態(tài)。由圖2可知，車輛以當(dāng)前姿態(tài)，從位置O1運動至下一個路徑點O2時，前輪轉(zhuǎn)角θ、轉(zhuǎn)彎半徑R以及步長B之間的關(guān)系為
結(jié)合式（1）和（2），可得當(dāng)前車輛姿態(tài)與下一個路徑點之間轉(zhuǎn)角θ需滿足θ≤σ，以此來保證每個路徑點都是可達(dá)的。
2 算法設(shè)計
為了避免在使用VFH+算法規(guī)劃局部路徑時陷入環(huán)境內(nèi)的障礙物死區(qū)，提高算法在不同環(huán)境下的適應(yīng)性，提出采用雙向RRT方向引導(dǎo)的VFH+路徑規(guī)劃算法，具體的算法流程如圖3所示。首先，通過優(yōu)化改進(jìn)的雙向RRT方法得到參考路徑，來獲取合理的參考方向。其次，使用VFH+算法計算當(dāng)前位置的候選方向集合，并篩選出滿足車輛轉(zhuǎn)角約束的前進(jìn)范圍。最后，根據(jù)代價函數(shù)來選取代價值最小的方向作為車輛前進(jìn)姿態(tài)，若候選方向不滿足條件，則將參考方向作為候選方向，在保證避開死區(qū)的同時，滿足車輛運動學(xué)約束要求。
2.1 改進(jìn)的雙向RRT算法
針對傳統(tǒng)的RRT算法生成的路徑存在折點較多、路徑不平滑以及路徑隨機(jī)性大等問題，提出改進(jìn)的雙向RRT算法。通過引入新的節(jié)點擴(kuò)展策略來保證生成的路徑滿足車輛的運動學(xué)限制，并為樹節(jié)點的擴(kuò)展過程提供一定的方向引導(dǎo)。圖4顯示了樹節(jié)點擴(kuò)展的過程，具體的算法步驟見算法1。
算法1 改進(jìn)的雙向RRT算法
輸入：起始點的坐標(biāo)值（xstart，ystart）、目標(biāo)點的坐標(biāo)值（xgoal，ygoal）。
輸出：路徑點集合PathFound。
a）定義兩個空集Tree1和Tree2分別用來存放隨機(jī)樹的擴(kuò)展節(jié)點坐標(biāo)，并把起始點和目標(biāo)點分別放置在內(nèi)；定義賦值運算符←，表示給左側(cè)變量賦值。
PathFound，Tree1，Tree2←?
Tree1←（xstart，ystart）
Tree2←（xgoal，ygoal）
b）如式（3）所示，定義隨機(jī)點計算函數(shù)SampleNode用于生成隨機(jī)點坐標(biāo)qrand。
其中：xrand和yrand分別表示生成的隨機(jī)點qrand的坐標(biāo)值；p表示在（0，1）內(nèi)生成隨機(jī)數(shù)；地圖map的邊界大小mapx、mapy。
c）設(shè)置隨機(jī)樹擴(kuò)展的步長Step，在獲取隨機(jī)點坐標(biāo)之后，計算隨機(jī)點和起始點及目標(biāo)點的距離，從樹節(jié)點Tree1集合中選出距離最小的點（xmin1，ymin1）作為擴(kuò)展的起始點，定義函數(shù)GetNode用于計算鄰近點qnear1的坐標(biāo)，如式（4）所示，并以qnear1為Tree2擴(kuò)展節(jié)點的方向，計算得到qnear2。同理，在qnear1和qnear2的基礎(chǔ)上獲取新節(jié)點qnew1和qnew2，（xmin2，ymin2）則表示Tree2中距離qnew1最近的節(jié)點。
qnear1←GetNode（Step，alpha1，xmin1，ymin1）
qnear2←GetNode（Step，alpha2，xmin2，ymin2）
Tree1←qnear1，Tree2←qnear2
其中：xnode和ynode表示生成的節(jié)點坐標(biāo);alpha1表示Tree1中（xrand，yrand）和（xmin，ymin）之間的夾角，alpha2表示生成的新節(jié)點qnew1和Tree2中（xmin2，ymin2）之間的夾角。
d）引用文獻(xiàn)［9］中鄰近點的選取策略，定義函數(shù)AngleCheck用于判斷擴(kuò)展的樹節(jié)點qstart、qnear和qnew是否滿足車輛的轉(zhuǎn)角約束，即圖4中的β角和γ角小于前輪最大轉(zhuǎn)角σ；添加函數(shù)CollisionCheck用于碰撞檢測。
Collisionflag←CollisionCheck（map，qnode）
Angleflag←AngleCheck（qnode，qnear，qnew）
//發(fā)生碰撞或者不滿足轉(zhuǎn)角約束則重新生成新節(jié)點
if ！（Collisionflag）‖！（Angleflag）
continue
其中：Collisionflag表示不發(fā)生碰撞；Angleflag表示滿足轉(zhuǎn)角約束；qnode此處表示擴(kuò)展得到新節(jié)點qnear1的父節(jié)點。
e）當(dāng)兩棵樹的擴(kuò)展得到的新節(jié)點距離小于一定閾值時，搜索結(jié)束，并將節(jié)點依次保存在集合PathFound中。
PathFound←{Tree1，Tree2}
改進(jìn)后的雙向RRT產(chǎn)生的路徑受限于擴(kuò)展步長的限制，整體路徑的節(jié)點過多，需要對生成的搜索路徑進(jìn)行冗余點的清除。圖5所示為去冗余的過程示意圖，圖中的紅色粗折線為最終保留的路徑。
去冗余的過程從路徑的終點qgoal開始，依次連接路徑上的節(jié)點，若連線與障礙物發(fā)生碰撞，則判斷碰撞點之前的點是否滿足轉(zhuǎn)角限制，在保證不與障礙物發(fā)生碰撞的前提下，圖中的qgoal、q1和q2點形成的夾角η以及q1、q2和qstart組成的夾角ζ必須小于車輛的限制轉(zhuǎn)角σ。在找到q1和q2點之后，以q1點為起點，重復(fù)以上過程，直到刪除所有的冗余節(jié)點。在此基礎(chǔ)上，采用如下B樣條曲線來平滑生成的初始路徑：
其中：Pi表示離散的路徑點坐標(biāo)集合;Bi，j表示路徑點生成的樣條基函數(shù)。多障礙物環(huán)境下，圖6（a）和（b）分別顯示了雙向RRT［21］和改進(jìn)后的雙向RRT算法效果。可見，雙向RRT算法路徑轉(zhuǎn)折點更多，改進(jìn)后雙向RRT算法規(guī)劃的路徑整體更平滑，從而更好地作為VFH+算法候選方向的參考。
2.2 雙向RRT引導(dǎo)的VFH+算法
文獻(xiàn)［13］所提出的傳統(tǒng)VFH+算法通過在圓形活動窗口范圍內(nèi)，將障礙物的距離信息轉(zhuǎn)換為閾值來表示當(dāng)前行駛環(huán)境內(nèi)障礙物的位置，并根據(jù)閾值范圍的大小來獲得代價值最小的前進(jìn)方向。當(dāng)活動窗口范圍較小時，對障礙物感知的敏感度降低，設(shè)置閾值的范圍往往是固定值，無法跟隨環(huán)境不斷調(diào)整，閾值的作用也就會減小，規(guī)劃的路徑可能陷入環(huán)境死區(qū)。為了避免上述情況，通過引入上述改進(jìn)的雙向RRT算法生成的引導(dǎo)路徑，作為車輛在活動窗口未檢測到障礙物時的參考行駛方向；在檢測到障礙物時，則會根據(jù)引導(dǎo)路徑的方向和障礙物位置情況，獲取代價值最小的前進(jìn)方向。具體的步驟如算法2所示。
算法2 修改的VFH+算法
輸入：引導(dǎo)路徑各離散點之間的角度值head集合；車輛位置坐標(biāo)（xveh，yveh）；地圖信息map。
輸出：規(guī)劃的路徑點集合Path。
a）從離散點角度集合中篩選出距離車輛位置坐標(biāo)最近的點，得到角度值head［i］并定義為臨時變量refdir作為車輛的參考方向。
// 選出距離最小的點，得到索引值i
i←Min{dist［i］}
refdir←head［i］
其中：Min{}表示選取最小值；dist表示引導(dǎo)路徑各點與車輛當(dāng)前位置的距離集合；（xi+1，yi+1）和（xi，yi）分別表示引導(dǎo)路徑上篩選出的第i+1個和第i個點坐標(biāo)。
b）對障礙物柵格進(jìn)行膨脹由式（6）得到障礙物占據(jù)的扇形區(qū)域φ，計算表示當(dāng)前位置內(nèi)活動窗口半徑內(nèi)車輛后軸中心與障礙物柵格的角度α，用于后續(xù)對可行區(qū)域的選擇。
φ=arcsinrsafed（7）
α←2×φ
其中：rsafe表示障礙物柵格向外膨脹的距離；d表示車輛位置距離障礙物柵格中心的距離。
c）獲取障礙物占據(jù)的范圍后，需要計算障礙物閾值。傳統(tǒng)的VFH+算法要求獲取地圖上障礙物的先驗條件，即障礙物柵格在活動窗口內(nèi)的所有占據(jù)狀態(tài)。而實際情況下，傳感器只能獲取到障礙物其中的一面，因此采用文獻(xiàn)［19］的閾值計算公式：
其中：mi表示第i個障礙物柵格的閾值;C表示扇區(qū)內(nèi)障礙物柵格的置信度;a為距離系數(shù);di表示車輛當(dāng)前位置與第i個障礙物柵格的距離;r表示活動窗口半徑;Hk表示當(dāng)前位置障礙物閾值mi的最大值。當(dāng)閾值Hk越小，表示車輛在該方向的扇區(qū)范圍內(nèi)與障礙物的距離d越大，反之距離d越小。
d）定義候選方向集合direction，并在候選方向的選取上，首先遍歷活動窗口內(nèi)除了α占據(jù)之外的所有單個可行扇區(qū)范圍，接著獲取可行區(qū)域的上界hdir和下界ldir，由式（8）計算得到各扇區(qū)內(nèi)的唯一候選方向dir，最后篩選出滿足車輛轉(zhuǎn)角約束的候選方向。
//若唯一候選方向也在可行區(qū)域內(nèi)，則加入候選方向集合中
if" （dir∈{－σ，+σ}）
direction←dir
//如果參考方向也在可行區(qū)域內(nèi)，則加入候選方向集合中
if （refdir∈{hdir，ldir}）
direction←refdir
其中：+σ和－σ分別表示車輛右側(cè)σ角和左側(cè)σ角度的范圍。
e）獲取候選方向集合后，使用式（9）所示代價函數(shù)計算最小代價值。選出代價值最小的候選方向Sdir作為下一步前進(jìn)的方向。
其中：ci表示方向集合中的第i個候選方向；cj表示需要對比的關(guān)聯(lián)方向；t則表示活動范圍劃分的扇區(qū)個數(shù)；k表示候選方向集合中的元素;Δ（ci，cj）表示關(guān)聯(lián)方向與候選方向的差值在限制的候選方向范圍內(nèi)所占據(jù)的比值；g（c）為候選方向c的代價值；kf表示車輛當(dāng)前的朝向；μ1、μ2和μ3分別表示候選方向與參考線方向refdir、候選方向與當(dāng)前朝向kf以及參考方向refdir與當(dāng)前朝向kf的權(quán)重系數(shù)。通過在代價函數(shù)中設(shè)置權(quán)重系數(shù)μ1gt;μ2+μ3，來保證車輛盡量靠近參考線方向。
f）由式（10）更新當(dāng)前位置的車輛后軸中心點，得到下一個車輛后軸的中心點坐標(biāo)，并重復(fù)以上過程，直到到達(dá)目標(biāo)點位置附近，獲取路徑點坐標(biāo)。
3 仿真與實驗
3.1 仿真分析
在搭載Windows平臺的PC機(jī)上，通過MATLAB R2021進(jìn)行仿真。通過提取二維地圖上障礙物的各邊界點坐標(biāo)，建立大小為500 m×500 m的地圖場景。仿真對比了傳統(tǒng)的VFH+算法［13］和本文雙向RRT引導(dǎo)下的VFH+算法的路徑規(guī)劃結(jié)果，仿真所用的參數(shù)如表1所示。
3.1.1 簡單環(huán)境下的仿真分析
在仿真中，選用半徑為r=30 m、r=80 m以及半徑為r=100 m的活動窗口。圖7（a）為在傳統(tǒng)VFH+算法在不同窗口半徑下的規(guī)劃情況，從圖中可以看出，選用r=30 m時的小半徑活動窗口時，算法會朝著目標(biāo)點方向擴(kuò)展，導(dǎo)致最終在活動范圍內(nèi)檢測不到可通行區(qū)域，并陷入局部死區(qū)。當(dāng)選用r=80 m的大范圍活動窗口時，算法能順利繞開障礙物死區(qū)，但路徑存在大量的拐點，且曲率較大，車輛無法跟蹤行駛。當(dāng)選用r=100 m的窗口半徑時，生成的路徑偏離目標(biāo)點，且在目標(biāo)點之后會產(chǎn)生局部振蕩。圖7（b）為方向引導(dǎo)下VFH+算法在三種窗口下的路徑規(guī)劃結(jié)果。相比于圖7（a）中未改進(jìn)前的算法而言，改進(jìn)后生成的路徑都更平滑，且都能到達(dá)目標(biāo)點。
表2為改進(jìn)后三種窗口下生成的路徑信息。從表中可以看出，當(dāng)選用r=80 m和r=100 m的窗口時，路徑的曲率絕對值和路徑長度都比r=30 m時更大。因此，當(dāng)選用r=30 m的活動窗口半徑時，規(guī)劃的路徑質(zhì)量更好。
3.1.2 未知障礙物環(huán)境下仿真分析
使用雙向RRT算法生成如圖8（a）所示的方向引導(dǎo)路徑，從中提取黑色靜態(tài)障礙物邊緣點坐標(biāo)，并添加部分黑的矩形方塊作為出現(xiàn)的未知障礙物，從而得到圖8（b）所示的最終仿真場景，以此來驗證改進(jìn)后的VFH+算法對未知環(huán)境的適應(yīng)性。
圖9（a）（b）分別表示傳統(tǒng)的VFH+算法和改進(jìn)后的VFH+算法在面對未知障礙物環(huán)境的路徑規(guī)劃結(jié)果。
從圖9（a）可見，傳統(tǒng)的VFH+算法在規(guī)劃路徑的過程中，由于目標(biāo)點方向的代價系數(shù)更大，算法在規(guī)劃的過程中更傾向于目標(biāo)點方向，故最終陷入了局部死區(qū)，且路徑拐點處的角度較大，不滿足車輛的避障與跟蹤要求。從圖9（b）改進(jìn)后的VFH+算法規(guī)劃結(jié)果可以看出，生成的路徑能夠順利到達(dá)目標(biāo)點附近，且生成的路徑更平滑，說明所使用的改進(jìn)方法能夠較好地應(yīng)對環(huán)境的變化，并滿足車輛的運動學(xué)約束。
3.1.3 算法規(guī)劃效率對比
由于快速隨機(jī)樹算法自身的隨機(jī)性，為了更客觀地評價全局引導(dǎo)路徑規(guī)劃的效果，分別對RRT［5］、雙向RRT［21］以及改進(jìn)的雙向RRT算法進(jìn)行10次仿真實驗。擴(kuò)展步長設(shè)置為10 m，統(tǒng)計生成全局路徑的信息，得到表3所示的統(tǒng)計結(jié)果。從表中數(shù)據(jù)可以看出，改進(jìn)后的雙向RRT相較于其他三種算法所花費的時間最少。在路徑長度上，改進(jìn)的雙向RRT算法在去除冗余節(jié)點后，路徑整體長度比其余三種算法更短。在路徑平滑度上，改進(jìn)的雙向RRT通過限制節(jié)點角度，生成的路徑曲率在車輛轉(zhuǎn)角范圍內(nèi)，路徑整體更平滑。
為避免因全局路徑規(guī)劃所花費的時間對算法效率產(chǎn)生影響，分別對傳統(tǒng)VFH+算法和基于引導(dǎo)路徑下的VFH+算法進(jìn)行10次規(guī)劃時間統(tǒng)計，得到圖10所示的時間統(tǒng)計結(jié)果。從圖中可以看出，改進(jìn)后的VFH+算法規(guī)劃所花費的時間更少，相比于傳統(tǒng)的VFH+算法，其平均時間降低了46.15%，主要是改進(jìn)后的VFH+算法在全局路徑的方向引導(dǎo)下，減少了對當(dāng)前窗口內(nèi)部分不合理方向的代價值計算，從而提高算法效率。
3.2 實驗驗證
設(shè)置未知障礙物和存在死區(qū)兩種實驗環(huán)境，用來驗證雙向RRT引導(dǎo)的VFH+算法對環(huán)境的適應(yīng)性以及死區(qū)避障能力。實驗采用搭載Rplidar A1單線激光雷達(dá)的智能小車。智能小車的車身長為445 mm，軸距為240 mm，車寬為331 mm，在限制最大前輪轉(zhuǎn)角為35°的情況下，由式（1）得到本次實驗的曲率限制為k2=±2.917，并在算法程序中設(shè)置窗口范圍為1 m。小車硬件配置包括運動控制器、IMU位姿傳感器，具體的安裝情況如圖11所示。實驗基于ROS（開源機(jī)器人操作系統(tǒng)）平臺，通過Rviz進(jìn)行可視化，得到的分辨率為5 cm的二維柵格地圖。
1）未知障礙物環(huán)境下實驗分析
針對未知的障礙物環(huán)境，搭建如圖12所示的實驗場景，地面的紙箱代表環(huán)境內(nèi)的未知障礙物。首先在沒有障礙物的情況下，建立如圖13（a）所示的二維柵格地圖，并使用改進(jìn)的雙向RRT算法生成圖中所示的黑色引導(dǎo)路徑。接著在環(huán)境中增加如圖12所示的障礙物，得到傳統(tǒng)的VFH+和雙向RRT引導(dǎo)的VFH+路徑規(guī)劃結(jié)果如圖13（b）和（c）所示。可見，傳統(tǒng)的VFH+算法雖然能夠規(guī)劃出一條到達(dá)目標(biāo)點的路徑，但路徑拐點較多，而后者能夠生成一條平滑的路徑，在滿足車輛的轉(zhuǎn)角限制下繞開障礙物到達(dá)目標(biāo)點。
兩種算法生成路徑的曲率和長度的對比結(jié)果如圖14所示，路徑的具體參數(shù)如表4所示。在路徑曲率上，傳統(tǒng)的VFH+算法的曲率極值超出了限制的最大曲率值k2，而雙向RRT引導(dǎo)的VFH+算法生成路徑的曲率極值較小。在路徑長度上，傳統(tǒng)的VFH+算法受目標(biāo)點方向的影響，在障礙物附近計算得到的路徑點不合理，存在多次方向調(diào)整，故路徑長度更長。
2）環(huán)境死區(qū)實驗對比
使用紙箱代表圍成的環(huán)境死區(qū)，保證小車周邊1 m范圍內(nèi)，無法檢測到可行區(qū)域來避開障礙物，以此來驗證算法對環(huán)境死區(qū)的適應(yīng)能力，如圖15所示。
圖16為傳統(tǒng)VFH+算法規(guī)劃的路徑情況，可以看出，由于目標(biāo)點方向的影響，算法規(guī)劃的路徑陷入了環(huán)境死區(qū)。圖17表示在雙向RRT引導(dǎo)的VFH+算法規(guī)劃結(jié)果，統(tǒng)計生成的路徑節(jié)點并計算得到路徑的曲率最大值為0.625 3 mm－1，曲率最小值為-2.313 6 mm－1，路徑長度為6 608.83 mm。可見，雙向RRT引導(dǎo)下的VFH+算法能規(guī)劃出一條滿足車輛轉(zhuǎn)角約束的路徑來繞開環(huán)境死區(qū)。
4 結(jié)束語
針對智能車在低速場景下的路徑規(guī)劃問題，提出基于方向引導(dǎo)的VFH+路徑規(guī)劃算法。a）在避障過程中，用車輛當(dāng)前環(huán)境的全局路徑引導(dǎo)方向來取代目標(biāo)點方向，避免車輛陷入環(huán)境死區(qū);b）對VFH+算法候選方向的選擇上增加了小車的轉(zhuǎn)角約束，限制了候選方向的范圍，保證路徑能夠滿足車輛運動學(xué)約束;c）通過兩種算法結(jié)合的方式，解決因全局路徑對環(huán)境變化的不敏感而導(dǎo)致跟蹤過程的碰撞風(fēng)險。仿真和實驗結(jié)果表明，該方法在低速場景下的智能車路徑規(guī)劃中，能夠有效地避免陷入環(huán)境死區(qū)，保證規(guī)劃的路徑能夠到達(dá)目標(biāo)位置且滿足車輛的轉(zhuǎn)角約束條件。但在面對移動障礙物時，改進(jìn)的VFH+算法無法準(zhǔn)確判斷出障礙物的具體位置，會將動態(tài)障礙物經(jīng)過的位置識別為一段障礙物占據(jù)柵格，使原本可行區(qū)域突變?yōu)榄h(huán)境死區(qū)。因此，考慮動態(tài)障礙物的避障是需要進(jìn)一步研究的問題。
參考文獻(xiàn)：
[1]馮凱，吉星，楊昕.A*算法在自動駕駛車輛路徑規(guī)劃中的應(yīng)用［J］.汽車實用技術(shù)，2020，45（22）：25-28.（Feng Kai， Ji Xing， Yang Xin. Application of A* algorithm in path planning of autonomous vehicles［J］.Automobile Applied Technology，2020，45（22）：25-28.）
［2]龐永旭，袁德成.融合改進(jìn)A*與DWA算法的移動機(jī)器人路徑規(guī)劃［J］.計算機(jī)與現(xiàn)代化，2022（1）：103-107.（Pang Yongxu， Yuan Decheng. Mobile robot path planning based on fusion of improved A* and DWA algorithms［J］.Computer and Modernization，2022，38（1）：103-107.）
［3]陳萬通，刁天茹，賈吉慶，等.基于扇形領(lǐng)域擴(kuò)展的同步雙向A*算法［J］.計算機(jī)應(yīng)用研究，2022，39（1）：118-122，127.（Chen Wantong， Diao Tianru， Jia Jiqing， et al. Research on synchronous bi-directional A* algorithm based on sector field expansion［J］.Application Research of Computers，2022，39（1）：118-122，127.）
［4]張輝，張瑞亮，許小慶，等.基于關(guān)鍵節(jié)點的改進(jìn)A*無人車路徑規(guī)劃算法［J］.汽車技術(shù)，2022，53（1）：1-9.（Zhang Hui， Zhang Ruiliang， Xu Xiaoqing， et al. Key nodes-based improved A* algorithm for path planning of unmanned vehicle［J］.Automobile Technology，2022，53（1）：1-9.）
［5]LaValle S M， Kuffner J J. Randomized kinodynamic planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，1999：473-479.
［6]李曉旭，馬興錄，王先鵬.移動機(jī)器人路徑規(guī)劃算法綜述［J］.計算機(jī)測量與控制，2022，30（7）：9-19.（Li Xiaoxu， Ma Xinglu， Wang Xianpeng. A survey of path planning algorithms for mobile robots［J］.Computer Measurement amp; Control，2022，30（7）：9-19.）
［7]Eshtehardian S A， Khodaygan S. A continuous RRT*-based path planning method for non-holonomic mobile robots using B-spline curves［J/OL］.Journal of Ambient Intelligence and Humanized Computing.（2022-01-21）.https：//doi.org/10.1007/s12652-021-03625-8.
［8]Jiang Huanyu， Chen Qiulian， Zheng Yijun， et al. Informed RRT* with adjoining obstacle process for robot path planning［C］//Proc of IEEE International Conference on Communication Technology.Pisca-taway，NJ：IEEE Press，2020：1471-1477.
［9]張衛(wèi)波，肖繼亮.改進(jìn)RRT算法在復(fù)雜環(huán)境下智能車路徑規(guī)劃中的應(yīng)用［J］.中國公路學(xué)報，2021，34（3）：225-234.（Zhang Weibo， Xiao Jiliang. Application of improved RRT algorithm in intelligent vehicle path planning under complicated environment［J］.China Journal of Highway and Transport，2021，34（3）：225-234.）
［10]Al-Ansarry S， Al-Darraji S. Hybrid RRT-A*： an improved path planning method for an autonomous mobile robots［J］.Iraqi Journal for Electrical And Electronic Engineering，2021，17（1）：1-9.
［11]Ge Qingying， Li Aijuan， Li Shaohua， et al. Improved bi-directional RRT path planning method for smart vehicle［J］.Mathematical Problems in Engineering，2021，2021（8）：1-14.
［12]劉冰雁，葉雄兵，王新波，等.基于改進(jìn)人工勢場的無人地面車輛路徑規(guī)避算法［J］.中國慣性技術(shù)學(xué)報，2020，28（6）：769-777.（Liu Bingyan， Ye Xiongbing， Wang Xinbo， et al. Path avoidance algorithm of unmanned ground vehicles based on improved artificial potential field［J］.Journal of Chinese Inertial Technology，2020，28（6）：769-777.）
［13]Borenstein J， Koren Y. The vector field histogram-fast obstacle avoi-dance for mobile robots［J］.IEEE Journal of Robotics and Automation，1991，7（2）：278-288.
［14]Ulrich I， Borenstein J. VFH+： reliable obstacle avoidance for fast mobile robots［C］//Proc of IEEE International Conference of Robotics and Automation.Piscataway，NJ：IEEE Press，1998：1572-1577.
［15]Babinec A， Duchon F， Dekan M. VFH*TDT （VFH* with time dependent tree）： a new laser range finder based obstacle avoidance method designed for environment with non-static obstacles［J］.Robotics and Autonomous Systems，2014，62（8）：1098-1115.
［16]Ulrich I， Borenstein J. VFH*： local obstacle avoidance with look-ahead verification［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2000：2505-2511.
［17]Daniel D， Leonardo M. VFH+D： an improvement on the VFH+algorithm for dynamic obstacle avoidance and local planning［J］.IFAC-PapersOnLine，2020，53（2）：9590-9595.
［18]屈盼讓，薛建儒，朱耀國，等.面向無人車運動規(guī)劃問題的VFH算法［J］.計算機(jī)仿真，2018，35（12）：245-251.（Qu Panrang， Xue Jianru， Zhu Yaoguo， et al. A constrained VFH algorithm for motion planning of autonomous vehicles［J］.Computer Simulation，2018，35（12）：245-251.）
［19]Yan Yingwei， Du Yu， Zhou Wenan. Study on the local path planning for intelligent vehicles based on an improved VFH method［J］.International Journal of Embedded Systems，2018，10（6）：445-456.
［20]Ye Feng， Yang Jing， Ma Chao， et al. Combing VFH with Bezier for motion planning of an autonomous vehicle［C］//Proc of the 2nd Annual International Conference on Information System and Artificial Intelligence.2017.
［21]韓豐鍵，邱書波，馮超，等.基于目標(biāo)導(dǎo)向的雙向RRT路徑規(guī)劃算法［J］.齊魯工業(yè)大學(xué)學(xué)報，2021，35（1）：35-43.（Han Fengjian， Qiu Shubo， Feng Chao， et al. Goal-oriented Bi-RRT path planning algorithm［J］.Journal of Qilu University of Technology，2021，35（1）：35-43.）
