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

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

關(guān)鍵詞：無(wú)人駕駛 路徑規(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 引言

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

路徑規(guī)劃是是指生成一條連接車輛行駛起點(diǎn)位置與終點(diǎn)位置，且不與環(huán)境中障礙物發(fā)生碰撞的幾何路徑[1]。實(shí)際上兩個(gè)關(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è)備感知行駛遇到的空間障礙物，在滿足動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)約束以及穩(wěn)定性、舒適性等評(píng)價(jià)指標(biāo)的條件下預(yù)先或者實(shí)時(shí)地為智能車輛規(guī)劃出從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑[2]。自動(dòng)駕駛技術(shù)路線很多是在輪式機(jī)器人的技術(shù)路線基礎(chǔ)上改進(jìn)和發(fā)展。

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

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

2.1 A*算法

A*算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，依據(jù)代價(jià)函數(shù)來(lái)搜索最優(yōu)路徑，既考慮從初始結(jié)點(diǎn)到當(dāng)前結(jié)點(diǎn)的代價(jià)值，又考慮了當(dāng)前結(jié)點(diǎn)到目標(biāo)結(jié)點(diǎn)的啟發(fā)值，是一種啟發(fā)式搜索算法[3]。在路徑搜索過(guò)程中，A*算法使用代價(jià)函數(shù)來(lái)評(píng)估節(jié)點(diǎn)的質(zhì)量，算法將選擇的代價(jià)函數(shù)數(shù)值最小的節(jié)點(diǎn)作為下一步的節(jié)點(diǎn)，然后它將繼續(xù)從下一個(gè)節(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à)，是一個(gè)預(yù)測(cè)值。g（n）經(jīng)常都是固定的數(shù)值，然而h（n）卻不是固定的，它的改變會(huì)影響到f（n）是否為最短路徑。因此在 A*算法中，啟發(fā)函數(shù)h（n）的設(shè)計(jì)非常重要，合理的啟發(fā)函數(shù)設(shè)計(jì)能幫助A*算法在路徑規(guī)劃中快速簡(jiǎn)潔，提高尋路效率。啟發(fā)函數(shù)一般有四種表達(dá)式，但是最常用的是用曼哈頓距離定義。

曼哈頓距離：坐標(biāo)橫縱方向的距離之和，允許向上、下、左、右移動(dòng)，啟發(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。但是針對(duì)不同的環(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*算法和動(dòng)態(tài)窗口法的全局動(dòng)態(tài)路徑規(guī)劃方法。設(shè)計(jì)了一種關(guān)鍵點(diǎn)選取策略，能夠去除傳統(tǒng)A*算法規(guī)劃路徑序列中的冗余點(diǎn)，從而提高路徑規(guī)劃性能[4]。

2.2 蟻群算法

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

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

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

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

3.1 人工勢(shì)場(chǎng)法

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

智能汽車受到的勢(shì)場(chǎng)是單個(gè)引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)之和，可表示為：

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

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

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

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

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

3.2.1 初始化

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

3.2.2 隨機(jī)采樣

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

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

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

3.2.4 終止條件

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

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

4 總結(jié)與展望

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

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

參考文獻(xiàn)：

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

[2]張朋飛，何克忠，歐陽(yáng)正柱等.多功能室外智能移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)-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]程傳奇，郝向陽(yáng)，李建勝等.融合改進(jìn)A*算法和動(dòng)態(tài)窗口法的全局動(dòng)態(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]譚寶成，宋潔.蟻群算法在無(wú)人駕駛智能車中的應(yīng)用及改進(jìn)[J].理論與方法，2012，31（9）：15-17.