基于蟻群算法的多采摘機(jī)器人路徑規(guī)劃與導(dǎo)航系統(tǒng)

張 璐，王 慧

(河南工業(yè)職業(yè)技術(shù)學(xué)院，河南 南陽 473000)

0 引言

近年來，人工智能已成為新一輪工業(yè)革命的基石，不僅廣泛應(yīng)用于工業(yè)控制領(lǐng)域，在農(nóng)業(yè)生產(chǎn)中的應(yīng)用越來越普及。在農(nóng)忙時節(jié)，農(nóng)場往往會出現(xiàn)勞動力大量匱乏的現(xiàn)象，因此機(jī)器人在農(nóng)業(yè)上的應(yīng)用需求越來越明顯。研發(fā)出一種能進(jìn)行遠(yuǎn)程控制的采摘機(jī)器人，代替?zhèn)鹘y(tǒng)人力的作業(yè)操作，對提高農(nóng)業(yè)生產(chǎn)水平及種植效率具有重要意義，也是現(xiàn)代化農(nóng)業(yè)的發(fā)展趨勢。為此，根據(jù)采摘機(jī)器人作業(yè)環(huán)境，采用蟻群算法，結(jié)合先全局后局部的路徑規(guī)劃方法，設(shè)計(jì)了一種多采摘機(jī)器人路徑規(guī)劃和導(dǎo)航系統(tǒng)。

1 蟻群算法簡述

蟻群算法是意大利學(xué)者Dorigo在20世紀(jì)90年代模擬和總結(jié)螞蟻尋找食物的一系列行為過程，得出的一種解決復(fù)雜組合優(yōu)化問題的方法。該方法從螞蟻、蜜蜂這些昆蟲尋找食物的行為入手，其實(shí)對于一個個體而言，它們的行為非常容易分析，但當(dāng)它們組合成一個龐大的群體后，群體行為分析難度就大大增加了。其原因是因?yàn)槲浵亗€體間是由一種分泌物作為信息素進(jìn)行交流通信，螞蟻在行走的路線上會留下這些物質(zhì)，而所有的螞蟻都能夠感知這些物質(zhì)的濃度，并根據(jù)該物質(zhì)濃度的強(qiáng)弱選擇自己的運(yùn)動方向。因此，蟻群的集體行為會形成一種特定的信息正反饋場景，即某路線上經(jīng)過的螞蟻數(shù)量越大，則后面選擇該路線的螞蟻就會越多。螞蟻間根據(jù)感知這種信息素物質(zhì)的強(qiáng)弱實(shí)現(xiàn)搜索食物的整個過程，這也就是蟻群算法的緣由。蟻群尋找食物過程示意圖如圖1所示。

圖1 蟻群尋找食物過程示意圖Fig.1 The schematic diagram of ant colony searching for food process

圖1中，假設(shè)A為蟻群群居的位置，E為有食物的地方，MN為AE之間的一個阻擋物。由于從A到E之間存在障礙物MN，那么蟻群在搬運(yùn)食物的過程中只能是從BCD或者BFD兩條路徑。其中，兩條路徑的長度分別為10和20。假設(shè)在1min內(nèi)有200只螞蟻從A運(yùn)動到E，螞蟻經(jīng)過后信息素總數(shù)增加為20，為了讓計(jì)算比較簡單，設(shè)定信息素從產(chǎn)生到消失的時間為10min。在最開始時，沒有信息素物質(zhì)的存在，螞蟻選擇兩條路線的概率相同；但因?yàn)锽CD路徑短，螞蟻選擇BCD需要的時間較少，因此在相同時間內(nèi)，走過BCD的螞蟻會多一些；這樣BCD路徑上的信息素物質(zhì)比BFD要多，也會吸引后面的螞蟻從BCD走，從而實(shí)現(xiàn)從A到E的最優(yōu)路徑。

蟻群算法最初是應(yīng)用于解決TSP問題，其原理為：在最初時刻，所有螞蟻都在同一起點(diǎn)，每條可供選擇前進(jìn)路徑的信息素物質(zhì)都為零，設(shè)定行程最短的路徑距離為smin，距離最長的為smax，經(jīng)過的螞蟻數(shù)量為M。其中，經(jīng)過smin和smax的螞蟻數(shù)量分別為M1和M2，則

M=M1+M2

(1)

那么，當(dāng)M只螞蟻經(jīng)過smin和smax后，接下來的螞蟻從smin經(jīng)過的概率為

(2)

其中，Psmin(M+1)為低M+1次螞蟻從smin經(jīng)過的概率；a和h為參數(shù)。

2 多采摘機(jī)器人路徑規(guī)劃

在多采摘機(jī)器人作業(yè)過程中，障礙物較多，機(jī)器人間相互影響，成為彼此的障礙物，因此多機(jī)器人路徑規(guī)劃難度較大。在實(shí)際研究中，多個采摘機(jī)器人在同一種植區(qū)作業(yè)，需要規(guī)劃多條最優(yōu)路徑，但在具體的作業(yè)過程中，由于障礙物的存在，最優(yōu)路徑會發(fā)生變化，也會影響其他機(jī)器人的移動路徑?；谏鲜鰡栴}，為了保證多采摘機(jī)器人實(shí)現(xiàn)無碰撞作業(yè)，該系統(tǒng)需要實(shí)時規(guī)劃路線，防止機(jī)器人間、機(jī)器人與障礙物間的碰撞。

2.1 多采摘機(jī)器人路徑規(guī)劃整體設(shè)計(jì)

由于產(chǎn)業(yè)種植園區(qū)環(huán)境不確定性因素較大，如種植人員園區(qū)工作以及其他機(jī)械的隨機(jī)移動，且傳感器探測具有一定的局限性，機(jī)器人在最初規(guī)劃時往往很難獲取全部的信息，采用簡單的規(guī)劃往往難以獲得理想的最優(yōu)路徑。因此，可在獲得一定園區(qū)環(huán)境信息的基礎(chǔ)上，進(jìn)行作業(yè)前的全局規(guī)劃，并將其作為理想的最優(yōu)路線。在采摘機(jī)器人群的實(shí)際運(yùn)動作業(yè)中，根據(jù)后續(xù)探測的環(huán)境信息，規(guī)劃出局部的最優(yōu)路徑，進(jìn)行簡單的微調(diào)，使機(jī)器人避開所有碰撞物，并最快地到達(dá)目的地?；谝陨蠁栴}，該系統(tǒng)采用全局和局部的規(guī)劃方法，實(shí)現(xiàn)多采摘機(jī)器人的路徑規(guī)劃。多采摘機(jī)器人路徑規(guī)劃框架如圖2所示。

1)全局路徑規(guī)劃。本章研究的是多采摘機(jī)器人在實(shí)際園區(qū)作業(yè)過程中的路徑規(guī)劃算法，對于其全局路徑規(guī)劃，首先要做到的是按照具體作業(yè)要求，根據(jù)已知環(huán)境信息，給每個機(jī)器人分配任務(wù)，并確定每個機(jī)器人作業(yè)的起始點(diǎn)和終點(diǎn)，然后結(jié)合多機(jī)器人間的合作機(jī)制，在兩點(diǎn)之間規(guī)劃一條無碰撞的優(yōu)化路線。在整個算法中，最重要的是提高路線規(guī)劃速度和實(shí)現(xiàn)路徑規(guī)劃的最優(yōu)性。由于螞蟻算法路徑規(guī)劃效率比較高，且其對于障礙物變化的適應(yīng)性較強(qiáng)，因此本文將采用蟻群算法對多采摘機(jī)器人進(jìn)行全局路徑規(guī)劃。

2)局部路徑規(guī)劃。有了全局路徑規(guī)劃之后，采摘機(jī)器人群就可以開始各自的作業(yè)任務(wù)。開始階段，采摘機(jī)器人將按照各自的規(guī)劃路徑進(jìn)行作業(yè)，并實(shí)時采集周邊環(huán)境信息，根據(jù)采集到的動態(tài)信息，進(jìn)行分析處理，根據(jù)碰撞檢測、決策算法和行為檢測等進(jìn)行局部的路徑修改，從而使自身具有更強(qiáng)的環(huán)境適應(yīng)能力。

本系統(tǒng)采用的局部規(guī)劃算法是利用機(jī)器人的車載傳感器對周邊環(huán)境信息進(jìn)行實(shí)時檢測，并與全局規(guī)劃時的信息進(jìn)行對比分析，根據(jù)碰撞檢測子系統(tǒng)對是否存在碰撞風(fēng)險進(jìn)行評估。若存在撞機(jī)危險，則對局部路徑進(jìn)行一定的調(diào)整，在調(diào)整過程中，始終以作業(yè)終點(diǎn)為最終目標(biāo)，避免出現(xiàn)局部的極值點(diǎn)而忽略機(jī)器人整體規(guī)劃。另外，在局部規(guī)劃過程中，行為檢測子系統(tǒng)需要實(shí)時對采摘機(jī)器人規(guī)劃的路徑進(jìn)行判斷，確定其是否符合全局規(guī)劃的目標(biāo)，若符合，則執(zhí)行。在機(jī)器人成功避開障礙物后，則可以按照全局規(guī)劃的路線繼續(xù)進(jìn)行作業(yè)操作，在執(zhí)行期望路徑的過程中，向最近的子路徑節(jié)點(diǎn)移動。如此反復(fù)，不管局部規(guī)劃路徑怎么實(shí)施，采摘機(jī)器人最終的目標(biāo)終點(diǎn)不會發(fā)生改變，直到其成功到達(dá)終點(diǎn)。

圖2 路徑規(guī)劃框架圖Fig.2 The path planning framework

2.2 路徑規(guī)劃的適應(yīng)度函數(shù)

在進(jìn)行路徑規(guī)劃中，除保證各機(jī)器人前進(jìn)路徑最優(yōu)外，還需要各機(jī)器人間協(xié)調(diào)，以避免出現(xiàn)兩采摘機(jī)器人發(fā)生碰撞。路徑規(guī)劃的適應(yīng)度函數(shù)是影響蟻群算法能否高效的計(jì)算出最優(yōu)解的主要原因，多采摘機(jī)器人路徑規(guī)劃要求包含避障和距離最短。因此，適應(yīng)度函數(shù)重點(diǎn)要考慮路徑長度、路徑平滑度及可行性，計(jì)算方法如下：

路徑長度為

(3)

其中，d(p)為路徑長度；i為路徑子節(jié)點(diǎn)編號；n為障礙物數(shù)量。

d(p)的倒數(shù)是決定路徑規(guī)劃是都最短的適應(yīng)度函數(shù)fit1，其表達(dá)式為

(4)

路徑平滑度為

(5)

其中，h(p)為路徑平滑度。在路徑規(guī)劃過程中，在選擇目標(biāo)函數(shù)時，要結(jié)合采摘機(jī)器人移動性能、光滑適應(yīng)性作為主要參數(shù)，盡可能使生成的路徑變化均勻，取h(p)的倒數(shù)為路徑平滑度的適應(yīng)度函數(shù)fit2為

(6)

躲障是采摘機(jī)器人工作的首要條件，是其正常作業(yè)的保證，因此可行性是指采摘機(jī)器人群是否能夠安全地避開障礙物并得到終點(diǎn)。以下是采摘機(jī)器人在最優(yōu)路徑行駛中會出現(xiàn)的幾種情況：

1)在優(yōu)化路徑中沒有出現(xiàn)障礙物，則可以按照蟻群算法確定其適應(yīng)度函數(shù)fit3為

(7)

由式(7)可以看出：當(dāng)規(guī)劃的路徑中沒有出現(xiàn)障礙物時，fit3值為1，否則為0。

2)假設(shè)一采摘機(jī)器人從路徑子節(jié)點(diǎn)i移動到路徑子節(jié)點(diǎn)i+1，則其與障礙物不想交的適應(yīng)度函數(shù)fit4為

(8)

綜上所述，采摘機(jī)器人最終可行性為

Fi1t=fit3·fit4

(9)

3 多采摘機(jī)器人導(dǎo)航系統(tǒng)

3.1 導(dǎo)航系統(tǒng)整體方案

多采摘機(jī)器人導(dǎo)航系統(tǒng)是指在執(zhí)行任務(wù)期間，對各采摘機(jī)器人的作業(yè)路線進(jìn)行導(dǎo)航，即根據(jù)空間位置的做出全局規(guī)劃，結(jié)合在前進(jìn)過程中感知到的部分環(huán)境信息，實(shí)時對路線進(jìn)行恰當(dāng)?shù)男薷模龑?dǎo)各機(jī)器人安全前進(jìn)到指定位置。該方案主要包括環(huán)境建模、自主定位和路徑規(guī)劃3部分。由于路徑規(guī)劃已在前一章詳細(xì)的闡述，這里不再重復(fù)。

1)環(huán)境建模。環(huán)境建模是指采用數(shù)學(xué)模型的方法，對采摘作業(yè)區(qū)域進(jìn)行詳細(xì)描述，并將其抽象成計(jì)算機(jī)系統(tǒng)能夠直接識別的數(shù)據(jù)信息。本文所采取的導(dǎo)航系統(tǒng)數(shù)據(jù)信息存在于后臺服務(wù)器中，并由該信息抽象出拓?fù)鋱D，對路線信息進(jìn)行簡單的描述。另外，也方便各采摘機(jī)器人識別道路之間的聯(lián)系，準(zhǔn)確實(shí)現(xiàn)全局、局部路徑規(guī)劃和實(shí)時導(dǎo)航。

2)采摘機(jī)器人定位。采摘機(jī)器人定位是指根據(jù)周邊環(huán)境信息和GPS確定其在作業(yè)區(qū)域相對于全局坐標(biāo)的具體位置，其流程圖如圖3所示。

圖3 采摘機(jī)器人定位流程圖Fig.3 The positioning flow chart of picking robot

3.2 環(huán)境建模的具體實(shí)現(xiàn)

環(huán)境建模的空間信息對實(shí)現(xiàn)多采摘機(jī)器人的導(dǎo)航具有決定性的意義，本文所采用的方法是將地圖信息存在后臺服務(wù)器，通過后臺服務(wù)器的數(shù)據(jù)庫進(jìn)行拓?fù)鋱D的繪制，并采用循環(huán)鏈表的方式將數(shù)據(jù)信息進(jìn)行保存。因此，后臺服務(wù)器中的數(shù)據(jù)庫存放有地圖的全部信息，可用于多采摘機(jī)器人的實(shí)時定位與導(dǎo)航，而拓?fù)鋱D則可以用于結(jié)合螞蟻算法實(shí)現(xiàn)路徑的最優(yōu)規(guī)劃。

1)地圖信息數(shù)據(jù)庫。先將地圖信息分為站和道路兩種，其中站面積比較大，占地較多，可以用來讓機(jī)器人進(jìn)行短暫的停留；而道路則是機(jī)器人前進(jìn)的路線，用來連接各個站。其次，在策略算法進(jìn)行規(guī)劃的過程中，可以將站和道路看成一段段或者一塊塊的矩陣，將其連接起來就可以用來表示地圖。地圖信息如圖4所示。

圖4 搭建導(dǎo)航地圖信息圖Fig.4 The establishment of navigation cartographic information

2)建立拓?fù)鋱D。建立拓?fù)鋱D需要以服務(wù)器中的數(shù)據(jù)信息為依據(jù)，其主要是用來描述各站點(diǎn)與道路之間的連接狀態(tài)，在內(nèi)存中以循環(huán)鏈表的方式進(jìn)行儲存。導(dǎo)航地圖拓?fù)鋱D如圖5所示。

圖5 導(dǎo)航地圖的拓?fù)鋱DFig.5 The topological graph of navigation map

循環(huán)鏈表是數(shù)據(jù)結(jié)構(gòu)中一種常用的保存數(shù)據(jù)信息的方法，可以靈活地表示數(shù)據(jù)間的前后關(guān)系，找到其中一個信息，便可以知道其前后內(nèi)存里面的信息。在導(dǎo)航過程中，需要經(jīng)常訪問節(jié)點(diǎn)間的連接信息，因此采用該方法可以大大提高數(shù)據(jù)讀取信息。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證多采摘機(jī)器人路徑規(guī)劃與導(dǎo)航系統(tǒng)的準(zhǔn)確性可靠性，本文利用MatLab仿真軟件對其進(jìn)行了仿真實(shí)驗(yàn)。其中，蟻群迭代步長設(shè)置為0.02，迭代次數(shù)為100次，運(yùn)行環(huán)境為200m×200m的區(qū)域，本實(shí)驗(yàn)中采摘機(jī)器人數(shù)量為3個。實(shí)驗(yàn)?zāi)康模涸诓烧鳂I(yè)區(qū)域，該系統(tǒng)經(jīng)過改變執(zhí)行和迭代次數(shù)，判斷多采摘機(jī)器人群是否能夠成功避開障礙物，并且安全達(dá)到目的地。仿真結(jié)果如圖6所示。

圖6 仿真結(jié)果圖Fig.6 The simulation result diagram

由圖6可以看出：實(shí)驗(yàn)中各采摘機(jī)器人均安全從起點(diǎn)到達(dá)終點(diǎn)，當(dāng)一條線路只能允許一臺機(jī)器人通過時，其他機(jī)器人能夠避讓到站點(diǎn)休息，然后再進(jìn)行前進(jìn)操作，成功解決了路徑?jīng)_突的問題。從路徑長度和光滑度可以看出，該系統(tǒng)優(yōu)化效果明顯，具有很強(qiáng)的避障、路徑規(guī)劃和導(dǎo)航能力，且可行性高。

5 結(jié)論

1)針對多采摘機(jī)器人聯(lián)合作業(yè)容易出現(xiàn)路徑?jīng)_突和碰撞的問題，采用蟻群算法，設(shè)計(jì)了多采摘機(jī)器人路徑規(guī)劃和導(dǎo)航系統(tǒng)。

2)針對產(chǎn)業(yè)種植園區(qū)環(huán)境不確定性因素較大及機(jī)器人在最初規(guī)劃的時候往往很難獲取全部的信息的問題，提出了先全局后局部的路徑規(guī)劃方法，實(shí)現(xiàn)了多采摘機(jī)器人移動路徑的實(shí)時動態(tài)規(guī)劃。

3)提出了多采摘機(jī)器人環(huán)境下，各機(jī)器人利用環(huán)境建模和自主定位進(jìn)行自主導(dǎo)航的方法，通過建立地圖信息數(shù)據(jù)庫和拓?fù)鋱D，實(shí)現(xiàn)了對多采摘機(jī)器人的安全引導(dǎo)。最后，運(yùn)用MatLab軟件仿真對路徑規(guī)劃和導(dǎo)航成功進(jìn)行了驗(yàn)證。