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

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

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

1 引言

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

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

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

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

2 改進的人工勢場路徑規(guī)劃算法

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

人工勢場算法的基本思想是將空間中被控對象的運動虛擬為虛擬力場中粒子的運動。目標點吸引被控對象，障礙物區(qū)域?qū)Ρ豢貙ο螽a(chǎn)生斥力作用，被控物體在二者的合力作用下移動。

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

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

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

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

式中，Vg是被控對象指向目標點的單位向量。

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

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

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

式中，Vob為被控對象指向障礙點的方向向量。

綜上所述，可以計算存在n 個障礙點時，被控對象收到斥力合力表示為式（6）：

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

1）目標無法接近的情況

當目標點附近有障礙物時，被控對象在目標點受到的斥力勢場超過目標點的引力勢場，導(dǎo)致被控對象無法接近目標點。

2）目標停止的情況

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

2.2 改進的人工勢場算法

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

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

式（7）可化簡為

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

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

改進后的斥力勢場可表示為式（9）：

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

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

當空間中存在n 個有效障礙物時，被控對象的合力可表示為式（12）：

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

2.3 基于障礙點的被控目標建模

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

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

圖3 橢圓勢場示意圖

式中ktruck為障礙車輛勢場剛度，表示第i個障礙的幾何中心坐標。

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

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

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

1）車道保持約束

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

2）最短路徑原則

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

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

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

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

4）路徑航向約束

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

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

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

3 仿真分析

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

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

圖4 不同速度下的實際運動路徑

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

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

4 結(jié)語

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