基于勢場蟻群算法的溫室機器人路徑規(guī)劃

王紅君+徐+軍+趙輝+岳有軍

摘要：針對工作在溫室環(huán)境下的機器人需要在壟間穿行的難點，建立柵格化環(huán)境模型，提出了基于勢場蟻群算法的溫室機器人路徑規(guī)劃算法。溫室環(huán)境下的機器人路徑規(guī)劃是要在溫室中長有農作物的情況下，使機器人能夠尋找到一條從起始點到目標點的連續(xù)、無碰撞的路徑，其中，機器人走過的路徑要包圍農作物的生長區(qū)域，以方便機器人對農作物進行施肥、摘取果實等行為。路徑規(guī)劃過程中，螞蟻根據當前節(jié)點與可達點之間是否存在障礙物分為確定性方向轉移和概率性方向轉移2種轉移方式，并且重新構造了啟發(fā)信息，指引螞蟻搜索。在不同的環(huán)境下進行了仿真試驗，結果表明，改進勢場蟻群算法能有效解決溫室環(huán)境的路徑規(guī)劃問題。

關鍵詞：溫室；機器人；蟻群算法；人工勢場；路徑規(guī)劃

中圖分類號： TP242 文獻標志碼： A 文章編號：1002-1302（2017）18-0222-04

收稿日期：2016-04-19

基金項目：天津市農業(yè)科技成果轉化與推廣項目（編號：201203060、201303080）。

作者簡介：王紅君（1963—），女，天津人，碩士，教授，主要從事工業(yè)先進控制技術、微機控制、智能控制等方面的研究。E-mail：hongewang@126.com。

通信作者：徐 軍，碩士研究生，主要從事機器人智能控制方面的研究。E-mail：598649947@qq.com。 利用機器人來完成溫室內各項工作，不僅可以大幅度減少勞動者的體力勞動，并且可以避免勞動者在高溫、高濕甚至有毒的作業(yè)環(huán)境中受到人身傷害[1]。路徑規(guī)劃是實現機器人自由移動的一項關鍵技術。目前，溫室內各種農用機具的作業(yè)路徑規(guī)劃一般是由操作者根據經驗、習慣進行選擇的，也有學者提出了一些路徑規(guī)劃的方法。陳濟勤從農機運營管理和作業(yè)工藝角度考慮，提出了針對耕地、播種、收割等不同農田作業(yè)方式下的直行、繞行、斜行等作業(yè)路徑模式[2]。Stoll提出根據地塊最長邊將田塊分割成子地塊進行路徑優(yōu)化的方法[3]。以上路徑規(guī)劃方法都是對地塊全區(qū)域覆蓋的路徑優(yōu)化方法，但是目前尚沒有一種合理的用于機器人在農作物的壟間狹窄路徑中穿行的路徑規(guī)劃方法。目前，用于移動機器人路徑規(guī)劃的方法有模擬退火法、模糊邏輯法、柵格法等。但是隨著環(huán)境系統(tǒng)的復雜性和任務難度的增加，基于數學模型的傳統(tǒng)路徑規(guī)劃已經難以取得理想的效果，于是出現了一些仿生智能算法，如蟻群算法[4]、免疫算法[5]、遺傳算法[6-8]等，但是這些方法存在搜索空間大、參數難以確定、搜索效率低及容易產生局部最優(yōu)等問題。

為了解決在溫室環(huán)境下機器人的路徑規(guī)劃難題，在本試驗中將改進的人工勢場算法和蟻群算法進行融合， 利用人工勢場的全局性和蟻群算法的正反饋及全局尋優(yōu)等特點，不僅可以實現機器人的實時避障，而且在復雜的環(huán)境中可以較好地改善局部最優(yōu)、徘徊等問題。

1 基本算法原理

1.1 基于柵格地圖的環(huán)境建模

假設機器人在二維空間中工作，并且將機器人看作二維空間中的點狀移動物體。柵格地圖的優(yōu)點是易于創(chuàng)建和維護，并且機器人感知的每個柵格信息能直接與環(huán)境相對應。柵格地圖中有障礙物的區(qū)域表示非自由區(qū)域，無障礙物的柵格表示自由區(qū)域，環(huán)境中的障礙物映射到柵格地圖中覆蓋面積不足1個柵格需要按1個柵格處理。

在本試驗中柵格地圖按照從上到下、從左到右的順序依次編號為1，2，3，…，n。如圖1所示，農作物種植區(qū)域表示為障礙區(qū)域（黑色部分），障礙區(qū)域的柵格中心坐標表示為障礙物的中心點，自由區(qū)域（白色部分）的柵格中心點作為機器人可行走的路徑點，這樣路徑規(guī)劃問題就可以描述為在自由區(qū)域中尋找一個連續(xù)柵格的子集，使機器人在自由區(qū)域中穿行的路徑最短，并且機器人所走路徑要包圍農作物的種植區(qū)域。

1.2 蟻群算法原理

蟻群算法是由意大利學者Dorigo等提出的[4]，以后所有改進的蟻群算法都是在此基礎上發(fā)展起來的。在基本蟻群算法中，第k只螞蟻在t時刻從節(jié)點i轉移到節(jié)點j的概率為

式中：τij（t）為t時刻節(jié)點i與節(jié)點j之間殘留的信息素濃度；ηij（t）表示t時刻節(jié)點i和節(jié)點j之間的期望啟發(fā)函數，在基本蟻群算法中期望啟發(fā)函數定義為節(jié)點i與節(jié)點j之間的距離dij的倒數，即ηij=1/dij；α為信息素啟發(fā)因子，表示前幾代螞蟻走過軌跡的相對重要性[9]；β為期望啟發(fā)因子，表示能見度的相對重要性[10]；allowedk為螞蟻k下一步可以選擇的節(jié)點。

螞蟻完成1次循環(huán)，各路徑上的信息素更新規(guī)則按公式（2）、公式（3）計算：

式中：ρ為信息素揮發(fā)系數，為有效防止信息素無限積累，ρ的取值范圍為[0，1）；Δτij（t）表示本次循環(huán)中節(jié)點i與節(jié)點j之間路徑上的信息素增量，在初始時刻Δτij（0）=0；Δτijk（t）為第k只螞蟻在本次循環(huán)中殘留在節(jié)點i與節(jié)點j之間的信息素，在本試驗中信息素的計算采用Dorigo等提出的Ant-Cycle模型[4]，即

式中：Q為信息素強度，它在一定程度上影響算法的收斂速度；Lk為螞蟻k在本次循環(huán)中所走過路徑的總長度，Pk（begin，end）為螞蟻k在本次循環(huán)中從起點到終點所走過的節(jié)點。

1.3 人工勢場算法原理

人工勢場算法是由Khatib提出的一種算法[11]，其基本思想是在機器人的工作環(huán)境中利用虛擬勢場力信息來指引機器人運動，環(huán)境中的障礙物對機器人產生斥力Frep，目標點對機器人產生引力Fatt，斥力和引力形成的合力Ftot指引機器人移動。假設機器人所在當前點為P，目標點對當前點的引力勢場函數為Uatt（P），如公式（5）所示，障礙物對當前點的斥力勢場函數為Urep（P），如公式（6）所示。

式中：katt和krep分別為引力勢場常量、斥力勢場常量；d（P，G）為當前點P和目標點G的距離；d（P，O）為當前點P和障礙物O的距離；d0為斥力勢場作用半徑，只有當機器人在障礙物的作用半徑范圍內時斥力才起作用。機器人在P點所受的引力和斥力分別如公式（7）、公式（8）所示。endprint

式中：nPG表示移動機器人當前所在位置O指向目標位置G的單位矢量；nOP表示由障礙物位置O指向移動機器人當前所處位置P的單位矢量。根據幾何疊加原理可以得到機器人在當前位置所受勢場合力為

2 改進算法原理

2.1 改進的勢場蟻群算法

由于在溫室環(huán)境下，農作物種植相對規(guī)范，根據人工勢場算法和蟻群算法的特點，將兩者有機結合，使之適合溫室環(huán)境下的機器人路徑規(guī)劃。根據螞蟻當前所在節(jié)點與目標節(jié)點間是否存在障礙物分為確定性方向選擇和概率性方向選擇2種方式來選擇下一節(jié)點，這種節(jié)點選擇方式既解決了蟻群算法搜索效率低的問題，又克服了人工勢場算法容易陷入局部極小點的缺點。

當螞蟻當前所在節(jié)點與目標節(jié)點之間不存在障礙物時，螞蟻選擇與勢場合力方向夾角最小的自由柵格作為下一個所要選擇的節(jié)點，如圖2所示，分別計算螞蟻所在柵格位置所受合力Ftot與相鄰可選擇柵格的角度差，選擇角度差最小的一個柵格min|θi-θ|（θi依次取0、45、…、315°）作為螞蟻下一步要選擇的節(jié)點。對于螞蟻根據勢場合力方向進行節(jié)點選擇所走的路徑，同樣加入蟻群算法禁忌表，已走路徑上信息素的更新策略同樣按照蟻群算法計算。

在螞蟻當前所在節(jié)點與目標節(jié)點存在障礙物時，螞蟻不能再按照勢場合力方向選擇下一個將要到達的節(jié)點，而是通過運用蟻群算法的概率性方向轉移選擇下一個將要到達的節(jié)點，如圖3所示，在當前情況下，螞蟻所在節(jié)點與當前目標點之間存在障礙物，螞蟻將通過概率性方向轉移選擇下一個節(jié)點，當前可供螞蟻選擇的節(jié)點有3個，分別為圖3中的1、2、3節(jié)點，只有從當前節(jié)點直接向節(jié)點3轉移才是最佳路徑。

由于是溫室環(huán)境下的路徑規(guī)劃，機器人需要像農民似的一排排地對農作物進行作業(yè)，所以會選取多個可達點作為機器人運動過程中需要經過的點，把規(guī)劃的機器人路徑是否經過所有的可達點，包圍了所有農作物生長區(qū)域，并且路徑長度是否最短作為路徑規(guī)劃是否成功的標準。在本試驗中，將2壟農作物之間路徑的中點選取為路徑規(guī)劃過程中需要經過的可達點，如圖4所示，對這些可達點進行編號，分別為圖4中的1、2、3節(jié)點，同時建立類似蟻群算法的禁忌表，這些可達點都是螞蟻在路徑規(guī)劃過程中必須經過的點，將螞蟻已經經過的可達點加入禁忌表，加入禁忌表的可達點不再起作用。

在利用蟻群算法進行傳統(tǒng)點到點的路徑規(guī)劃時，將當前

節(jié)點與目標節(jié)點距離的倒數作為啟發(fā)信息，啟發(fā)信息的大小隨著螞蟻向目標點的移動而增加，而將蟻群算法應用在溫室環(huán)境下的路徑規(guī)劃時，隨著螞蟻的移動，當前點與目標點的距離會增加，導致啟發(fā)信息越來越小，因而在本試驗算法中直接使用距離信息作為啟發(fā)信息，從而使啟發(fā)信息隨著螞蟻的移動越來越大，同時將當前節(jié)點的勢場力信息加入到啟發(fā)信息中，構成綜合的啟發(fā)信息，充分利用對已知環(huán)境的感知信息來規(guī)避障礙。本試驗中改造后的綜合啟發(fā)信息為

式中：dig（t）表示當前點與當前可達點之間的距離。

2.2 算法流程

采用勢場蟻群算法進行機器人路徑規(guī)劃的算法流程如下：

步驟1：采用0和1組成的矩陣抽象描述溫室環(huán)境，確定障礙物分布與可行路徑，確定機器人初始點與目標點；步驟2：初始化勢場蟻群算法的基本參數，如設置螞蟻數量m、最大迭代次數Nmax、信息啟發(fā)因子α、期望啟發(fā)因子β、信息素揮發(fā)因子ρ、初始信息素矩陣τ、信息素強度Q、初始化禁忌表Tabu、引力增益系數katt、斥力增益系數krep、斥力作用半徑d0；步驟3：將m只螞蟻置于起始節(jié)點；步驟4：判斷螞蟻所在當前節(jié)點i與當前目標點之間是否存在障礙物，如果存在障礙物，按公式（1）選擇下一個節(jié)點j，如果不存在障礙物，按照勢場合力方向與可選節(jié)點方向夾角的最小值選擇下一個節(jié)點j；步驟5：將節(jié)點j加入禁忌表Tabu；步驟6：重復步驟4、步驟5，直至螞蟻到達所有目標點，并保存所有螞蟻所走路徑及其長度，選擇最優(yōu)路徑；步驟7：根據公式（10）更新各路徑信息素；步驟8：判斷是否達到最大迭代次數，若達到，則算法終止并輸出最優(yōu)路徑，否則，重復步驟3至步驟7。

3 仿真試驗

3.1 關鍵參數設置

為了驗證本試驗所提算法的有效性，進行了相應仿真試驗，仿真試驗環(huán)境為Matlab R2014a。在本試驗中用25×25的柵格地圖模擬真實的溫室環(huán)境，柵格大小為單位長度為1的正方形，機器人起始點設置在地圖的左上角，將2壟農作物之間路徑的中點選取為可達點。在設置勢場蟻群算法的基本參數時，由于目前尚沒有科學的求取適合參數的方法，本試驗所設置的參數完全是在仿真環(huán)境下試驗所得結果，設置的參數為信息素啟發(fā)因子α=0.7、期望啟發(fā)因子β=0.3、信息素揮發(fā)系數ρ=0.2、信息素強度Q=10、引力增益系數katt=2、斥力增益系數krep=1、斥力作用半徑d0=2。

3.2 仿真試驗結果

考慮到溫室內的實際種植環(huán)境，溫室內可能會分布一些其他設施，比如溫室內的承重柱，各種監(jiān)測溫室內溫度、濕度的設施，農作物種植時要避開這些設施，利用柵格地圖描述的實際種植效果和勢場蟻群算法規(guī)劃的路徑結果如圖5所示。改進后的勢場蟻群算法的最大迭代次數Nmax設置為50，每次釋放螞蟻數目m=100，規(guī)劃結果最小路徑長度為Lmin=233941 1，并且機器人所走路徑包圍了農作物種植區(qū)域，達到了本試驗路徑規(guī)劃的目的。

路徑規(guī)劃過程中會產生大量的迷失螞蟻，迷失的螞蟻會在數據篩選過程中剔除，并且迷失的螞蟻所走路徑不能進行信息素更新。由圖6可知，在50次的迭代過程中，在第39次產生最短路徑，經過2次波動后，最終收斂于最短路徑。

在目前的溫室種植條件下，農民為了避免種植單種農作物帶來的經濟風險，根據農作物生長周期安排合理種植等問題，可能會在同一溫室內的不同區(qū)域同時種植多種農作物。這些農作物由于種類不同，機器人工作的區(qū)域就不是整個溫室，而是其中的某一個區(qū)域或多個區(qū)域，如圖7所示，溫室內同時種植了4種作物，只有其中2塊區(qū)域的作物需要機器人工作，由于此時的路徑規(guī)劃相對復雜，勢場蟻群算法的基本參數不變，機器人起始位置仍然在地圖的左上角，每次釋放螞蟻數量增加到300只，當機器人的路徑包圍了所有需要工作的區(qū)域后停止工作，最終的規(guī)劃結果為最小路徑長度為 Lmin=152.355 3，達到了規(guī)劃目的，機器人所走路徑包圍了需要工作的農作物覆蓋區(qū)域，并且路徑長度最短。由圖8可知，在50次的迭代過程中，在第40次產生最短路徑，經過1次波動后，最終收斂于最短路徑。endprint

4 結論

本試驗針對在溫室環(huán)境下機器人的特殊路徑規(guī)劃問題，采用人工勢場算法和蟻群算法融合的方法，提出螞蟻根據當前所在位置采用確定性方向轉移或者概率性方向轉移選擇下一個節(jié)點的策略。確定性方形轉移保證了螞蟻能快速地向可達點或者目標點的方向運動，概率性方向轉移保證了該算法解的多樣性。同時，將蟻群算法中的不適應溫室環(huán)境路徑規(guī)劃的約束條件優(yōu)化為適應溫室路徑規(guī)劃的條件。在Matlab的仿真環(huán)境下，對本試驗所提算法進行了驗證，結果表明，本試驗算法對溫室環(huán)境下路徑規(guī)劃的效果較好，同時具有動態(tài)的收斂過程。

參考文獻：

[1]高國琴，李 明. 基于K-means算法的溫室移動機器人導航路徑識別[J]. 農業(yè)工程學報，2014，30（7）：25-33.

[2]陳濟勤. 農業(yè)機械運用管理學[M]. 北京：中國農業(yè)出版社，1999.

[3]Stoll A. Automatic operation planning for GPS-guided machiner[C]//Proceedings of 4th European Conference on Precision Agriculture. 2003：657-664.

[4]Dorigo M，Maniezzo V，Colorni A. Ant system：optimization by a colony of cooperating agents[J]. IEEE Transactions on Systems，Man，and Cybernetics，1996，26（1）：29-41.

[5]葉兆莉，袁明新，程 帥，等. 移動機器人的一種煙花爆炸式新免疫規(guī)劃算法[J]. 計算機仿真，2013，30（3）：323-326，375.

[6]Manikas T W，Ashenayi K，Wainwright R L. Genetic algorithms for autonomous robot navigation[J]. IEEE Instrumentation and Measurement Magazine，2007，10（6）：26-31.

[7]Alvarez A，Caiti A，Onken R. Evolutionary path planning for autonomous underwater vehicles in a variable ocean[J]. IEEE Journal of Oceanic Engineering，2004，29（2）：418-429.

[8]Cobano J A，Conde R，Alejo D，et al. Path planning based on genetic algorithms and the Monte-Carlo method to avoid aerial vehicle collisions under uncertainties[C]//International Conference on Robotics and Automation. 2011：4429-4434.

[9]周明秀，程 科，汪正霞. 動態(tài)路徑規(guī)劃中的改進蟻群算法[J]. 計算機科學，2013，40（1）：314-316.

[10]段海濱. 蟻群算法原理及其應用[M]. 北京：科學出版社，2005.

[11]Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research，1986，5（1）：90-98.張啟宇，劉 峰，陳英義，等. 海參病害防治診斷專家系統(tǒng)的研究[J]. 江蘇農業(yè)科學，2017，45（18）：226-229.endprint