智能汽車路徑規(guī)劃應(yīng)用研究

洪 順

（華南理工大學(xué)機(jī)械與汽車工程學(xué)院，廣東 廣州 510640）

前言

路徑規(guī)劃是智能車輛自主行駛的方向，具有影響智能車輛行駛平順性和安全性，研究智能車輛的路徑規(guī)劃算法尤其重要。路徑規(guī)劃[1]，即為了滿足一定性能通過搜索策略，規(guī)劃出一條從起始點到目標(biāo)終點的最優(yōu)化路徑。目前常用的路徑規(guī)劃算法有A*算法、蟻群算法、RRT算法等[2-3]，蟻群算法雖然求得的路徑相對較短，但其耗時較多，不利于智能車輛的路徑規(guī)劃[4-5]。王海梅等人對Dijkstra算法進(jìn)行優(yōu)化，結(jié)合數(shù)據(jù)結(jié)構(gòu)和路徑搜索策略，具有啟發(fā)函數(shù)性質(zhì)的A*算法，兩種算法的融合證明有較好的效果[6]。RRT算法[7]適用于多自由度路徑規(guī)劃問題，但隨機(jī)分布特點會引起算法計算時間較長、路徑規(guī)劃非最優(yōu)等不足。A*算法[8-9]發(fā)源于Dijkstra算法，具有啟發(fā)性的路徑搜索算法，其算法的核心在于從當(dāng)前目標(biāo)點搜索下一最小代價函數(shù)值，作為下一目標(biāo)起點，循環(huán)往復(fù)以達(dá)到最優(yōu)路徑規(guī)劃的目的。基于A*算法最優(yōu)規(guī)劃，計算效率高且較容易規(guī)劃出最優(yōu)路徑，本文提出采用基于改進(jìn)擴(kuò)展搜索領(lǐng)域的、基于優(yōu)化搜索算法的A*算法，通過Matlab/Simulink建模，搭建仿真模型，模擬室外特定區(qū)域，以驗證該算法的實用性和有效性，達(dá)到智能車輛路徑行駛要求。

1 傳統(tǒng) A＊算法

傳統(tǒng)A*算法是一種通過啟發(fā)函數(shù)來搜索路徑，每次確定當(dāng)前起點，重新計算代價函數(shù)，循環(huán)搜索下一目標(biāo)點的路徑搜索算法。啟發(fā)函數(shù)常用以下公式描述：

上述公式（1）中，f（n）為路徑搜索總的代價函數(shù)估計值，g（n）為實際移動代價值，h（n）為目標(biāo)點到終點的估算成本，其值是估算來的。f（n）是g（n）和h（n）的和，表示智能車輛在搜索算法中總的估算成本。

傳統(tǒng)A*算法流程圖如圖1所示：

圖1 傳統(tǒng)A星算法

傳統(tǒng) A*算法的具體實現(xiàn)方式：初始化 open集和 close集，將智能車輛起點s放入open集，同時將s加入到path集，判斷 open集是否為空，若為空則算法結(jié)束；否則更新open集合，重新計算open集中對應(yīng)的g值、h值、f值，選出代價值f的最小值，將對應(yīng)的節(jié)點n加入到path集，移除open集中的n節(jié)點，同時將n移動到close集中，表示已經(jīng)成功的節(jié)點或者遍歷過的節(jié)點，若節(jié)點n為終點則表示尋路成功，則輸出最優(yōu)路徑退出算法規(guī)劃；否則，更新open集重新計算g值、h值、f值，重復(fù)循環(huán)直至找到最優(yōu)路徑，找到最優(yōu)路徑時則退出算法搜索。

2 改進(jìn) A＊算法

2.1 基于擴(kuò)大搜索領(lǐng)域的優(yōu)化

傳統(tǒng)A*算法常采用領(lǐng)域8節(jié)點搜索方式，這種搜索方式有一定的局限性，較難全方位為智能車輛提供可行駛區(qū)域且存在路徑局部最小等不足，基于該搜索方法，提出一種改進(jìn)的搜索方式，擴(kuò)大可搜索領(lǐng)域的方法，增加智能車輛可行駛區(qū)域的可能性。

搜索領(lǐng)域示意圖如圖2所示，白色圓形表示搜索起始位置，灰色圓形代表待搜索領(lǐng)域，黑色圓形代表非搜索領(lǐng)域。傳統(tǒng)A*算法搜索領(lǐng)域示意如圖（a）所示，在當(dāng)前搜索起始位置，通常是8領(lǐng)域搜索。擴(kuò)展搜索領(lǐng)域示意如圖（b）所示，將當(dāng)前起始位置進(jìn)行搜索領(lǐng)域擴(kuò)展，增加領(lǐng)域搜索范圍，向周圍擴(kuò)展一倍的方式進(jìn)行下一可能的目標(biāo)點搜索，A*算法的代價估算將有更多的選擇，避免出現(xiàn)局部最優(yōu)的情況，在細(xì)分的搜索范圍中，促使搜索算法可以達(dá)到全局最優(yōu)，同時對避障功能也可遠(yuǎn)離障礙物，提高規(guī)避障礙物的可能性，提供智能車輛可能的可行駛區(qū)域。

圖2 優(yōu)化搜索領(lǐng)域示意圖

2.2 基于改進(jìn)算法的優(yōu)化

A*算法是啟發(fā)性搜索算法代表算法，啟發(fā)函數(shù)的選取直接影響了搜索算法的效果和實用型，該算法常用的啟發(fā)函數(shù)有如下幾種：

歐式距離：為兩個坐標(biāo)點之間的直線距離，也是路徑規(guī)劃中常用的距離。其公式表達(dá)式為：

上式中，（x1，y1）表示為當(dāng)前坐標(biāo)，（x2，y2）表示為目標(biāo)點坐標(biāo)。

切比雪夫距離：為兩個坐標(biāo)點之間最大的坐標(biāo)差的絕對值，其公式可描述為：

上式中，（x1，y1）表示為當(dāng)前坐標(biāo)，（x2，y2）表示為目標(biāo)點坐標(biāo)。

曼哈頓距離：在平面或者空間，為兩個坐標(biāo)點之間的差值的絕對值的和，隨著兩個節(jié)點的動態(tài)變化，其值也會跟隨變化，其計算公式可表達(dá)為：

上式中，（x1，y1）表示為當(dāng)前坐標(biāo)，（x2，y2）表示為目標(biāo)點坐標(biāo)。

A*算法是一種啟發(fā)性的搜索算法，通過代價函數(shù)對每個節(jié)點進(jìn)行代價評估，代價函數(shù)的選取尤其重要，本文采用距離最短的歐式距離作為代價評估，真實反應(yīng)各個節(jié)點之間的相對關(guān)系，為智能車輛路徑規(guī)劃提供可能性的路徑航點。

改進(jìn)算法的優(yōu)化示意圖如圖3所示。

圖3 改進(jìn)算法

改進(jìn)算法具體實現(xiàn)方式：

（1）算法開始，需要將起始點加入到open集中，同時初始化close集、path集，將起始點并入path集，open集表示起點或者可能的行駛目標(biāo)點，即可能的待檢測計算的節(jié)點序號，close代表已經(jīng)找的節(jié)點或者障礙物點、不可到達(dá)的節(jié)點，path集用以存放成功的目標(biāo)節(jié)點。

（2）將節(jié)點n作為當(dāng)前目標(biāo)點，將節(jié)點n的open集更新，加入新的可搜索區(qū)域，對可能的目標(biāo)節(jié)點進(jìn)行遍歷重新計算對應(yīng)的g值、h值、f值。

（3）選出最小的f值，其所對應(yīng)的節(jié)點為n，同時刪除open集中的節(jié)點n，將n加入到path中。此時屬于暫代目標(biāo)點，需對目標(biāo)點進(jìn)行優(yōu)化處理。

（4）判斷n是否為終點，若是終點，則代表算法結(jié)束，尋求到最優(yōu)的目標(biāo)路徑。

（5）若n不為終點則，優(yōu)化判斷n的子open集是否存在上一節(jié)點的open數(shù)據(jù)。

（6）若滿足則重復(fù)步驟2；若不滿足則重新計算暫定節(jié)點的路徑f值，若最小則不用重復(fù)計算，否則重復(fù)計算路徑該點的f值，若存在最小的值則優(yōu)化替換，重新更新open集和path集。

（7）整個算法完成，則找到最優(yōu)的智能車輛的路徑。

3 仿真結(jié)果與分析

建立基于Matlab/Simulink模型，對室外特定區(qū)域?qū)嶋H測繪與記錄，根據(jù)實際場地的構(gòu)造畫出室外特定區(qū)域電子地圖。其電子地圖如圖4所示，藍(lán)色方形代表綠化區(qū)域或者堆積區(qū)域，淺綠色區(qū)域代表停車位區(qū)域，圖中藍(lán)點代表路徑航點，航點代表目標(biāo)節(jié)點的位置、航向、轉(zhuǎn)向信息等屬性特點。圖中，紅色星號代表目標(biāo)起始點，藍(lán)色星號代表目標(biāo)終點，規(guī)劃出一條可行駛區(qū)域，即紅色的線條代表規(guī)劃的一次路徑，當(dāng)行駛的過程中遇到障礙物時，該優(yōu)化算法重新規(guī)劃出最新的路徑，實現(xiàn)了避障的功能，且重新規(guī)劃出的最優(yōu)避障路徑達(dá)到了行駛要求同時規(guī)避障礙物較遠(yuǎn)的距離，確保了行駛的安全距離，滿足功能性的要求外也保證了行駛的安全性的要求。在直道線，路徑相對稀疏考慮到直線部分對目標(biāo)航點的要求較低，但是在彎道曲線部分，通過樣條曲線擬合優(yōu)化，路徑航點較為密集，滿足智能車輛對彎道曲線的復(fù)雜要求屬性，同時彎道曲線過度平穩(wěn)和曲線較為平滑。通過實驗驗證了，該算法的有效性能，為后續(xù)的智能車開發(fā)提供一定的技術(shù)積累。

圖4 仿真結(jié)果