基于改進(jìn)人工勢(shì)場法的無人車路徑規(guī)劃算法

羅潔，王中訓(xùn)，潘康路，盧中原，劉言

（煙臺(tái)大學(xué)光電信息科學(xué)技術(shù)學(xué)院，山東煙臺(tái) 264005）

隨著無人車技術(shù)的發(fā)展，在多個(gè)場景中，無人車都得到了廣泛的應(yīng)用，但障礙物的出現(xiàn)會(huì)阻礙無人車的工作。因此，如何做到有效規(guī)避障礙物是無人駕駛路徑規(guī)劃技術(shù)方面永恒不變的話題。

為了使無人車在有障礙物環(huán)境下實(shí)現(xiàn)無碰撞路徑規(guī)劃，目前比較主流的全局路徑規(guī)劃[1]算法有Dijkstra算法[2]、A*算法[3]、蟻群算法[4]、遺傳算法[5]以及強(qiáng)化學(xué)習(xí)[6]等。全局路徑規(guī)劃算法需要掌握全局的環(huán)境信息，再根據(jù)環(huán)境信息作出路徑規(guī)劃，缺乏對(duì)未知環(huán)境的適應(yīng)性。局部路徑規(guī)劃[7]算法有DWA 算法[8]、人工勢(shì)場法[9]等。局部路徑規(guī)劃算法存在不能保證最優(yōu)解的情況，容易陷入局部最優(yōu)。并且上述的方法大多適用于靜態(tài)環(huán)境，不適用于動(dòng)態(tài)環(huán)境。

因此，為了實(shí)現(xiàn)無人車在未知?jiǎng)討B(tài)環(huán)境中的路徑規(guī)劃，該文提出了基于改進(jìn)的人工勢(shì)場法來實(shí)現(xiàn)局部在線路徑規(guī)劃，并且結(jié)合LSTM 的Q-Learning 強(qiáng)化學(xué)習(xí)算法來規(guī)避動(dòng)態(tài)障礙物。該文創(chuàng)新性地提出了虛擬勢(shì)場檢測(cè)圓模型，根據(jù)圓上的虛擬勢(shì)場檢測(cè)點(diǎn)與障礙物斥力場接觸的分布信息和數(shù)量信息來檢測(cè)“最小值陷阱[10]”，并改變無人車運(yùn)動(dòng)狀態(tài)實(shí)現(xiàn)有效規(guī)避。另外檢測(cè)點(diǎn)信息可作為LSTM 的信息輸入，利用LSTM 處理時(shí)序數(shù)據(jù)的特性，獲取障礙物的運(yùn)動(dòng)預(yù)測(cè)狀態(tài)空間，以此實(shí)現(xiàn)無人車在未知?jiǎng)討B(tài)環(huán)境中的無碰撞路徑規(guī)劃。

1 人工勢(shì)場法

作為一種經(jīng)典的路徑規(guī)劃算法，人工勢(shì)場法被廣泛地用于局部路徑規(guī)劃。人工勢(shì)場法是在無人車工作場景中人工模擬出一個(gè)勢(shì)場，類比于電磁場，無人車受吸力和斥力的影響，根據(jù)勢(shì)場的特性來規(guī)劃路徑的方法。無人車如果要從一個(gè)地點(diǎn)到另一個(gè)指定目標(biāo)地點(diǎn)，可以在目標(biāo)地點(diǎn)增加一個(gè)對(duì)無人車的吸力場；同理，要避開障礙物，就需要在檢測(cè)到的障礙物及其周圍加一個(gè)隨距離增大而衰減的斥力場，賦予無人車規(guī)避障礙物的能力。以此建立一個(gè)人工勢(shì)場，無人車沿著勢(shì)場行進(jìn)，最后到達(dá)目標(biāo)點(diǎn)，如圖1所示。

圖1 人工勢(shì)場中無人車的勢(shì)場分析

人工勢(shì)場法的引力勢(shì)場函數(shù)一般形式為：

式中，ka是一個(gè)引力增益系數(shù)，xn-xg是當(dāng)前位置與目標(biāo)點(diǎn)的距離。

障礙物的斥力勢(shì)場函數(shù)一般形式為：

式中，kr是一個(gè)斥力增益系數(shù)，r0為斥力作用范圍，r為無人車距離障礙物斥力場中心的距離。

人工勢(shì)場法與其他經(jīng)典避障算法相比有很多突出優(yōu)點(diǎn)，比如：計(jì)算量小、可以解決局部避障問題、可以解決突發(fā)威脅等。因此，該算法被廣泛用于避障算法中。

但人工勢(shì)場法也有很明顯的缺點(diǎn)。無人車依靠從各個(gè)方向檢測(cè)到的勢(shì)場的疊加獲得合勢(shì)場，以合勢(shì)場[11]的方向和大小來判斷接下來運(yùn)動(dòng)軌跡。但如果出現(xiàn)合勢(shì)場近似為0 的情況，那么無人車就不會(huì)移動(dòng)，停滯不前。區(qū)域內(nèi)障礙物越多，出現(xiàn)合勢(shì)場為0 的情況的概率越高，越容易停滯不前。這種現(xiàn)象被稱為“最小值陷阱”。

2 改進(jìn)的人工勢(shì)場法

2.1 “最小值陷阱”

在傳統(tǒng)人工勢(shì)場法的基礎(chǔ)上，文中提出了虛擬勢(shì)場檢測(cè)圓模型。該圓由N個(gè)虛擬檢測(cè)點(diǎn)以無人車K為中心，以R為半徑，形成虛擬檢測(cè)圓T。每個(gè)點(diǎn)均可檢測(cè)到障礙物斥力場，并且隨著無人車對(duì)障礙物的靠近，圓T與障礙物斥力場接觸的點(diǎn)的數(shù)量n會(huì)呈正態(tài)分布，如圖2 所示。

圖2 虛擬勢(shì)場檢測(cè)圓模型及接觸點(diǎn)分布圖

當(dāng)檢測(cè)圓T上出現(xiàn)兩個(gè)離散的正態(tài)分布，并且推算出來的障礙物之間的距離d1，2小于安全距離dsafe，dsafe為可能形成最小值陷阱的兩個(gè)障礙物之間的距離，如圖3 所示，則可預(yù)測(cè)為“最小值陷阱”，需滿足的以下兩個(gè)條件如式（3）所示：

圖3 有兩處障礙物的“最小值陷阱”示意圖

2.2 規(guī)避“最小值陷阱”

2.2.1 建立無人車運(yùn)動(dòng)學(xué)模型

人工勢(shì)場法應(yīng)用于無人車的路徑規(guī)劃，為了保證無人車運(yùn)動(dòng)的穩(wěn)定性，需要考慮無人車自身運(yùn)動(dòng)學(xué)約束。該研究采用自行車運(yùn)動(dòng)學(xué)模型[12]來簡化解讀，如圖4 所示。

圖4 自行車運(yùn)動(dòng)學(xué)模型

簡化后得式（4）：

式中，δ為前輪轉(zhuǎn)向角，L為軸距，R為無人車在該轉(zhuǎn)向角下運(yùn)動(dòng)形成圓的半徑。

由曲率半徑R、角速度ω與速度v之間的等式：v=ωR，可得無人車的運(yùn)動(dòng)學(xué)模型如式（5）所示：

其中，v為無人車的速度，x˙為無人車在世界坐標(biāo)系中X軸方向上的分速度，y˙為無人車在世界坐標(biāo)系中Y軸方向上的分速度，θ·為無人車在世界坐標(biāo)中的航向角，θ為無人車的角速度。

2.2.2 更新航向角

設(shè)檢測(cè)圓與兩個(gè)障礙物斥力場的兩個(gè)外側(cè)接觸點(diǎn)為a1、b1。兩個(gè)點(diǎn)與無人車K以及目標(biāo)點(diǎn)X形成兩個(gè)夾角α和β，取其中最小角度作為無人車K的更新航向角，如圖5 所示。即：θ=min(α，β)，由此以較短路徑駛離“最小值陷阱”。

圖5 無人車更新航向角示意圖

3 基于LSTM改進(jìn)的強(qiáng)化學(xué)習(xí)動(dòng)態(tài)環(huán)境路徑規(guī)劃算法

3.1 LSTM

LSTM[13]是長短時(shí)記憶網(wǎng)絡(luò)，由RNN（循環(huán)神經(jīng)網(wǎng)絡(luò)）[14]發(fā)展而來，與RNN 相比，具有良好的記憶能力，在解決傳統(tǒng)RNN 難以解決的梯度消失和長依賴問題上有顯著作用。因此LSTM 成為了一種應(yīng)用廣泛的神經(jīng)網(wǎng)絡(luò)。

經(jīng)典LSTM 的公式為：

其中，式（6）是遺忘門，式（7）～（8）是輸入門，式（9）是輸出門，通過上式可以畫出如圖6 所示的LSTM 結(jié)構(gòu)圖。

圖6 LSTM結(jié)構(gòu)圖

與傳統(tǒng)RNN 相比，LSTM 最大的特色是增加了Ct-1和Ct，它們被稱為細(xì)胞狀態(tài)。細(xì)胞狀態(tài)在每次迭代過程中都會(huì)進(jìn)行一次更新，將歷次最優(yōu)結(jié)果保留下來，形成長期記憶。這樣就可以使用較長的歷史信息去預(yù)測(cè)未來的情況，與短期的歷史信息配合，形成一個(gè)長短時(shí)記憶網(wǎng)絡(luò)，提高無人車的避障學(xué)習(xí)能力。避免由于短期記憶的偏差對(duì)神經(jīng)網(wǎng)絡(luò)整體的影響，提高了系統(tǒng)的抗干擾能力。

3.2 結(jié)合LSTM的Q-Learning強(qiáng)化學(xué)習(xí)算法

改進(jìn)的人工勢(shì)場法雖然能夠有效地避開“最小值陷阱”，但是對(duì)未知的動(dòng)態(tài)環(huán)境，具有比較差的適應(yīng)性。原因在于虛擬勢(shì)場檢測(cè)圓模型無法預(yù)測(cè)障礙物的運(yùn)動(dòng)狀態(tài)，并且針對(duì)移動(dòng)速度較快的障礙物，該模型容易造成信息缺失。針對(duì)該問題，提出了LSTM循環(huán)神經(jīng)網(wǎng)絡(luò)結(jié)合Q-Learning 的強(qiáng)化學(xué)習(xí)算法結(jié)構(gòu)來優(yōu)化虛擬勢(shì)場檢測(cè)圓模型。

首先利用LSTM 循環(huán)神經(jīng)網(wǎng)絡(luò)來處理虛擬勢(shì)場檢測(cè)圓模型返回的障礙物斥力場變化信息，利用LSTM 算法結(jié)合前序檢測(cè)信息來生成當(dāng)前時(shí)刻的輸出，當(dāng)前時(shí)刻的輸出參考之前時(shí)刻信息的變化趨勢(shì)的特點(diǎn)[15]，來預(yù)測(cè)障礙物的運(yùn)動(dòng)狀態(tài)，進(jìn)一步得到障礙物的預(yù)測(cè)狀態(tài)空間SO，并且調(diào)整虛擬勢(shì)場檢測(cè)圓模型的半徑R，實(shí)現(xiàn)跟蹤動(dòng)態(tài)障礙物的目的，實(shí)現(xiàn)步驟如下：

步驟1：構(gòu)建可調(diào)半徑R的虛擬勢(shì)場檢測(cè)圓模型。

步驟2：獲取圓T與障礙物斥力場接觸的點(diǎn)的數(shù)量n分布信息和數(shù)量信息作為LSTM 的輸入。

步驟3：獲取障礙物預(yù)測(cè)狀態(tài)空間SO。

步驟4：結(jié)合預(yù)測(cè)狀態(tài)空間SO的空間位置和無人車的狀態(tài)來調(diào)整半徑R。

其中獲取的障礙物預(yù)測(cè)狀態(tài)空間SO添加到QLearning 中的無人車狀態(tài)空間S，重新設(shè)置回報(bào)函數(shù)R，使無人車以最佳的回報(bào)來躲避動(dòng)態(tài)障礙物。

式（11）中，v·Δt代表小車沿當(dāng)前航向角繼續(xù)以速度v行駛。該設(shè)計(jì)添加無人車預(yù)測(cè)狀態(tài)重新設(shè)置回報(bào)函數(shù)[16]。當(dāng)無人車與障礙物中心距離大于無人車預(yù)測(cè)距離，則設(shè)置reward=-1，給予正回報(bào)。反之，則預(yù)測(cè)為“相撞”，給予負(fù)回報(bào)來使無人車更改方向，遠(yuǎn)離該預(yù)測(cè)區(qū)域。

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

4.1 實(shí)驗(yàn)環(huán)境和模型搭建

實(shí)驗(yàn)搭建時(shí)用Python構(gòu)建出一個(gè)120 m×120 m二維地圖?？臻g內(nèi)隨機(jī)設(shè)置障礙物個(gè)數(shù)N0∈(20，50)，設(shè)置單個(gè)障礙物的斥力場影響范圍r=5 m。并對(duì)每個(gè)障礙物添加隨機(jī)擾動(dòng)因素，令障礙物以不規(guī)則運(yùn)動(dòng)模式運(yùn)動(dòng)。為方便結(jié)果觀察，實(shí)驗(yàn)添加了固定和移動(dòng)的“最小值陷阱”。構(gòu)建虛擬勢(shì)場檢測(cè)圓模型，圓模型檢測(cè)點(diǎn)數(shù)N與其半徑R的函數(shù)關(guān)系式為：如圖2 所示均勻分布在圓周。仿真結(jié)果如圖7-8 所示，圖中，點(diǎn)(0，0)為無人車起點(diǎn)；點(diǎn)(120，120)為無人車目標(biāo)點(diǎn)；圓球代表障礙物及其影響范圍；實(shí)線代表無人車運(yùn)動(dòng)軌跡。

圖7 傳統(tǒng)勢(shì)場法

圖8 改進(jìn)的人工勢(shì)場法

4.2 改進(jìn)的人工勢(shì)場法對(duì)比

分析圖7-8 可知，在傳統(tǒng)人工勢(shì)場法條件下，無人車在遇到“最小值陷阱”時(shí)，出現(xiàn)了來回振蕩的情況。在增加虛擬勢(shì)場檢測(cè)圓模型后的改進(jìn)的人工勢(shì)場法條件下，可以看出，面臨“最小值陷阱”，無人車做到提前感知，并且有效規(guī)避了該陷阱，驗(yàn)證了該方法的有效性。

4.3 基于LSTM Q-Learning人工勢(shì)場法對(duì)比

將固定半徑R的虛擬勢(shì)場檢測(cè)圓模型放置于動(dòng)態(tài)障礙物環(huán)境中，發(fā)現(xiàn)無人車對(duì)于不規(guī)則運(yùn)動(dòng)或者高速運(yùn)動(dòng)的障礙物，會(huì)出現(xiàn)來不及規(guī)避而相撞的情況。針對(duì)這一情況該文利用LSTM Q-Learning 算法提出新的可調(diào)半徑R的虛擬勢(shì)場檢測(cè)圓模型來跟蹤和預(yù)測(cè)障礙物的空間狀態(tài)，做到有效規(guī)避。

由圖9 可知，為障礙物添加隨機(jī)擾動(dòng)因素后，對(duì)移動(dòng)速度較快的障礙物，無人車會(huì)出現(xiàn)來不及躲避的情況，并與之相撞。并且經(jīng)過多次實(shí)驗(yàn)得出，二者相撞的概率為78%。由圖10 可知，模型經(jīng)過改進(jìn)后，無人車能夠規(guī)避高速障礙物，并且路線中有較為明顯的預(yù)判行為，證實(shí)了該方法的可行性。

圖9 原虛擬勢(shì)場檢測(cè)圓模型

圖10 改進(jìn)的虛擬勢(shì)場檢測(cè)圓模型

表1 數(shù)據(jù)中，信息缺失率以模型上的檢測(cè)點(diǎn)的坐標(biāo)與障礙物斥力場的坐標(biāo)重合后，第二次獲取的坐標(biāo)信息不符合無人車與障礙物的運(yùn)動(dòng)情況為準(zhǔn)。規(guī)避成功率以無人車做出規(guī)避動(dòng)作，并觀察是否成功規(guī)避為準(zhǔn)。分析表1數(shù)據(jù)可知，基于LSTM Q-Learning算法提出的改進(jìn)模型在信息缺失率和規(guī)避成功率方面均有較好的提升。

表1 模型改進(jìn)前后效果對(duì)比

仿真實(shí)驗(yàn)結(jié)果表明，傳統(tǒng)人工勢(shì)場法利用斥力場和引力場相互作用來控制無人車，當(dāng)無人車遇到“最小值陷阱”時(shí)，無人車常出現(xiàn)“卡死”和振蕩的情況，而通過增加虛擬勢(shì)場檢測(cè)圓模型，無人車提前感知到障礙物，并且能夠?qū)Α白钚≈迪葳濉弊龀鲇行б?guī)避。

同時(shí)在動(dòng)態(tài)環(huán)境中，固定半徑R的虛擬勢(shì)場檢測(cè)圓模型對(duì)以不規(guī)則運(yùn)動(dòng)的障礙物和高速運(yùn)動(dòng)障礙物無法做到準(zhǔn)確躲避，甚至相撞，并出現(xiàn)一定的信息缺失的情況。而通過LSTM Q-Learning 算法對(duì)障礙物運(yùn)動(dòng)狀態(tài)進(jìn)行預(yù)測(cè)，并將虛擬勢(shì)場檢測(cè)圓模型的半徑R設(shè)置為可調(diào)，以達(dá)到一定程度的跟隨障礙物，避免信息缺失。由結(jié)果可知，無人車能夠順利躲避動(dòng)態(tài)障礙物，做到無碰撞路徑規(guī)劃。證明了該算法的有效性。

5 結(jié)束語

針對(duì)傳統(tǒng)人工勢(shì)場法存在的“最小值陷阱”問題，文中提出了虛擬勢(shì)場檢測(cè)圓模型。利用檢測(cè)點(diǎn)返回的障礙物斥力場信息來提前感知到潛在的“最小值陷阱”，并且根據(jù)模型上的接觸點(diǎn)，更新無人車的航向角，實(shí)現(xiàn)以較短路徑做到有效規(guī)避。同時(shí)針對(duì)動(dòng)態(tài)環(huán)境，該文提出LSTM Q-Learning 算法來獲取障礙物的預(yù)測(cè)運(yùn)動(dòng)狀態(tài)空間，并且針對(duì)固定半徑R的虛擬勢(shì)場檢測(cè)圓模型在該環(huán)境下的信息缺失情況，提出可調(diào)半徑R的改進(jìn)模型，使無人車在動(dòng)態(tài)環(huán)境中實(shí)現(xiàn)無碰撞的路徑規(guī)劃。利用Python 對(duì)上述兩種方法進(jìn)行了仿真，結(jié)果證明了其有效性和可行性。