智能采摘機器人路徑規(guī)劃的設(shè)計與研究

吳妮妮

(長江職業(yè)學(xué)院,武漢 430074)

0 引言

隨著人工智能化的發(fā)展,我國果園的采摘任務(wù)逐漸由人力勞作向機器自主作業(yè)進行轉(zhuǎn)變。機器自主作業(yè)時,需要識別一條無障礙、短距離的采摘路線[1]。然而,我國大部分的果園地處山林丘壑間,路面寬窄不一,起伏不平,加之行駛過程中出現(xiàn)的果樹等障礙物,導(dǎo)致采摘機器人無法快速、安全地到達目標(biāo)點,故不能有效地完成采摘任務(wù),使得果園智能化作業(yè)陷入瓶頸[2]。

針對上述現(xiàn)象,借助圖像處理、蟻群算法、PID閉環(huán)控制等技術(shù)對采摘機器人的行駛路線進行規(guī)劃和跟蹤,并運用嵌入式為手段設(shè)計了一款適用于采摘機器人的路徑規(guī)劃-跟蹤控制系統(tǒng)。系統(tǒng)能夠?qū)ふ页鲆粭l避開障礙且距離較短的行駛路線,使得采摘機器人能夠更智能化作業(yè),對今后智慧農(nóng)業(yè)的發(fā)展具有現(xiàn)實意義。

1 總體設(shè)計方案

果園采摘機器人實現(xiàn)自主智能采摘的關(guān)鍵是能夠找到一條最優(yōu)路徑,并設(shè)計出合理的路徑跟蹤控制方法,使其能夠準(zhǔn)確、安全地到達目標(biāo)點[3]。因此,采摘機器人的導(dǎo)航任務(wù)分為路徑規(guī)劃和路徑跟蹤。路徑規(guī)劃是指機器人在有障礙物的工作環(huán)境中如何找到一條最優(yōu)運動路徑,使機器人在運動過程中能安全、無碰撞地繞過所有障礙物;路徑追蹤需要采摘機器人根據(jù)自身情況,從某一處出發(fā),按照某種控制規(guī)律到達該路徑上,實現(xiàn)其跟蹤運動。因此,從果園圖像的采集開始,設(shè)計的采摘機器人的路徑規(guī)劃及路徑跟蹤思路如圖1所示。

圖1 總體設(shè)計思路Fig.1 Overall design idea

由無人機搭載相機拍攝果園圖像,并通過WiFi無線通訊發(fā)送給機器人;經(jīng)過相關(guān)圖像處理后,確定相關(guān)比例尺寸[4];采用柵格法對圖像的比例尺寸進行建模,由蟻群算法完成路徑規(guī)劃,篩選出一條最優(yōu)線路;根據(jù)運動學(xué)模型得到采摘機器人的位姿數(shù)據(jù),由PID控制器轉(zhuǎn)換為實際的控制信號,控制采摘機器人行走[5]。

2 圖像采集及處理

通常情況下,無人機搭載相機采集到的果園圖像需進行以下處理,從而轉(zhuǎn)化為對應(yīng)的數(shù)字信息。

1)原始果園圖像屬于YUV顏色空間,需將其轉(zhuǎn)換為RGB顏色,轉(zhuǎn)換表達式為

2)對得到的RGB圖像進行灰度處理,即處理成黑白二值圖像,其表達式為[6]

Gray=0.2989R+0.5870G+0.1140B

3)得到的二值圖像需區(qū)分果樹與道路,故進行Otsu分割。假設(shè)閾值為T,則表達式為

g(T)=ω0ω1(μ0-μ1)2

其中,ω0、ω1分別為目標(biāo)、背景像素點數(shù)量;μ0、μ1分別為目標(biāo)、背景平均灰度值。

4)將果園實際坐標(biāo)值與圖像中點的坐標(biāo)進行對應(yīng),得到圖像坐標(biāo)向?qū)嶋H坐標(biāo)轉(zhuǎn)換的矩陣,實現(xiàn)圖像的幾何校正。轉(zhuǎn)換矩陣的表達式為

5)為了確定后續(xù)柵格尺寸的大小,需要將圖像尺寸按比例轉(zhuǎn)換為實際果園的尺寸[7],故進行比例尺寸的轉(zhuǎn)化,即

其中,k為比例因子;l為圖像上的兩點間的距離;L為果園實際兩點間的距離;(x1,y1)為起點的坐標(biāo)位置;(x2,y2)為終點的坐標(biāo)位置。

3 基于蟻群算法的路徑規(guī)劃

依據(jù)收集的環(huán)境圖像信息的程度可以將路徑規(guī)劃分為全局路徑和局部路徑[8]。

鑒于采摘機器人能夠最大程度地找到出發(fā)地到目標(biāo)點的最佳線路,采用全局路徑進行規(guī)劃。首先以實際圖像建立柵格圖,然后采用蟻群算法進行路徑最優(yōu)篩選。

柵格圖是將果園環(huán)境轉(zhuǎn)換為二值信息的圖片,規(guī)定黑色柵格為果樹等障礙物,白色柵格為采摘機器人能夠行駛的區(qū)域,實體的障礙物如果不滿1格,按1格計算。柵格尺寸越小,計算越準(zhǔn)確,復(fù)雜程度會越高,如圖2所示。

圖2 柵格圖事例Fig.2 Grid case

假設(shè)柵格圖為m×n,單個柵格表示(i,j),則所有柵格的集合為E[9],所得表達式為

E={(i,j)|0

當(dāng)采摘機器人用八領(lǐng)域行走,則其在位置(a,b)時,下一步的柵格集合為D(a,b),表達式為

D(a,b)={(a-1,b-1),(a-1,b),(a-1,b+1),

(a,b-1)(a,b+1),(a+1,b-1),

(a+1,b),(a+1,b+1)}|(i,j)∈E

根據(jù)柵格圖構(gòu)建障礙物矩陣為

則最短路徑可表示為

其中,t為實際時間;T為運行最多時間。

由于采摘機器人按八領(lǐng)域行走,故其行走必須滿足位移、規(guī)避障礙物和行走停止等要求。

1)位移要求:采摘機器人可以向8個方向移動,但每次只能移動1步,故位移必須滿足的條件為

|Xi-Xi-1|≤1 ?t>1

|Yi-Yi-1|≤1 ?t>1

|(Xi+Yi)-(Xi-1+Yi-1)|≤1 ?t>1

2)規(guī)避障礙物要求:假設(shè)障礙物在柵格中的坐標(biāo)為(r,c),則必須滿足的條件為

|(Xi-r)+(Yi-c)|≥1 ?t>1

3)行走停止要求:當(dāng)采摘機器人到達目標(biāo)點后,需要停止前進[10],則必須滿足的條件為

|(Xi+Yi)-(Xi-1+Yi-1)|≤|(Xi-1+Yi-1)-(Xi-2+Yi-2)|

?t>2

基于上述模型,運用蟻群算法,從采摘機器人當(dāng)前所在點出發(fā),在最短的時間內(nèi)遍歷所有的果樹,尋求最優(yōu)路徑[11]。

蟻群算法是指螞蟻在路徑上釋放信息素,碰到還沒走過的路口,就隨機挑選一條路走;同時,釋放與路徑長度有關(guān)的信息素,信息素濃度與路徑長度成反比;后來的螞蟻再次碰到該路口時,就選擇信息素濃度較高路徑;由于最優(yōu)路徑上的信息素濃度越來越大,最終找到最優(yōu)路徑。

以果樹為障礙,遍歷采集圖像內(nèi)的全局采摘路徑,流程如圖3所示。

圖3 全局采摘路徑規(guī)劃流程圖Fig.3 Flow chart of global picking path planning

4 基于PID閉環(huán)控制的路徑跟蹤

路徑跟蹤需要采用某種控制理論,對采摘機器人的行走速度和方向進行調(diào)節(jié)和控制,使其按照期望的路線行駛。假設(shè)采摘機器人當(dāng)前的位置為O,期望到達的目標(biāo)點為P,以O(shè)p為目標(biāo)點坐標(biāo)原點,以速度v方向為Xp軸,建立XpOpYp坐標(biāo)系[12],其跟蹤如圖4所示。

圖4 運動模型Fig.4 Motion model

θp=arctan2(yp(u),xp(u))

采摘機器人在期望目標(biāo)點的位姿為

因此,期望路徑與期望速度的關(guān)系為

綜上所述,當(dāng)采摘機器人開始行走(即速度v>0)時,采摘機器人的旋轉(zhuǎn)角度、位姿[13]、期望路徑和速度不斷按照上述約束關(guān)系進行更新,從而實現(xiàn)按照期望的路徑和速度進行跟蹤。

基于上述運動模型,使用PID控制理論對已規(guī)劃的路徑進行跟蹤。PID控制應(yīng)用到采摘機器人的路徑追蹤過程中,就是對周圍環(huán)境的識別和分析,并將這些信息整理后循環(huán)反饋給控制器,使得控制器發(fā)送相應(yīng)的控制指令,實現(xiàn)采摘機器人閉環(huán)行駛控制,原理如圖5所示。

圖5 PID控制原理Fig.5 PID control principle

其中,wset為底層控制器理論角速度;wout為電機實際角速度。當(dāng)輸入底層控制器的角速度后,微處理器根據(jù)比例積分調(diào)節(jié)器PI輸出更新的PWM波,從而輸出對應(yīng)變化的電流[14],驅(qū)動電機轉(zhuǎn)速發(fā)生相應(yīng)的變化。

5 路徑規(guī)劃-跟蹤控制系統(tǒng)的設(shè)計

以上述理論為依據(jù),設(shè)計一款路徑規(guī)劃-跟蹤控制系統(tǒng),擬基于嵌入式技術(shù)實現(xiàn),一般分為硬件和軟件兩部分。

硬件部分主要由主控模塊、運動控制器模塊、圖像處理單元、路徑規(guī)劃單元、傳感器采集模塊及通訊單元和電源模塊組成,如圖6所示。

圖6 硬件框圖Fig.6 Hardware block diagram

主控制器選用Exynos4412;運動控制器選用微處理器STM32F407,通過CAN總線與主控制器相連;電機驅(qū)動模塊選用L298N,根據(jù)接收到的PWM波輸出電機轉(zhuǎn)速驅(qū)動信號;電源模塊給主控制器提供4V的電壓;WiFi接口用于控制系統(tǒng)與無人機之間的果園圖像數(shù)據(jù)的傳輸;UART接口用于系統(tǒng)調(diào)試;JTAG接口用于軟件程序的下載;OTG接口用于U盤燒寫文件。

控制系統(tǒng)的軟件是將各個部分模塊化,其實現(xiàn)框架如圖7所示。

圖7 軟件實現(xiàn)框架Fig.7 Software implementation framework

主控制軟件讀取傳感器收集到的數(shù)據(jù),進行PID算法驗證[15],并輸出控制信號;運動軟件收到控制命令后,對電機的轉(zhuǎn)速進行調(diào)控,從而控制采摘機器人的行動。

6 實驗結(jié)果與分析

為了驗證路徑規(guī)劃-追蹤控制系統(tǒng)的可行性,以任意數(shù)量、體積大小不同的紙箱代替果樹障礙物,任意設(shè)定采摘其機器人的起始點和目標(biāo)點,并做如下規(guī)定:若在實驗中采摘機器人碰撞果樹,則視為未通過;若采摘機器人未按照期望的路徑行駛,也視為未通過。改變紙箱的位置,形成不同的障礙結(jié)構(gòu),共進行10組實驗。其中,一組具有代表性的柵格圖像如圖8所示。

圖8 仿真柵格圖Fig.8 Simulation Grid

對應(yīng)該該柵格圖的實驗數(shù)據(jù)如表1所示。

表1 實驗數(shù)據(jù)表Table 1 Experimental data sheet

對表1的實驗數(shù)據(jù)進行分析可得:采摘機器人按期望路徑安全到達目標(biāo)點的概率在90%以上,平均通過率為94.5%,實際路徑與期望路徑的誤差控制在0.244m之內(nèi)。這表明,控制系統(tǒng)能夠有效進行路徑規(guī)劃,并實現(xiàn)精準(zhǔn)跟蹤。

7 結(jié)論

針對果園采摘機器人在采摘作業(yè)時路徑規(guī)劃及路徑跟蹤問題,提出了相應(yīng)的解決方案,并設(shè)計了一款路徑規(guī)劃-跟蹤控制系統(tǒng)。實驗表明:系統(tǒng)能夠快速尋找出相對平坦且距離最短的路線,提高了采摘機器人的作業(yè)效率,為智能化采摘機器人的推廣提供了參考依據(jù)。