陸向龍，吳春篤，楊官學，張 波，陳 振

江蘇大學電氣信息工程學院，江蘇鎮(zhèn)江212013

農業(yè)是國民經濟建設和社會發(fā)展的根基。其中，水果產業(yè)由于經濟效益高，在農業(yè)生產中占有相當大的地位[1]。果園規(guī)模一般較為龐大，在進行生產作業(yè)時將會耗費較多人工成本和體力。為減輕農業(yè)生產人員的勞動負擔，降低人工成本，減少工作重復率，越來越多的無人操作智能裝備已被廣泛研發(fā)并應用于農業(yè)生產之中。因此果園噴霧機器人在果園植保中替代人工作業(yè)將會大大降低人力成本。應用在果園環(huán)境中的果園噴霧機器人因其具有負載大量藥劑、實現(xiàn)持續(xù)作業(yè)的優(yōu)點，得到了研究人員的密切關注。由于果園地形復雜，光照變化較為明顯，這些因素會對傳感器產生干擾。通過傳感器感知所在環(huán)境果樹和障礙物位置等信息時，為了將數(shù)據映射到地圖中，需對傳感器數(shù)據優(yōu)化處理，處理后數(shù)據與實際環(huán)境會存在一定偏差，將會對機器人的路徑規(guī)劃產生影響。因此機器人作業(yè)時應與作物和障礙物保持安全距離，實時避開動態(tài)障礙物，在保證安全的前提下，規(guī)劃出一條可行性最優(yōu)的路徑，完成噴霧工作[2-3]。

針對路徑規(guī)劃領域的研究，A*算法[4]、Dijkstra 算法[5]、遺傳算法[6]、蟻群算法[7]、人工勢場法[8]以及DWA算法[9]等已經被廣泛應用于無人駕駛。其中，A*算法是靜態(tài)網絡中求解路徑最優(yōu)的直接搜索方法，同時具有根據搜索方式設計啟發(fā)函數(shù)的優(yōu)點。但是A*算法也存在著規(guī)劃的路徑拐點大、平滑性差、算法運行時間長、單向遞進式搜索的缺點[10]?？桌^利等[11]通過加入雙向搜索機制并優(yōu)化評價函數(shù)，提高了A*算法搜索效率，縮短了路徑長度。焦錚等[12]通過改進A*算法函數(shù)并引入避障冗余距離，同時結合質量評價函數(shù)，實現(xiàn)全局—局部融合的礫石土均勻攤鋪路徑規(guī)劃。

DWA 算法廣泛用于移動機器人的局部路徑規(guī)劃，具有良好的動態(tài)避障能力，適用于動態(tài)環(huán)境中的移動機器人路徑規(guī)劃，但是在復雜的大規(guī)模環(huán)境中存在陷入局部最優(yōu)的死循環(huán)，導致路徑規(guī)劃失敗。劉建娟等[13]通過調節(jié)A*算法啟發(fā)函數(shù)權重，設計了路徑節(jié)點優(yōu)化算法和DWA 算法評價函數(shù)，實現(xiàn)路徑的全局最優(yōu)和動態(tài)避障。龐永旭等[14]通過將障礙物占比引入傳統(tǒng)A*算法，改進A*算法評價函數(shù)，提高其在不同環(huán)境下的搜索效率，然后改進子節(jié)點選擇方式，融合動態(tài)窗口法，實現(xiàn)復雜環(huán)境下機器人的實時動態(tài)避障。王子靜等[15]通過動態(tài)改變步長方法設計了一種改進的快速平滑A*算法，并在DWA 的評價函數(shù)中增加路徑偏差項，將全局規(guī)劃與局部規(guī)劃相融合，實現(xiàn)了動態(tài)環(huán)境下無人艇的路徑規(guī)劃。勞彩蓮等[16]通過對關鍵點選取策略進行改進，結合動態(tài)窗口法，構建路徑評價函數(shù)，減少了路徑多余轉折點，提高了路徑的平滑性。

本文對果園噴霧機器人作業(yè)需求進行研究。針對以上不同的算法改進，結合果園實際環(huán)境，本文提出了一種改進A*和DWA 算法的全局路徑規(guī)劃方法。首先對A*算法啟發(fā)函數(shù)進行優(yōu)化，確保機器人在躲避障礙物的前提下能沿著果樹行間中心線行走，接著加入引導點實現(xiàn)果園的初步全局路徑規(guī)劃，然后通過DWA 算法精準的追蹤初步全局作業(yè)路徑，在規(guī)避作業(yè)過程中的動態(tài)障礙物前提下，實現(xiàn)減少路徑冗余點和提高作業(yè)安全性的目的。

1 果園噴霧機器人環(huán)境模型建立

在果園進行路徑規(guī)劃時，首先需要建立簡化后的果園環(huán)境地圖[17]。為了便于實驗研究，本文在進行地圖邊界、果樹和障礙物構建時，考慮到機器人行駛的安全距離，將果樹和障礙物點化后適當膨脹，以保障機器人作業(yè)時的安全。在果園環(huán)境模型建立時將果園噴霧機器人簡化為一個質點，果樹與障礙物也進行簡化，近似認為單個果樹和障礙物所占面積相等。模擬果園環(huán)境構建的30 m×30 m柵格地圖如圖1所示。圖1果園地圖中的果樹位置和作業(yè)生產中臨時的一些障礙物統(tǒng)一用黑色正方塊表示。此外，把地圖分為兩部分，一部分為施藥區(qū)域，另一部分稱為地頭調轉區(qū)域。在圖1建立的柵格地圖中共有8 行果樹，兩果樹行間組成一塊施藥區(qū)域，共計7塊施藥區(qū)域。施藥區(qū)域已在圖中用紅色矩形框包圍，施藥區(qū)域統(tǒng)一用T1 表示。地頭調轉區(qū)域為地圖中施藥區(qū)域之外的區(qū)域，如圖1中綠色矩形框包絡的區(qū)域，地頭調轉區(qū)域統(tǒng)一用T2表示。

圖1 模擬果園環(huán)境地圖構建Fig.1 Construction of simulated orchard environment map

2 基于改進A*算法的路徑規(guī)劃

A*算法是在靜態(tài)環(huán)境中求解全局最短路徑的尋路算法，是一種快速高效的啟發(fā)式算法，屬于最佳優(yōu)先算法。但是A*算法在復雜果園環(huán)境規(guī)劃的路徑不適合果園噴霧機器人作業(yè)。因此本文通過對傳統(tǒng)A?算法的評價函數(shù)進行改進，再通過加入引導點，引導A*算法迭代，實現(xiàn)果園噴霧機器人初步全局路徑規(guī)劃。

2.1 傳統(tǒng)A＊算法

A*算法從起點開始搜索，通過代價函數(shù)對鄰域周圍8個方向的點代價進行計算[18]。代價函數(shù)的運動搜索方向如圖2所示。紅色的點為當前位置p，箭頭所指方向為可移動方向，由圖中可以看出移動過程中最小轉角為π/4，單次移動最小距離為1或

圖2 運動搜索方向Fig.2 Motion search direction

A*算法通過函數(shù)表達式計算運動搜索方向節(jié)點的代價值，然后進行選取代價最小的節(jié)點作為下一步移動的節(jié)點，重復此操作，直至找到目標點。A*啟發(fā)函數(shù)如下：

其中，f(p)為從起點S到終點E的預估代價函數(shù)；g(p)為當前點p到起點S的實際代價函數(shù)；h(p)為當前點p到終點E的估計代價值。在搜索路徑過程中，p點每移動一個單位，g(p)值將增加1 或者。由于果園環(huán)境較為復雜，果園噴霧機器人在移動過程中并不能完全沿著相對徑直的線路行走，因而當h(p)采用曼哈頓距離估計從當前節(jié)點p到終點S距離時，h(p)值會比實際移動距離大，采用歐幾里德距離得到的h(p)會比實際移動距離小。本文采用了一種介于兩者之間，更能準確估算節(jié)點p到終點距離的相對復雜的對角線距離，其公式可表示為：

其中，Ex為終點E在柵格地圖的橫坐標，Ey為終點E在柵格地圖的縱坐標，E為當前點p所在柵格地圖的橫坐標，py為當前點p在柵格地圖的縱坐標，D為移動1個單位所消耗距離代價。A*算法路徑規(guī)劃過程中，每次搜索下一步路徑均需依靠預估代價函數(shù)進行估值，從而規(guī)劃出相對最優(yōu)的下一步節(jié)點，直至抵達終點。通過對預估函數(shù)進行改進，可以得到一條與預期接近的路徑，進而滿足果園噴霧機器人作業(yè)需求。

2.2 A＊算法優(yōu)化

針對果園環(huán)境的施藥任務要求，需要對A*算法預估函數(shù)進行優(yōu)化：確保在果樹施藥區(qū)域時，果園噴霧機器人能夠沿著果樹行間中心線施藥；當在地頭調轉區(qū)域，能夠規(guī)劃出最短路徑以進行施藥工作，以此減少不必要的能量、時間浪費。實現(xiàn)方式是通過加入施藥區(qū)域代價函數(shù)w(p)計算施藥區(qū)域中每個節(jié)點距離最近的果樹或者障礙物位置的比較復雜的對角線距離的倒數(shù)與施藥區(qū)域代價增益的乘積。由此可知，當施藥區(qū)域代價增益一定時，與果園噴霧機器人距離最近的果樹相對位置越接近，施藥區(qū)域代價就越大。因此，可以利用控制施藥區(qū)域代價函數(shù)的增益可以實現(xiàn)控制果園噴霧機器人的大概走向。此外，在地頭調轉區(qū)域無需進行w(p)計算，即w(p)=0，只需果園噴霧機器人尋找到最短路徑即可。優(yōu)化后的啟發(fā)函數(shù)為：

其中，ox為距離當前點p直線距離最近的果樹或障礙物的橫坐標，oy為距離當前點p直線距離最近的果樹或障礙物的縱坐標，?為施藥區(qū)域代價增益，T1為施藥區(qū)域，T2為地頭調轉區(qū)域。優(yōu)化前后的A*算法在14×14模擬果園地圖規(guī)劃出的仿真路徑如圖3、圖4所示。

圖3 A*算法Fig.3 A* algorithm

圖4 優(yōu)化的A*算法Fig.4 Optimized A* algorithm

圖3、圖4 中設定S點為路徑規(guī)劃起點，E點為路徑規(guī)劃終點。傳統(tǒng)A*算法路徑規(guī)劃耗時1.043 4 s，距離代價11.242 6 m，加入施藥區(qū)域代價函數(shù)的A*算法路徑規(guī)劃耗時1.055 8 s，距離代價12.242 6 m。比較兩種不同預估函數(shù)的路徑規(guī)劃，兩種方法耗時幾乎相等，優(yōu)化的A*算法所規(guī)劃出的路徑在果園場景作業(yè)更為適合。故加入施藥區(qū)域代價函數(shù)的A*算法路徑規(guī)劃優(yōu)于傳統(tǒng)A*算法路徑規(guī)劃。

2.3 改進A＊算法

預估函數(shù)優(yōu)化后的A*算法在路徑規(guī)劃中，只能在果園中實現(xiàn)單行作業(yè)。一般果園種植規(guī)模較大，單行作業(yè)是遠遠不能滿足任務要求的。本文在加入施藥區(qū)域代價函數(shù)的基礎上，在保證算法區(qū)域有效性的基礎前提下，加入多個引導點，通過A*算法迭代實現(xiàn)全局路徑規(guī)劃。算法以第一個引導點S為起點開始規(guī)劃路徑，直至尋找到第二個引導點，再以第二個引導點為起點，尋找第三個引導點，依次迭代，直至尋找到終點E，實現(xiàn)果園全局作業(yè)。通過加入引導點迭代的改進A*算法流程如圖5所示。

圖5 改進A*算法流程Fig.5 Improved A* algorithm process

3 DWA算法

DWA算法是一種考慮機器人動力學約束的局部路徑規(guī)劃算法。它采樣了機器人運動過程中的多組速度，模擬出機器人在一定時間內在該速度下的軌跡，對軌跡進行評價，選取評價最高的一組軌跡。DWA 算法根據機器人的性能在一個動態(tài)范圍內限定速度采樣空間。因此DWA算法可以用于在復雜的果園環(huán)境中實時動態(tài)避障，以保障機器人安全作業(yè)，但是該算法不能保證規(guī)劃出的路徑全局最優(yōu)，導致路徑不利于機器人作業(yè)。本文通過調整DWA 算法的評價函數(shù)系數(shù)，實現(xiàn)機器人在實時動態(tài)避障的前提下更精準的對改進A*算法的初步全局路徑追蹤。

3.1 機器人運動模型

在動態(tài)窗口中首先要建立機器人的運動學模型，機器人的運動狀態(tài)可由機器人的線速度vt、角速度wt表示。(vt,wt)代表一組運動軌跡。在時間間隔Δt內，設機器人做勻速直線運動，機器人只能進行前進和旋轉，不能全向運動。在很短的時間間隔內，可以將運動軌跡近似為一段直線，只需要將Δt的位移增量累計求和，運動學模型可表示為：

3.2 速度采樣

在速度(vt,wt)空間中，存在多組速度，根據機器人的硬件性能和環(huán)境等多種限制可以得到速度約束。

機器人自身速度大小限制：

機器人受電機性能影響，加速度會受到限制，機器人速度約束：

式中，vc、wc表示當前速度，va、wa表示機器人最大加速度，vb、wb表示機器人最大減速度。

機器人安全距離約束：為了保證機器人運動的安全性，當機器人遇到障礙物前可以及時停止運動，需要對機器人速度進行約束，機器人運動學約束模型為：

式中，dist(v,w)是(v,w)對應軌跡距離障礙物的最近距離。

3.3 評價函數(shù)

在采樣的速度中，機器人有多組采樣軌跡，不同的采樣軌跡可以規(guī)劃出不同路徑，需要用一組評價標準對多組軌跡進行評價。評價標準函數(shù)如下：

式中，heading(v,w)為方位角評價函數(shù)，表示當前速度下軌跡與目標點的方位角偏差，dist(v,w)為軌跡與靜態(tài)障礙物的最近距離，velocity(v,w)為當前速度大小的評價函數(shù)。σ為平滑系數(shù)，α、β、γ為各項的加權系數(shù)。

4 算法融合

由于果園規(guī)模龐大，改進的A*算法只能規(guī)劃出果樹行間中心線路徑，但機器人很難實現(xiàn)全局覆蓋作業(yè)。通過對改進的A*算法加入引導點，引導改進A*算法規(guī)劃出初步全局路徑。為了更好地完成機器人噴藥作業(yè)，提取初步全局路徑關鍵節(jié)點，引導DWA 算法在含有動態(tài)障礙的果園實現(xiàn)全局作業(yè)。通過改進A*和DWA 算法結合，兼顧全局路徑規(guī)劃和局部路徑規(guī)劃的優(yōu)點，實現(xiàn)全局果樹行間中心線作業(yè)和實時動態(tài)避障。融合算法具體實現(xiàn)步驟如下：

（1）將機器人所獲數(shù)據處理后建立果園環(huán)境地圖，選定起點和終點。

（2）在地圖中選取節(jié)點作為改進A*算法引導點。

（3）改進A*算法通過啟發(fā)函數(shù)計算出從起點到第一個引導點路徑，接著計算出第一個引導點到第二個引導點路徑，依次迭代，直至終點，規(guī)劃出初步全局路徑并儲存。

（4）在地圖中加入動態(tài)障礙物，驗證融合算法躲避動態(tài)障礙物能力。

（5）提取步驟（3）規(guī)劃的初步全局路徑關鍵點，作為DWA算法的子目標點。

（6）初始化DWA算法初始參數(shù)，根據評價函數(shù)選取最優(yōu)軌跡，然后按照最優(yōu)軌跡移動，追蹤子目標點。

（7）判斷是否到達終點，若未到達終點，繼續(xù)執(zhí)行步驟（5），直至到達終點。

5 仿真研究

為驗證改進A*算法與DWA 算法路徑規(guī)劃的有效性，在環(huán)境為30 m×30 m的模擬果園的柵格地圖上進行仿真實驗，如圖6，起點S（3.5，2.5），終點E（28.5，28.5），障礙物覆蓋率約為25.8%。在模擬果園環(huán)境地圖中，機器人在靜態(tài)地圖中首先通過A*算法進行全局路徑規(guī)劃，然后在模擬果園地圖隨機加入動態(tài)障礙物，如圖6（c）的紅色正方塊，驗證融合算法躲避動態(tài)障礙物能力。傳統(tǒng)A*算法、改進A*算法與融合算法進行仿真驗證，規(guī)劃的路徑如圖6（a）~（c）所示。圖中S點為機器人起點，E點機器人為終點，圖6（b）中G點為加入的引導點。

圖6 三種算法仿真結果Fig.6 Simulation results of three algorithms

對比傳統(tǒng)A*算法，改進的A*算法在果樹中心線沒有障礙物時，在工作區(qū)域中心線行走路徑100%，且能實現(xiàn)對果園環(huán)境進行全局路徑規(guī)劃，但A*算法存在著無法躲避地圖中的動態(tài)障礙物的問題，不能保障機器人的安全作業(yè)。融合算法在全局路徑的基礎上進行路徑規(guī)劃，可以進行實時避障，高效地追蹤全局路徑。本文算法輸出的機器人控制參數(shù)如圖7（a）~（c）所示，輸出結果為機器人姿態(tài)、線速度和角速度參數(shù)。

圖7 機器人控制參數(shù)Fig.7 Robot control parameters

在30 m×30 m 果園環(huán)境地圖中，對比改進A*算法和融合算法的路徑及路徑數(shù)據，轉折點個數(shù)從20 個減小到14 個，轉折點優(yōu)化了30%，累積轉彎角從2 250°減少到1 409.3°，累積轉彎角度優(yōu)化了36.17%。相對于傳統(tǒng)A*算法，融合算法能夠根據周圍環(huán)境做出速度匹配，保證安全與高效，且規(guī)劃出的路徑平滑，路徑曲率連續(xù)，更利于機器人的動力學控制。機器人在規(guī)劃時實時進行姿態(tài)、線速度和角速度輸出，方便對機器人狀態(tài)進行監(jiān)控與控制。通過以上數(shù)據和任務需求對比，融合算法在工作任務限制的前提下，減小了距離代價，提高了機器人工作的有效性和可靠性。整體來說，本文算法兼顧了A*算法與DWA 算法的優(yōu)點，彌補了A*算法與DWA算法的缺陷，能夠滿足果園噴霧機器人的作業(yè)需求。

6 結論

本文針對果園噴霧機器人在果園環(huán)境作業(yè)這一情景，提出了一種基于改進A*與DWA算法的全局路徑規(guī)劃方法。在A*算法啟發(fā)函數(shù)中加入工作區(qū)域代價函數(shù)，使果園噴霧機器人能夠規(guī)劃出沿著果樹行間中心線路徑，地頭調轉時，尋找最短距離進行調頭；通過對柵格地圖中加入多個引導點，引導A*算法進行迭代，實現(xiàn)初步全局規(guī)劃；最后，DWA 算法對初步全局路徑追蹤，實現(xiàn)對果園全局的規(guī)劃和動態(tài)障礙物的實時躲避，保障噴霧機器人作業(yè)的安全，提高了作業(yè)效率。將改進A*算法與DWA算法進行融合，通過仿真實驗分析比較，改進的A*算法與DWA 算法能夠規(guī)劃出果園噴霧機器人在果園環(huán)境的工作路徑，算法具有較好的實時性，更高的安全性，規(guī)劃出的路徑平滑度較高，更適合果園噴霧機器人作業(yè)。