余騰偉，劉昌力

(1. 重慶交通大學(xué) 交通工程應(yīng)用機器人重慶市工程實驗室，重慶 400041；2. 重慶嘉陵華光光電科技有限公司 技術(shù)開發(fā)部，重慶 400700)

0 引 言

移動機器人的路徑規(guī)劃是指在給定的地圖環(huán)境或者根據(jù)不斷探測到的障礙物信息下找到一條從起始點到目標(biāo)點的一條無碰撞的安全行進路線，其本質(zhì)是在幾個約束條件下得到最優(yōu)或者可行解的問題[1]。根據(jù)移動機器人對環(huán)境信息的不同獲取情況，路徑規(guī)劃算法主要劃分為全局路徑規(guī)劃和局部路徑規(guī)劃兩類算法[2]。全局路徑規(guī)劃算法一般在環(huán)境信息大部分已知的情況下得到廣泛應(yīng)用。近年來的研究中全局的路徑規(guī)劃算法包括蜂群算法、遺傳算法、快速隨機搜索樹算法、拓撲法等。局部路徑規(guī)劃一般應(yīng)用于移動機器人對環(huán)境信息局部未知或完全未知的情況下，其大部分情況下只能在短時間內(nèi)有效。根據(jù)適用情況的不同，局部路徑規(guī)劃算法大部分采用了人工勢場法、A*算法和模糊算法等[3]。

基于大部分移動機器人的工作環(huán)境，全局、局部路徑規(guī)劃算法的融合是目前研究的一個重點方向。在不同情況下常見的融合方法有兩個大的分類：基于行為的混合路徑規(guī)劃方法與基于全局引導(dǎo)的混合式路徑規(guī)劃方法[4]。前者主要將路徑規(guī)劃看作兩個并列的模塊，在進行全局路徑規(guī)劃和局部路徑規(guī)劃時交替使用，是兩個并列的模塊；后一種方法則是在先驗地圖上使用全局路徑規(guī)劃算法求出一條可行的路徑，然后移動機器人在該路徑上進行實時的動態(tài)避障。

目前現(xiàn)有的每個路徑規(guī)劃算法均有不同的缺點，在實際環(huán)境下的使用中適用性并不高。筆者針對人工勢場法所存在的缺陷對其進行改進。

人工勢場法是一種局部路徑規(guī)劃算法，具有計算量小、響應(yīng)迅速、適用性高等優(yōu)點，但在實際問題的應(yīng)用中，易陷入局部最小值導(dǎo)致移動機器人無法抵達目標(biāo)點的問題。針對該算法的缺陷，國內(nèi)外許多學(xué)者提出了數(shù)種解決方案。汪首坤等[5]運用幾何法與人工勢場法相結(jié)合來解決傳統(tǒng)人工勢場法的局部極小值問題，但該種方法在路徑規(guī)劃時運用了逆運動的方法，需從8組解中進行篩選，計算效率較低；姬偉等[6]引入了虛擬目標(biāo)點的概念，建立新的移動向量將移動機器人從局部最小點引導(dǎo)出去，從而解決了該問題。但利用幾何的方法確定虛擬目標(biāo)點在較為復(fù)雜的環(huán)境中適用性較差。

基于各種分析與歸納，目前針對人工勢場法的主要改進思路為建立虛擬目標(biāo)點作為引導(dǎo)、增加虛擬障礙物、建立等勢線等方法[7]。因此，筆者參考了各種改進方法的缺點，從建立虛擬目標(biāo)點的思路出發(fā)，結(jié)合改進快速擴展隨機樹算法，使移動機器人能夠適應(yīng)動態(tài)環(huán)境變化并更好地解決了人工勢場法中的局部極小值問題，讓移動機器人滿足路徑規(guī)劃的要求。

1 人工勢場法

1.1 傳統(tǒng)人工勢場法

人工勢場法的本質(zhì)是對移動機器人工作環(huán)境的一種抽象描述，該方法將移動機器人、目標(biāo)點、障礙物看做質(zhì)點，進而在移動機器人的工作空間中人為構(gòu)造一種虛擬勢場[8]。虛擬勢場由兩種勢場組成，一種是目標(biāo)點產(chǎn)生的引力勢場，另一種是由障礙物產(chǎn)生的斥力勢場。引力勢場具有負勢能，移動機器人具有向低勢能方向移動的趨勢；斥力勢場具有高勢能，移動機器人會優(yōu)先向低勢能方向移動而避開高勢能區(qū)域。移動機器人的路徑規(guī)劃是進行二維平面空間下的路徑規(guī)劃，但人工勢場是表現(xiàn)為三維的，由引力勢場所引起，這也是移動機器人在路徑規(guī)劃中的主要驅(qū)動力。在圖1中目標(biāo)點所代表的“低谷”具有低勢能，對移動機器人產(chǎn)生吸引力；而每個障礙物均會構(gòu)成一座“高山”，在這些區(qū)域中具有高勢能，移動機器人所具有的動能無法靠近這些高斥力勢能區(qū)域，使得移動機器人在進行路徑規(guī)劃時會避開這些障礙物。這種避障策略的思路非常巧妙且具有實用性。

圖1 人工勢場法三維模型Fig. 1 3D Model of artificial potential field algorithm

傳統(tǒng)的人工勢場法中只會考慮到移動機器人與障礙物、目標(biāo)點之間的距離因數(shù)。移動機器人與目標(biāo)點所產(chǎn)生的引力勢場與兩者之間的距離成正比，與目標(biāo)點越遠，目標(biāo)點對移動機器人產(chǎn)生的引力越強。引導(dǎo)移動機器人到達目標(biāo)點，目標(biāo)點所產(chǎn)生的引力勢場作用于全局域。障礙物與移動機器人之間的斥力勢場成反比關(guān)系，這些障礙物所組成的勢場僅作用于局部域，只有在移動機器人進入斥力勢場作用域內(nèi)后才會對規(guī)劃路徑產(chǎn)生影響，規(guī)避掉障礙物來達到避障的目的。移動機器人與障礙物、目標(biāo)點之間的引力斥力關(guān)系如式(1)、式(2)：

(1)

(2)

式中：k為引力勢場增益系數(shù)；m為斥力勢場增益系數(shù)，通常m≥2；‖pr-pg‖為機器人的實時位置相對于最終目標(biāo)點的歐幾里得距離；p=‖pr-po‖為移動機器人相對于障礙物的歐幾里得距離；po為勢力場中障礙物對移動機器人產(chǎn)生斥力的距離條件，即當(dāng)移動機器人與障礙物的距離大于po時不受障礙物產(chǎn)生的斥力勢場影響，反之則受到斥力影響。引力Fatt與斥力Frep可由相對應(yīng)的勢場函數(shù)求得，分別為：

Fatt(p)=-grad[Uatt(p)]=-k‖pr-pg‖

(3)

Frep(p)=-grad[Urep(p)]=f(x)

(4)

(5)

Ftotal=-?U=Fatt+Frep

(6)

人工勢場法具有計算量小、路徑規(guī)劃軌跡平滑、實時性高等優(yōu)點，但也存在易使移動機器人陷入局部極小值點的問題，在復(fù)雜環(huán)境下的實際應(yīng)用受到較大的局限，動態(tài)規(guī)劃也受到一定影響。

1.2 傳統(tǒng)人工勢場法的改進思路

動態(tài)障礙物的分布存在不確定性和實時性，部分特定的靜態(tài)障礙物分布情況也會使移動機器人在進行路徑規(guī)劃時遇到某些區(qū)域，如圖2。區(qū)域內(nèi)障礙物對移動機器人產(chǎn)生斥力Frep與目標(biāo)點產(chǎn)生吸引力Fatt大小趨近于0，方向相反，此區(qū)域被稱為局部極小值區(qū)域。當(dāng)移動機器人陷入該區(qū)域內(nèi)時將無法脫離出去，導(dǎo)致路徑規(guī)劃失敗[9]；當(dāng)存在復(fù)雜的障礙物，如U型障礙物時，移動機器人被障礙物環(huán)繞無法逃出，亦將導(dǎo)致路徑規(guī)劃失敗[10]。

圖2 局部極小值點問題Fig. 2 Local minimum point problem

國內(nèi)外學(xué)者對如何解決局部極小值問題提出了多種改良方法，比如改進勢場函數(shù)減少局部極小值點；添加啟發(fā)式搜索算法等方法。筆者針對這一問題，提出了結(jié)合快速擴展隨機樹算法的方法，在起始點至目標(biāo)點之間的可行路徑上增加臨時目標(biāo)點，解決路徑規(guī)劃對象在局部極小值點附近徘徊震動的問題。

人工勢場法在解決路徑規(guī)劃問題時由于缺少全局信息，且移動機器人所面臨的環(huán)境是未知、動態(tài)且復(fù)雜的，引入快速擴展隨機樹算法為其增加數(shù)個虛擬目標(biāo)點的改進方法可在不過多增加計算量的前提下有效提高移動機器人使用人工勢場法進行動態(tài)路徑規(guī)劃的適用性。

2 快速擴展隨機樹算法

2.1 快速擴展隨機樹算法

快速擴展隨機樹算法最早由La Valle于1998年提出，是一個有效的樹類數(shù)據(jù)結(jié)構(gòu)和隨機采樣方案，如圖3。該算法結(jié)合了隨機路徑規(guī)劃理論，設(shè)定移動機器人的起始點為根節(jié)點出發(fā)，通過在可行自由空間內(nèi)生長出一顆擴展隨機樹的方法來選定一條路徑。在生長過程中，隨機選取新的節(jié)點，直至到達目標(biāo)點?；镜目焖贁U展隨機樹算法包括兩個階段：

圖3 快速擴展隨機樹擴展示意Fig. 3 Expansion schematic of rapidly exploring random tree

1)構(gòu)建擴展隨機樹階段。在工作空間中通過迭代的方式，每一次均在空間中隨機選取一個不屬于擴展樹的隨機節(jié)點xrand，隨后在擴展樹中找出一個與xrand度量最近的節(jié)點xnear，再在xnear與xrand的連線上尋求一個新的狀態(tài)節(jié)點xnew，xnew與xnear間的距離滿足擴展隨機樹的擴展步長條件。在此過程中，度量最近的節(jié)點xnear向隨機節(jié)點xrand的方向遞增了一個擴展步長的距離，同時需要保證xnew不與障礙物發(fā)生碰撞。重復(fù)上述的過程直到新的節(jié)點包含于目標(biāo)點內(nèi)后，結(jié)束迭代并構(gòu)成完整的擴展隨機樹。

2)路徑查詢階段。在完成擴展隨機樹的構(gòu)建后，停止再添加進新的節(jié)點。根據(jù)上一階段所構(gòu)造出的擴展隨機樹，從目標(biāo)節(jié)點開始，逆向搜索整個隨機樹，層層迭代至起始根節(jié)點，計算出一條包含起始根節(jié)點與目標(biāo)節(jié)點的路徑，且該路徑不與障礙物發(fā)生碰撞，滿足全局路徑規(guī)劃要求。

2.2 改進的快速擴展隨機樹算法

快速擴展隨機樹的具體擴展類似于不斷生長向四周發(fā)散的樹，由于其隨機性，不可避免的存在擴展太過平均，導(dǎo)致所得到的路徑質(zhì)量不高的問題。筆者針對此問題，提出了一種改進的擴展隨機樹算法，使隨機樹的生長朝向目標(biāo)點，以提高搜索的效率。

改良后具體的算法描述如下：

Step 1在自由空間中確定起始根節(jié)點xpstart和隨機選取的節(jié)點xprand，并判斷起始根節(jié)點不在障礙物內(nèi)且隨機選取的節(jié)點不在擴展隨機樹內(nèi)。

Step 2在擴展樹中找到xpnear，節(jié)點xpnear是擴展樹中距離xprand最近的一個點。

Step 3在xpnear與xprand的連線上求解出xpnew，xpnew必須在路徑規(guī)劃區(qū)域的自由空間內(nèi)且與xpnear的距離為擴展隨機樹的擴展步長。如果xpnew存在且不在障礙物內(nèi)，將該點添加至隨機擴展樹中；否則退回Step 1，重新隨機選取節(jié)點xprand。

Step 4判斷新增的節(jié)點xpnew是否是目標(biāo)節(jié)點xpgoal，如果是則求解成功，結(jié)束算法；否則重新進行Step 1。

在此迭代過程中，擴展隨機樹共有Tk個節(jié)點，新增節(jié)點xpnew的求解可由式(7)～式(9)求出：

xpnew=xpnear+

(7)

ypnew=ypnear+

(8)

zpnew=zpnear+

(9)

在擴展隨機樹的擴展過程中，prand為其隨機生成的局部目標(biāo)點。區(qū)別于其構(gòu)造方式的不同，隨機擴展樹的形狀亦會隨之改變。筆者為提高擴展隨機樹的搜索效率，改進了prand的構(gòu)造方式，引入了隨機函數(shù)與參數(shù)pgoal-Possibility相結(jié)合共同生成局部目標(biāo)點的方式。人工設(shè)定參數(shù)pgoal-Possibility表示了算法選取目標(biāo)點pgoal為prand的可能性，如果pgoal-Possibility的值大于算法產(chǎn)生的隨機數(shù)時自動選取pgoal作為局部目標(biāo)點prand，否則隨機生成局部目標(biāo)點。該構(gòu)造方式能夠有效引導(dǎo)快速擴展隨機樹向著目標(biāo)點pgoal方向進行生長，提高生長速度，優(yōu)化算法的搜索效率。

RRT算法的擴展流程類似樹枝的生長過程，原理明確構(gòu)造簡單，通過相同的算法不斷隨機采樣新的狀態(tài)節(jié)點，反復(fù)迭代遞增生成擴展隨機樹。在引入?yún)?shù)pgoal-Possibility對擴展階段進行優(yōu)化后，擴展隨機樹的生長方向更具一致性，逆向查詢出的路徑更加平滑。

根據(jù)筆者的實際需求，改進的快速擴展隨機樹算法用于為移動機器人在路徑中搜索臨時目標(biāo)點。因此適度增大擴展隨機樹的擴展步長γ，增大步長可以降低移動機器人調(diào)整位置姿態(tài)的頻率，提高搜索效率，降低運動的時間，且移動機器人在進行實時路徑規(guī)劃時利用人工勢場法可對障礙物進行有效的躲避，有效避免了增大步長后產(chǎn)生的與障礙物碰撞幾率增加的問題，因此在APF-RRT融合算法的基礎(chǔ)上增大擴展步長γ可有效改善路徑的平順性。

3 APF-RRT融合算法

3.1 融合算法的必要性

路徑規(guī)劃算法可以根據(jù)規(guī)劃出的路徑分為完備的路徑規(guī)劃算法和概率完備的路徑規(guī)劃算法。第一類算法在進行路徑規(guī)劃時如果起始點與目標(biāo)點之間有路徑規(guī)劃的解存在，那么就一定可以計算并得出該解。此類算法為完備的路徑規(guī)劃算法，其優(yōu)勢為對路徑是否可得問題的答案是肯定的，但隨之亦會存在算法計算量大、算法結(jié)構(gòu)復(fù)雜的問題，在環(huán)境信息較為復(fù)雜的情況下適用性很低。第二類算法如基于采樣的路徑規(guī)劃算法，運行速度較快且其得到的路徑規(guī)劃解的集合概率完備，在高維空間下依舊適用；其缺點是此類算法可能會出現(xiàn)無法求出解的情況，且其生成路徑的代價相較于第一類算法太高。

人工勢場法在解決移動機器人路徑規(guī)劃問題時由于缺乏全局信息，在復(fù)雜環(huán)境中經(jīng)常陷入局部最小值點，無法完成路徑規(guī)劃；快速擴展隨機樹算法作為一種概率完備的路徑規(guī)劃算法，在環(huán)境信息已知下的靜態(tài)路徑規(guī)劃能力十分突出，但缺乏面對動態(tài)環(huán)境下實時避障的能力。筆者根據(jù)此兩種算法的優(yōu)缺點進行融合，在移動機器人使用人工勢場法進行動態(tài)路徑規(guī)劃之前，使用優(yōu)化的快速擴展隨機樹算法對靜態(tài)已知環(huán)境中障礙物的分布情況提前進行預(yù)搜索；設(shè)置每個新狀態(tài)節(jié)點為數(shù)個臨時子目標(biāo)點，對之后移動機器人使用人工勢場法進行動態(tài)路徑規(guī)劃進行路徑干涉與引導(dǎo)；通過優(yōu)化的快速擴展隨機樹算法解決了人工勢場法的局部最小值問題，并使得移動機器人在動態(tài)環(huán)境中亦具有很強的避障性能。

3.2 APF-RRT融合算法設(shè)計

在已知環(huán)境信息的靜態(tài)地圖上，先行使用RRT算法在先驗地圖上規(guī)劃出一條可行的路徑。該路徑的起點和終點亦為移動機器人路徑規(guī)劃的起點和終點。再選取此條路徑上的每一個新狀態(tài)節(jié)點xpnew作為臨時目標(biāo)點，當(dāng)移動機器人沿該條路徑行駛時，使用人工勢場法把初始路徑上的每一個xpnew依次作為目標(biāo)點；當(dāng)移動機器人抵達第一個點后隨即切換第二個點為目標(biāo)點，依次類推直至抵達最終的目標(biāo)點。APF-RRT算法流程如圖4。

圖4 APF-RRT算法流程Fig. 4 APF-RRT algorithm flow chart

APF-RRT算法具體步驟如下：

1)獲取當(dāng)前移動機器人初始位置xpstart，并檢測該點是否處于障礙物內(nèi)。

2)在空間中隨機選取xprand作為臨時目標(biāo)點，判斷人工設(shè)定參數(shù)pgoal-Possibility是否大于算法產(chǎn)生的隨機數(shù)，是則選取最終目標(biāo)點作為臨時目標(biāo)點，否則繼續(xù)隨機選取。

3)RRT算法規(guī)劃完成后，選取路徑上每一個xpnew依次作為移動機器人使用人工勢場法進行路徑規(guī)劃時的臨時目標(biāo)點，引導(dǎo)移動機器人在該路徑上行駛。

4)計算移動機器人與各障礙物間的歐幾里得距離，求解障礙物對移動機器人產(chǎn)生的斥力與臨時目標(biāo)點產(chǎn)生的吸引力，得到斥力勢能與引力勢能之和。

5)計算障礙物與移動機器人間的向量與角度，并進行角度弧度轉(zhuǎn)換，依次在各虛擬目標(biāo)點間進行避障。

6)判斷移動機器人是否抵達最終目標(biāo)點。

4 仿真驗證

筆者主要針對移動機器人在動態(tài)環(huán)境下的路徑規(guī)劃進行研究。為驗證APF-RRT融合算法的必要性以及適用性，使用MATLAB平臺進行仿真驗證。移動機器人從起始點出發(fā)，成功到達目標(biāo)點且過程中未與障礙物及地圖邊界發(fā)生碰撞，說明移動機器人避障成功。單獨對傳統(tǒng)的人工勢場法進行仿真驗證，與APF-RRT融合算法進行仿真對比。移動機器人運動控制參數(shù)如表1，仿真結(jié)果如圖5～圖8。

圖5 人工勢場法仿真示意Fig. 5 Schematic diagram of artificial potentialfield algorithm simulation

表1 移動機器人運動控制參數(shù)Table 1 Parameter of moblie robot motion control

在障礙物環(huán)境中設(shè)置L型障礙物、多個黑色靜態(tài)障礙物與紫色動態(tài)障礙物。為測試移動機器人使用APF-RRT算法進行避障時不會陷入局部最小值點，并在路徑規(guī)劃過程中能夠躲避動態(tài)障礙物，能夠應(yīng)對較為復(fù)雜的障礙物環(huán)境，在驗證中加入道路約束。仿真結(jié)果顯示，在10 m寬(y)、50 m長(x)的障礙物環(huán)境下，該算法能成功生成動態(tài)避障路徑，而且能夠保證移動機器人在低俗環(huán)境下的安全行進。在仿真驗證進行的同時筆者將對移動機器人進行碰撞檢測，如果移動機器人撞上地圖邊界或預(yù)先設(shè)置的障礙物，仿真過程將停止并表示路徑規(guī)劃失敗。

圖5表明，使用傳統(tǒng)人工勢場法的移動機器人在L型障礙物前、數(shù)個點狀靜態(tài)障礙物和一個動態(tài)障礙物附近陷入局部最小值區(qū)域且陷入震蕩工況，移動機器人無法逃離該區(qū)域，路徑規(guī)劃失敗。

圖6 快速擴展隨機樹擴展過程Fig. 6 Expansion process of rapidly exploring random tree

圖7的快速擴展隨機樹算法成功在靜態(tài)障礙物地圖上實現(xiàn)擴展隨機樹生長，并成功從最終目標(biāo)點逆向迭代搜索出一條合理的路徑。在該路徑上存在的數(shù)個臨時目標(biāo)點引導(dǎo)下，移動機器人順利地利用人工勢場法規(guī)避掉移動障礙物，并最終到達目標(biāo)點，路徑規(guī)劃成功。

圖7 快速擴展隨機樹的路徑查詢Fig. 7 Path query for rapidly exploring random tree

圖8 APF-RRT算法的路徑規(guī)劃Fig. 8 Path planning of APF-RRT algorithm

仿真結(jié)果表明，與快速擴展隨機樹算法融合之后，在利用該算法預(yù)先獲得了部分的全局地圖信息的情況下，成功解決了人工勢場法中移動機器人易陷入局部極小值點的問題，在預(yù)規(guī)劃路徑上存在的數(shù)個臨時目標(biāo)點可以引導(dǎo)移動機器人進行路徑規(guī)劃。

5 結(jié) 語

在動態(tài)障礙物地圖中，單一的路徑規(guī)劃算法難以滿足復(fù)雜的環(huán)境情況，目前提出的路徑規(guī)劃算法或多或少均存在各自的優(yōu)缺點。筆者提出的利用人工勢場法與快速擴展隨機樹相融合的算法結(jié)合了兩種算法的優(yōu)點并互補了缺點，能夠在動態(tài)環(huán)境中進行穩(wěn)定的路徑規(guī)劃，生成清晰合理的避障路徑，同時具有很高的適用性，概率完備，路徑平順性較好，能夠確保移動機器人不與障礙物之間發(fā)生碰撞且有效減少了算法的計算量。仿真結(jié)果表明，筆者所研究的路徑規(guī)劃方法，在動態(tài)障礙物環(huán)境下能夠進行有效的避障，同時具有較好的平順性。在后續(xù)的研究中筆者將繼續(xù)提高路徑規(guī)劃的舒適性、安全性。