基于改進人工勢場法的農用機器人路徑規劃技術研究*

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

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

0 引言

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

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

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

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

1 人工勢場法原理

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

在利用人工勢場法對果蔬農用機器人移動路徑進行規劃時,把果蔬基地內機器人看成一個質點,將起始位置向目標位置移動的過程看成求解勢場最優方向。將基地中障礙物看成斥力產生源,通過計算合力大小及方向,驅動農用機器人移動,詳細參數如圖1所示。

圖1 人工勢場法原理圖

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

(1)

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

Katt——引力勢能場常量。

可得引力

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

(2)

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

(3)

Krep——斥力勢場常量。

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

(4)

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

(5)

式中:i——障礙物的個數;

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

2 傳統人工勢場法存在問題及改進方法

2.1 傳統人工勢場法存在問題

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

2.1.1 目標不可達問題

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

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

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

2.1.2 局部極小值問題

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

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

如圖3所示,障礙物產生的斥力或多個障礙物產生的總斥力與目標位置產生的引力在同一條直線上時,就會出現局部極小值問題。農用機器人受到的勢能場引力和總斥力最終會出現數值相同、方向相反的情況?；趧菽軋龅娜斯輬龇o法為機器人提供下一步的移動方向,此時農用機器人陷入了局部最小值,農用機器人停在原地不動。

2.2 傳統人工勢場法的改進

根據2.1節中描述的傳統人工勢場法在運行時出現的問題,提出了人工勢場法改進方法,以滿足果蔬基地中農用機器人對路徑規劃需求。方法主要為勢能場修正、Follow-Wall算法和最大轉向角約束。

2.2.1 勢能場修正

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

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

(6)

m——勢能場修正系數,m>0。

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

(7)

(8)

(9)

(10)

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

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

2.2.2 Follow-Wall算法

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

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

(a) 當ρ≥ρ0時

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

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

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

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

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

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

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

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

2.2.3 最大轉向角約束

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

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

圖9 最大轉向角示意圖

(11)

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

3.1 試驗環境模型搭建

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

圖10 試驗基地現場圖

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

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

圖11 試驗基地環境柵格圖

3.2 人工勢場法參數選擇

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

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

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

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

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

表2 人工勢場法參數試驗結果表Tab. 2 Experimental results of artificial potential field method

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

3.3 人工勢場法仿真結果對比

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

3.3.1 局部最小值改進對比

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

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

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

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

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

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

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

3.3.3 最大轉向角改進對比

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

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

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

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

4 結論

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

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