楊時(shí)川　胡曉曉　胡漢橋

摘 要：路徑規(guī)劃是自動駕駛關(guān)鍵技術(shù)之一，各種路徑規(guī)劃算法現(xiàn)已不斷改進(jìn)并不斷涌現(xiàn)。本文根據(jù)全局和局部路徑規(guī)劃對常用路徑規(guī)劃算法進(jìn)行分類，對幾種常用的路徑規(guī)劃算法原理進(jìn)行介紹，并分析其發(fā)展方向。

關(guān)鍵詞：無人駕駛 路徑規(guī)劃 算法原理 發(fā)展方向

Analysis of Several Unmanned Vehicle Path Planning Algorithms

Yang Shichuan，Hu Xiaoxiao，Hu Hanqiao

Abstract：Path planning is one of the key technologies of autonomous driving. Various path planning algorithms have been continuously improved and emerging. This article classifies common path planning algorithms based on global and local path planning， introduces the principles of several common path planning algorithms， and analyzes their development direction.

Key words：unmanned driving， path planning， algorithm principle， development direction

1 引言

自動駕駛汽車能夠自動感知周圍環(huán)境、根據(jù)環(huán)境信息自動做出運(yùn)動決策并實(shí)施行駛。自動駕駛系統(tǒng)的關(guān)鍵技術(shù)主要由四個部分組成，分別為：環(huán)境感知、行為決策、路徑規(guī)劃和運(yùn)動控制。環(huán)境感知即為車輛憑借自身的各傳感器相互配合工作來保證車輛正常行駛，常用的傳感器有圖像傳感器，激光雷達(dá)，毫米波雷達(dá)，超聲波雷達(dá)，生物傳感器。行為決策是根據(jù)傳感器獲得的信息對未來一段時(shí)間的行車環(huán)境進(jìn)行預(yù)測，如車道保持、轉(zhuǎn)彎、制動、加速等。運(yùn)動控制是指根據(jù)行為決策和路徑規(guī)劃對車輛本身運(yùn)動狀態(tài)進(jìn)行控制，以達(dá)到期望的路徑和期望的速度。

路徑規(guī)劃是是指生成一條連接車輛行駛起點(diǎn)位置與終點(diǎn)位置，且不與環(huán)境中障礙物發(fā)生碰撞的幾何路徑[1]。實(shí)際上兩個關(guān)鍵點(diǎn)分別是車輛從A點(diǎn)到B點(diǎn)，不與環(huán)境中障礙物發(fā)生碰撞。一般前者稱為全局路徑規(guī)劃，根據(jù)全局靜態(tài)地圖規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行駛的最優(yōu)路徑。后者則體現(xiàn)了局部路徑規(guī)劃含義，智能車輛的局部路徑規(guī)劃是指基于傳感器等設(shè)備感知行駛遇到的空間障礙物，在滿足動力學(xué)、運(yùn)動學(xué)約束以及穩(wěn)定性、舒適性等評價(jià)指標(biāo)的條件下預(yù)先或者實(shí)時(shí)地為智能車輛規(guī)劃出從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑[2]。自動駕駛技術(shù)路線很多是在輪式機(jī)器人的技術(shù)路線基礎(chǔ)上改進(jìn)和發(fā)展。

2 全局路徑規(guī)劃算法

對于全局路徑規(guī)劃和局部路徑規(guī)劃因?yàn)橐紤]的內(nèi)容不一樣，所以在選擇算法時(shí)也會有各自傾向。自動駕駛技術(shù)路線很多是在輪式機(jī)器人的技術(shù)路線基礎(chǔ)上改進(jìn)和發(fā)展，因此很多算法也是在此基礎(chǔ)之上改進(jìn)以適合自動駕駛的情形，全局路徑規(guī)劃算法主要有A*算法、可視圖法、模擬退火法、遺傳算法、粒子群算法、蟻群算法等，現(xiàn)在將全局路徑規(guī)劃典型算法進(jìn)行簡單介紹。

2.1 A*算法

A*算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，依據(jù)代價(jià)函數(shù)來搜索最優(yōu)路徑，既考慮從初始結(jié)點(diǎn)到當(dāng)前結(jié)點(diǎn)的代價(jià)值，又考慮了當(dāng)前結(jié)點(diǎn)到目標(biāo)結(jié)點(diǎn)的啟發(fā)值，是一種啟發(fā)式搜索算法[3]。在路徑搜索過程中，A*算法使用代價(jià)函數(shù)來評估節(jié)點(diǎn)的質(zhì)量，算法將選擇的代價(jià)函數(shù)數(shù)值最小的節(jié)點(diǎn)作為下一步的節(jié)點(diǎn)，然后它將繼續(xù)從下一個節(jié)點(diǎn)搜索，直到達(dá)到目標(biāo)點(diǎn)。

A*算法的代價(jià)函數(shù)如下：

f（n）=g（n）+h（n）

其中函數(shù)f（n）是結(jié)點(diǎn)n的當(dāng)前代價(jià)值，函數(shù)g（n）是從初始結(jié)點(diǎn)到當(dāng)前結(jié)點(diǎn) n的實(shí)際代價(jià)值，h（n）是啟發(fā)函數(shù)，為當(dāng)前結(jié)點(diǎn)n 到目標(biāo)結(jié)點(diǎn)最短路徑的估計(jì)代價(jià)，是一個預(yù)測值。g（n）經(jīng)常都是固定的數(shù)值，然而h（n）卻不是固定的，它的改變會影響到f（n）是否為最短路徑。因此在 A*算法中，啟發(fā)函數(shù)h（n）的設(shè)計(jì)非常重要，合理的啟發(fā)函數(shù)設(shè)計(jì)能幫助A*算法在路徑規(guī)劃中快速簡潔，提高尋路效率。啟發(fā)函數(shù)一般有四種表達(dá)式，但是最常用的是用曼哈頓距離定義。

曼哈頓距離：坐標(biāo)橫縱方向的距離之和，允許向上、下、左、右移動，啟發(fā)函數(shù)表示為：h（n）=D*[abs（xb-xn）+abs（yb-yn）]，（xb，yb）為目標(biāo)節(jié)點(diǎn)坐標(biāo)，（xn，yn）為當(dāng)前結(jié)點(diǎn)坐標(biāo)，其中D表示h（n）的代價(jià)系數(shù)，一般取為10。但是針對不同的環(huán)境，可以選用不同的代價(jià)系數(shù)，例如文獻(xiàn)3就根據(jù)當(dāng)前節(jié)點(diǎn)和目標(biāo)結(jié)點(diǎn)的距離不同，嘗試用不同代價(jià)系數(shù)D，當(dāng)障礙物環(huán)境地圖不變的情況下減少了搜索時(shí)間。文獻(xiàn)4提出了一種融合改進(jìn)A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃方法。設(shè)計(jì)了一種關(guān)鍵點(diǎn)選取策略，能夠去除傳統(tǒng)A*算法規(guī)劃路徑序列中的冗余點(diǎn)，從而提高路徑規(guī)劃性能[4]。

2.2 蟻群算法

蟻群算法是一種仿生算法，它是用來尋找優(yōu)化路徑的機(jī)率型算法。螞蟻從蟻穴出發(fā)，在尋找食物過程中，在路徑上會釋放信息素來提醒同伴這條路徑是否為一條優(yōu)秀路徑。蟻群算法的尋路模型可以簡化如圖1所示，1號螞蟻選擇{e1}路徑取得食物并返回，2號螞蟻選擇{e2+e3}路徑，信息素會隨著時(shí)間的推移而逐漸揮發(fā)，在1號螞蟻返回時(shí)，{e1}路徑上會存在兩倍于{e2+e3}路徑上的信息素，剩下的螞蟻在尋找食物時(shí)，根據(jù)信息素的濃度選擇行走的方向，幾個周期后，大部分螞蟻就會逐漸集中在最短路徑{e1}上。

它最開始被用來解決旅行商問題，即從原點(diǎn)出發(fā)，經(jīng)過若干個給定的需求點(diǎn)，最終返回原點(diǎn)的最短路徑。后來世界各地研究工作者對蟻群算法進(jìn)行了研究和應(yīng)用開發(fā)，該算法現(xiàn)已被大量應(yīng)用于數(shù)據(jù)分析，路徑規(guī)劃，機(jī)器人協(xié)作問題，水利，采礦等領(lǐng)域。在建立模型時(shí)，初始化參數(shù)會對結(jié)果產(chǎn)生較大影響，容易陷入局部最優(yōu)，所以在建立模型時(shí)要選擇合適螞蟻數(shù)量，信息素常量，迭代次數(shù)，信息素?fù)]發(fā)因子等大小。為了加快運(yùn)算速度并得到最優(yōu)解，有不同學(xué)者對蟻群算法進(jìn)行改進(jìn)。在路徑規(guī)劃研究領(lǐng)域中，文獻(xiàn)5對于地圖環(huán)境靜態(tài)不變的問題，在路徑規(guī)劃的初始階段，改進(jìn)算法根據(jù)地圖的結(jié)構(gòu)和目標(biāo)點(diǎn)的位置信息給螞蟻正確的方向引導(dǎo)，并以此來優(yōu)化初始信息素的分布，加快了算法的搜索速度[5]。文獻(xiàn)6從信息素的更新方式及局部搜索策略方面進(jìn)行了改進(jìn)，并將虛擬路徑這一概念應(yīng)用于動態(tài)路徑規(guī)劃中[6]。

3 局部路徑規(guī)劃算法

局部路徑規(guī)劃，也稱為避障路徑規(guī)劃，即考慮本車和障礙物之間的幾何關(guān)系尋找出一條避免與障礙物發(fā)生碰撞的路徑，是無人駕駛汽車的重要功能模塊之一。常用的局部路徑規(guī)劃算法有人工勢場法、神經(jīng)網(wǎng)絡(luò)算法、快速搜索隨機(jī)樹法、智能水滴算法等。

3.1 人工勢場法

人工勢場法路徑規(guī)劃的基本思想是將汽車在環(huán)境中的運(yùn)動設(shè)計(jì)成為一種抽象的人造引力場中的運(yùn)動。在無人駕駛汽車的運(yùn)行環(huán)境中，人工勢場模型的引力主要來源于目標(biāo)點(diǎn)對汽車的引力勢場，與目標(biāo)點(diǎn)距離越大引力勢能越強(qiáng)，引力方向朝向目標(biāo)點(diǎn)。障礙物對汽車產(chǎn)生斥力，斥力的大小隨著與障礙物的距離增大而減小，方向指向遠(yuǎn)離障礙物方向。最后根據(jù)引力和斥力的合力來控制汽車的運(yùn)動。

智能汽車受到的勢場是單個引力勢場和斥力勢場之和，可表示為：

Utotal（X）=Uatt（X）+Urep（X）

汽車所受合力為：Ftotal（X）=Fatt（X）+Frep（X）

汽車在實(shí)際環(huán)境中自主駕駛時(shí)，外部環(huán)境是動態(tài)的和變化的，并沒有考慮車輛行駛動力學(xué)和運(yùn)動學(xué)，因此在實(shí)際使用時(shí)需要增加約束條件。人工勢場法也有自己的局限性，如當(dāng)車輛離目標(biāo)點(diǎn)很遠(yuǎn)時(shí)，引力特別大，相對較小的斥力在甚至可以忽略的情況下，物體路徑上可能會碰到障礙物，又如在某個點(diǎn)，引力斥力剛好大小相等，方向相反，則車輛容易陷入局部最優(yōu)解或震蕩。

3.2 快速搜索隨機(jī)樹算法

快速搜索隨機(jī)樹算法（RRT）RRT是一種在完全已知的環(huán)境中通過采樣擴(kuò)展搜索的算法，它以一個初始點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一個隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或進(jìn)入了目標(biāo)區(qū)域，便可以在隨機(jī)樹中找到一條由從初始點(diǎn)到目標(biāo)點(diǎn)的路徑。采用RRT算法進(jìn)行無人駕駛車輛路徑規(guī)劃步驟如下：

3.2.1 初始化

需要提供已知環(huán)境地圖，包括障礙物位置以及終點(diǎn)位置。然后將車輛的起點(diǎn)作為隨機(jī)樹的根節(jié)點(diǎn)。

3.2.2 隨機(jī)采樣

隨機(jī)樹在生長時(shí)理論上是朝著終點(diǎn)進(jìn)行生長，但是由于有障礙物的存在，所以在每次選擇生長方向時(shí)，有一定的概率會向著目標(biāo)點(diǎn)延伸，也有一定的概率會隨機(jī)在地圖內(nèi)選擇一個方向延伸一段距離。

3.2.3 樹節(jié)點(diǎn)擴(kuò)展

在選取采樣點(diǎn)之后，如何判斷采樣點(diǎn)是否為隨機(jī)樹節(jié)點(diǎn)？這里采用的策略是尋找距離采樣點(diǎn)最近的樹中的節(jié)點(diǎn)，將其作為新擴(kuò)展的節(jié)點(diǎn)的父節(jié)點(diǎn)，然后以該父節(jié)點(diǎn)為基礎(chǔ)，向采樣點(diǎn)的方向延申一定距離，將延伸后的節(jié)點(diǎn)作為新節(jié)點(diǎn)并加入樹中。這個步驟的前提條件是新節(jié)點(diǎn)以及父節(jié)點(diǎn)到新節(jié)點(diǎn)的路徑不發(fā)生碰撞，如果發(fā)生碰撞，則放棄該采樣點(diǎn)和新節(jié)點(diǎn)。

3.2.4 終止條件

一般情況下，在樹節(jié)點(diǎn)擴(kuò)展的方法基礎(chǔ)上，可以將擴(kuò)展的新節(jié)點(diǎn)是終點(diǎn)作為終止條件，為了避免不存在路徑導(dǎo)致的死循環(huán)問題，增加最大迭代次數(shù)作為另一個終止條件。

該算法也有自己的局限性，作為一種隨機(jī)性算法，在同樣的環(huán)境中可能得到不同的路徑，而且隨機(jī)采樣的特性會導(dǎo)致路徑不平滑。

4 總結(jié)與展望

無人駕駛中的路徑規(guī)劃算法有很多，但是都有自己的局限性，可以結(jié)合各自算法優(yōu)缺點(diǎn)使多種算法相融合。在路徑規(guī)劃中要考慮動態(tài)障礙物、道路安全法規(guī)、車輛行駛動力學(xué)和運(yùn)動學(xué)等問題。車輛運(yùn)動速度快，也需要考慮算法的復(fù)雜程度，減少計(jì)算的時(shí)間，更快的做出決策。

課題來源：湖北省武漢軟件工程職業(yè)學(xué)院“一專一項(xiàng)”教學(xué)改革項(xiàng)目。

參考文獻(xiàn)：

[1]袁師召，李軍.無人駕駛汽車路徑規(guī)劃研究綜述[J].汽車工程師，2019，05：11-13.

[2]張朋飛，何克忠，歐陽正柱等.多功能室外智能移動機(jī)器人實(shí)驗(yàn)平臺-THMR-V[J].機(jī)器人，2002，06：88-95.

[3]關(guān)泉珍，鮑泓，史志堅(jiān).基于A*算法的駕駛地圖路徑規(guī)劃實(shí)現(xiàn)[J].北京聯(lián)合大學(xué)學(xué)報(bào)（自然科學(xué)版），2016，30（02）：31-39.

[4]程傳奇，郝向陽，李建勝等.融合改進(jìn)A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃[J].西安交通大學(xué)學(xué)報(bào)，2017，51（11）：137-142.

[5]喻環(huán).改進(jìn)蟻群算法在機(jī)器人路徑規(guī)劃上的應(yīng)用研究.[D].安徽：安徽大學(xué)，2017：11-20.

[6]譚寶成，宋潔.蟻群算法在無人駕駛智能車中的應(yīng)用及改進(jìn)[J].理論與方法，2012，31（9）：15-17.