余騰偉,劉昌力
(1. 重慶交通大學(xué) 交通工程應(yīng)用機(jī)器人重慶市工程實(shí)驗(yàn)室,重慶 400041;2. 重慶嘉陵華光光電科技有限公司 技術(shù)開發(fā)部,重慶 400700)
移動(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ī)劃的要求。
人工勢(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ī)劃也受到一定影響。
動(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ī)劃的適用性。
快速擴(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ī)劃要求。
快速擴(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)γ可有效改善路徑的平順性。
路徑規(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)的避障性能。
在已知環(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)。
筆者主要針對(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ī)劃。
在動(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ī)劃的舒適性、安全性。