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

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

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

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

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

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

1 人工勢場法

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

圖1 人工勢場中無人車的勢場分析

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

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

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

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

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

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

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

2.1 “最小值陷阱”

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

圖2 虛擬勢場檢測圓模型及接觸點分布圖

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

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

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

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

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

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

簡化后得式（4）：

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

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

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

2.2.2 更新航向角

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

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

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

3.1 LSTM

LSTM[13]是長短時記憶網(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)在每次迭代過程中都會進(jìn)行一次更新，將歷次最優(yōu)結(jié)果保留下來，形成長期記憶。這樣就可以使用較長的歷史信息去預(yù)測未來的情況，與短期的歷史信息配合，形成一個長短時記憶網(wǎng)絡(luò)，提高無人車的避障學(xué)習(xí)能力。避免由于短期記憶的偏差對神經(jīng)網(wǎng)絡(luò)整體的影響，提高了系統(tǒng)的抗干擾能力。

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

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

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

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

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

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

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

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

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

4 實驗及仿真結(jié)果分析

4.1 實驗環(huán)境和模型搭建

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

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

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

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

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

4.3 基于LSTM Q-Learning人工勢場法對比

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

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

圖9 原虛擬勢場檢測圓模型

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

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

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

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

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

5 結(jié)束語

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