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

洪 順

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

前言

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

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

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

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

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

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

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

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

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

傳統(tǒng)A*算法常采用領(lǐng)域8節(jié)點(diǎn)搜索方式，這種搜索方式有一定的局限性，較難全方位為智能車輛提供可行駛區(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)點(diǎn)搜索，A*算法的代價(jià)估算將有更多的選擇，避免出現(xiàn)局部最優(yōu)的情況，在細(xì)分的搜索范圍中，促使搜索算法可以達(dá)到全局最優(yōu)，同時(shí)對(duì)避障功能也可遠(yuǎn)離障礙物，提高規(guī)避障礙物的可能性，提供智能車輛可能的可行駛區(qū)域。

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

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

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

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

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

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

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

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

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

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

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

圖3 改進(jìn)算法

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

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

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

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

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

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

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

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

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

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

圖4 仿真結(jié)果