改進(jìn)人工勢(shì)場(chǎng)法機(jī)器人避障路徑規(guī)劃研究

高飛翔,郝萬(wàn)君,吳 宇,孫 誠(chéng)

(1. 蘇州科技大學(xué)電子與信息工程學(xué)院,江蘇 蘇州 215009;2. 蘇州科技大學(xué)物理科學(xué)與技術(shù)學(xué)院,江蘇 蘇州 215009)

1 引言

隨著人工智能的快速發(fā)展,機(jī)器人技術(shù)得到進(jìn)一步開(kāi)發(fā),具有各種功能的機(jī)器人開(kāi)始逐漸融入到日常生活中[1]。機(jī)器人工作時(shí)都需要進(jìn)行路徑規(guī)劃,路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃,它可以為機(jī)器人在各種復(fù)雜環(huán)境中規(guī)劃一條安全、快速的路徑。常用的路徑規(guī)劃算法有:蟻群算法(Ant Colony Optimization,ACO)、遺傳算法(Genetic Algorithm,GA)、快速搜索隨機(jī)樹(shù)法(Rapid-Exploration Random Tree,RRT)和人工勢(shì)場(chǎng)法(Artificial Potential Field,APF)等[1]。

APF法不同于其它的路徑規(guī)劃智能算法,APF是基于局部路徑規(guī)劃的智能算法,運(yùn)用在路徑規(guī)劃中需要根據(jù)當(dāng)前機(jī)器人的位姿以及距離障礙物的距離來(lái)規(guī)劃下一步動(dòng)作,其算法本身具有計(jì)算量小、算法反應(yīng)速度快、生成路徑順滑等優(yōu)點(diǎn)[1]。徐小強(qiáng)[4]等人提出從安全距離概念避免多余路徑,引入預(yù)測(cè)距離解決目標(biāo)不可達(dá)問(wèn)題,在機(jī)器人陷入局部極小值時(shí)利用虛擬障礙點(diǎn)逃離局部極小點(diǎn),但是未曾考慮復(fù)雜障礙物下機(jī)器人是否可以完整規(guī)劃路線。何乃峰[5]等人提出利用模擬退火算法重建APF函數(shù)模型,重新規(guī)劃全局路徑,雖然避免了局部極小值但是所規(guī)劃的路徑不是全局最優(yōu)路徑。吳宇[6]等人提出的利用A*與DWA算法進(jìn)行融合優(yōu)化,提出自適應(yīng)參數(shù)融合改進(jìn)算法,在柵格地圖中可以有效避開(kāi)障礙物建立安全且平滑的行進(jìn)路線,但是這種方法的計(jì)算量大,運(yùn)行緩慢,收斂精度低。楊周[7]等人提出的利用ACO與DWA算法融合,但融合的算法在面對(duì)復(fù)雜環(huán)境問(wèn)題時(shí),區(qū)域搜索最佳轉(zhuǎn)折點(diǎn)不完整可能會(huì)導(dǎo)致可行區(qū)域的收斂性低,從而影響整個(gè)路徑規(guī)劃的完整性。

針對(duì)以上TAPF應(yīng)用于路徑規(guī)劃存在的弊端,提出了利用自適應(yīng)切線偏轉(zhuǎn)角確定虛擬障礙點(diǎn)的APF算法,改進(jìn)斥力函數(shù)引入機(jī)器人與目標(biāo)點(diǎn)的動(dòng)態(tài)影響距離以及調(diào)節(jié)因子系數(shù),避免遇到目標(biāo)不可達(dá)問(wèn)題,利用當(dāng)前機(jī)器人行徑位置提出自適應(yīng)切線偏轉(zhuǎn)角法,以最遠(yuǎn)障礙物為參照物在另一側(cè)確定虛擬障礙點(diǎn),通過(guò)引入虛擬斥力解決振蕩易陷入局部最小值問(wèn)題,使得機(jī)器人可以更快、平穩(wěn)的到達(dá)目標(biāo)點(diǎn)。然后通過(guò)MATLAB仿真驗(yàn)證該算法的正確性,并進(jìn)行了實(shí)際論證。

2 APF算法

2.1 地圖構(gòu)建

ROS智能機(jī)器人進(jìn)行路徑規(guī)劃前利用激光雷達(dá)對(duì)周?chē)鷱?fù)雜環(huán)境進(jìn)行數(shù)據(jù)掃描,將掃描到的數(shù)據(jù)建立模擬地圖模型。本文利用柵格地圖進(jìn)行復(fù)雜環(huán)境的搭建模擬,柵格地圖具有簡(jiǎn)潔的特點(diǎn),可以直觀的顯示出仿真結(jié)果,在眾多路徑規(guī)劃仿真中得到廣泛應(yīng)用。

本文采用如圖1所示柵格地圖進(jìn)行復(fù)雜環(huán)境復(fù)現(xiàn),黑色柵格表示障礙物,白色部分表示無(wú)障礙物可以通過(guò)區(qū)域。

圖1 柵格地圖

2.2 APF算法基本原理

APF是路徑規(guī)劃中應(yīng)用較廣泛的智能算法之一,最早由Khatib在80年代提出[4]。在APF算法中,整個(gè)機(jī)器人運(yùn)動(dòng)過(guò)程比作一顆運(yùn)動(dòng)粒子在帶有正負(fù)極的電勢(shì)場(chǎng)中運(yùn)動(dòng)的過(guò)程。將整個(gè)復(fù)雜的地圖環(huán)境看作一個(gè)虛擬電勢(shì)場(chǎng),將機(jī)器人當(dāng)作電勢(shì)場(chǎng)正極,目標(biāo)點(diǎn)當(dāng)作電勢(shì)場(chǎng)負(fù)極,障礙物視為正極,機(jī)器人利用與障礙物引起的斥力場(chǎng)以及與目標(biāo)點(diǎn)產(chǎn)生的引力場(chǎng)的合力使得機(jī)器人有效地繞過(guò)障礙物最終到達(dá)目標(biāo)點(diǎn)。

假設(shè)機(jī)器人在二維空間中的受力情況如圖2所示,且設(shè)機(jī)器人在二維空間內(nèi)的位置表示為j=(x,y),目標(biāo)點(diǎn)位置表示為jobj=(xT,yT),障礙物的位置表示為jobs=(xo,yo)。

圖2 傳統(tǒng)APF受力分析

設(shè)定目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力場(chǎng)表示為Uatt(j),障礙物對(duì)機(jī)器人產(chǎn)生的斥力場(chǎng)表示為Urep(j),即機(jī)器人最終受到的合力場(chǎng)U(j),則合力如式(1)所示:

U(j)=Uatt(j)+Urep(j)

(1)

其中,引力場(chǎng)函數(shù)如式(2)所示:

(2)

式中,β為引力勢(shì)場(chǎng)增益常數(shù),β>0;‖j-jobj‖為機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)的距離。機(jī)器人與障礙物之間的斥力勢(shì)場(chǎng)函數(shù)如式(3)所示

(3)

勢(shì)場(chǎng)力為負(fù)梯度時(shí),機(jī)器人受到的勢(shì)場(chǎng)引力如式(4)所示

Fatt(j)=-?Uatt(j)=β(jobj-j)

(4)

機(jī)器人受到的勢(shì)場(chǎng)斥力如式(5)所示

Frep(j)=-?Uatt(j)=

(5)

則機(jī)器人受到的合力如式(6)所示

(6)

2.3 APF法的缺陷

TAPF法不同于其它智能算法,不需要進(jìn)行全局路徑規(guī)劃找出全局關(guān)鍵節(jié)點(diǎn),因此其行進(jìn)路線都是利用障礙物斥力及目標(biāo)點(diǎn)引力的合力來(lái)推動(dòng)機(jī)器人前進(jìn)。一旦途中沒(méi)有受到力的驅(qū)使或力達(dá)到平衡狀態(tài)時(shí),就會(huì)出現(xiàn)以下問(wèn)題:

1)由于力的不平衡才驅(qū)使機(jī)器人可以向目標(biāo)點(diǎn)移動(dòng),一旦行徑策略中存在某點(diǎn)使其達(dá)到合力平衡的效果,機(jī)器人會(huì)產(chǎn)生目標(biāo)不可達(dá)現(xiàn)象,如圖3所示。

圖3 傳統(tǒng)APF目標(biāo)不可達(dá)

2)機(jī)器人行進(jìn)策略位于兩個(gè)障礙物夾縫之中或遇到L型等復(fù)雜障礙物時(shí),極易陷入徘徊抖動(dòng)造成振蕩狀態(tài),如圖4所示。

圖4 傳統(tǒng)APF“振蕩”

3 改進(jìn)TAPF算法

3.1 改進(jìn)斥力場(chǎng)函數(shù)

針對(duì)機(jī)器人在TAPF存在合力平衡導(dǎo)致目標(biāo)不可達(dá)現(xiàn)象,引入機(jī)器人與目標(biāo)點(diǎn)之間的動(dòng)態(tài)影響距離參數(shù),在每個(gè)障礙物斥力范圍之內(nèi),將其代入斥力場(chǎng)函數(shù)式

(7)

(8)

(9)

其中,Fm(j)表示機(jī)器人朝向與障礙物相反,Fn(j)表示機(jī)器人朝向與障礙物同向。

考慮改進(jìn)的改進(jìn)斥力函數(shù)打破力的平衡,因此引入?yún)f(xié)調(diào)力Fx,其定義為

(10)

式中,η為協(xié)調(diào)系數(shù),η的取值范圍在0≤η≤1,一般取值0.5。由此,改進(jìn)后人工勢(shì)場(chǎng)合力函數(shù)為

(11)

3.2 虛擬障礙點(diǎn)

在機(jī)器人行進(jìn)策略中,遇到L型等障礙物或在障礙物中間穿行會(huì)出現(xiàn)振蕩情況,機(jī)器人就陷入死鎖。本文提出利用自適應(yīng)偏轉(zhuǎn)角根據(jù)障礙物的分布情況引入額外虛擬障礙點(diǎn),當(dāng)機(jī)器人陷入振蕩情況時(shí),利用虛擬障礙點(diǎn)提供的斥力使其合力發(fā)生改變推動(dòng)機(jī)器人向目標(biāo)點(diǎn)移動(dòng)。在接下來(lái)行進(jìn)過(guò)程中再遇此情況,重復(fù)進(jìn)行以上操作,直到機(jī)器人到達(dá)目標(biāo)點(diǎn)為止。

3.2 自適應(yīng)切線偏轉(zhuǎn)角

當(dāng)機(jī)器人在進(jìn)行過(guò)程中遇到復(fù)雜結(jié)構(gòu)障礙物時(shí),例如U型障礙物、L型障礙物,機(jī)器人會(huì)產(chǎn)生振蕩狀態(tài)即陷入局部最小值中。當(dāng)機(jī)器人處于振蕩狀態(tài)時(shí),以機(jī)器人當(dāng)前位置為切點(diǎn),做當(dāng)前振蕩軌跡的切線l1,如圖5所示;以當(dāng)前切線為基準(zhǔn)線建立坐標(biāo)軸;再以機(jī)器人為圓心,距離機(jī)器人受到斥力最遠(yuǎn)障礙物距離Ri為半徑順時(shí)針做圓;機(jī)器人與最遠(yuǎn)障礙物之間的距離為dis(Robot,Obsm),利用式(12)計(jì)算得出自適應(yīng)切線l1的偏轉(zhuǎn)角ζ的大小,將切線l1順時(shí)針旋轉(zhuǎn)偏轉(zhuǎn)角ζ得直線l2與圓相交得兩點(diǎn)γ1與γ2;將最遠(yuǎn)障礙物另一側(cè)的為設(shè)定的虛擬障礙點(diǎn)Obsset,如圖6所示。

圖5 軌跡切線示意圖

圖6 設(shè)定模擬障礙點(diǎn)示意圖

自適應(yīng)切線偏轉(zhuǎn)角ζ可表示為

(12)

4 改進(jìn)APF算法流程

本文改進(jìn)傳統(tǒng)APF避障算法,解決傳統(tǒng)APF在做局部路徑規(guī)劃時(shí)遇到的目標(biāo)不可達(dá)以及振蕩現(xiàn)象。其具體步驟如下:

步驟 1:初始化柵格地圖及算法參數(shù)。

步驟2:確定柵格地圖中機(jī)器人的初始位置及機(jī)器人的目標(biāo)點(diǎn)。

步驟3:利用傳統(tǒng)APF法計(jì)算機(jī)器人在行進(jìn)過(guò)程中每一步的斥力場(chǎng)及引力場(chǎng)。

步驟4:通過(guò)機(jī)器人周?chē)恼系K物計(jì)算出斥力與目標(biāo)點(diǎn)對(duì)其的引力得出合力,判斷合力是否為零,進(jìn)而判斷是否遇到傳統(tǒng)APF法的兩大問(wèn)題之一,使用改進(jìn)型斥力場(chǎng)函數(shù),尋找新的局部路徑;通過(guò)機(jī)器人判斷,對(duì)于仿真中出現(xiàn)振蕩狀態(tài)從而陷入局部最小值問(wèn)題時(shí),利用自適應(yīng)偏轉(zhuǎn)角ζ找出位于最遠(yuǎn)障礙物另一側(cè)的虛擬障礙點(diǎn),添加額外斥力使得機(jī)器人擺脫局部最小值。

步驟5:通過(guò)APF法計(jì)算下一時(shí)刻機(jī)器人前進(jìn)位置,最終抵達(dá)終點(diǎn),如再遇上述問(wèn)題返回步驟4重新找尋新的下一步路徑。

改進(jìn)融合算法的機(jī)器人路徑規(guī)劃流程圖,如圖7所示。

圖7 改進(jìn)APF算法的機(jī)器人避障示意圖

5 仿真及結(jié)果分析

為驗(yàn)證本文所提算法的可行性以及普適性,利用MATLAB2016a仿真平臺(tái)分別在20×20、25×25、30×30三種大小的柵格地圖下進(jìn)行改進(jìn)后避障算法仿真。

將本文所提的改進(jìn)APF算法與TAPF算法以及文獻(xiàn)[4]所提A-APF算法分別在三種大小的地圖中進(jìn)行對(duì)比實(shí)驗(yàn)。將移動(dòng)機(jī)器人視為一個(gè)質(zhì)點(diǎn),黑色矩形代表柵格地圖中的障礙物,綠色正三角S表示起點(diǎn),紅色正三角T表示目標(biāo)點(diǎn)。主要設(shè)置參數(shù)如表(1)所示,環(huán)境一、二、三中障礙率分別為0.075、0.136、0.1444,如圖8-圖10所示。

表1 仿真參數(shù)設(shè)定

圖8 環(huán)境一下三種算法的路徑規(guī)劃圖

圖9 環(huán)境二下三種算法的路徑規(guī)劃圖

圖10 環(huán)境三下三種算法的路徑規(guī)劃圖

從圖8-圖10可以看出,在不同障礙率和不同尺寸的環(huán)境下,TAPF算法面對(duì)復(fù)雜障礙物通常會(huì)在行進(jìn)途中陷入局部極小值從而走入死胡同,A-APF算法提出虛擬目標(biāo)點(diǎn)方法以及本文所提利用自適應(yīng)切線角所確定虛擬障礙點(diǎn)算法均能規(guī)劃出完整路徑。如表2-4所示,給出三種環(huán)境下,三種不同算法的性能參數(shù)。從表2-4可以看出:

表2 20×20復(fù)雜環(huán)境地圖中仿真數(shù)據(jù)

表3 25×25復(fù)雜環(huán)境地圖中仿真數(shù)據(jù)

表4 30×30復(fù)雜環(huán)境地圖中仿真數(shù)據(jù)

在20×20的柵格地圖環(huán)境中,TAPF算法由于復(fù)雜障礙物存在陷入局部極小值,無(wú)法規(guī)劃完整路線,而本文算法較A-APF算法在步長(zhǎng)、時(shí)間方面分別降低了14.2%、13.1%;

在25×25的柵格地圖環(huán)境中,TAPF算法規(guī)劃出完整路線,但振蕩頻率較大,而本文算法較TAPF及A-APF算法在步長(zhǎng)、時(shí)間方面分別降低了34.2%、26.3%,20.6%、15.8%;

在30×30的柵格地圖環(huán)境中,TAPF算法因復(fù)雜障礙物無(wú)法規(guī)劃出完整路線,而本文算法較A-APF算法在步長(zhǎng)、時(shí)間方面分別降低了9.6%、12.5%;相同仿真環(huán)境下,利用TAPF、本文所提算法對(duì)TAPF算法所存在兩個(gè)缺陷進(jìn)行對(duì)比仿真,如圖11、圖12所示。

圖11 TAP目標(biāo)不可達(dá)情況及改進(jìn)

圖12 TAPF陷入局部極小值情況及改進(jìn)

如圖11所示,TAPF面對(duì)合力為零導(dǎo)致目標(biāo)不可達(dá)就此算法進(jìn)入死胡同,本文所提算法改進(jìn)斥力場(chǎng)函數(shù)引入動(dòng)態(tài)距離影響參數(shù)以及斥力場(chǎng)調(diào)節(jié)因子,避開(kāi)了合力為零的狀態(tài);如圖12所示,面對(duì)復(fù)雜障礙物例如L型障礙物等,TAPF會(huì)陷入局部極小值出現(xiàn)振蕩現(xiàn)象,TAPF又會(huì)進(jìn)入死胡同,本文改進(jìn)后的算法提出自適應(yīng)切線偏轉(zhuǎn)角,確定合適的虛擬障礙點(diǎn),從而使得機(jī)器人擺脫局部極小值,順利到達(dá)目標(biāo)點(diǎn)。

通過(guò)上述分析可以看出本文所提算法與TAPF、A-APF算法在路徑長(zhǎng)度、時(shí)間都有較大的優(yōu)化;在路徑長(zhǎng)度方面與A-APF算法相差不多,但本文所提算法的時(shí)間以及平滑度與A-APF算法相比有明顯提高。綜上分析,本文所提改進(jìn)TAPF避障算法有進(jìn)一步的改進(jìn),路徑更平滑,拐點(diǎn)更少,通過(guò)不同大小環(huán)境對(duì)比而言更加具有普適性。

6 實(shí)際論證

為了驗(yàn)證在實(shí)際環(huán)境中本文所提算法的有效性以及可靠性,將本文所提改進(jìn)TAPF避障算法應(yīng)用到圖13(a)中的輪式激光雷達(dá)機(jī)器人中,進(jìn)行實(shí)物論證。利用激光雷達(dá)對(duì)周?chē)h(huán)境進(jìn)行數(shù)據(jù)掃描,如圖13(b)所示。

圖13 實(shí)際驗(yàn)證環(huán)境

如圖14所示,為實(shí)驗(yàn)驗(yàn)證過(guò)程圖。黑色方框?yàn)檩喪街悄軝C(jī)器人,粉紅色為實(shí)際障礙物,紅色線為智能機(jī)器人局部路徑規(guī)劃路線。

圖14 輪式機(jī)器人行進(jìn)策略軌跡圖

起點(diǎn)為圖中紅綠交叉直角處,設(shè)置好后,智能機(jī)器人向目標(biāo)點(diǎn)移動(dòng),通過(guò)激光雷達(dá)掃描到障礙物時(shí),通過(guò)改進(jìn)算法可以成功避開(kāi)障礙物,繼續(xù)向目標(biāo)點(diǎn)前進(jìn)。規(guī)避障礙物軌跡如圖14(a)-圖14(d)所示,通過(guò)圖15(a)、圖15(b)可以看出規(guī)劃局部路線時(shí)能夠與障礙物保持一定的安全距離并且避障軌跡較為平滑,符合機(jī)器人實(shí)際運(yùn)行動(dòng)力學(xué)特點(diǎn)。

圖15 輪式機(jī)器人實(shí)際避障圖

7 結(jié)論

1)針對(duì)TPF算法目標(biāo)不可達(dá)情況,本文對(duì)其傳統(tǒng)斥力場(chǎng)函數(shù)進(jìn)行改進(jìn),引入機(jī)器人與目標(biāo)點(diǎn)之間影響距離以及障礙物斥力場(chǎng)調(diào)節(jié)因子,通過(guò)仿真與TAPF和文獻(xiàn)[4]所提算法A-APF進(jìn)行對(duì)比,本文優(yōu)化算法路徑更加平滑,用時(shí)更少。

2)針對(duì)復(fù)雜環(huán)境下TAPF會(huì)出現(xiàn)振蕩陷入死區(qū)現(xiàn)象,本文提出自適應(yīng)偏轉(zhuǎn)角確定虛擬障礙點(diǎn),通過(guò)額外斥力使得機(jī)器人擺脫局部極小值,繼續(xù)向目標(biāo)點(diǎn)前進(jìn)直到到達(dá)目標(biāo)點(diǎn)。

3)本文利用ROS輪式智能機(jī)器人進(jìn)行算法實(shí)際測(cè)試,測(cè)試結(jié)果顯示,在復(fù)雜環(huán)境下機(jī)器人也能夠安全,快速的進(jìn)行障礙物避障直到抵達(dá)設(shè)定目標(biāo)點(diǎn)。

綜上,本文所提避障算法能夠?yàn)橹悄軝C(jī)器人行進(jìn)策略規(guī)劃好安全路線,為智能駕駛提供一定的行進(jìn)決策借鑒意義。