羅智杰，黃子濤，許嘉志，潘仲宇，曹 亮，劉雙印

（1.仲愷農(nóng)業(yè)工程學(xué)院信息科學(xué)與技術(shù)學(xué)院，廣東 廣州 510225；2.仲愷農(nóng)業(yè)工程學(xué)院智慧農(nóng)業(yè)工程技術(shù)研究中心，廣東 廣州 510225；3.廣州市農(nóng)產(chǎn)品質(zhì)量安全溯源信息技術(shù)重點實驗室，廣東 廣州 510225）

0 引言

進入電子信息化時代，農(nóng)業(yè)領(lǐng)域已向機械化、智能化、信息化方向靠攏。農(nóng)業(yè)機器人已成為一種具備路徑規(guī)劃,環(huán)境障礙感知和能夠完成一系列動作行為等功能為一體的智能化農(nóng)業(yè)設(shè)備。現(xiàn)階段，國內(nèi)外現(xiàn)有的農(nóng)業(yè)機器人的主要類型有播種機器人、采摘機器人、果蔬分選機器人、除草機器人等[1,2]。為保障機器人能夠適應(yīng)復(fù)雜多變的農(nóng)業(yè)環(huán)境，其主要技術(shù)主要包括定位導(dǎo)航技術(shù)、運動控制技術(shù)、傳感器技術(shù)、圖像識別技術(shù)[3]，要求農(nóng)業(yè)機器人具備較高的環(huán)境感知、定位和避障的能力[4]。對于企業(yè)用戶來說，農(nóng)業(yè)機器人的效率和路徑合理度是一個重要的指標(biāo)。因此如何合理規(guī)劃路徑是農(nóng)業(yè)機器人研究的熱點之一。近年來，有關(guān)機器人路徑規(guī)劃算法的研究不斷被提出和研究，其中幾種典型的算法有：人工勢場法[5]、蟻群算法[6]、模擬退火法[7]、粒子群算法[8]、A*算法[9]等。

在實際農(nóng)業(yè)場景中，尤其是對大型農(nóng)場而言，單靠一個農(nóng)業(yè)機器人運作處理是難以實現(xiàn)的，而多個機器人作業(yè)又涉及到協(xié)作問題，因此有必要考慮多個機器人的路徑規(guī)劃。為了解決蟻群算法在多移動機器人路徑規(guī)劃應(yīng)用中的不足，本文在基本蟻群算法的基礎(chǔ)上，對啟發(fā)信息函數(shù)做出了改進，同時提出死鎖問題的解決方案，既保證了算法的速度，又避免了算法的提前收斂。另外，針對未知運動狀態(tài)和已知運動狀態(tài)的障礙物，本文分別討論了動態(tài)窗口搭配區(qū)域膨脹以及正碰、側(cè)碰兩種避碰策略，進一步加強了機器人應(yīng)對多變的室內(nèi)農(nóng)業(yè)環(huán)境的能力。本文所研究改進的算法對于用在障礙物較多，路徑規(guī)劃較復(fù)雜的大型農(nóng)場中有較好的效果。本文最后也討論了多個機器人在同一環(huán)境下的路徑規(guī)劃問題。

1 空間模型建立

本研究首先采用柵格法[10-12]進行環(huán)境地圖的創(chuàng)建，以此來模擬應(yīng)用場景，規(guī)格為30×30。對著環(huán)境地圖從上到下，從左到右分別進行1～900 的編號，如圖1（a）所示。在這個環(huán)境中，黑色柵格表示靜態(tài)障礙物，白色柵格表示機器人的自由可行區(qū)域，分別如圖1（b）所示。建立工作空間環(huán)境時，把實際的障礙物經(jīng)過膨脹處理后映射到柵格地圖中，因此可認(rèn)為柵格地圖中的邊界均為安全邊界，并將機器人視為質(zhì)點。

圖1 環(huán)境的柵格地圖

環(huán)境地圖中各柵格的中心點坐標(biāo)由其對應(yīng)的柵格序號（xi，yi）決定，其算法為

式中 i為柵格序號；Nx為每行柵格的個數(shù)；Ny為每列柵格的個數(shù)。

根據(jù)實際的農(nóng)業(yè)場景情況，規(guī)定機器人的行走方向為西北、北、東北、東、東南、南、西南、西8個方向，且每次前進一個步長的距離。如圖2 所示。

圖2 機器人行走方向

2 改進的蟻群算法研究

2.1 基本的蟻群算法

蟻群算法[13]是一種應(yīng)用于組合優(yōu)化問題的啟發(fā)式搜索算法，在解決離散組合優(yōu)化方面具有良好的性能，最早應(yīng)用在旅行商（TSP）問題上。通常，我們用(t)表示t 時刻螞蟻k 選擇從柵格i 轉(zhuǎn)移到柵格j 的概率，也稱隨機比例規(guī)則。信息素濃度τxy(t)和局部啟發(fā)信息ηxy(t)共同決定(t)，其算法為

螞蟻k 下一步允許選擇的柵格的集合為

式中 m為蟻群中螞蟻的數(shù)量，只；禁忌表tabuk(i)記錄了螞蟻k當(dāng)前所走過的柵格；α為信息素啟發(fā)因子；β為期望值啟發(fā)因子；τij(t)為t時刻在柵格i和j中間殘留的信息素，初始時刻，各條路徑上的信息素相等，即τij(0)=C(const)。

啟發(fā)信息函數(shù)ηij(t)計算公式為

螞蟻完成一次遍歷后，需要對各條路徑上的殘留信息素作消散處理，避免信息素過多影響遍歷結(jié)果。更新機制為

其中，式(6)表示第k 只螞蟻留在路徑（i,j）上的信息素增量，Q 為常數(shù)，Lk為優(yōu)化問題的目標(biāo)函數(shù)值，表示第k 只螞蟻在本次循環(huán)中所走路徑的長度；式(7)表示每只螞蟻在柵格i 和j 路徑上增加的信息素之和；ρ為信息素?fù)]發(fā)系數(shù)。

值得注意的是，現(xiàn)階段，在不同的數(shù)學(xué)模型（如螞蟻數(shù)量系統(tǒng)、螞蟻密度系統(tǒng)）中，對于Δτkij(t)給出的定義略有區(qū)別。

2.2 改進蟻群算法

2.2.1 死鎖回退策略

基本蟻群算法存在的一個問題是容易過早收斂，導(dǎo)致規(guī)劃的路徑得不到較優(yōu)解。另外，在復(fù)雜的空間模型中（如大面積農(nóng)場），螞蟻容易陷入死鎖問題（如圖3），造成算法停滯。一種解決的辦法是采用早期死亡螞蟻策略[14]，即當(dāng)螞蟻陷入死鎖時，為防止程序停滯，結(jié)束本只螞蟻的搜索，并且不對當(dāng)前這只螞蟻做任何處理，即不將該螞蟻的搜索路徑、留下的信息素等信息進行保留。這種方案會導(dǎo)致有效螞蟻的數(shù)量在減少，此外還可能會導(dǎo)致蟻群算法得不到較優(yōu)解。

圖3 螞蟻陷入死鎖現(xiàn)象

考慮到室內(nèi)農(nóng)業(yè)應(yīng)用的實際情況，本文利用死鎖回退策略[15,16]，該策略通過犧牲一些算法時間來得到路徑的較優(yōu)解。螞蟻死鎖回退策略具體為：若螞蟻陷入死鎖，則回退至上一次所在的柵格，并將螞蟻陷入死鎖的所在柵格臨時標(biāo)記為靜態(tài)障礙物，防止螞蟻再次陷入同樣的死鎖問題。在回退處理之后，再次判斷該只螞蟻在當(dāng)前柵格是否還是陷入死鎖。若是，則再次進入死鎖回退的策略當(dāng)中；否則，進入下一個柵格的選擇步驟。在本文的研究試驗中，我們將各只螞蟻視為相互獨立，即它們在陷入死鎖問題后所產(chǎn)生的靜態(tài)障礙物只對自己有效，一只螞蟻完成一輪搜索后，其產(chǎn)生的臨時靜態(tài)障礙物也隨之消失。這種方法能夠有效避免早期螞蟻陷入死鎖，產(chǎn)生過多的靜態(tài)障礙物導(dǎo)致算法提前收斂，規(guī)劃出不理想的搜索路徑。在空間越大的模型（如大面積的農(nóng)場）中，這種效果表現(xiàn)的就越顯著。

2.2.2 啟發(fā)信息函數(shù)改進

此外，本文對啟發(fā)信息函數(shù)做了一些針對性的優(yōu)化，即將每一柵格到目標(biāo)柵格的直線距離的倒數(shù)作為啟發(fā)信息函數(shù)，減少了一定的計算量，提高算法速度。修改后的啟發(fā)信息函數(shù)表示為

式中 i為當(dāng)前所在的柵格；j為可前往的柵格的編號；e為目標(biāo)柵格的編號。

2.2.3 路徑規(guī)劃步驟

本算法的程序流程如圖4 所示，具體算法步驟如下。

圖4 本研究的算法程序流程圖

Step1：根據(jù)實際農(nóng)業(yè)應(yīng)用場景，利用柵格法對整體空間進行二維平面建模。

Step2：對算法的相關(guān)參數(shù)進行初始化操作。

Step3：根據(jù)輪盤賭法選擇下一個相鄰的自由柵格。

Step4：更新禁忌表，將當(dāng)前所在柵格標(biāo)記為已去過柵格，避免該柵格被本只螞蟻再次選擇。

Step5：當(dāng)本次迭代的所有螞蟻完成路徑搜索后，對所有路徑上的信息素進行更新操作。

Step6：如果迭代完畢，則輸出算法結(jié)果；否則，重置禁忌表，并跳轉(zhuǎn)到Step3。

2.3 算法的試驗數(shù)據(jù)對比

為了對兩種算法進行比較，在Matlab R2018b 平臺上進行仿真試驗。試驗中算法的各項參數(shù)分別為α=1.5，β=7，ρ=0.3，Q=5，螞蟻數(shù)量為50 只，最大迭代次數(shù)為100。圖5—圖8 分別為采用基本蟻群算法和改進蟻群算法對單個機器人進行路徑規(guī)劃的收斂曲線趨勢圖及運動軌跡圖。

圖5 基本蟻群算法的運動路徑

圖6 改進蟻群算法的運動路徑

圖7 基本蟻群算法的收斂曲線

圖8 改進蟻群算法的收斂曲線

值得注意的是，從本次試驗比較結(jié)果來看，雖然兩種算法都能得到給定空間模型的最優(yōu)解，但從多次試驗數(shù)據(jù)觀察發(fā)現(xiàn)，改進蟻群算法得到優(yōu)解的概率要高于基本蟻群算法，顯得更加穩(wěn)定。表1 給出了在參數(shù)相同的情況下兩種算法的各10 次試驗數(shù)據(jù)統(tǒng)計。從表1 中的數(shù)據(jù)可以看到，改進蟻群算法10 次試驗得到的平均路徑長度要優(yōu)于基本蟻群算法得到的平均路徑長度，反映了改進蟻群算法的穩(wěn)定性要高于基本蟻群算法的穩(wěn)定性，且改進蟻群算法出現(xiàn)的最差路徑長度接近于平均值，沒有出現(xiàn)像基本蟻群算法那樣有較大的波動。

表1 10次試驗數(shù)據(jù)統(tǒng)計

3 討論

3.1 動態(tài)環(huán)境下單個機器人的路徑規(guī)劃

在實際的室內(nèi)農(nóng)業(yè)環(huán)境中，為保證蔬菜等農(nóng)產(chǎn)品的精準(zhǔn)培育，智能監(jiān)測設(shè)備的數(shù)目會多于室外農(nóng)業(yè)環(huán)境。因此其不可預(yù)測事件發(fā)生的概率也較高。例如，為了改善室內(nèi)農(nóng)業(yè)的監(jiān)測系統(tǒng)，會將技術(shù)更加先進的傳感器設(shè)備添加到室內(nèi)農(nóng)業(yè)場景中，而這個設(shè)備相對于系統(tǒng)之前建模的空間來說，是未知的，對于室內(nèi)農(nóng)業(yè)機器人而言，新添加的傳感器設(shè)備則視為一個未知的動態(tài)障礙物。因此，為了提高農(nóng)業(yè)機器人的有效運作，未知物體的存在因素是必須考慮的。假設(shè)未知物體的運動狀態(tài)是不可預(yù)測的，此時，需要借助機器人的傳感器設(shè)備完成對動態(tài)障礙物的檢測，必要時采取預(yù)先設(shè)定的避障策略進行避障處理。本文采用的是基于滾動窗口策略對動態(tài)障礙物進行檢測[17,18]。機器人每到達一個柵格位置，進行一次窗口檢測。如果檢測到動態(tài)障礙物在滾動窗口范圍內(nèi)，則以動態(tài)障礙物當(dāng)前位置為中心做膨脹處理，即將其附近的柵格設(shè)為灰色障礙物（不包括機器人所在柵格，如圖9）。如果膨脹區(qū)域占據(jù)了機器人的原路徑，則在原路徑的基礎(chǔ)上查找機器人當(dāng)前所在柵格之后的第一個未被占據(jù)的柵格，并以該節(jié)點為目標(biāo)節(jié)點，當(dāng)前節(jié)點為源節(jié)點，做局部路徑規(guī)劃，將該局部路徑添加到原路徑中，形成機器人的實際行走路徑。

圖9 動態(tài)未知障礙物膨脹處理

本試驗在環(huán)境地圖及各項參數(shù)依舊不變的情況下，在柵格地圖空間中設(shè)置了兩個動態(tài)障礙物，這兩個動態(tài)障礙物相對機器人而言其運動狀態(tài)是未知的。如圖10（a）所示，兩個星號分別代表兩個動態(tài)障礙物，紅色質(zhì)點代表機器人，紅色虛線表示機器人初始規(guī)劃的路徑。如圖10（b）所示，藍色實線代表機器人實際的行走路線。圖10（c）表示當(dāng)機器人遇到第一個動態(tài)障礙物時，且該動態(tài)障礙物在滾動窗口范圍內(nèi)，對動態(tài)障礙物進行膨脹處理。圖10（d）表示機器人觸發(fā)了避障策略，進行局部路徑規(guī)劃動作。圖10（e）表示機器人遇到第二個動態(tài)障礙物，且該動態(tài)障礙物與機器人相鄰，在對其作膨脹處理時，機器人所在的柵格不作處理，并進行局部路徑規(guī)劃。圖10（f）、圖10（g）表示機器人成功避開第二個動態(tài)障礙物后，繼續(xù)按原路徑前進。圖10（h）表示機器人到達目的地，并給出了實際路線（藍色實線）和初始路線（紅色虛線）的對照。

圖10 改進算法的動態(tài)避障仿真試驗圖

從試驗結(jié)果可以看出，依賴本研究改進的蟻群算法，農(nóng)業(yè)機器人可以順利避開靜態(tài)和動態(tài)障礙物，順利的到達目標(biāo)位置。并且對比默認(rèn)路徑與動態(tài)修改后的路徑，其運動軌跡較為接近。因此表示局部規(guī)劃的路徑較為有效和穩(wěn)定。

3.2 動態(tài)環(huán)境下多個機器人的路徑規(guī)劃

一般來說，單個移動機器人的工作能力是有限的，而多個移動機器人的協(xié)調(diào)工作能使得工作效率得到大幅度的提升。在室內(nèi)農(nóng)業(yè)的應(yīng)用場景下，多個機器人各自規(guī)劃的路線難免重合產(chǎn)生碰撞的風(fēng)險，從而影響有序的正常工作狀態(tài)。假設(shè)在各個機器人之間具備一定的通信能力的情況下，機器人彼此之間能夠知道對方的運動狀態(tài)（位置）。如果能夠?qū)討B(tài)窗口掃描到的機器人障礙物確定為已知運動狀態(tài)的機器人，即可以采用更合理的避碰策略。因此在有限的空間范圍內(nèi)，如何讓機器人相互之間避免碰撞是提高工作效率的關(guān)鍵所在。在本研究中，把機器人之間可以看作是已知運動狀態(tài)的動態(tài)障礙物。但是與未知運動狀態(tài)的動態(tài)障礙物的避碰策略不同，將不再采用膨脹的方法進行處理，而是根據(jù)運動方向判定兩個機器人是否存在碰撞的可能性。根據(jù)實際應(yīng)用調(diào)研發(fā)現(xiàn)，碰撞大致可以分為兩大類：正碰和側(cè)碰。

本文采用的一種判定是否碰撞的思想是：將機器人的當(dāng)前節(jié)點和下一節(jié)點兩點之間看作是一條線段，兩個機器人則有兩條線段，通過判定這兩條線段是否相交，則可判定兩個機器人下一步是否產(chǎn)生碰撞。實現(xiàn)這種思想的一種常用的方法是通過向量叉積來判斷，這種方法判斷線段是否相交需要完成兩個關(guān)鍵步驟：快速排斥試驗和跨立試驗。

1）快速排斥試驗。判斷兩條線段在坐標(biāo)軸x 以及在坐標(biāo)軸y 上的投影是否有重合。即判斷一條線段中x 軸坐標(biāo)較大的端點是否小于另一條線段中x 軸坐標(biāo)較小的端點，若是，則說明兩條線段必然沒有交點。否則，同理判斷y 軸坐標(biāo)。

2）跨立試驗。如果兩線段相交那么就意味著它們互相跨立。如圖11 所示，點A 和點B 分別在線段CD 兩側(cè)，點C 和點D 分別在線段AB 兩側(cè)。判斷A點與B 點是否在線段CD 的兩側(cè)，即向量AD 與向量BD 分別在向量CD 的兩端，也就是其叉積是異號的。同時證明C 點與D 點在線段AB 的兩端，兩個條件同時滿足，則表示線段相交。

圖11 兩線段相交

當(dāng)經(jīng)過快速排斥試驗和跨立試驗判定兩條線段是否相交后，如果不相交，則兩個機器人按原路徑繼續(xù)前進。如果相交，則將機器人的當(dāng)前節(jié)點和下一節(jié)點當(dāng)作向量計算，通過比較兩個向量是否方向相反，如果是則為正碰，如果不是則為側(cè)碰。若為正碰，根據(jù)機器人的優(yōu)先級決定優(yōu)先級低的機器人重新規(guī)劃路徑，優(yōu)先級高的機器人按原路徑前進。若為側(cè)碰，根據(jù)機器人的優(yōu)先級決定優(yōu)先級低的機器人等待一個步長，讓優(yōu)先級高的機器人優(yōu)先通過。正碰時的局部路徑規(guī)劃：將下一個節(jié)點設(shè)為臨時的灰色障礙物，以下個節(jié)點為目標(biāo)節(jié)點，當(dāng)前節(jié)點為源節(jié)點，進行局部路徑規(guī)劃。圖12 給出了基于本研究算法的一次多個農(nóng)業(yè)機器正碰試驗演示。在本試驗中，算法的各項參數(shù)分別是：α=1.5，β=7，ρ=0.3，Q=5，螞蟻數(shù)量為50 只，最大迭代次數(shù)為100，紅色機器人的優(yōu)先級低于藍色機器人。

如圖12（a）所示，兩個機器人各自從起點出發(fā)，且其初始路徑正好反向重疊。圖12（b）、圖12（c）、圖12（d）表示低優(yōu)先級的機器人做出了臨時的局部路徑規(guī)劃以避免碰撞。圖12（e）表示兩個機器人避開碰撞后順利到達終點。

4 結(jié)語

本文在基礎(chǔ)蟻群算法的基礎(chǔ)上，提出了死鎖問題的解決方案，并對啟發(fā)信息函數(shù)進行了改進，在Matlab-R2018b 平臺上進行仿真試驗。針對室內(nèi)農(nóng)業(yè)復(fù)雜多變的場景環(huán)境，本文考慮了動態(tài)障礙物的存在和多農(nóng)業(yè)機器人作業(yè)兩個重要方面，提出相應(yīng)的避碰策略，分別為機器人與障礙物之間的動態(tài)窗口掃描搭配區(qū)域膨脹和機器人與機器人之間的正碰、側(cè)碰做出了針對性的處理。仿真試驗結(jié)果證明，研究的改進蟻群算法在多農(nóng)業(yè)機器人運作場景中能取得較好的運行效果，在有靜態(tài)和動態(tài)障礙物的環(huán)境下，具備較好的穩(wěn)定性與實時性。因此認(rèn)為該研究可以推廣應(yīng)用在室內(nèi)農(nóng)業(yè)多機器人作業(yè)的復(fù)雜場景中。