基于改進人工勢場法的農(nóng)用機器人路徑規(guī)劃技術(shù)研究*

邵毅帆，梅松，石志剛，宋志禹，童一飛

(1. 南京理工大學機械工程學院,南京市,210094; 2. 農(nóng)業(yè)農(nóng)村部南京農(nóng)業(yè)機械化研究所,南京市,210014;3. 寧夏農(nóng)林科學院枸杞工程技術(shù)研究所,銀川市,750002)

0 引言

在農(nóng)業(yè)生產(chǎn)中,農(nóng)用移動機器人需要利用路徑規(guī)劃算法計算出機器人到達目標工作位置的路徑。路徑規(guī)劃的運行效率以及路徑的平滑性,決定農(nóng)用移動機器人在果蔬采摘以及播種等工作的運行效率以及運行穩(wěn)定性,不同的路徑規(guī)劃算法應用于不同的農(nóng)用生產(chǎn)環(huán)境都存在著不同的問題,如何選用一種適用于特定場地的高效農(nóng)用移動機器人路徑規(guī)劃方法,是農(nóng)業(yè)生產(chǎn)現(xiàn)代化的一個研究熱點。因此,研究農(nóng)用移動機器人的路徑規(guī)劃技術(shù),實現(xiàn)目標位置的快速平穩(wěn)到達,對農(nóng)業(yè)裝備的現(xiàn)代化發(fā)展有著重要的意義。

路徑規(guī)劃是移動機器人研究領域的重要研究方向,一般依照某個性能指標如工作能耗最小、行走時間最少和行走路線最短等,在工作空間內(nèi)找到一條從起點到終點并可以避開環(huán)境障礙物的最優(yōu)或近似最優(yōu)的路徑。在現(xiàn)有的研究中,路徑規(guī)劃主要根據(jù)對環(huán)境信息的掌握程度分為全局路徑規(guī)劃[1]和局部路徑規(guī)劃[2],主要分為A-star算法[3]、RRT算法[4]、人工勢場法[5]、蟻群算法[6]、遺傳算法[7]和粒子群算法[8]等。其中,A-star算法、RRT算法和人工勢場法為傳統(tǒng)路徑規(guī)劃算法,蟻群算法、遺傳算法和粒子群算法為智能路徑規(guī)劃算法。

移動機器人的路徑規(guī)劃要根據(jù)各算法應用場景進行選擇,路徑規(guī)劃算法需根據(jù)環(huán)境建立關(guān)于目標位置與障礙物的代價函數(shù),通過求取最速梯度下降方向驅(qū)動機器人移動[9],因此,機器人在復雜環(huán)境中路徑進行規(guī)劃時,智能路徑規(guī)劃算法和全局路徑規(guī)劃算法需要進行大量的計算,無法保證運行的實時性,采用人工勢場法能保證移動機器人的運行實時性,但傳統(tǒng)人工勢場法仍存在局部最小值問題、目標不可達以及路徑不平穩(wěn)等問題[10]。王麗[11]在機器人當前位置圓周上均勻取點,向勢場最小方向運行;郭梟鵬[12]對修正勢場函數(shù)和距離調(diào)整因子,使修正后的斥力為勢能最小點。因此需根據(jù)農(nóng)用機器人運行場地及現(xiàn)場仿真對算法進行優(yōu)化及改進。

本文對人工勢場法的原理進行闡述,針對傳統(tǒng)人工勢場法中存在的局部最小值點問題、目標不可達問題和運行轉(zhuǎn)向角大的問題,分別采用勢能場修正函數(shù)、Follow-Wall改進方法和最大轉(zhuǎn)向角約束函數(shù),建立適用于果蔬農(nóng)用機器人的改進人工勢場法,建立柵格環(huán)境模型,并對改進前后的人工勢場法進行比較,滿足農(nóng)用移動機器人對路徑規(guī)劃計算高效、運行平穩(wěn)的要求。

1 人工勢場法原理

人工勢場法是由Khatib[5]首先提出將障礙物和目標抽象為虛擬勢能場,通過建立對應勢能場函數(shù)解決機械臂避障問題的一種算法。該算法通過建立關(guān)于障礙物的斥力場函數(shù)和關(guān)于目標的引力場函數(shù),確定機器人的行進方向,并確保不會與障礙物碰撞,以實現(xiàn)路徑規(guī)劃的功能。

在利用人工勢場法對果蔬農(nóng)用機器人移動路徑進行規(guī)劃時,把果蔬基地內(nèi)機器人看成一個質(zhì)點,將起始位置向目標位置移動的過程看成求解勢場最優(yōu)方向。將基地中障礙物看成斥力產(chǎn)生源,通過計算合力大小及方向,驅(qū)動農(nóng)用機器人移動,詳細參數(shù)如圖1所示。

圖1 人工勢場法原理圖

為了實現(xiàn)移動方向的計算,假設農(nóng)用機器人所在位置為X=(x,y),Xt=(xt,yt)為目標位置向量,則目標位置與農(nóng)用機器人之間的引力場

(1)

式中:ρ(X,Xt)——農(nóng)用機器人與目標位置的距離;

Katt——引力勢能場常量。

可得引力

Fatt(X)=-?Uatt=-Katt|X-Xt|

(2)

由于農(nóng)用機器人與障礙物間距離對規(guī)劃路徑會產(chǎn)生不同程度的影響,因此設置以地障礙物為中心的斥力場影響半徑為ρ0,可得斥力場

(3)

Krep——斥力勢場常量。

定義斥力勢場的負梯度方向為農(nóng)用機器人所受斥力方向,可得

(4)

對目標位置產(chǎn)生的引力和所有障礙物產(chǎn)生的斥力進行矢量和求解,可得合力

(5)

式中:i——障礙物的個數(shù);

Fatt(X)——目標位置產(chǎn)生對農(nóng)用機器人的斥力;

2 傳統(tǒng)人工勢場法存在問題及改進方法

2.1 傳統(tǒng)人工勢場法存在問題

常規(guī)人工勢場算法中需要對每個障礙物斥力場產(chǎn)生的斥力進行計算,能夠很好保證機器人運行時不會與障礙物發(fā)生碰撞,但在面對相對復雜多障礙物或特殊障礙物與目標位置放置組合時會出現(xiàn)機器人無法到達目標位置的情況。

2.1.1 目標不可達問題

當障礙物接近目標位置時,農(nóng)用機器人會很大幾率出現(xiàn)目標不可達問題。當目標不可達問題出現(xiàn)時,農(nóng)用機器人會在目標位置附近不斷迂回,無法完整路徑規(guī)劃,目標不可達問題示意如圖2所示。

圖2 目標不可達問題示意圖

如圖2所示,當農(nóng)用機器人向目標位置靠近時,會受到障礙物產(chǎn)生的斥力,此時目標位置處產(chǎn)生的合力小于障礙物產(chǎn)生的合斥力,機器人會向遠離目標位置的方向移動;當機器人與目標位置的距離變大后,此時受到的斥力變小,引力變大,機器人會沿著合力方向即向目標位置處靠近,因此會出現(xiàn)機器人循環(huán)往復移動但始終無法達到目標位置的情況。由于果蔬基地中障礙物較多,在機器人運行中很容易出現(xiàn)圖2的障礙物目標位置組合,因此需要對該種目標不可達到問題進行改進。

2.1.2 局部極小值問題

當農(nóng)用機器人、障礙物和目標位置位于同一個直線上或者多個特殊組合時,就會出現(xiàn)局部極小值問題。該種情況出現(xiàn)時,機器人會到達局部最小值位置而不再運行,算法無法繼續(xù)運行,局部極小值問題示意圖如圖3所示。

圖3 局部極小值問題示意圖

如圖3所示,障礙物產(chǎn)生的斥力或多個障礙物產(chǎn)生的總斥力與目標位置產(chǎn)生的引力在同一條直線上時,就會出現(xiàn)局部極小值問題。農(nóng)用機器人受到的勢能場引力和總斥力最終會出現(xiàn)數(shù)值相同、方向相反的情況。基于勢能場的人工勢場法無法為機器人提供下一步的移動方向,此時農(nóng)用機器人陷入了局部最小值,農(nóng)用機器人停在原地不動。

2.2 傳統(tǒng)人工勢場法的改進

根據(jù)2.1節(jié)中描述的傳統(tǒng)人工勢場法在運行時出現(xiàn)的問題,提出了人工勢場法改進方法,以滿足果蔬基地中農(nóng)用機器人對路徑規(guī)劃需求。方法主要為勢能場修正、Follow-Wall算法和最大轉(zhuǎn)向角約束。

2.2.1 勢能場修正

由于本文果蔬基地中容易出現(xiàn)障礙物位于目標位置前,這種特殊的目標與障礙物組合會導致目標不可達問題的出現(xiàn),因此要對傳統(tǒng)人工勢場法存在的問題進行改進。

目標不可達問題出現(xiàn)的主要原因是機器人靠近目標位置時,障礙物產(chǎn)生的斥力會使得機器人在目標位置附近反復震蕩,因此對人工勢場法的改進需要對斥力場進行修正[11]。定義一個與距離相關(guān)的修正項ρm(X,Xt),可得修正后斥力場函數(shù)

(6)

m——勢能場修正系數(shù),m>0。

對式(5)梯度求導可得斥力

(7)

(8)

(9)

(10)

由于式(7)中包含了勢能場修正系數(shù)m,該系數(shù)對修正后的勢能場函數(shù)有較大影響。根據(jù)對m的分類討論采用的m范圍為0

圖4 斥力場修正后的人工勢場法示意圖

2.2.2 Follow-Wall算法

該算法主要用于改進人工勢場法運行中出現(xiàn)局部最小值的問題,為了讓農(nóng)用系統(tǒng)機器人逃離局部最小值點,引入改進的Follow-Wall算法[13]。這種改進的Follow-Wall算法能夠?qū)崿F(xiàn)當農(nóng)用機器人判斷進入局部最小值時,沿著障礙物四周進行巡線,通過判斷障礙物和目標之間的位置關(guān)系,實現(xiàn)對局部最小值問題的改進。

根據(jù)式(3),傳統(tǒng)人工勢場法中設置了障礙物與農(nóng)用機器人之間的影響距離ρ0,能夠?qū)崿F(xiàn)農(nóng)用機器人在一定的范圍內(nèi)對障礙物進行斥力的計算,保證運行的效率,在這個基礎上增設一個接近距離,利用對障礙物的影響范圍二次分層,實現(xiàn)Follow-Wall算法的運行,運行示意圖如圖5所示。

(a) 當ρ≥ρ0時

如圖5所示,判斷進入局部極小值后開始利用二次分層法驅(qū)動農(nóng)用機器人沿障礙物運行。當ρ≥ρ0時,農(nóng)用機器人只受到來自目標位置的引力,運行方向為此時合力的方向,能夠保證農(nóng)用機器人在運動時快速接近障礙物;當ρ1≤ρ<ρ0時,農(nóng)用機器人受到來自目標位置的引力和障礙物的斥力,機器人不斷向障礙物接近時出現(xiàn)局部最小值;當0≤ρ<ρ1時,ρ1大于局部最小值出現(xiàn)的停止位置,定義一個虛擬引力,引力大小等于目標位置產(chǎn)生的引力,方向為逆時針90°垂直于斥力方向。

如圖6所示,農(nóng)用機器人會不斷圍繞障礙物運動,無法向目標位置的移動。為了實現(xiàn)Follow-Wall算法的退出,對障礙物、目標位置和農(nóng)用機器人的位置關(guān)系進行判斷,當障礙物與目標位置處于機器人兩側(cè)且夾角φ超過90°,則繼續(xù)人工勢場法,基于Follow-Wall的改進人工勢場法流程如圖7所示。

圖6 Follow-Wall算法退出示意圖

圖7 基于Follow-Wall的改進人工勢場法流程圖

如圖7所示,基于Follow-Wall算法的人工勢場法通過判斷是否進入極小值點來進入Follow-Wall算法的子程序,并利用是否退出極小值點和障礙物與目標點的位置關(guān)系,實現(xiàn)Follow-Wall算法的退出。

根據(jù)農(nóng)用機器人在果蔬中運行時可能出現(xiàn)的障礙物與目標位置的組合進行算法仿真,可得基于Follow-Wall的改進人工勢場法對比圖,結(jié)果如圖8所示。

(a) 局部極小值問題改進前

當農(nóng)用機器人位于如圖8(a)所示位置處,農(nóng)用機器人處于停止狀態(tài),無法計算下一步的移動方向;使用基于Follow-Wall的改進人工勢場法后,當檢測到農(nóng)用機器人運行至局部極小值時,開始進行Follow-Wall算法,實現(xiàn)對障礙物的圍繞,并能根據(jù)障礙物與目標位置的位置關(guān)系判斷,繼續(xù)使用傳統(tǒng)人工勢場法對路徑進行規(guī)劃,該方法能夠解決局部極小值問題。

2.2.3 最大轉(zhuǎn)向角約束

由于農(nóng)用機器人相比于其他平面移動機器人高度較高,因此,頻繁轉(zhuǎn)向會降低運行精度并導致運行不穩(wěn)定,為保證路徑的平滑性與農(nóng)用機器人的運行穩(wěn)定性,需要對傳統(tǒng)的人工勢場法的轉(zhuǎn)動角度進行限制。

最大轉(zhuǎn)向角是用來限制農(nóng)用機器人在地面行進時的最大轉(zhuǎn)彎范圍,可以保障農(nóng)用機器人不會因為頻繁轉(zhuǎn)向而導致運行不穩(wěn)定。定義農(nóng)用機器人最小轉(zhuǎn)彎半徑為R,根據(jù)人工勢場法運行時步長s,可以得到農(nóng)用機器人的最大轉(zhuǎn)向角φ以及受到最大轉(zhuǎn)向角約束的新位置,示意圖如圖9所示。

圖9 最大轉(zhuǎn)向角示意圖

(11)

3 基于改進人工勢場法路徑仿真與結(jié)果分析

3.1 試驗環(huán)境模型搭建

由于路徑規(guī)劃對環(huán)境模型依賴程度較高,不同環(huán)境模型對路徑規(guī)劃算法的運行效率有較大影響,為保證農(nóng)用機器人能夠根據(jù)果蔬基地(圖10)環(huán)境模型進行路徑規(guī)劃,需要對設施蔬菜基地模型進行建立。

圖10 試驗基地現(xiàn)場圖

環(huán)境模型主要分為柵格法[14]和自由空間法[15],由于果蔬基地中常見障礙物形狀均為矩形,且自由空間法計算量較大,不能保證運行的實時性[16]。因此,采用柵格法對障礙物和試驗基地進行建模。

對用于試驗的果蔬基地進行測量,可得基地機器人運行區(qū)域長度為25 m,寬度為10 m。對障礙物的長寬進行測量,并根據(jù)進行試驗的農(nóng)用機器人的大小,設置柵格的大小為1 m,繪制的柵格圖如圖11所示,其中黑色方塊為障礙物。

圖11 試驗基地環(huán)境柵格圖

3.2 人工勢場法參數(shù)選擇

由于傳統(tǒng)人工勢場法通常根據(jù)經(jīng)驗對障礙物影響范圍ρ0、引力增益系數(shù)Katt和斥力增益系數(shù)Krep進行選擇。其中,根據(jù)柵格大小為1 m×1 m以及機器人運行步長0.5 m,設置障礙物影響ρ0為2,可實現(xiàn)農(nóng)用機器人對下一步路徑移動前2 m范圍內(nèi)所有障礙物產(chǎn)生的合斥力進行計算,并能在降低計算量的基礎上保證農(nóng)用機器人不與障礙物發(fā)生碰撞。

而引力增益系數(shù)Katt和斥力增益系數(shù)Krep通常憑借經(jīng)驗進行選擇,為了選取適用于果蔬基地試驗的最優(yōu)算法運行參數(shù),需要對不同經(jīng)驗參數(shù)值進行規(guī)劃路徑比較,常用經(jīng)驗參數(shù)取值如表1所示。其中,Katt取值范圍通常大于1,能夠保證機器人在離目標位置較遠時仍有較大引力;Krep取值范圍應大于0小于1,該值與Katt相對應,若選取的參數(shù)大于Katt則會出現(xiàn)目標不可達問題。

表1 人工勢場法參數(shù)取值表Tab. 1 Magnetic field matching results

路徑規(guī)劃的主要目標是找到一條長度較短且能夠避開所有障礙物的路徑,因此將路徑長度作為衡量人工勢場法選用參數(shù)是否最優(yōu)的指標。設置三條盡量不出現(xiàn)局部極小值的路徑。其中,路徑1的起點為(2,1),終點為(5.5,8.5);路徑2的起點為(10.5,1),終點為(7.5,7.5);路徑3的起點為(18.5,1),終點為(23.5,7)。路徑如圖12所示。

根據(jù)表1參數(shù)取值對人工勢場法路徑長度進行計算,計算結(jié)果如表2所示。

表2 人工勢場法參數(shù)試驗結(jié)果表Tab. 2 Experimental results of artificial potential field method

根據(jù)表2的結(jié)果可知,當系數(shù)Katt=2,Krep=0.4和Katt=2,Krep=0.3時,農(nóng)用機器人運行的路徑長度均較小,但為了保證農(nóng)用機器人不會與障礙物發(fā)生碰撞,最終選擇Katt=2,Krep=0.4。

3.3 人工勢場法仿真結(jié)果對比

為了驗證改進后人工勢場法的運行效果,利用圖12中柵格環(huán)境模型進行農(nóng)用機器人路徑規(guī)劃仿真。其中,農(nóng)用機器人運行的起點為(2,1),目標位置為(15.5,9.5),引力增益系數(shù)為2,斥力增益系數(shù)為0.4,試驗基地障礙物影響范圍為2,對2.2節(jié)中改進后人工勢場法與傳統(tǒng)人工勢場法進行比較。

3.3.1 局部最小值改進對比

針對2.2節(jié)中基于Follow-Wall的改進人工勢場法的對比結(jié)果如圖13所示。

(a) 沒有改進局部最小值問題

根據(jù)圖13可知,圖13 (a)為未改進局部最小值的人工勢場法,當處于局部最小值位置時農(nóng)用機器人處于停滯狀態(tài),無法實現(xiàn)農(nóng)用機器人向目標位置的路徑規(guī)劃。圖13 (b)為使用了Follow-Wall算法的改進人工勢場法,農(nóng)用機器人能利用沿著障礙物逃逸出局部最小值,最終通過判斷障礙物與機器人的位置關(guān)系跳出Follow-Wall算法。

3.3.2 目標不可達問題改進對比

根據(jù)2.2節(jié)中對目標不可達問題進行了改進方法的研究,采用了對勢能場函數(shù)進行優(yōu)化,其中勢能場修正系數(shù)m并沒有進行賦值,根據(jù)李奕銘等[17-19]在論文中修正系數(shù)的經(jīng)驗取值,選擇勢能場修正系數(shù)m=0.9,修正后的人工勢場法如圖14所示。

圖14 基于勢能場修正的改進人工勢場法對比圖

根據(jù)圖14可知,直線路徑為勢能場函數(shù)改進前的路徑,虛線路徑為基于勢能場修正的人工勢場法。根據(jù)直線圈出的規(guī)劃路徑可以看出,當農(nóng)用機器人接近障礙物時,受到的來自目標位置的引力變大,受到障礙物產(chǎn)生的斥力相應減小。并且使用基于勢能場函數(shù)修正能夠在一定程度上降低局部極小值點出現(xiàn)的幾率,避免農(nóng)用機器人采用人工勢場法路徑規(guī)劃時無解情況的出現(xiàn)。對運行時農(nóng)用機器人移動的距離進行計算,可知改進前農(nóng)用機器人的路徑長度為18.72 m,改進后的農(nóng)用機器人的路徑長為16.83 m,使用勢能場對人工勢場法進行修正降低了10%的路徑長度,并在一定程度上平滑了規(guī)劃路徑曲線。

3.3.3 最大轉(zhuǎn)向角改進對比

為了保證農(nóng)用機器人在果蔬基地中運行的平穩(wěn)性,在2.2節(jié)中采用了對人工勢場法運行最大轉(zhuǎn)向角進行限制,可得基于最大轉(zhuǎn)向角的人工勢場法對比如圖15所示。

對圖15中基于最大轉(zhuǎn)向角的人工勢場法和未考慮最大轉(zhuǎn)向角的人工勢場法進行農(nóng)用機器人運行路程和累積轉(zhuǎn)折角進行統(tǒng)計,統(tǒng)計結(jié)果如表3所示。

表3 基于最大轉(zhuǎn)向角的人工勢場法對比Tab. 3 Comparison of artificial potential field method based on maximum steering angle

對人工勢場法運行時農(nóng)用機器人最大轉(zhuǎn)向角進行限制后,規(guī)劃的路徑相比之前的路徑規(guī)劃結(jié)果路徑長度稍有增加,但是累計轉(zhuǎn)折角從原來的165.2°降低至143.3°,降低了13.3%,大大減小了農(nóng)用機器人在果蔬試驗基地中運行的轉(zhuǎn)向角,保證了農(nóng)用機器人的運行穩(wěn)定性。

4 結(jié)論

1) 本文對人工勢場法的原理進行闡述,針對傳統(tǒng)人工勢場法中存在的局部最小值點問題、目標不可達問題和運行轉(zhuǎn)向角大的問題改進了算法。采用勢能場修正函數(shù)對局部最小值問題進行改進;采用Follow-Wall方法解決目標不可達問題,并在一定程度上減少了路徑規(guī)劃的長度;采用最大轉(zhuǎn)向角約束函數(shù)減少農(nóng)用移動機器人在運行時的頻繁轉(zhuǎn)向。

2) 根據(jù)試驗基地建立了柵格環(huán)境模型,采用改進人工勢場法進行路徑仿真,對改進前后的人工勢場法進行了結(jié)果比較,改進后的人工勢場法實現(xiàn)了農(nóng)用移動機器人逃逸局部最小值點,規(guī)劃路徑長度降低10%,累計轉(zhuǎn)折角降低13.3%。滿足農(nóng)用移動機器人對路徑規(guī)劃計算高效、運行平穩(wěn)的要求。