余騰偉，劉昌力

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

0 引 言

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

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

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

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

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

1 人工勢(shì)場(chǎng)法

1.1 傳統(tǒng)人工勢(shì)場(chǎng)法

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

圖1 人工勢(shì)場(chǎng)法三維模型Fig. 1 3D Model of artificial potential field algorithm

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

(1)

(2)

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

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

1.2 傳統(tǒng)人工勢(shì)場(chǎng)法的改進(jìn)思路

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

圖2 局部極小值點(diǎn)問題Fig. 2 Local minimum point problem

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

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

2 快速擴(kuò)展隨機(jī)樹算法

2.1 快速擴(kuò)展隨機(jī)樹算法

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

圖3 快速擴(kuò)展隨機(jī)樹擴(kuò)展示意Fig. 3 Expansion schematic of rapidly exploring random tree

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

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

2.2 改進(jìn)的快速擴(kuò)展隨機(jī)樹算法

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

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

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

Step 2在擴(kuò)展樹中找到xpnear，節(jié)點(diǎn)xpnear是擴(kuò)展樹中距離xprand最近的一個(gè)點(diǎn)。

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

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

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

xpnew=xpnear+

(7)

ypnew=ypnear+

(8)

zpnew=zpnear+

(9)

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

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

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

3 APF-RRT融合算法

3.1 融合算法的必要性

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

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

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

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

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

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

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

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

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

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

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

6)判斷移動(dòng)機(jī)器人是否抵達(dá)最終目標(biāo)點(diǎn)。

4 仿真驗(yàn)證

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

圖5 人工勢(shì)場(chǎng)法仿真示意Fig. 5 Schematic diagram of artificial potentialfield algorithm simulation

表1 移動(dòng)機(jī)器人運(yùn)動(dòng)控制參數(shù)Table 1 Parameter of moblie robot motion control

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

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

圖6 快速擴(kuò)展隨機(jī)樹擴(kuò)展過程Fig. 6 Expansion process of rapidly exploring random tree

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

圖7 快速擴(kuò)展隨機(jī)樹的路徑查詢Fig. 7 Path query for rapidly exploring random tree

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

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

5 結(jié) 語

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