陸向龍，吳春篤，楊官學(xué)，張 波，陳 振

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

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

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

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

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

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

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

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

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

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

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

A*算法從起點(diǎn)開(kāi)始搜索，通過(guò)代價(jià)函數(shù)對(duì)鄰域周?chē)?個(gè)方向的點(diǎn)代價(jià)進(jìn)行計(jì)算[18]。代價(jià)函數(shù)的運(yùn)動(dòng)搜索方向如圖2所示。紅色的點(diǎn)為當(dāng)前位置p，箭頭所指方向?yàn)榭梢苿?dòng)方向，由圖中可以看出移動(dòng)過(guò)程中最小轉(zhuǎn)角為π/4，單次移動(dòng)最小距離為1或

圖2 運(yùn)動(dòng)搜索方向Fig.2 Motion search direction

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

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

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

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

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

其中，ox為距離當(dāng)前點(diǎn)p直線距離最近的果樹(shù)或障礙物的橫坐標(biāo)，oy為距離當(dāng)前點(diǎn)p直線距離最近的果樹(shù)或障礙物的縱坐標(biāo)，?為施藥區(qū)域代價(jià)增益，T1為施藥區(qū)域，T2為地頭調(diào)轉(zhuǎn)區(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 中設(shè)定S點(diǎn)為路徑規(guī)劃起點(diǎn)，E點(diǎn)為路徑規(guī)劃終點(diǎn)。傳統(tǒng)A*算法路徑規(guī)劃耗時(shí)1.043 4 s，距離代價(jià)11.242 6 m，加入施藥區(qū)域代價(jià)函數(shù)的A*算法路徑規(guī)劃耗時(shí)1.055 8 s，距離代價(jià)12.242 6 m。比較兩種不同預(yù)估函數(shù)的路徑規(guī)劃，兩種方法耗時(shí)幾乎相等，優(yōu)化的A*算法所規(guī)劃出的路徑在果園場(chǎng)景作業(yè)更為適合。故加入施藥區(qū)域代價(jià)函數(shù)的A*算法路徑規(guī)劃優(yōu)于傳統(tǒng)A*算法路徑規(guī)劃。

2.3 改進(jìn)A＊算法

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

圖5 改進(jìn)A*算法流程Fig.5 Improved A* algorithm process

3 DWA算法

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

3.1 機(jī)器人運(yùn)動(dòng)模型

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

3.2 速度采樣

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

機(jī)器人自身速度大小限制：

機(jī)器人受電機(jī)性能影響，加速度會(huì)受到限制，機(jī)器人速度約束：

式中，vc、wc表示當(dāng)前速度，va、wa表示機(jī)器人最大加速度，vb、wb表示機(jī)器人最大減速度。

機(jī)器人安全距離約束：為了保證機(jī)器人運(yùn)動(dòng)的安全性，當(dāng)機(jī)器人遇到障礙物前可以及時(shí)停止運(yùn)動(dòng)，需要對(duì)機(jī)器人速度進(jìn)行約束，機(jī)器人運(yùn)動(dòng)學(xué)約束模型為：

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

3.3 評(píng)價(jià)函數(shù)

在采樣的速度中，機(jī)器人有多組采樣軌跡，不同的采樣軌跡可以規(guī)劃出不同路徑，需要用一組評(píng)價(jià)標(biāo)準(zhǔn)對(duì)多組軌跡進(jìn)行評(píng)價(jià)。評(píng)價(jià)標(biāo)準(zhǔn)函數(shù)如下：

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

4 算法融合

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

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

（2）在地圖中選取節(jié)點(diǎn)作為改進(jìn)A*算法引導(dǎo)點(diǎn)。

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

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

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

（6）初始化DWA算法初始參數(shù)，根據(jù)評(píng)價(jià)函數(shù)選取最優(yōu)軌跡，然后按照最優(yōu)軌跡移動(dòng)，追蹤子目標(biāo)點(diǎn)。

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

5 仿真研究

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

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

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

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

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

6 結(jié)論

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