曹 凱，陳陽泉，高 嵩，高佳佳

1.西安工業(yè)大學機電工程學院，西安 710021

2.西安工業(yè)大學電子信息工程學院，西安 710021

3.加州大學默塞德分校工程學院，美國默塞德95343

近年來智能移動機器人在軍事國防、航空航天以及星球探索方面起到了重要的作用[1-4]，因而成為學術(shù)界研究和關(guān)注的熱點問題。移動機器人在任務空間自由活動的過程中，首先需要解決路徑規(guī)劃問題。路徑規(guī)劃是在靜態(tài)和動態(tài)環(huán)境、模型約束以及不確定性的情況下，自動生成到達預定目標點的可行和最佳路徑的過程。目前已有許多路徑規(guī)劃算法用于解決靜態(tài)和動態(tài)環(huán)境中的運動規(guī)劃問題[5]，例如人工勢場[6-7]、基于網(wǎng)格的方法[8-9]、神經(jīng)網(wǎng)絡[10]、進化仿生的方法[11-13]和基于采樣的規(guī)劃[14-15]。

人工勢場（artificial potential fields，APF）包括目標點周圍的吸引勢場和障礙物周圍的排斥勢場。吸引勢場使機器人朝向目標移動，斥力勢場使機器人遠離障礙物，最終機器人將朝著吸引和排斥合力的方向移動。雖然APF 具有簡單的數(shù)學表達式和較高的計算效率，但也存在一些固有問題：

（1）容易陷入局部最??；

（2）障礙物密集且間隔較小的環(huán)境中找不到可行路徑；

（3）在狹窄通道和目標點附近存在障礙物導致無法到達的情況下出現(xiàn)機器人振蕩。

以往的研究大多假設障礙物遠離目標點，在這種情況下，當機器人接近目標位置時，由于障礙物引起的排斥力可以忽略不計，機器人會被吸引力吸引到目標位置。然而，在實際環(huán)境中目標位置或許會非常接近障礙物，導致當機器人靠近目標時，也會接近附近的障礙物，而排斥力將遠大于吸引力，并且目標位置不是總勢能的全局最小值。因此，由于附近的障礙物，機器人無法到達目標。針對這一問題，考慮到機器人與目標之間的相對距離，對路徑規(guī)劃中的排斥勢函數(shù)進行了修正，從而確?？倓菰谀繕宋恢糜幸粋€全局最小值，機器人最終會達到目標[16]。

基于采樣的規(guī)劃（sampling-based planning，SBP）算法對配置空間進行純粹的探索，以保證算法的概率完備性。其中Kavraki 等人于1994 年提出的概率路線圖（probabilistic roadmap，PRM）[17]和Lavalle 于1998 年提出的快速隨機探索樹（rapidly-exploring random tree，RRT）[18]是最有效的SBP 算法。然而，PRM對障礙物信息的依賴使得它只適用于靜態(tài)環(huán)境。相比之下，RRT 更適合動態(tài)環(huán)境和未知場景，并且可以包含非完整約束條件。

RRT 算法是從根（起始點）開始向未探索的區(qū)域隨機生成一棵可行軌跡樹，當其中一條無碰撞軌跡到達目標區(qū)域時產(chǎn)生一條可行路徑。而路徑間隙、路徑長度以及規(guī)劃時間都取決于節(jié)點產(chǎn)生的隨機性。因此，RRT 算法主要問題有：

（1）在初始迭代期間，目標區(qū)域附近的有用樣本無法最快做出貢獻，增加了規(guī)劃時間；

（2）在整個探索空間中進行探索采樣，產(chǎn)生較多節(jié)點，擴大了內(nèi)存需求；

（3）尋找到的路徑往往不是最優(yōu)的，路徑成本較高，且向最優(yōu)路徑收斂緩慢；

（4）車輛與障礙物之間的距離，會改變生成路徑的間隙，從而影響路徑安全。

目前，有大量文獻關(guān)于RRT 算法以及RRT*算法存在的問題提出了一些解決方案，例如文獻[19]提出RRT-Connect 方法，它從起始點和目標點生長兩棵不同的樹，同時不斷嘗試用貪婪的啟發(fā)式方法連接它們，以改善規(guī)劃時間。在文獻[20]中，限制節(jié)點擴展的最大數(shù)量，并通過刪除無效節(jié)點，添加高性能節(jié)點，保證了完整節(jié)點重新布線后的路徑長度最短，但是收斂速度比RRT*慢。RRT*算法作為RRT 的一種重要變體。文獻[21]中的RRT*算法通過配置路徑成本函數(shù)來連接樹中的節(jié)點，從而降低路徑長度。文獻[22]和文獻[23]提出在找到可行路徑之后，對路徑再進行優(yōu)化以得到較短的路徑。文獻[24]使用啟發(fā)式滑動窗口采樣減少B-RRT*算法隨機采樣所帶來的誤差，并將車輛運動學約束加入到重選父節(jié)點和重布隨機樹的過程，使用貝塞爾曲線對所規(guī)劃的路徑進行平滑處理。文獻[25]采用目標偏向策略和氣味擴散法來改善擴展節(jié)點的選取，使得隨機樹的生長趨向于目標點，并提出一種基于三次B 樣條曲線的路徑平滑方法，極大地提升了搜索效率和路徑質(zhì)量。

本研究在APF 中加入渦流的引導，并與RRT*采樣的方法相結(jié)合來解決RRT*存在的一些問題。該方法利用渦流人工勢場（vortex-APF，VAPF）引導探索更有前途的配置空間，減少了得到最優(yōu)路徑的迭代次數(shù)，從而降低執(zhí)行時間，加快收斂速度。在軌跡樹中拒絕插入無效節(jié)點和高成本節(jié)點，以降低內(nèi)存需求。然后對可行路徑進行修剪和平滑處理，在無障礙區(qū)域連接盡可能少的節(jié)點得到一條長度較短、更為平滑的最優(yōu)或次優(yōu)路徑。仿真結(jié)果表明，該方法在內(nèi)存需求、執(zhí)行時間和路徑復雜性方面均具有有效性。

1 相關(guān)工作

1.1 RRT*算法

RRT 算法是通過隨機探索自由空間，并構(gòu)建一棵從起始點（zstart）開始尋找朝向目標狀態(tài)的可行路徑的樹。在迭代的過程中，創(chuàng)建隨機點并搜索距離該點最近的頂點，然后以固定步長得到該方向上的新節(jié)點，并檢查其是否屬于自由空間，直至達到目標（zgoal）。

RRT*算法使用了三個新的概念對RRT 進行了改進：第一個是新增節(jié)點的本地鄰域；第二個是成本函數(shù)，它計算了從初始狀態(tài)到樹中節(jié)點的總成本；第三個是重新布線，它利用路徑成本函數(shù)在新增節(jié)點的本地鄰域中重新布線，從而降低路徑成本。RRT*算法的流程與RRT 算法類似，在迭代的過程中，創(chuàng)建隨機采樣節(jié)點，搜索到該點的最近節(jié)點znearest，根據(jù)固定步長連接到最近的節(jié)點，就形成探索樹中的新節(jié)點。新節(jié)點領(lǐng)域中的點znear由新節(jié)點為中心半徑為r的區(qū)域來確定，如果通過新節(jié)點的路徑比通過當前父節(jié)點的路徑具有更低的成本并且兩點之間是無障礙區(qū)域，那么新節(jié)點被設置為最佳父節(jié)點以保持樹的結(jié)構(gòu)，如圖1 所示。

重復上述過程來維護節(jié)點樹的增長，直到達到目標。如果目標狀態(tài)位于障礙物空間中，則迭代過程被中止或者尋找路徑失敗，稱為陷阱狀態(tài)。RRT*算法的基本流程如下所示。

Fig.1 RRT*node extension process圖1 RRT*節(jié)點擴展過程

算法1T=(V,E)←RRT*(zstart,zgoal)

1.2 人工勢場法

1986 年，Khatib 提出利用勢場進行避障和路徑規(guī)劃的思想[26]，即人工勢場法APF。它來自物理學中場的概念，它將物體的運動視為兩種力相互作用的結(jié)果。因此在該方法中，機器人被建模為受吸引力和排斥力影響的粒子。障礙物表示為斥力場Urep，目標則是吸引場Uatt。在吸引力和排斥力的共同作用下，機器人沿著合力的方向移動，安全地到達目標區(qū)域，即不發(fā)生任何碰撞。目標點對于機器人的引力場和斥力場如式（1）和式（2）所示，目標對于機器人的吸引力Fatt和排斥力Frep分別為吸引勢場和排斥勢場的負梯度，如式（3）和式（4）所示。吸引勢場和排斥勢場的矢量和是勢場的總和Ftotal，其計算公式如式（5）所示。

式（1）～（5）中，x是機器人當前的位置，xg為目標點，機器人與障礙物之間的最短距離用ρ表示，ρ0表示勢場的最小影響距離，Ka和Kr分別表示吸引場和斥力場的縮放因子。

2 路徑規(guī)劃混合優(yōu)化方法

2.1 改進的RRT*算法（Improved RRT*）

在RRT*算法中遠離生成路徑的擴展節(jié)點重復連線，通常這些節(jié)點不僅沒有降低路徑成本，還因為無效節(jié)點間的重新布線增加了時間成本。因此，在生成路徑的節(jié)點附近進行有意義的重新布線操作，可以有效地改善路徑成本。

改進RRT*算法僅在有前途的區(qū)域中探索，生成更有前景的樹T，從而以更少的時間和更小的內(nèi)存需求找到最佳路徑。它以zstart為根來初始化探索樹T，先確定zstart和zgoal之間的偏向區(qū)域BRegion，然后隨機選擇BRegion中的節(jié)點進行采樣，找到可行的初始路徑。同時對初始路徑使用節(jié)點拒絕技術(shù)，去除高成本節(jié)點和無效節(jié)點，然后進行原始路徑修剪，獲得一條長度更小，節(jié)點更少的優(yōu)化路徑，如圖2 所示。

首先，確定探索區(qū)域，使用偏向采樣探索區(qū)域，并對采樣產(chǎn)生的節(jié)點頻繁地進行重新布線操作，這有利于迅速地降低當前探索路徑的成本。當發(fā)現(xiàn)目標節(jié)點或目標區(qū)域內(nèi)（最小步長范圍內(nèi)）的節(jié)點時，找到一條從起始位置到目標位置的初始路徑，如圖2（a）所示。如果當前區(qū)域探索完成后未找到可行路徑，則重新改變區(qū)域的大小，直到找到目標位置，如圖2（c）所示。

傾向得分匹配法可以控制處理組與對照組之間不可觀測且不隨時間變化的組間差異（Heckman，LaLonde & Smith，1999）。此方法的核心邏輯是針對樣本數(shù)據(jù)中不存在實施過境免簽政策地區(qū)在沒有實施此項政策情況下的入境旅游績效的“反事實情形”，采用半?yún)?shù)估計方法估計實施地區(qū)未實施該項政策的傾向得分值，進而為實施地區(qū)選擇匹配特征盡可能接近的未實施地區(qū)，保證處理組與對照組在政策實施前的發(fā)展軌跡基本“平行”。

Fig.2 Improved RRT*processing flow圖2 改進的RRT*算法處理流程

探索區(qū)域的確定主要是利用起始點和目標點之間的連線的斜率，計算出延長線與地圖邊界之間的交點P和Q，然后對P和Q分別加減100 單位（可根據(jù)地圖比例調(diào)節(jié)），得到與地圖相交的新交點P1、P2、Q1 和Q2，并連接P1 和Q1，P2 和Q2，得到新的直線方程。如果這些新交點超出地圖范圍，則以地圖頂點和直線方程重新計算得到交點P3 和Q3，P3、P2、Q1 和Q3 形成連通區(qū)域，如圖3 所示。

Fig.3 Determining biased region圖3 偏向區(qū)域確定的方法

其次，節(jié)點拒絕的工作是去除高成本節(jié)點和無效節(jié)點。如果新的節(jié)點和目標之間的距離成本大于當前路徑的距離成本，那么新采樣的節(jié)點就被拒絕插入樹中。此外，如果某一個節(jié)點不能被擴展（沒有子節(jié)點或者僅有一個子節(jié)點），這也意味著它不在達到目標狀態(tài)的路徑上，這樣的節(jié)點也會從樹杈中被刪除掉。因此，只通過有用節(jié)點來擴展樹，不僅可以維持樹的成本，還可以快速地尋找到一條路徑成本較小、收斂時間最短、內(nèi)存需求最少的可行路徑，如圖2（c）所示。

最后，對先前得到的路徑進行修剪（類似于樹木生長過程中需要將多余的樹枝修剪掉，以保證樹主干的營養(yǎng)吸收）。因為RRT*算法是概率完備的，它傾向于隨機點方向構(gòu)建可行軌跡樹，所以大多數(shù)解決方案不是最優(yōu)的。因此，需要采用修剪算法以獲得路程更短，噪聲更少的路徑，如圖2（d）所示。

路徑修剪的原理如圖4 所示，獲得初始路徑后，從起點開始遍歷每一個路徑節(jié)點并相互連線，如果起點和其中的一個節(jié)點Zp1之間存在障礙物（藍色虛線），則退回Zp1的上一個節(jié)點Zp1-1，而起點到Zp1-1之間的線段就是一段平滑路徑。同時，Zp1-1作為新的起點遍歷剩余的路徑節(jié)點。以此類推，直到遍歷到目標點形成一條優(yōu)化后的低成本路徑（橙色虛線）。

Fig.4 Path pruning圖4 路徑修剪

2.2 改進的APF 方法

APF 方法將障礙物作為移動機器人的斥力場，斥力場是以障礙物為中心產(chǎn)生向外發(fā)散的排斥力，從而使機器人遠離障礙物，如圖5 所示。圖5 中的紅色線條表示3 個圓形障礙物向外產(chǎn)生的發(fā)散的排斥力。雖然在吸引力的作用下可以到達目標點，往往難以遵循能量最小、時間和路徑最優(yōu)等優(yōu)化準則。

Fig.5 Repulsive field distribution圖5 斥力場分布

本研究在APF 的基礎上將渦流引入勢場，從而形成渦流場。渦流場可以產(chǎn)生切向梯度，利用迫使機器人繞過每個障礙物的動作來代替原本的排斥動作，從而引導機器人避開指向自然運動方向的有害或不利梯度，如圖6 所示。

通過對機器人與障礙物之間位置關(guān)系進行反切運算可以得到渦流場的方向。同時，在原有的斥力場計算中加入了梯度信息和機器人與障礙物之間的距離可以得到渦流斥力場。渦流人工勢場（VAPF）的引入，能夠使機器人的路徑更加平滑。

Fig.6 Vortex repulsive field distribution圖6 渦流斥力場分布

2.3 VAPF 與改進的RRT*混合優(yōu)化方法

首先利用RRT*算法在偏向區(qū)域中進行隨機采樣，然后根據(jù)機器人的位置和障礙物的影響范圍，計算了作用于機器人的渦流場，并利用隨機吸引場梯度下降對隨機采樣點zrand進行改造，得到一個引導采樣點zprand，同時利用節(jié)點拒絕技術(shù)將路徑中無效的節(jié)點（不在主路徑中的節(jié)點）進行剔除，通過節(jié)點擴展的迭代得到一條初始路徑，然后進行路徑修剪和路徑平滑處理，最終可以得到一條最優(yōu)路徑。VAPFRRT*混合優(yōu)化方法的基本流程如下所示：

算法2T=(V,E)←VAPF-RRT*(zstart,zgoal)

算法第6 行中的隨機梯度下降（randomized gradient descent，RGD）子函數(shù)表示隨機吸引場梯度下降，它用吸引勢場對隨機采樣點zrand進行擴展，得到一個改進的采樣點xprand，并將xprand作為后續(xù)算法的隨機采樣點。算法流程偽代碼如下所示：

函數(shù)1RGD(zrand)

3 算法仿真與結(jié)果分析

本研究在Matlab2016 中設計仿真場景，分別使用RRT、改進RRT*和VAPF-RRT*方法進行路徑規(guī)劃，考慮到RRT 類算法具有概率隨機性，因此每種方法進行了32 次實驗，取平均路徑長度、執(zhí)行時間和采樣節(jié)點用來分析路徑規(guī)劃方法的性能。為了將執(zhí)行時間限制在合理范圍，迭代次數(shù)設定為2 000 次。

仿真場景由多個不同半徑的圓形障礙物組成，使用RRT 和改進RRT*方法完成路徑規(guī)劃的效果圖，如圖7 所示。使用VAPF-RRT*方法對障礙物產(chǎn)生的渦流場和路徑規(guī)劃效果圖，如圖8 所示。

使用3 種算法在上述場景分別進行32 次仿真實驗，表1 給出了每種算法用于達到最優(yōu)路徑解的平均執(zhí)行時間、采樣節(jié)點和路徑成本。

Fig.7 RRT and improved RRT*path planning圖7 RRT 與改進的RRT*方法路徑規(guī)劃

Fig.8 VAPF-RRT*vortex and path planning圖8 VAPF-RRT*方法產(chǎn)生的渦流場和路徑規(guī)劃

Table 1 Simulation results for calculating optimal or approximate optimal path表1 計算最優(yōu)或近似最優(yōu)路徑的仿真結(jié)果

從圖7 中可以直觀地看出，改進RRT*方法不論從擴展節(jié)點數(shù)量還是路徑成本都要優(yōu)于RRT 算法，而圖8 顯示了VAPF-RRT*算法沒有擴展多余的節(jié)點，形成的路徑更加平滑，而且斥力場的存在使機器人與障礙物之間保持相對距離，從而保證了機器人在運動過程中的安全性。通過表1 可以得到，改進RRT*算法的平均執(zhí)行時間和路徑成本是3 種方法中最優(yōu)的，相對于RRT 算法和VAPF-RRT*算法，執(zhí)行時間分別減少了12.1%和33.1%，路徑成本分別縮短了21.1%和10.3%，說明偏向區(qū)域搜索可以明確搜索方向，加快收斂速度，降低執(zhí)行時間；路徑修剪可以修剪多余節(jié)點，達到優(yōu)化路徑的效果。在擴展節(jié)點方面，VAPF-RRT*方法的節(jié)點數(shù)量要優(yōu)于另外兩種方法，因為VAPF 的渦流場引導RRT*的隨機采樣節(jié)點，并將引導節(jié)點作為路徑形成的導航點，從而降低擴展節(jié)點的數(shù)量。改進RRT*方法在偏向區(qū)域中隨機采樣再進行節(jié)點拒絕，收斂速度要優(yōu)于使用渦流場來引導采樣節(jié)點。雖然路徑修剪使路徑成本有明顯的下降，但是容易出現(xiàn)優(yōu)化路徑貼著障礙物的情況，而且折線路徑在實際應用中也存在較多問題。

從子函數(shù)RGD 流程中可以看出，障礙物勢場的影響距離ρ0會影響引導節(jié)點的選取，從而影響執(zhí)行時間、擴展節(jié)點和路徑成本。本研究在實驗過程中分別選取圓形障礙物半徑的1.0、1.5、2.0 和3.0 倍作為障礙物勢場的影響距離，圖9 顯示了選取不同影響距離對于最終路徑形成產(chǎn)生的影響，其中黑色圓圈為障礙物，包圍障礙物的青色圓圈為最小影響距離，綠色圓圈和深藍色圓圈分別為起始點和目標點。

Fig.9 Path planning under different influence radii圖9 不同影響距離下的路徑規(guī)劃

從圖9（a）和圖9（b）中可以看出，選取1.0 倍和1.5 倍障礙物半徑作為勢場的影響距離，通過VAPFRRT*算法形成的路徑與障礙物之間保持的距離較小，甚至出現(xiàn)了貼著障礙物運動的情況，這不僅增加了執(zhí)行時間和路徑成本，也無法保證機器人在運動過程中的安全性。而對2.0 倍半徑的影響距離而言，規(guī)劃出的路徑不僅相對平滑，而且可以和障礙物之間保持相對安全的距離。對比圖9（c）和圖9（d）可以發(fā)現(xiàn)，路徑與障礙物之間的距離雖然稍有擴大，但后半段的路徑卻出現(xiàn)了波動。通過仿真實驗發(fā)現(xiàn)，影響距離在大于2.0 倍障礙物半徑后，逐漸增加影響距離對路徑規(guī)劃已無明顯的改變。

綜上所述，仿真結(jié)果證明了VAPF-RRT*算法在執(zhí)行時間、采樣節(jié)點、路徑成本方面優(yōu)化的有效性，如：

（1）在偏向區(qū)域內(nèi)探索，加快了收斂到可行軌跡的速度，減少了規(guī)劃時間；

（2）VAPF 算法的渦流場效應使節(jié)點產(chǎn)生的方向性增強，從而生長軌跡更為集中的擴展樹；

（3）降低了內(nèi)存需求，使RRT*算法也可以應用于內(nèi)存有限的嵌入式系統(tǒng)；

（4）路徑修剪和路徑平滑，可以得到路徑長度相對較短且平滑的路徑；

（5）RRT*算法的隨機性可以克服APF 算法容易陷入局部最優(yōu)的問題。

4 結(jié)束語

在本研究中，提出了一種渦流人工勢場引導下的RRT*算法來解決路徑規(guī)劃中存在的一些問題。通過多次仿真實驗，其仿真結(jié)果可以定量分析改進算法的優(yōu)越性、高效性和實用性。實驗結(jié)果表明：本文方法可以有效地減少采樣節(jié)點，降低規(guī)劃時間并優(yōu)化得到一條相對較短且平滑的路徑。同時，RRT*算法的隨機性又克服了APF 算法容易陷入局部最優(yōu)的問題。

重復進行實驗后發(fā)現(xiàn)，雖然本研究中的方法可以使用較少的時間和較小的內(nèi)存，但是得到的路徑并非全局最優(yōu)路徑。此外，也沒有考慮實際場景中工業(yè)機器人和移動機器人的運動學和動力學約束條件下的安全問題。未來研究工作中希望可以根據(jù)搜索的初始路徑擬合空間貝塞爾曲線，并通過優(yōu)化曲線控制點使行駛軌跡滿足各類約束的同時，提高路徑的平順性與安全性。最后希望本研究將可以應用于自主無人車輛和工業(yè)生產(chǎn)加工的高維運動規(guī)劃研究領(lǐng)域。