A*算法在自動(dòng)駕駛車輛路徑規(guī)劃中的應(yīng)用

馮凱 吉星 楊昕

摘 要：路徑規(guī)劃是自動(dòng)駕駛系統(tǒng)研究的重要內(nèi)容之一，A*算法是一種啟發(fā)式的搜索算法，可以大幅度減少搜索過程中的擴(kuò)展節(jié)點(diǎn)，從而可以快速找到一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑。結(jié)合高精度地圖在自動(dòng)駕駛系統(tǒng)中的應(yīng)用，文章將A*算法用于自動(dòng)駕駛車輛在高精度地圖中的全局路徑規(guī)劃，通過在自動(dòng)駕駛車輛平臺上實(shí)驗(yàn)測試表明該算法能夠快速準(zhǔn)確地規(guī)劃出一條最短路徑。

關(guān)鍵詞：路徑規(guī)劃;A*算法;無人駕駛;高精度地圖

中圖分類號：U469.7? 文獻(xiàn)標(biāo)志碼：A? 文章編號：1671-7988（2020）22-25-04

Abstract： Path planning is one of the important contents of automated driving system research. A* algorithm is a heuristic search algorithm， which can significantly reduce the extended nodes in the search process， so as to quickly find an optimal path from the starting point to the end point. Combined with the application of high-precision map in the automated driving system， this paper applies the A* algorithm to the global path planning of the autonomous vehicle in the high-precision map. The experimental test on the platform of the autonomous vehicle shows that the algorithm can quickly and accurately plan a shortest path.

Keywords： Path planning; A* algorithm; Automated driving; High precision map

CLC NO.： U469.7? Document Code： A? Article ID： 1671-7988（2020）22-25-04

引言

無人駕駛車輛也稱無人車、自動(dòng)駕駛汽車，是指搭載有先進(jìn)的車載傳感器、控制器、執(zhí)行器等裝置，具備復(fù)雜環(huán)境感知、智能決策和控制等功能，能根據(jù)自身對周圍環(huán)境條件的感知、理解，自動(dòng)進(jìn)行合理的車輛運(yùn)動(dòng)控制，且能夠達(dá)到人類駕駛員駕駛水平的車輛[1][2]。路徑規(guī)劃是無人駕駛車輛系統(tǒng)最基本的環(huán)節(jié)之一，它是指在真實(shí)的道路環(huán)境中，無人駕駛系統(tǒng)按照設(shè)定的性能指標(biāo)（如無碰撞、距離最短、時(shí)間最短等）尋找一條從開始位置到目標(biāo)位置的最優(yōu)路徑[3][4]。根據(jù)對周圍環(huán)境信息的知道程度不同，可以將路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃[5]。全局路徑規(guī)劃是指在已知的地圖數(shù)據(jù)中規(guī)劃出從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑;局部路徑規(guī)劃是指依靠感知數(shù)據(jù)、局部地圖數(shù)據(jù)和車輛位姿信息規(guī)劃出一條無碰撞、滿足約束條件的可行駛軌跡。

目前常用的全局路徑規(guī)劃算法有Dijkstra算法、A*算法、蟻群算法和粒子群算法等[6][7]，其中由尼爾森提出的A*算法在路徑規(guī)劃領(lǐng)域廣泛應(yīng)用。本文在基于無人駕駛高精度地圖的基礎(chǔ)上，通過A*算法進(jìn)行了全局路徑規(guī)劃的設(shè)計(jì)與實(shí)現(xiàn)，將算法在無人車平臺上進(jìn)行了真實(shí)環(huán)境下的測試與驗(yàn)證，結(jié)果表明A*算法能夠更加快速的找到一條最短路徑。

1 A*算法描述

A*算法是在Dijkstra算法的基礎(chǔ)上提出的，它引入了啟發(fā)式函數(shù)和預(yù)估代價(jià)，是在一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，也是許多其他問題的常用啟發(fā)式算法。算法的核心部分在于估價(jià)函數(shù)的設(shè)計(jì)[8]，其估價(jià)函數(shù)如式1所示：

其中，f（n）是從初始狀態(tài)經(jīng)由狀態(tài)n到目標(biāo)狀態(tài)的代價(jià)估計(jì)，g（n）是從初始狀態(tài)到狀態(tài)n的實(shí)際代價(jià)，h（n）是從狀態(tài)n到目標(biāo)狀態(tài)的最佳路徑的估計(jì)代價(jià)。

在A*算法中由于g（n）表示實(shí)際距離，而h（n）表示估計(jì)距離，所以在A*算法估價(jià)函數(shù)中h（n）的選擇具有關(guān)鍵性的作用，決定了A*算法的效率，其中常用的估價(jià)函數(shù)有曼哈頓距離、對角線距離和歐幾里德距離等。在估價(jià)函數(shù)中，如果h（n）等于0，那么f（n）= h（n），則A*算法就演變成為了Dijkstra算法，能夠找到從起點(diǎn)到終點(diǎn)的最短路徑，但是擴(kuò)展節(jié)點(diǎn)會增多，效率變差。如果h（n）比從節(jié)點(diǎn)n移動(dòng)到目標(biāo)節(jié)點(diǎn)的真實(shí)代價(jià)小，此時(shí)A*也能保證找到一條最短路徑;但是h（n）越小，A*的擴(kuò)展節(jié)點(diǎn)越多，耗費(fèi)資源越大，運(yùn)行效率降低。如果h（n）精確的等于從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的真實(shí)代價(jià)，則A*將會快速的尋找到最佳的路徑并且不會進(jìn)行額外的搜索擴(kuò)展，此時(shí)算法運(yùn)行效率最高。如果h（n）比從節(jié)點(diǎn)n移動(dòng)到目標(biāo)節(jié)點(diǎn)的真實(shí)代價(jià)大，那么A*不能夠保證找到最短的路徑，但是此時(shí)算法的運(yùn)行速度快。另一種極端的情況為h（n）遠(yuǎn)遠(yuǎn)大于g（n），那么此時(shí)f（n）≈ h（n），則A*算法演變?yōu)锽FS算法。

2 高精度地圖的有向圖建模

高精度地圖是無人駕駛的基礎(chǔ)，它包含了車道模型、道路部件、道路屬性和其它的圖層信息。在無人駕駛領(lǐng)域高精度地圖必須要滿足車道級的自動(dòng)駕駛導(dǎo)航，為了能為自動(dòng)駕駛車輛進(jìn)行精準(zhǔn)的轉(zhuǎn)向、制動(dòng)等，地圖中除了需要包含車道線、車道中心線、車道屬性變化等道路細(xì)節(jié)信息，還需要包含道路的曲率、坡度、航向、橫坡等參數(shù)，此外還應(yīng)包含交通標(biāo)志牌、路面標(biāo)志等道路部件等，這些綜合信息數(shù)據(jù)一起構(gòu)成了無人駕駛高精度地圖。

目前高精度地圖的主流形式有Opendrive和NDS兩種。本文中無人駕駛車輛所用的高精度地圖采用OpenDrive格式進(jìn)行存儲，由于地圖數(shù)據(jù)元素眾多，而全局路徑規(guī)劃關(guān)注的是地圖路網(wǎng)的連接關(guān)系，所以必須先對其進(jìn)行解析然后進(jìn)行路網(wǎng)數(shù)據(jù)提取。我們需要從地圖數(shù)據(jù)中提取表示一條路段的必要元素：id號、起點(diǎn)、終點(diǎn)和長度以及連接關(guān)系，因?yàn)榈貓D數(shù)據(jù)中的路網(wǎng)連接關(guān)系是有方向的，為了將路網(wǎng)數(shù)據(jù)抽象為有向圖的形式進(jìn)行表達(dá)，所以可將每條路段抽象為一個(gè)節(jié)點(diǎn)并將路段A的長度作為路段A到其相鄰路段權(quán)重，并最終采用鄰接表進(jìn)行路網(wǎng)數(shù)據(jù)的表示，如圖1所示為從采集的高精度地圖中提取的路網(wǎng)數(shù)據(jù)（左圖中標(biāo)紅的路線）。

3 基于A*算法的路徑規(guī)劃設(shè)計(jì)與實(shí)現(xiàn)

3.1 估值函數(shù)選擇

在真實(shí)道路環(huán)境中一條路段的起點(diǎn)至終點(diǎn)的距離長度一定大于等于兩點(diǎn)之間的直線距離，結(jié)合A*算法中估計(jì)函數(shù)h（n）的選取原則，本文中選取歐幾里德距離作為A*算法的估計(jì)函數(shù)，例如地圖中存在點(diǎn)A和點(diǎn)B，并且在地圖數(shù)據(jù)中的坐標(biāo)分別表示為（A.x，A.y）和（B.x，B.y），則AB之間的估值計(jì)算如式2所示。

采用歐幾里德距離作為估計(jì)函數(shù)時(shí)，因?yàn)閔（n）一定小于等于道路的真實(shí)距離，所以此時(shí)A*算法一定能夠規(guī)劃出最短路徑且運(yùn)行效率高。

3.2 A*算法數(shù)據(jù)接口設(shè)計(jì)與實(shí)現(xiàn)

根據(jù)對真實(shí)路網(wǎng)數(shù)據(jù)的有向圖表達(dá)形式結(jié)合A*算法對數(shù)據(jù)結(jié)構(gòu)的需求，對路網(wǎng)數(shù)據(jù)結(jié)構(gòu)進(jìn)行接口設(shè)計(jì)。其中，路網(wǎng)中一條Section路段表示為結(jié)構(gòu)體SectionInfo。

3.3 A*算法流程設(shè)計(jì)

A*算法在進(jìn)行路徑規(guī)劃時(shí)，需要建立兩個(gè)列表用來進(jìn)行輔助規(guī)劃搜索。一個(gè)是用來存放已經(jīng)被檢測過的節(jié)點(diǎn)列表Close_List，另一個(gè)是用來存放待檢測節(jié)點(diǎn)的列表Open_List。本文設(shè)計(jì)實(shí)現(xiàn)的A*算法具體步驟如下所示。

4 算法測試與驗(yàn)證

4.1 實(shí)驗(yàn)平臺

本文中所實(shí)現(xiàn)的路徑規(guī)劃算法的實(shí)驗(yàn)是在陜汽汽車工程研究院智能服務(wù)研究所無人駕駛車輛平臺上進(jìn)行的，如圖4所示為實(shí)驗(yàn)所用的無人車駕駛車輛，無人車上裝配有激光雷達(dá)、相機(jī)和組合導(dǎo)航等傳感器。

4.2 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)所用的無人駕駛車輛平臺中所使用的地圖均為同向單車道且道中間為實(shí)線，無人車在行駛過程中禁止變道。測試驗(yàn)證時(shí)無人車的姿態(tài)朝向?yàn)槲飨颍▓D中朝左方向），路徑規(guī)劃的測試結(jié)果如圖5所示，其中綠色路段為規(guī)劃出的路徑。

5 結(jié)語

本文將A*算法應(yīng)用于從高精度地圖數(shù)據(jù)中提取的路網(wǎng)上，可以高效準(zhǔn)確地規(guī)劃出最短路徑。實(shí)驗(yàn)結(jié)果表明采用A*算法能夠滿足封閉園區(qū)無人駕駛車輛對路徑規(guī)劃的需求，本文實(shí)現(xiàn)的路徑規(guī)劃模塊已經(jīng)部署在自動(dòng)駕駛車輛平臺上并且可以很好的運(yùn)行。

參考文獻(xiàn)

[1] 宋飛揚(yáng).無人駕駛汽車及其發(fā)展[J].中國高新科技，2019（05）：24-27.

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

[3] ZAMIRIAN M，KAMYAD A V，F(xiàn)ARAHI M H. A novel algorithm for solving optimal path planning problems based on parametrizationmethod and fuzzy aggregation[J]. Physics Letters A， 2009.373（38）.

[4] 朱大奇，顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述[J].控制與決策， 2010.25（7）：961-967.

[5] 黃金豪，吳建悍.基于改進(jìn)A*算法的室內(nèi)服務(wù)機(jī)器人路徑規(guī)劃[J].技術(shù)與市場，2020，27（03）：62-63.

[6] 趙曉，王錚，黃程侃，等.基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].機(jī)器人，2018，40（06）：903-910.

[7] 張廣林，胡小梅，柴劍飛，等.路徑規(guī)劃算法及其應(yīng)用綜述[J].現(xiàn)代機(jī)械，2011（5）：85-90.

[8] 武雅杰，楊晶東.基于A*算法的機(jī)器人路徑規(guī)劃[J].電子科技，2017， 30（06）：124-127.