基于改進(jìn)人工勢(shì)場(chǎng)算法的自主車輛局部路徑規(guī)劃方法研究*

王翔昌 吳訓(xùn)成 張偉偉 王鴻超

（上海工程技術(shù)大學(xué) 上海 201620）

1 引言

近年來，智能輔助駕駛隨著汽車技術(shù)的發(fā)展而逐漸普及，自主車輛局部路徑規(guī)劃和動(dòng)態(tài)避障作為智能駕駛的重要組成部分一直是專家學(xué)者的研究熱點(diǎn)。自主車輛的局部路徑規(guī)劃通過車載傳感器進(jìn)行道路環(huán)境信息感知，通過系統(tǒng)決策進(jìn)行車輛的行為規(guī)劃，完成從目標(biāo)起點(diǎn)至目標(biāo)終點(diǎn)路徑規(guī)劃［1］，并通過車輛控制器完成估計(jì)的跟蹤。程虹霞等通過對(duì)超聲波傳感器、紅外傳感器等進(jìn)行傳感器融合系統(tǒng)建立，研究多傳感器信息融合技術(shù)在無人駕駛車輛以及機(jī)器人中自主避障行為中的應(yīng)用［2］。張瑜等結(jié)合無人駕駛避障系統(tǒng)的應(yīng)用需求，將多傳感器數(shù)據(jù)融合技術(shù)應(yīng)用于無人駕駛避障導(dǎo)航控制系統(tǒng)，改進(jìn)了基于模糊神經(jīng)網(wǎng)絡(luò)的人車路徑規(guī)劃避障導(dǎo)航算法［3］。宋建輝等提出了一種基于BP（Back Propagation）神經(jīng)網(wǎng)絡(luò)的無人車避障技術(shù)。建立了以BP神經(jīng)網(wǎng)絡(luò)作為環(huán)境信息識(shí)別模型的無人車輛避障方法［4］。遺傳算法作為一種通過模擬自然過程搜索最優(yōu)解的方法常用于局部路徑規(guī)劃［5］。近年來隨著計(jì)算機(jī)圖形處理器（GPU）計(jì)算能力的增強(qiáng)，基于卷積神經(jīng)網(wǎng)絡(luò)［6］的路徑規(guī)劃算法也逐漸被重視起來。相比于兩外兩種算法，人工勢(shì)場(chǎng)算法不需要進(jìn)行全區(qū)域搜索，路徑規(guī)劃速度快，執(zhí)行效率高，有較高的精確度和實(shí)時(shí)性，滿足車載計(jì)算平臺(tái)的需求。張建英等通過在附加條件上增加控制力來建立相同直線方向上的重力和排斥力動(dòng)作線，以執(zhí)行機(jī)器人的局部路徑規(guī)劃［7］。張琦等學(xué)者將障礙速度和加速度矢量引入了足球機(jī)器人避障控制［8］。在文獻(xiàn)［9］中，作者從車輛的避障的實(shí)際情況考慮，針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法的不足，通過引入速度矢量，結(jié)合模糊控制方法，實(shí)時(shí)調(diào)節(jié)斥力場(chǎng)系數(shù)，進(jìn)行無人車輛局部路徑規(guī)劃方法研究。譚寶成等［10］根據(jù)傳統(tǒng)人工勢(shì)場(chǎng)路徑規(guī)劃中存在的避障較大太大問題提出了隨機(jī)改變目標(biāo)點(diǎn)的方法，并在此基礎(chǔ)上避免安全避障中的圓形虛擬力場(chǎng)范圍。LaValle等在RRT 的基礎(chǔ)上提出了連接型RRT算法［11］，該算法通過擴(kuò)展隨機(jī)樹的進(jìn)行雙向搜索，提高傳統(tǒng)RRT 的搜索效率。Sangwoo Moon 等基于虛擬人工勢(shì)場(chǎng)算法實(shí)現(xiàn)了在未知環(huán)境的動(dòng)態(tài)路徑規(guī)劃，結(jié)合了車輛的位置和方位信息［12］。Carvalho等基于模型預(yù)測(cè)控制MPC 進(jìn)行車輛的局部路徑規(guī)劃［13］，模型預(yù)測(cè)控制算法能夠協(xié)調(diào)系統(tǒng)約束和系統(tǒng)的非線性問題。Rui Oliveira等提出了一種基于“晶格”的路徑規(guī)劃及其優(yōu)化方法［14］，該方法能狗為自動(dòng)駕駛卡車提供實(shí)時(shí)軌跡規(guī)劃。Chu 等人基于最優(yōu)控制方法提出了一種局部路徑規(guī)劃算法［15］。該算法在滿足車輛橫向、縱向等約束下進(jìn)行局部路徑規(guī)劃。

為了規(guī)劃自主車輛的最優(yōu)路徑，實(shí)現(xiàn)對(duì)自主車輛的準(zhǔn)確平滑控制，本文將對(duì)自主車輛的軌跡規(guī)劃跟蹤控制分為環(huán)境感知、系統(tǒng)決策、路徑規(guī)劃和路徑跟蹤四個(gè)部分，不同部分依賴相應(yīng)的車載設(shè)備進(jìn)行處理，如圖1所示。

圖1 路徑規(guī)劃與跟蹤流程

環(huán)境感知部分由車載傳感器收集和處理道路以及障礙物的信息；系統(tǒng)決策部分處理收集到的信息，根據(jù)駕駛行為決策對(duì)障礙進(jìn)行分析判斷，完成基于目標(biāo)點(diǎn)、障礙點(diǎn)和目標(biāo)點(diǎn)模型建立，確定相對(duì)位置。路徑規(guī)劃部分采用改進(jìn)的人工勢(shì)場(chǎng)算法，根據(jù)所建立的障礙點(diǎn)、目標(biāo)點(diǎn)模型和車輛行駛狀態(tài)進(jìn)行避障路徑規(guī)劃。在完成道路感知、行為決策以及路徑規(guī)劃后，路徑跟蹤執(zhí)行系統(tǒng)發(fā)出的指令進(jìn)行車輛轉(zhuǎn)向以及加速制動(dòng)控制，實(shí)現(xiàn)自主車輛的軌跡跟蹤。

2 改進(jìn)的人工勢(shì)場(chǎng)路徑規(guī)劃算法

2.1 傳統(tǒng)的人工勢(shì)場(chǎng)算法原理

人工勢(shì)場(chǎng)算法的基本思想是將空間中被控對(duì)象的運(yùn)動(dòng)虛擬為虛擬力場(chǎng)中粒子的運(yùn)動(dòng)。目標(biāo)點(diǎn)吸引被控對(duì)象，障礙物區(qū)域?qū)Ρ豢貙?duì)象產(chǎn)生斥力作用，被控物體在二者的合力作用下移動(dòng)。

圖2 傳統(tǒng)人工勢(shì)場(chǎng)算法原理

在二維空間中將被控對(duì)象假定為質(zhì)點(diǎn)并設(shè)定其坐標(biāo)為X=(x，y) ，目標(biāo)點(diǎn)坐標(biāo)為Xg=(xg，yg) 。根據(jù)傳統(tǒng)人工勢(shì)場(chǎng)算法的二力合成原理進(jìn)行被控對(duì)象的引力勢(shì)場(chǎng)Uatt和斥力勢(shì)場(chǎng)Urep。

式中，k為引力場(chǎng)系數(shù)；X-Xg是被控對(duì)象到目標(biāo)點(diǎn)的距離，用ρg表示，ρg可以通過式（2）進(jìn)行計(jì)算。

據(jù)此可以得到被控對(duì)象的引力如式（3）所示：

式中，Vg是被控對(duì)象指向目標(biāo)點(diǎn)的單位向量。

若在障礙物存在的情況下，假設(shè)其歐式空間中的坐標(biāo)位置為Xob=(xob，yob)，改障礙點(diǎn)對(duì)被控對(duì)象的斥力勢(shì)場(chǎng)函數(shù)可由式（4）計(jì)算：

式中η和ρo為斥力勢(shì)場(chǎng)的作用系數(shù)和距離范圍；ρob是被控對(duì)象與障礙點(diǎn)之間的距離。

根據(jù)斥力勢(shì)場(chǎng)函數(shù)，當(dāng)ρob≤ρo，控制對(duì)象的斥力通過式（5）給出：

式中，Vob為被控對(duì)象指向障礙點(diǎn)的方向向量。

綜上所述，可以計(jì)算存在n 個(gè)障礙點(diǎn)時(shí)，被控對(duì)象收到斥力合力表示為式（6）：

通過對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)算法進(jìn)行分析，我們可以發(fā)現(xiàn)，傳統(tǒng)人工勢(shì)場(chǎng)算法僅是對(duì)作用在被控對(duì)象上的力進(jìn)行矢量合成，存在一些不足。

1）目標(biāo)無法接近的情況

當(dāng)目標(biāo)點(diǎn)附近有障礙物時(shí)，被控對(duì)象在目標(biāo)點(diǎn)受到的斥力勢(shì)場(chǎng)超過目標(biāo)點(diǎn)的引力勢(shì)場(chǎng)，導(dǎo)致被控對(duì)象無法接近目標(biāo)點(diǎn)。

2）目標(biāo)停止的情況

當(dāng)被控對(duì)象在同一點(diǎn)所受到的障礙物斥力和目標(biāo)點(diǎn)引力大小相等，方向相反時(shí)，被控對(duì)象處于動(dòng)態(tài)平衡的狀態(tài)，出現(xiàn)被控目標(biāo)停止的情況。

2.2 改進(jìn)的人工勢(shì)場(chǎng)算法

針對(duì)以上問題，本文通過引入調(diào)和函數(shù)來計(jì)算障礙物斥力與目標(biāo)點(diǎn)引力，從而解決目標(biāo)遙不可及或者無法靠近的情況。

調(diào)和函數(shù)是二階連續(xù)可導(dǎo)且滿足拉普拉斯方程，即f:U→R，其中U滿足式（7）中的方程且為Rn的開子集：

式（7）可化簡(jiǎn)為

式中的Δ 表示拉普拉斯算子。

因此，可將障礙物勢(shì)場(chǎng)函數(shù)換用調(diào)和函數(shù)來表達(dá)，因其滿足極大值存在于障礙物的邊界上，所以能夠使其免陷入極值。

改進(jìn)后的斥力勢(shì)場(chǎng)可表示為式（9）：

式中，m是一個(gè)大于零的實(shí)數(shù)。

修正的斥力表達(dá)式為可由式（10）和式（11）給出：

當(dāng)空間中存在n 個(gè)有效障礙物時(shí)，被控對(duì)象的合力可表示為式（12）：

據(jù)此開支，通過引入調(diào)和函數(shù)，能夠有效的在車輛路徑規(guī)劃中的運(yùn)用人工勢(shì)場(chǎng)法。

2.3 基于障礙點(diǎn)的被控目標(biāo)建模

本文基于改進(jìn)人工勢(shì)場(chǎng)算法進(jìn)行被控目標(biāo)建模，首先進(jìn)行障礙物橢圓勢(shì)場(chǎng)的建立，完成目標(biāo)點(diǎn)基于障礙點(diǎn)的目標(biāo)建模；然后基于自然三次樣條曲線和全局優(yōu)化在滿足車道保持約束、最短路徑原則、最優(yōu)曲率原則和路徑航向約束的前提下進(jìn)行路徑規(guī)劃。

本文通過建立橢圓勢(shì)場(chǎng)區(qū)域來調(diào)整障礙車輛的勢(shì)場(chǎng)作用區(qū)域，能夠在考慮障礙車輛車身形狀的情況下區(qū)別于傳統(tǒng)圓形勢(shì)場(chǎng)。橢圓勢(shì)場(chǎng)的示意圖如圖3 所示，以障礙車輛的幾何中心為橢圓勢(shì)場(chǎng)的圓心，長(zhǎng)短軸分別為a和b。障礙勢(shì)場(chǎng)Utruck可用式（13）來表示：

圖3 橢圓勢(shì)場(chǎng)示意圖

式中ktruck為障礙車輛勢(shì)場(chǎng)剛度，表示第i個(gè)障礙的幾何中心坐標(biāo)。

本節(jié)通過考慮車身形狀在傳統(tǒng)圓形勢(shì)場(chǎng)建立“橢圓勢(shì)場(chǎng)”調(diào)整障礙車輛勢(shì)場(chǎng)的作用區(qū)域，利用橢圓勢(shì)場(chǎng)進(jìn)行路徑規(guī)劃能夠使規(guī)劃的路徑過渡更自然，曲率變化曲線更平緩。

2.4 基于自然三次樣條曲線和全局優(yōu)化的路徑規(guī)劃

本節(jié)在前節(jié)改進(jìn)人工勢(shì)場(chǎng)算法的基礎(chǔ)上，通過引入車道保持約束、最短路徑原則、最優(yōu)曲率原則和路徑航向約束等約束，使得生成的路徑盡可能地滿足駕駛平順性、曲率最小等要求。

1）車道保持約束

車道保持約束作為路徑規(guī)劃中首要被考慮的約束指標(biāo)，是避免規(guī)劃的路徑與車道發(fā)生過度偏離的關(guān)鍵約束。本文通過假設(shè)測(cè)試車輛行駛在與車道中心線的距離為L(zhǎng)d道路下，車道保持約束可用式（14）來描述：

2）最短路徑原則

規(guī)劃路徑應(yīng)盡可能再保證車輛安全的情況下通過減少路徑長(zhǎng)度來提高卡車運(yùn)輸效率，減少運(yùn)輸成本。本文通過計(jì)算相鄰控制節(jié)點(diǎn)的路徑長(zhǎng)度變化量來計(jì)算最短路徑：

3）最優(yōu)曲率原則

大曲率意味著車輛在行駛過程中會(huì)有較大程度的轉(zhuǎn)向角度變化，這對(duì)于車輛行駛是及其危險(xiǎn)的因素，因此在路徑規(guī)劃時(shí)要滿足最優(yōu)曲率原則，且曲率變化在趨于平緩，過渡自然。本文中將路徑簡(jiǎn)化為由若干分段式自然三次樣條曲線連接各控制節(jié)點(diǎn)組成的，則有：

于是其路徑曲率的二次式即為

4）路徑航向約束

路徑航向約束是指車輛的路徑規(guī)劃滿足于車道中心線航向角度相同，避免車輛偏離車道，本文通過控制相鄰節(jié)點(diǎn)間的航向角來實(shí)現(xiàn)路徑航向約束。相鄰控制節(jié)點(diǎn)間的航向角可用式（19）來表示，即：

從本文選擇的四個(gè)約束原則可以看出，路徑規(guī)劃應(yīng)滿足這四個(gè)約束來確保所規(guī)劃路徑的合理與安全。因此，可將該問題轉(zhuǎn)為為一個(gè)多目標(biāo)優(yōu)化問題。考慮安全因素，在上述四個(gè)多目標(biāo)優(yōu)化問題，本文的優(yōu)化目標(biāo)是使計(jì)劃路徑形狀盡可能流暢自然，同時(shí)也確保計(jì)劃路徑保持在一定的距離盡可能中心線的道路。在這個(gè)范圍內(nèi)，路徑長(zhǎng)度盡可能短，并且路徑朝向盡可能與道路的中心線一致。換句話說，它希望在相應(yīng)的計(jì)劃節(jié)點(diǎn)，路徑的曲率變化的總合是最小的，同時(shí)，累積的和剩下的三個(gè)約束條件的變化（車道保持、最短路徑和路徑標(biāo)題）也計(jì)劃節(jié)點(diǎn)的最小，因此有：

上式中I為控制變量，它包含以上四個(gè)約束表達(dá)式中的參數(shù)。

3 仿真分析

本節(jié)利用Prescan 軟件搭建測(cè)試環(huán)境，同時(shí)結(jié)合Matlab/Simulink 軟件對(duì)基于人工勢(shì)場(chǎng)的路徑規(guī)劃算法編程控制進(jìn)行了改進(jìn)，完成本文提出的基于改進(jìn)人工勢(shì)場(chǎng)算法路徑規(guī)劃方法性能驗(yàn)證。

單向車道寬度為3.5m 的雙車道道路被選中作為車輛測(cè)試道路，在Prescan 中進(jìn)行相關(guān)道路信息的設(shè)置并將相應(yīng)的環(huán)境感知傳感器添加到自主車輛控制和動(dòng)態(tài)模型中。

圖4 不同速度下的實(shí)際運(yùn)動(dòng)路徑

設(shè)定智能車恒速行駛36km/h、54km/h 和72km/h，通過車載感知傳感器進(jìn)行障礙物的檢測(cè)。在檢測(cè)遇到障礙物后，通過局部路徑規(guī)劃系統(tǒng)進(jìn)行路徑規(guī)劃和車道變換，完成車輛避障，得到路徑如下仿真結(jié)果。

通過添加橢圓人工勢(shì)場(chǎng)進(jìn)行障礙點(diǎn)的勢(shì)場(chǎng)建立改進(jìn)，并通過引入四個(gè)約束基于自然樣條曲線進(jìn)行路徑規(guī)劃和全局約束，所規(guī)劃的自主車輛的移動(dòng)路徑與目標(biāo)路徑基本一致，且車輛軌跡的曲率比那花十分平順，未出現(xiàn)大曲率路徑的現(xiàn)象。表明所提的基于改進(jìn)人工勢(shì)場(chǎng)的局部路徑規(guī)劃能夠滿足平滑變道與平緩駕駛的要求。

4 結(jié)語(yǔ)

本文提出了一種基于改進(jìn)人工勢(shì)場(chǎng)算法的局部路徑規(guī)劃算法，引入了橢圓勢(shì)場(chǎng)調(diào)整障礙車輛勢(shì)場(chǎng)的作用，建立了對(duì)稱點(diǎn)障礙模型的方法保證了局部目標(biāo)點(diǎn)在對(duì)稱軸上。仿真結(jié)果表明，本文提出的路徑規(guī)劃方法能有效地規(guī)劃一條曲率平滑的路徑，且滿足自主車輛行駛控制的需求，車輛行駛安全平穩(wěn)，解決了傳統(tǒng)人工勢(shì)場(chǎng)算法存在的無法接近問題，具有較好的魯棒性。