基于局部勢場Ａ算法和動態(tài)窗口法的農(nóng)用輪式機(jī)器人路徑規(guī)劃

劉順 閆榮蘭 臺浩嬌 姜春陽 許立成 王克凱

摘要：針對未知農(nóng)業(yè)生產(chǎn)環(huán)境中，現(xiàn)有移動機(jī)器人路徑規(guī)劃方法實時性差和適應(yīng)性弱等問題。在視覺傳感器實時定位和地圖構(gòu)建的基礎(chǔ)上，提出一種混合規(guī)劃方法用于農(nóng)用輪式機(jī)器人。在全局路徑規(guī)劃中使用局部勢場A*算法進(jìn)行路徑規(guī)劃，在局部路徑修正中使用改進(jìn)的動態(tài)窗口法進(jìn)行路徑修正。通過仿真將提出的混合路徑規(guī)劃方法與傳統(tǒng)路徑規(guī)劃方法進(jìn)行比較，驗證該方法的優(yōu)越性和魯棒性。結(jié)果表明，相對于傳統(tǒng)路徑規(guī)劃方法，混合路徑規(guī)劃方法通行代價降低63.89%，在保持最大偏移不變的情況下，搜索時間節(jié)省27.01%。該混合路徑規(guī)劃方法可以實現(xiàn)未知環(huán)境的規(guī)劃路徑，有很好的壁障和路徑規(guī)劃能力，具有一定的應(yīng)用價值。

關(guān)鍵詞：移動機(jī)器人;局部勢場A*算法;動態(tài)窗口法;混合路徑規(guī)劃方法;視覺傳感器

中圖分類號：S24;TP242 ??文獻(xiàn)標(biāo)志碼： A

文章編號：1002-1302（2022）02-0192-06

收稿日期：2021-05-17

基金項目：河北省青年科學(xué)支持基金（編號：18HB01661）。

作者簡介：劉 順（1988—），男，河北正定人，工程師，主要從事農(nóng)業(yè)自動控制等研究。E-mail：hbjyls@126.com。

隨著農(nóng)業(yè)智能技術(shù)的不斷發(fā)展，農(nóng)業(yè)機(jī)器人領(lǐng)域越來越受到重視。它是一種以農(nóng)業(yè)生產(chǎn)任務(wù)為主要目的，同時具有信息識別、路線識別、人工智能等前沿技術(shù)于一身，可以解決大量農(nóng)業(yè)生產(chǎn)力的不足[1]。輪式移動機(jī)器人以其運動穩(wěn)定靈活、能耗低、結(jié)構(gòu)簡單等優(yōu)點，在農(nóng)業(yè)領(lǐng)域廣泛應(yīng)用[2]。農(nóng)業(yè)機(jī)器人可以利用自動導(dǎo)航技術(shù)實現(xiàn)施肥、噴灑和除草等智能農(nóng)業(yè)作業(yè)，而路徑規(guī)劃是自主導(dǎo)航的重要基礎(chǔ)。因此，對農(nóng)用輪式機(jī)器人的路徑規(guī)劃方法進(jìn)行研究具有重要的現(xiàn)實意義。近年來，國內(nèi)外對輪式移動機(jī)器人的路線規(guī)劃方法進(jìn)行了大量的研究，并獲得許多優(yōu)異的成果。李鳳玲等提出一種適用于未知環(huán)境的路徑規(guī)劃方法，通過相機(jī)獲取周圍環(huán)境信息，通過螢火蟲算法進(jìn)行路徑規(guī)劃，以幫助機(jī)器人找到最佳路線并避免障礙物，仿真結(jié)果表明該算法具有有效性和實時性[3]。吳東林等設(shè)計一種基于人工勢場法的采摘機(jī)器人動態(tài)路徑規(guī)劃系統(tǒng)，并對系統(tǒng)方法進(jìn)行仿真分析，發(fā)現(xiàn)該系統(tǒng)優(yōu)化效果明顯，具有良好的避障和路徑規(guī)劃能力[4]。雷蕾等提出一種基于啟發(fā)式搜索算法的自動揀選機(jī)器人路徑規(guī)劃方法，通過改進(jìn)的A*算法進(jìn)行路徑規(guī)劃，發(fā)現(xiàn)改進(jìn)的A*算法可以明顯改善采摘機(jī)器人的行進(jìn)路徑，并使規(guī)劃路徑更平滑[5]。李國輝提出一種用于農(nóng)業(yè)機(jī)器人的壁障路徑智能規(guī)劃方法，通過分析農(nóng)用機(jī)器人的運動系統(tǒng)結(jié)構(gòu)和定位原理，建立農(nóng)用機(jī)器人的運動模型，完成避障路徑規(guī)劃的結(jié)構(gòu)設(shè)計，將模糊控制算法用于合理規(guī)劃農(nóng)業(yè)機(jī)器人的避障過程，發(fā)現(xiàn)農(nóng)業(yè)機(jī)器人可以成功避開障礙物[6]。上述研究存在算法復(fù)雜、內(nèi)存開銷大、實時性差等問題，但這些研究仍為農(nóng)業(yè)輪式機(jī)器人的路徑規(guī)劃方法提供了理論依據(jù)。本研究提出一種混合路徑規(guī)劃方法用于農(nóng)用輪式機(jī)器人。全局路徑規(guī)劃使用局部勢場A*（local potential field A*，LPF-A*）算法，而局部路徑校正使用自適應(yīng)參數(shù)動態(tài)窗口方法（adaptive parameters dynamic window approach，APDWA），應(yīng)用混合規(guī)劃算法可以有效避開周圍的障礙，找到最優(yōu)到達(dá)目標(biāo)點的路徑。

1 系統(tǒng)結(jié)構(gòu)

農(nóng)用輪式移動機(jī)器人路徑規(guī)劃方法的設(shè)計與移動機(jī)器人測試平臺密不可分，因此，本研究須要建立一種可在農(nóng)業(yè)環(huán)境下運行的輪式移動機(jī)器人的原型[7]。輪式移動機(jī)器人原型采用開放式結(jié)構(gòu)和控制系統(tǒng)，可安裝不同的農(nóng)具進(jìn)行不同的作業(yè)。圖1為農(nóng)用輪式移動機(jī)器人的系統(tǒng)結(jié)構(gòu)，該系統(tǒng)分為導(dǎo)航管理系統(tǒng)和機(jī)器人主體2個部分。導(dǎo)航管理系統(tǒng)具有遠(yuǎn)程控制機(jī)器人的功能，但側(cè)重于設(shè)置機(jī)器人的自主運動參數(shù)、自主作業(yè)并監(jiān)視機(jī)器人的運動狀態(tài)。機(jī)器人主體由控制系統(tǒng)、行走部分和操作裝置3個部分組成，負(fù)責(zé)分解、協(xié)調(diào)和執(zhí)行任務(wù)[8]。

2 混合路徑規(guī)劃方法

機(jī)器人環(huán)境用柵格法表示[9]。在生成的柵格地圖中，全局路徑規(guī)劃使用LPF-A*算法，而APDWA算法用于局部障礙物碰撞檢測和局部路徑修改。

2.1 LPF-A*全局路徑規(guī)劃

2.1.1 A*算法原理

A*算法通過啟發(fā)式函數(shù)對路徑的代價進(jìn)行估算，采用代價最低的節(jié)點作為下一個擴(kuò)展節(jié)點[10]。代價函數(shù)為

f（n）=g（n）+h（n）。（1）

式中：f（n）表示搜索節(jié)點的代價函數(shù);g（n）、h（n）分別表示實際代價值（起點-搜索節(jié)點）和評估代價值（搜索節(jié)點-終點）。

A*算法運行時，有2個列表（open和close），未訪問的節(jié)點在open列表中，已訪問的節(jié)點在close列表中[11]。具體算法步驟如下：步驟1，當(dāng)前節(jié)點為起點，加入close列表，設(shè)open列表為空。步驟2，確定當(dāng)前節(jié)點是否為目標(biāo)節(jié)點。如果是，則規(guī)劃完成[12];如果不是，請繼續(xù)下一步。步驟3，獲取與當(dāng)前節(jié)點相鄰的8個節(jié)點位置，并在每個節(jié)點上執(zhí)行以下操作。（1）判斷節(jié)點是否加入close列表。如果是，則忽略;如果不是，請繼續(xù)下一步。（2）判斷節(jié)點是否加入open列表，如果是，則重新計算g值。如果g值小于原始值，則用父節(jié)點替換當(dāng)前節(jié)點，對g值和f值重新計算，轉(zhuǎn)到步驟4，否則繼續(xù)下一步。（3）創(chuàng)建1個新節(jié)點，并計算新節(jié)點的g值、h值和f值，用父節(jié)點替換當(dāng)前節(jié)點[12]。步驟4，在open列表中找到f值最小的節(jié)點，并將其分配給當(dāng)前節(jié)點（current_node）。步驟5，當(dāng)前節(jié)點加入close列表，對open列表進(jìn)行判斷，為空則固化失敗，否則轉(zhuǎn)到“步驟2”。該算法的流程見圖2。

啟發(fā)式函數(shù)的選擇是A*算法成功的關(guān)鍵[13]。啟發(fā)式函數(shù)的估計值與實際值越接近，代價函數(shù)越準(zhǔn)確，且A*算法規(guī)劃的路徑越適合實際環(huán)境。

本研究使用8個鄰域路徑搜索算法，父節(jié)點可以搜索8個相鄰子節(jié)點。如果相鄰節(jié)點之間的距離為d，則對角節(jié)點距離為2d。本研究采用的啟發(fā)式函數(shù)如下[14]

h（n）=2min[d1（n），d2（n）]+|d1（n）-d2（n）|。（2）

式中：d1（n）、d2（n）分別表示當(dāng)前節(jié)點與目標(biāo)節(jié)點的橫坐標(biāo)、縱坐標(biāo)之差。

為了找到通行代價最低的路徑，須要結(jié)合先前的局部勢場函數(shù)S（n），并將其引入代價函數(shù)f（n）中，從而獲得更安全有效的路徑。代價函數(shù)變換見公式（3）[15]

f（n）=g（n）+h（n）+ωS（n）。（3）

式中：ω表示權(quán)重因子，值越大，則搜索節(jié)點離障礙物越遠(yuǎn)，但整體搜索路徑越長。

2.1.2 柵格地圖優(yōu)化

Elfes最早提出柵格地圖模型[16]。環(huán)境中的障礙模型用網(wǎng)格占有率表示，此方法廣泛應(yīng)用于路線規(guī)劃系統(tǒng)[17]。建立水平x軸和垂直y軸的直角坐標(biāo)系xOy（圖3），設(shè)環(huán)境帶下為w×h，每個柵格的邊長為a，x、y軸柵格數(shù)分別為 nx=w/a、ny=h/a。常用的柵格表示方法包括序列號方法和直角坐標(biāo)系法。

傳統(tǒng)的柵格圖模型僅使用二元函數(shù)來劃分環(huán)境圖中的障礙區(qū)域和可執(zhí)行區(qū)域。對于機(jī)器人而言，規(guī)劃的路徑只能在可行區(qū)域。在這種情況下，A* 算法采用最短路徑方式通過障礙物，但由于尺寸、誤差等原因，存在一定的碰撞風(fēng)險[18]。

為了解決上述問題，采用人工勢場對柵格進(jìn)行優(yōu)化。通過對鄰域內(nèi)障礙物數(shù)進(jìn)行計算，可以得到空閑節(jié)點的局部障礙物勢場函數(shù)[19]。

S（n）=k，0≤k≤m。（4）

式中：m表示搜索鄰域范圍，設(shè)置為m=8，采用8個鄰域路徑搜索;k表示可搜索鄰域中占用的柵格節(jié)點數(shù)。

2.2 混合路徑規(guī)劃算法

DWA算法可以通過局部障礙物信息獲得最優(yōu)的無碰撞路徑。但該方法在長距離規(guī)劃中極易出現(xiàn)局部極小值。LPF-A*算法的全局路徑規(guī)劃中，軌跡由大量柵格節(jié)點組成。都視為局部目標(biāo)點，移動機(jī)器人會出現(xiàn)左右反復(fù)旋轉(zhuǎn)的現(xiàn)象。因此，機(jī)器人在移動時需要執(zhí)行許多轉(zhuǎn)向運動，控制無法做到精確。

本研究結(jié)合LPF-A*算法和APDWA算法進(jìn)行混合路徑規(guī)劃。具體的實現(xiàn)過程如下：步驟1，提取關(guān)鍵點，如果對多有節(jié)點進(jìn)行局部規(guī)劃，會導(dǎo)致計算量大和結(jié)果軌跡的曲率大，所以需要提取關(guān)鍵節(jié)點進(jìn)行目標(biāo)搜索，關(guān)鍵節(jié)點提取的基本原理是確定當(dāng)前節(jié)點是否與上一個和下一個目標(biāo)在同一條直線上。如在一條直線上，則認(rèn)為該點沉余，繼續(xù)遍歷整個路徑。只保留起始點、目標(biāo)點和關(guān)鍵點。步驟2，約束方位角。基于單個目標(biāo)點進(jìn)行搜索可能會發(fā)生碰撞。因此，提出一種自適應(yīng)全局最優(yōu)路徑代價函數(shù)[20]。

G（v，w）=α·localhead（v，ω）+β·dis（v，ω）+γ·vel（v，ω）;（5）

α=a·kphead（v，ω）+b。（6）

式中：localhead（v，ω）表示預(yù)測軌跡終點和下一個關(guān)鍵點之間的方向夾角;kphead（v，ω）表示拐點是軌跡終點與下一個關(guān)鍵點之間的方向夾角;α、β、γ表示各函數(shù)相對應(yīng)的加權(quán)系數(shù);dis（v，ω）表示軌跡點到障礙物的最小距離;vel（v，ω）表示當(dāng)前軌跡速度;a、b表示縮小α范圍的固定參數(shù)。

規(guī)劃算法流程圖見圖4。

3 仿真結(jié)果與分析

3.1 仿真參數(shù)

通過對全局優(yōu)化算法和混合優(yōu)化算法進(jìn)行仿真比較，可以驗證算法的優(yōu)越性。試驗中使用的計算機(jī)是Intel i5處理器，8 g內(nèi)存和Win10系統(tǒng)，并使用MATLAB 2018a軟件進(jìn)行仿真。在柵格圖中，黑色網(wǎng)格節(jié)點是障礙節(jié)點，白色網(wǎng)格節(jié)點是空閑節(jié)點。根據(jù)勢場函數(shù)的值，使用MATLAB的colormap函數(shù)標(biāo)記從紅色到紫色的勢場函數(shù)。對于有局部障礙物存在的空閑節(jié)點，紅色勢場最大，紫色勢場最小。β=0.9、γ=0.2、T=3 s、a=0.4、b=1。

3.2 全局規(guī)劃算法仿真分析

本研究選擇的試驗環(huán)境是20 m×20 m的柵格圖，分析 LPF-A*算法在不同ω參數(shù)（0、0.1、1、6）下獲得的路徑圖，ω=0即為A*算法。傳統(tǒng)A*算法和LPF-A*算法之間的比較見圖5，全局規(guī)劃算法仿真結(jié)果見表1。

由圖5可知，帶有紅色圓圈的柵格是起點，帶有綠色圓圈的柵格是目標(biāo)點。 ω的增加使得路徑通行代價更低，但路徑長度增加。

由表1可知，ω=6通行代價最低，但是總路徑長度卻明顯增加。這是因為對于多個障礙而言，本研究選擇繞行策略而不是穿越，在不改變路徑長度的情況下，可以大幅降低通行成本。另外，本研究基于A*算法添加局部勢場，這會稍微增加搜索時間和通行節(jié)點的總數(shù)。

3.3 混合算法仿真分析

本研究采用全局算法得到的關(guān)鍵柵格節(jié)點序列生成全局最優(yōu)路徑節(jié)點，使用APDWA算法實時避障和局部路徑修改。并對該規(guī)劃方法進(jìn)行仿真分析，選擇ω=6的最低通行代價。圖6為靜態(tài)環(huán)境下調(diào)整α值（1、5、自適應(yīng)）的仿真結(jié)果，紅色軌跡為全局規(guī)劃，藍(lán)色軌跡為混合規(guī)劃。

由圖6可知，混合算法可以實現(xiàn)攻臺壁障。由表2可知，與A*-DWA算法相比，本研究規(guī)劃方法通行代價降低了63.89%。在保持最大偏移不變的情況下，搜索時間可以節(jié)省27.01%。將動態(tài)障礙物添加到原始靜態(tài)障礙物環(huán)境中，以驗證算法的魯棒性。動態(tài)障礙物仿真結(jié)果見圖7。

由圖7可知，本研究規(guī)劃方法不僅可以滿足全局最優(yōu)軌跡，還能夠根據(jù)環(huán)境中的動態(tài)障礙物修改局部軌跡，以實時避開障礙物。

4 結(jié)束語

本研究提出一種基于LPF-A*算法和APDWA算法的農(nóng)用輪式移動機(jī)器人混合路徑規(guī)劃算法，仿真驗證了混合規(guī)劃算法的優(yōu)越性和魯棒性。第一，全局路徑規(guī)劃使用LPF-A*算法，而局部路徑使用APDWA算法，該混合規(guī)劃算法遇到靜態(tài)障礙和動態(tài)障礙時可以有效地避開周圍的障礙，找到最優(yōu)到達(dá)目標(biāo)點的路徑。第二，將本研究算法與未改進(jìn)前的A*-DWA算法進(jìn)行比較，結(jié)果顯示，本研究算法可以將通行代價降低63.89%，在保持最大偏移不變的情況下，搜索時間可以節(jié)省27.01%?？紤]到當(dāng)前的試驗設(shè)備和數(shù)據(jù)規(guī)模，本研究仍處于起步階段，因此逐步改進(jìn)和完善將是下一步的重點。

參考文獻(xiàn)：

[1]李 瑞，敖 雁，孫啟洵，等. 大田農(nóng)業(yè)物聯(lián)網(wǎng)應(yīng)用現(xiàn)狀與展望[J]. 北方園藝，2018（14）：148-153.

[2]劉國斌，車宇彤. 農(nóng)業(yè)信息化與農(nóng)業(yè)現(xiàn)代化融合發(fā)展研究[J]. 情報科學(xué)，2019，37（1）：148-155.

[3]李鳳玲，陳 珊，范興江，等. 基于螢火蟲算法動態(tài)未知環(huán)境的路徑規(guī)劃[J]. 自動化與儀表，2019，34（6）：53-58.

[4]吳東林，張玉華. 采摘機(jī)器人動態(tài)路徑規(guī)劃系統(tǒng)研究——基于人工勢場法[J]. 農(nóng)機(jī)化研究，2019，41（6）：219-223.

[5]雷 蕾，石 晨. 基于啟發(fā)式搜索算法的自動采摘機(jī)器人路徑規(guī)劃研究[J]. 農(nóng)機(jī)化研究，2021，43（7）：240-244.

[6]李國輝. 農(nóng)業(yè)機(jī)器人避障路徑智能規(guī)劃研究[J]. 農(nóng)機(jī)化研究，2021，43（3）：236-239.

[7]王銘銘，徐 浩. 基于物聯(lián)網(wǎng)的安徽省農(nóng)田灌溉實時監(jiān)測及自動灌溉系統(tǒng)研究[J]. 節(jié)水灌溉，2017（1）：68-70，75.

[8]余國雄，王衛(wèi)星，謝家興，等. 基于物聯(lián)網(wǎng)的荔枝園信息獲取與智能灌溉專家決策系統(tǒng)[J]. 農(nóng)業(yè)工程學(xué)報，2016，32（20）：144-152.

[9]白秋產(chǎn). 基于物聯(lián)網(wǎng)的農(nóng)田智能灌溉系統(tǒng)[J]. 江蘇農(nóng)業(yè)科學(xué)，2017，45（22）：247-251.

[10]朱洪清，于利娟，汪燁歡，等. 基于物聯(lián)網(wǎng)的太陽能遠(yuǎn)程精準(zhǔn)灌溉系統(tǒng)的研制和應(yīng)用[J]. 中國農(nóng)機(jī)化學(xué)報，2014，35（2）：246-249.

[11]Liu G M，Ouyang M G，Lu L G，et al. Analysis of the heat generation of lithium-ion battery during charging and discharging considering different influencing factors[J]. Journal of Thermal Analysis and Calorimetry，2014，116（2）：1001-1010.

[12]Krewer U，Rder F，Harinath E，et al. Review-dynamic models of Li-ion batteries for diagnosis and operation：A review and perspective[J]. Journal of the Electrochemical Society，2018，165（16）：3656-3673.

[13]Patel G K，Dabhi V K，Prajapati H B.Clustering using a combination of particle swarm optimization and K-means[J]. Journal of Intelligent Systems，2017，26（3）：457-469.

[14]Gautam J V，Prajapati H B，Dabhi V K，et al. Empirical study of job scheduling algorithms in hadoop MapReduce[J]. Cybernetics and Information Technologies，2017，17（1）：146-163.

[15]Caetano C E F，Lima A B，Paulino J O S，et al. A conductor arrangement that overcomes the effective length issue in transmission line grounding[J]. Electric Power Systems Research，2018，159：31-39.

[16]吳宇豪，安籽鵬. 面向圖像三維重建的無人機(jī)航線規(guī)劃[J]. 計算機(jī)技術(shù)與應(yīng)用，2019，45（3）：76-79，87.

[17]蘇海鋒，許道林，李汶江，等. 基于改進(jìn)蟻群A*算法的輸電線路路徑搜索[J]. 河北大學(xué)學(xué)報（自然科學(xué)版），2017，37（1）：92-100.

[18]王慶斌，石亮緣，黃 輝，等. 基于改進(jìn)粒子群算法的輸電線路路徑規(guī)劃研究[J]. 廣東電力，2018，31（9）：135-141.

[19]劉先正，溫家良，潘 艷，等. 采用改進(jìn)粒子群算法的直流電網(wǎng)最優(yōu)潮流控制[J]. 電網(wǎng)技術(shù)，2017，41（3）：715-720.

[20]王 毅，劉 波，何 宇，等. 果園移動機(jī)器人路徑識別系統(tǒng)[J]. 傳感器與微系統(tǒng)，2020，39（9）：69-72.