任書宇 吳欽木 周還籍

（貴州大學(xué)電氣工程學(xué)院 貴陽(yáng) 550025）

1 引言

自動(dòng)駕駛汽車是一種集環(huán)境感知、路徑規(guī)劃、決策控制于一身的綜合性系統(tǒng)，已經(jīng)被世界各國(guó)所重視，并且將之視為汽車行業(yè)發(fā)展中新的動(dòng)力與發(fā)展方向。路徑規(guī)劃模塊是自動(dòng)駕駛汽車研究的關(guān)鍵模塊，此模塊的性能直接影響到汽車的運(yùn)行路線和運(yùn)行的平順性，其直接關(guān)系到自動(dòng)駕駛汽車的安全性和舒適性。

根據(jù)目標(biāo)范圍劃分，可以將路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃。根據(jù)已有的環(huán)境地圖所提供的所有精確信息，以此為依據(jù)，從給定的起始點(diǎn)到目的地，利用全局路徑規(guī)劃規(guī)劃算法，尋求一條最優(yōu)或近似最優(yōu)的路徑進(jìn)行全局路徑規(guī)劃。最優(yōu)路徑的定義標(biāo)準(zhǔn)包括距離最近、規(guī)劃時(shí)間最短、路徑轉(zhuǎn)折曲率最小等。但當(dāng)有動(dòng)態(tài)障礙物突然出現(xiàn)時(shí)會(huì)造成不必要的沖突，或者車輛想要進(jìn)行變道、超車等行為時(shí)，這就要求在全局路徑規(guī)劃的基礎(chǔ)上，利用傳感器對(duì)車輛周邊環(huán)境信息進(jìn)行實(shí)時(shí)的采集，并根據(jù)其在地圖中的具體位置和周邊車輛及行人等障礙物的分布，實(shí)現(xiàn)由當(dāng)前節(jié)點(diǎn)到任一子目標(biāo)節(jié)點(diǎn)的局部最優(yōu)安全路徑。

A*算法是最常用的全局路徑規(guī)劃算法之一，其在Dijstra算法基礎(chǔ)上，引入啟發(fā)式函數(shù)提高節(jié)點(diǎn)搜索速度，但其啟發(fā)式函數(shù)僅考慮距離信息，使規(guī)劃所得路徑因存在大量不必要冗余拐點(diǎn)而造成路徑不平滑。使用Floyd 算法對(duì)A*算法的結(jié)果路徑進(jìn)行迭代優(yōu)化可以保證得到最優(yōu)路徑［1］，但其規(guī)劃路徑不一定滿足車輛本身的動(dòng)力學(xué)要求；文獻(xiàn)［2］提出了一種二次規(guī)劃方法使規(guī)劃所得路徑在運(yùn)動(dòng)學(xué)上可行，且可以在復(fù)雜環(huán)境中避免碰撞。人工勢(shì)場(chǎng)法和動(dòng)態(tài)窗口法是常用的局部路徑規(guī)劃算法，文獻(xiàn)［3］提出了一種結(jié)合人工勢(shì)場(chǎng)法構(gòu)造同時(shí)包含距離信息和障礙物信息的啟發(fā)函數(shù)改進(jìn)A*算法，使得拐點(diǎn)數(shù)量明顯減少，但其規(guī)劃路徑仍不滿足車輛動(dòng)力學(xué)約束；由于A*算法不具備動(dòng)態(tài)避障的特性，文獻(xiàn)［4］將其與動(dòng)態(tài)窗口法相結(jié)合，使A*算法能夠?qū)崿F(xiàn)動(dòng)態(tài)避障，且所得規(guī)劃路徑的平滑性也有所改善，但其算法效率仍需提高。對(duì)此，本文提出了一種改進(jìn)A*算法和Lattice 算法相融合的路徑規(guī)劃算法，可以快速規(guī)劃得到一條舒適安全的運(yùn)動(dòng)軌跡。最后通過在CARLA 模擬器上進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證了該算法的有效性和可靠性。

2 A*算法及其改進(jìn)

2.1 基本A*算法

傳統(tǒng)的A*算法由Nilsson 提出，其為一種經(jīng)典的啟發(fā)式全局路徑規(guī)劃算法，也是尋找最短路徑最有效的直接路徑規(guī)劃方法，由于其完備性、最優(yōu)性和最優(yōu)效率的優(yōu)點(diǎn)，它在全局路徑規(guī)劃算法中應(yīng)用廣泛［5］。

公式為

式中：f(n)是經(jīng)過指定節(jié)點(diǎn)n，從給定的初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)，最小的預(yù)估代價(jià)值函數(shù)；g(n)是從給定的初始節(jié)點(diǎn)到指定節(jié)點(diǎn)n，最小的當(dāng)前實(shí)際代價(jià)值函數(shù)；h(n)是從指定節(jié)點(diǎn)n到給定的目標(biāo)節(jié)點(diǎn)，最小的當(dāng)前預(yù)估代價(jià)值函數(shù)。對(duì)于路徑搜索問題，代價(jià)就是距離。按f(n)尋找代價(jià)最小的路徑，確定一條從給定起始點(diǎn)到目的地的無(wú)碰撞最優(yōu)路線。

2.2 改進(jìn)A*算法

首先，在精度要求較高的復(fù)雜地圖環(huán)境，如果想進(jìn)行路徑規(guī)劃，需將地圖網(wǎng)格劃分較小，就會(huì)使A*算法運(yùn)算過程中，消耗內(nèi)存嚴(yán)重，并且加大計(jì)算量；其次，A*算法是利用柵格法進(jìn)行路徑規(guī)劃，網(wǎng)格型的路徑連接有可能造成規(guī)劃路徑過度轉(zhuǎn)折的情形，由于汽車本身具有動(dòng)力學(xué)約束，路徑不平滑會(huì)使其不能正常行駛［4，6］。針對(duì)上述問題，對(duì)A*算法的搜索策略和權(quán)重系數(shù)進(jìn)行優(yōu)化，提高了算法效率。

2.2.1 搜索策略優(yōu)化

A*算法的搜索策略是將當(dāng)前節(jié)點(diǎn)周圍8 鄰域中，除障礙物以外的全部節(jié)點(diǎn)均加入Openlist中，利用評(píng)價(jià)函數(shù)選出代價(jià)最小的節(jié)點(diǎn)擴(kuò)展為下一節(jié)點(diǎn)。若環(huán)境復(fù)雜、規(guī)劃路徑長(zhǎng)就會(huì)導(dǎo)致Openlist 中的節(jié)點(diǎn)數(shù)量很多，使評(píng)價(jià)函數(shù)的計(jì)算量增大，導(dǎo)致路徑規(guī)劃效率不理想。為了提高搜索效率，設(shè)計(jì)了一種動(dòng)態(tài)調(diào)整鄰域搜索范圍的方法，A*算法的鄰域示意圖如圖1所示。

圖1 A*算法鄰域示意圖

在真實(shí)世界坐標(biāo)系下，設(shè)當(dāng)前節(jié)點(diǎn)為C(Xc，Yc)，目標(biāo)節(jié)點(diǎn)為G(XG，YG)，θ為向量CG與X軸單位向量的夾角，則使用動(dòng)態(tài)鄰域法的搜索范圍如表1所示。

表1 動(dòng)態(tài)鄰域法搜索范圍

從表1 中可以看出，使用動(dòng)態(tài)鄰域法進(jìn)行搜索時(shí)，不再將當(dāng)前節(jié)點(diǎn)周圍8 鄰域中，除障礙物外的所有柵格均放入Openlist 中，而是根據(jù)值實(shí)時(shí)調(diào)整需搜索的柵格點(diǎn)。動(dòng)態(tài)鄰域法在進(jìn)行路徑搜索時(shí)，更傾向于選擇從初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的連線方向的路徑，從而減小了算法的搜算范圍，在運(yùn)行過程中可以顯著降低內(nèi)存占用，提高了算法的效率。

2.2.2 權(quán)重系數(shù)優(yōu)化

作為啟發(fā)式搜索算法的一種，其最核心的部分在于啟發(fā)式函數(shù)h(n)的設(shè)計(jì)上，它決定了算法執(zhí)行的準(zhǔn)確率和效率，h(n) 傳統(tǒng)的取值方法包括曼哈頓距離、對(duì)角線距離、歐拉距離等，經(jīng)實(shí)驗(yàn)表明，在搜索路徑長(zhǎng)度和搜索時(shí)間長(zhǎng)短方面，采用曼哈頓距離作為h(n) 函數(shù)的值效果都更好，因此本文采用曼哈頓距離［4］。其函數(shù)表達(dá)式如式（2）所示：

式中：(xn，yn)表示當(dāng)前節(jié)點(diǎn)在柵格地圖中的坐標(biāo)值，(xgoal，ygoal)表示目標(biāo)節(jié)點(diǎn)在柵格地圖中的坐標(biāo)值。

但當(dāng)前節(jié)點(diǎn)存在多條啟發(fā)式函數(shù)值相近的搜索結(jié)果時(shí)，很難搜索出最優(yōu)擴(kuò)展結(jié)點(diǎn)，因此對(duì)啟發(fā)式函數(shù)賦予一個(gè)動(dòng)態(tài)權(quán)重系數(shù)w(n)，公式為

式中：d(n)表示當(dāng)前節(jié)點(diǎn)n，與指定的目標(biāo)節(jié)點(diǎn)之間的距離，d(s) 表示指定的起始節(jié)點(diǎn)，與指定的目標(biāo)節(jié)點(diǎn)之間的距離。使用動(dòng)態(tài)加權(quán)提高A*算法快速性的原則為在規(guī)劃末尾階段，w(n) 減小，此時(shí)A*算法更傾向于搜索最優(yōu)路徑，其運(yùn)算時(shí)間與改進(jìn)之前幾乎保持一致；在規(guī)劃開始階段，w(n) 較大，此時(shí)A*算法會(huì)快速向目標(biāo)節(jié)點(diǎn)區(qū)域擴(kuò)展，其運(yùn)算時(shí)間較改進(jìn)之前大大減少。

3 Lattice 算法及其與改進(jìn)A*算法融合

無(wú)人駕駛汽車所處環(huán)境是一個(gè)動(dòng)態(tài)環(huán)境，傳統(tǒng)的全局路徑規(guī)劃算法只能規(guī)劃出一條靜態(tài)路徑，并且規(guī)劃出的路徑不一定滿足無(wú)人駕駛車輛的非完整性約束，同時(shí)目前常見的傳統(tǒng)路徑規(guī)劃算法可能存在局部最優(yōu)、模型計(jì)算量大甚至模型無(wú)解的情況，因此提出了基于Lattice算法的路徑與速度同時(shí)規(guī)劃的路徑規(guī)劃方法。采用改進(jìn)的A*算法規(guī)劃得到全局最優(yōu)路徑，并將其作為參考線，應(yīng)用于Frenet坐標(biāo)系下的Lattice算法，進(jìn)行局部路徑規(guī)劃［7］。

3.1 Fenet坐標(biāo)系

無(wú)人駕駛汽車的局部路徑規(guī)劃需要對(duì)周圍環(huán)境中的車輛、行人等動(dòng)態(tài)障礙物的未來(lái)軌跡進(jìn)行預(yù)測(cè)，在規(guī)劃過程中除了考慮車輛的位置信息，還需要考慮時(shí)間維度t，此時(shí)二維路徑點(diǎn)(x，y)就變?yōu)榱巳S軌跡點(diǎn)(x，y，t)，因此在自動(dòng)駕駛技術(shù)領(lǐng)域局部路徑規(guī)劃也可以稱為軌跡規(guī)劃，道路軌跡規(guī)劃直接反映了自動(dòng)駕駛汽車在城市道路上行駛時(shí)的智能水平［8］。如果使用笛卡爾坐標(biāo)系進(jìn)行軌跡規(guī)劃，對(duì)當(dāng)前車輛位置，與當(dāng)前所處車道的關(guān)系，很難進(jìn)行精確描述，并且與二維規(guī)劃相比，三維規(guī)劃更復(fù)雜。而在Frenet坐標(biāo)系下，車輛與當(dāng)前所處車道之間的橫向及縱向距離關(guān)系，可以很輕松的得到，由此可以忽略非直線車道曲率的影響。同時(shí)還可以將車輛運(yùn)動(dòng)解耦為縱向運(yùn)動(dòng)和橫向運(yùn)動(dòng)進(jìn)行分別計(jì)算［9］，F(xiàn)renet坐標(biāo)系簡(jiǎn)化了，特別是在道路環(huán)境復(fù)雜的情況下，道路曲線擬合問題的求解［10］，因此引入Frenet坐標(biāo)系。

無(wú)人駕駛車輛在Frenet 坐標(biāo)系下的位置表示如圖2所示。

圖2 Frenet坐標(biāo)系與笛卡爾坐標(biāo)系轉(zhuǎn)換關(guān)系圖

在Frenet 坐標(biāo)系中，使用改進(jìn)后A*算法所得全局規(guī)劃路徑作為參考線，在參考線上找到距離車輛位置最近的點(diǎn)定為投影點(diǎn)，以車輛自身為原點(diǎn)，使用投影點(diǎn)在參考線上的切線方向和法線方向建立一個(gè)坐標(biāo)軸互相垂直的坐標(biāo)系，車輛位置在此下表示為(s，l)。其中s表示從起點(diǎn)到投影點(diǎn)的曲線距離，l表示車輛位置到參考點(diǎn)的法向距離。

3.2 Lattice算法

為了解決車輛行駛過程中縱向運(yùn)動(dòng)對(duì)橫向運(yùn)動(dòng)產(chǎn)生的影響，設(shè)計(jì)了一種Frenet 坐標(biāo)系下的lattice 算法［11］。Frenet 坐標(biāo)系將車輛運(yùn)動(dòng)解耦為一種，不受約束的、互相獨(dú)立的縱向移動(dòng)和橫向移動(dòng)規(guī)劃系統(tǒng)，利用Lattice算法，根據(jù)初始配置信息，通過更改采樣時(shí)間間隔，隨機(jī)采樣生成一系列候選軌跡多項(xiàng)式，并利用可行性、安全性、舒適性等因素進(jìn)行綜合考量，對(duì)候選軌跡簇，使用評(píng)價(jià)函數(shù)對(duì)路徑成本進(jìn)行評(píng)估，選擇代價(jià)值最小的候選軌跡作為局部最優(yōu)軌跡［12］。由于Lattice 算法執(zhí)行過程中有采樣階段，因此其可以使用離散化搜索空間，用于高效計(jì)算非結(jié)構(gòu)化復(fù)雜環(huán)境中，最優(yōu)路徑規(guī)劃問題的局部最優(yōu)解［13］。使用Lattice 算法可以在規(guī)劃時(shí)考慮運(yùn)動(dòng)的不確定性，獲得安全和最佳的路徑［14］。

3.2.1 Lattice軌跡生成

Lattice 軌跡是由橫向軌跡簇和縱向軌跡簇進(jìn)行二維運(yùn)動(dòng)合成得到的。橫向規(guī)劃主要是用于解決動(dòng)態(tài)避障和換道等任務(wù)，與車輛的方向盤轉(zhuǎn)角直接相關(guān)；縱向規(guī)劃根據(jù)車輛在實(shí)際行駛場(chǎng)景中的要求分為定速巡航、避障、停車等，與車輛的油門和剎車直接相關(guān)。

首先獲得車輛在Frenet坐標(biāo)系下的初始位置、初始速度和初始加速度，記為，然后在Frenet 坐標(biāo)系下，任意確定一采樣時(shí)間間隔，采樣得到t1時(shí)刻的車輛位置、車輛速度和車輛加速度，記為，通過五次多項(xiàng)式擬合，將將t0時(shí)刻和t1時(shí)刻的狀態(tài)信息量，作為已知確定變量，帶入五次多項(xiàng)式求其未知數(shù)，就可以獲得橫向多項(xiàng)式軌跡和縱向多項(xiàng)式軌跡，見圖3。

圖3 多項(xiàng)式軌跡

1）縱向軌跡五次多項(xiàng)式s*=s(t*)如式（4）所示：

2）橫向軌跡五次多項(xiàng)式l*=l(s*)如式（5）所示：

更改采樣時(shí)間間隔，得到一系列待選的橫向和縱向路徑點(diǎn)，采用改進(jìn)A*算法生成的全局路徑，作為L(zhǎng)attice局部路徑規(guī)劃算法的參考線，計(jì)算出各個(gè)采樣時(shí)刻點(diǎn)，車輛相對(duì)于參考點(diǎn)的縱向偏移量和橫向偏移量。再將縱向規(guī)劃路徑和橫向規(guī)劃路徑，利用坐標(biāo)變換進(jìn)行二維運(yùn)動(dòng)合成，最后還原得到二維平面內(nèi)的軌跡簇。

3.2.2 Lattice評(píng)價(jià)函數(shù)

為了滿足無(wú)人駕駛車輛必須到達(dá)目的地、符合交通規(guī)則、避免障礙物碰撞以及行駛平穩(wěn)舒適等要求，本文設(shè)計(jì)了5 個(gè)cost評(píng)價(jià)函數(shù)，分別為碰撞cost、到達(dá)cost、側(cè)向偏移cost、橫向加速度cost、縱向加速度cost。

1）碰撞cost：汽車在行駛時(shí)，計(jì)算其周圍環(huán)境中的車輛、行人等障礙物中心點(diǎn)，與采樣生成的軌跡簇上的點(diǎn)之間的最短距離，進(jìn)行加權(quán)平均得到A，將其作為障礙物碰撞cost值。

2）到達(dá)cost：計(jì)算車輛距離最終目標(biāo)點(diǎn)之間的距離B作為任務(wù)到達(dá)cost值。

3）側(cè)向偏移cost：Lattice 算法更改采樣間隔所得的一系列路徑點(diǎn)，將其連接為候選軌跡簇，計(jì)算其上的點(diǎn)與改進(jìn)A*算法生成的全局路徑上的點(diǎn)之間的距離C，將其作為側(cè)向偏移cost值。

4）橫向加速度cost：計(jì)算方向盤的轉(zhuǎn)角D 作為橫向加速度cost值。

5）縱向加速度cost：計(jì)算沿軌跡線行駛方向的最大加速度E作為縱向加速度cost值。

將上述5 個(gè)cost評(píng)價(jià)函數(shù)進(jìn)行加權(quán)得到最終的cost值如式（6）所示：

式中，a 為碰撞cost的權(quán)重系數(shù)；b 為到達(dá)cost的權(quán)重系數(shù)；c為側(cè)向偏移cost的權(quán)重系數(shù)；d為橫向加速度cost的權(quán)重系數(shù)；e為縱向加速度cost的權(quán)重系數(shù)。根據(jù)最終的cost值，從軌跡簇中篩選出具有最小代價(jià)值的軌跡，將其作為局部最優(yōu)軌跡。

3.3 算法融合

采用改進(jìn)A*算法生成的全局路徑作為參考線，應(yīng)用于Frenet 坐標(biāo)系下的Lattice 算法實(shí)現(xiàn)，將縱向規(guī)劃和橫向規(guī)劃進(jìn)行解耦，更改采樣時(shí)間間隔，生成一系列縱向和橫向的候選路徑點(diǎn)，然后通過二維運(yùn)動(dòng)合成將其轉(zhuǎn)變?yōu)槎S空間的一系列軌跡點(diǎn)，再結(jié)合動(dòng)態(tài)障礙物信息及周圍環(huán)境道路信息，根據(jù)評(píng)價(jià)函數(shù)選出候選軌跡簇中代價(jià)值最小的候選軌跡作為平滑無(wú)碰撞的局部最優(yōu)軌跡。

4 仿真實(shí)驗(yàn)及結(jié)果分析

本文將改進(jìn)的A*算法和基于Frenet 坐標(biāo)系的Lattice算法融合，將經(jīng)過搜索策略和權(quán)重系數(shù)優(yōu)化改進(jìn)后的A*算法，所得到的全局規(guī)劃靜態(tài)路徑，作為局部規(guī)劃Lattice 算法的參考線。在Frenet 坐標(biāo)系下將縱向和橫向規(guī)劃進(jìn)行解耦，實(shí)現(xiàn)路徑與速度同時(shí)規(guī)劃。根據(jù)參考線和坐標(biāo)變換，得到二維平面內(nèi)的軌跡簇，通過評(píng)價(jià)函數(shù)選出代價(jià)值最小的最優(yōu)局部軌跡。這樣的算法融合方法，將靜態(tài)全局路徑規(guī)劃和動(dòng)態(tài)局部路徑規(guī)劃綜合考慮，可得到更好的汽車行駛軌跡，基于此汽車行駛更安全舒適。無(wú)人駕駛汽車基于融合算法的軌跡規(guī)劃邏輯圖如圖4所示。

圖4 基于融合算法的軌跡規(guī)劃邏輯圖

為驗(yàn)證本算法的有效性，在Matlab2018a 版本中進(jìn)行仿真實(shí)驗(yàn)。在柵格地圖場(chǎng)景（30m×30m，網(wǎng)格間距1m）下進(jìn)行實(shí)驗(yàn)，陰影柵格為障礙物區(qū)域。實(shí)驗(yàn)中，起始點(diǎn)坐標(biāo)為（1，1），目標(biāo)點(diǎn)坐標(biāo)為（27，27），A*算法、改進(jìn)A*算法及融合算法的規(guī)劃路徑圖如圖5、圖6所示。

圖5 A*算法和改進(jìn)A*算法路徑規(guī)劃圖

圖6 融合算法路徑規(guī)劃圖

三種不同算法所得規(guī)劃路徑的曲率如圖7 所示。

圖7 規(guī)劃路徑曲率圖

由圖7可以看出，改進(jìn)A*算法的路徑曲率相較于基本的A*算法雖然在數(shù)值上并沒有減小太多，但是其曲率變化速度明顯改善，有利于無(wú)人駕駛汽車方向盤的穩(wěn)定；融合算法的路徑曲率數(shù)值明顯減小，使得路徑平滑性大大提高，有利于無(wú)人駕駛汽車行駛。

CARLA 作為一個(gè)開源的自動(dòng)駕駛模擬器，可以提供多種開放的城市環(huán)境，包括城市空間的分布、建筑物以及車道和車輛行人等數(shù)字化模型；同時(shí)它還支持多種不同的傳感器組合和不同的天氣條件，對(duì)于研究自動(dòng)駕駛具有重要意義［15］。將融合算法在CARLA中仿真如圖8、圖9所示。

圖8 CARLA模擬環(huán)境下的路徑規(guī)劃圖

圖9 融合算法規(guī)劃所得候選路徑及最優(yōu)路徑

圖9 中藍(lán)色曲線為融合算法規(guī)劃路徑過程中生成的全部候選路徑，紅色曲線為根據(jù)Lattice代價(jià)函數(shù)選出的最優(yōu)局部路徑。

從圖8 可以看出，本文提出的融合算法可以規(guī)劃出，同時(shí)具有穩(wěn)定性、舒適性以及避障性能的車輛行駛軌跡，使汽車在真實(shí)環(huán)境中正常行駛。

5 結(jié)語(yǔ)

針對(duì)A*算法規(guī)劃出的路徑平滑性較差、不滿足無(wú)人駕駛車輛的動(dòng)力學(xué)約束、不具有避免碰撞動(dòng)態(tài)障礙物等問題，本文提出了一種改進(jìn)A*算法和Lattice 算法相融合的路徑規(guī)劃算法，優(yōu)化A*算法的搜索策略和權(quán)重系數(shù)，提高全局路徑規(guī)劃的效率，再將生成的全局靜態(tài)最優(yōu)路徑，作為參考線，應(yīng)用于Lattice 算法，并通過Frenet 坐標(biāo)系進(jìn)行解耦，同時(shí)進(jìn)行速度規(guī)劃和路徑規(guī)劃，采樣生成一系列候選軌跡點(diǎn)，結(jié)合動(dòng)態(tài)障礙物信息以及其他道路信息，根據(jù)候選軌跡代價(jià)值大小，選出平滑無(wú)碰撞的最優(yōu)軌跡。