吳東林，張玉華

(漯河食品職業(yè)學(xué)院，河南 漯河 462300)

0 引言

隨著自動(dòng)化、計(jì)算機(jī)和電子技術(shù)的發(fā)展，人類(lèi)已從刀耕火種過(guò)渡到機(jī)械化作業(yè)，各國(guó)都在引入各類(lèi)農(nóng)業(yè)機(jī)器人。我國(guó)水果種植面積大，人工采摘水果工作量巨大，研發(fā)出能實(shí)現(xiàn)自動(dòng)控制的各類(lèi)水果采摘機(jī)器人已是現(xiàn)代化農(nóng)業(yè)的迫切需求。本文基于人工勢(shì)場(chǎng)法設(shè)計(jì)了一套采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃系統(tǒng)，對(duì)采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃具有一定的參考意義。

1 采摘機(jī)器人作業(yè)環(huán)境及障礙物建模

1.1 采摘機(jī)器人作業(yè)環(huán)境建模

在采摘機(jī)器人作業(yè)環(huán)境建模過(guò)程中，合適的環(huán)境表示方法能夠有效地建立路徑規(guī)劃和路徑搜索算法，從而制定出時(shí)間短、路程短的最優(yōu)路徑。對(duì)已知的作業(yè)環(huán)境建立模型時(shí)，采用比較多的方法是柵格建模法。該方法是采用柵格的極坐標(biāo)和直角坐標(biāo)的變換關(guān)系，將作業(yè)環(huán)境區(qū)域分割成單位大小的柵格。只要柵格內(nèi)存在障礙物或其他物體，則標(biāo)記該區(qū)域?yàn)檎系K物柵格。該方法可以表達(dá)多種不規(guī)則障礙物，具有實(shí)現(xiàn)過(guò)程簡(jiǎn)單及計(jì)算時(shí)間短等優(yōu)點(diǎn)。

本文采用柵格法建立采摘機(jī)器人作業(yè)環(huán)境模型，具體流程為：①建立如圖1所示的直角坐標(biāo)系，無(wú)需劃分柵格，而是以障礙物頂點(diǎn)(見(jiàn)圖2)確定障礙物的位置；②在采摘機(jī)器人移動(dòng)過(guò)程中，實(shí)時(shí)判斷是否會(huì)從各個(gè)障礙物的頂點(diǎn)間經(jīng)過(guò)，若存在這樣的現(xiàn)象，則判定機(jī)器人將和障礙物發(fā)生碰撞行為；③為了更好地實(shí)現(xiàn)路徑規(guī)劃過(guò)程，本文將每個(gè)障礙物的頂尖之間連線，路徑規(guī)劃時(shí)不允許采摘機(jī)器人規(guī)劃出的路徑穿過(guò)連線，從而保證采摘機(jī)器人和障礙物不會(huì)發(fā)生碰撞行為。

圖1 建立作業(yè)環(huán)境模型Fig.1 Setting up a working environment model

圖2 障礙物簡(jiǎn)易位置圖Fig.2 The simple position diagram of obstacles

采用該方法表示障礙物，會(huì)加大采摘機(jī)器人路徑規(guī)劃所需的時(shí)間，也加大了其運(yùn)算工作量，對(duì)硬件要求較高。因此，本文采用一圓形區(qū)域表示各種不規(guī)則的障礙物，方便采摘機(jī)器人進(jìn)行處理。環(huán)境模型中障礙物表示如圖3所示。

圖3 環(huán)境模型障礙物示意圖Fig.3 The schematic diagram of environmental model obstacle

1.2 采摘機(jī)器人障礙物建模

在采摘機(jī)器人作業(yè)中，會(huì)存在兩類(lèi)障礙物：一類(lèi)是靜止不動(dòng)的；另一類(lèi)是會(huì)以一定的速度運(yùn)動(dòng)的。本節(jié)將對(duì)運(yùn)動(dòng)的障礙物進(jìn)行建模。

(1)

可以得到障礙物運(yùn)動(dòng)時(shí)的模型為

p(t+Δt)=p(t)+v(t)×Δt

(2)

式(2)可以轉(zhuǎn)換為

(3)

繼續(xù)化簡(jiǎn)為

(4)

式(4)就是所求的障礙物運(yùn)動(dòng)模型。

2 人工勢(shì)場(chǎng)法概述

人工勢(shì)場(chǎng)法的基本思路是建立一個(gè)人工勢(shì)場(chǎng)，使得采摘機(jī)器人與障礙物之間存在斥力，與目標(biāo)地點(diǎn)存在引力，然后根據(jù)勢(shì)函數(shù)的下降方向規(guī)劃最優(yōu)路徑。傳統(tǒng)的引力勢(shì)場(chǎng)計(jì)算公式為

(5)

Fat(P)=-

(6)

負(fù)梯度值隨著采摘機(jī)器人和目標(biāo)距離的減小而減小，直至0。

采用自制調(diào)查表,對(duì)患兒家屬對(duì)治療的滿意度進(jìn)行調(diào)查,量表總分0~100分,≥80為滿意,60~79分為一般滿意,<60分為不滿意。

本文研究的內(nèi)容是采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃，其目標(biāo)并非單一靜止的，而是在時(shí)刻運(yùn)動(dòng)變化的。人工勢(shì)場(chǎng)的勢(shì)場(chǎng)引力示意如圖4所示。

圖4 人工勢(shì)場(chǎng)的勢(shì)場(chǎng)引力Fig.4 Potential field gravitation of artificial potential field

因此，傳統(tǒng)的人工勢(shì)場(chǎng)需要進(jìn)行適當(dāng)改進(jìn)，定義為

Uat(P,V)=

ξp‖Pgoal(t)-P(t)‖m+lv‖Vtar(t)-V(t)‖n

(7)

其中，Vtar(t)和V(t)分別為目標(biāo)和采摘機(jī)器人在t時(shí)間點(diǎn)的移動(dòng)速度；ξp、lv為正參數(shù)；m、n為人工勢(shì)場(chǎng)系數(shù)。由該引力勢(shì)場(chǎng)生成的負(fù)梯度為

Fat(p,v)=-Uat(p,v)=-pUat(p,v)-

(8)

(9)

(10)

將式(7)帶入式(8)可得

Fat(p,v)=Fat1(p)+Fat2(v)

(11)

Fat1(p)=kξp‖Ptar(t)-p(t)‖mNprt

(12)

Fat2(v)=flv‖Vtar(t)-V(t)‖nNvrt

(13)

其中，Nprt為采摘機(jī)器人指向目標(biāo)終點(diǎn)的單位矢量；Nvrt為采摘機(jī)器人與目標(biāo)終點(diǎn)相對(duì)速度的單位矢量。

3 采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃

人工勢(shì)場(chǎng)是將采摘機(jī)器人作業(yè)區(qū)域進(jìn)行一種抽象的描述，在解決采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃問(wèn)題中，障礙物作為斥力產(chǎn)生目標(biāo)，其具有一定的運(yùn)動(dòng)性。采摘機(jī)器人作為抽象勢(shì)場(chǎng)主動(dòng)方，其負(fù)梯度方向表示采摘機(jī)器人受到抽象力的方向。在這種力的作用下，采摘機(jī)器人避開(kāi)障礙物，朝目標(biāo)終點(diǎn)移動(dòng)。

本文研究中采用人工勢(shì)場(chǎng)法，采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃程序框圖如圖5所示。

圖5 采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃程序框圖Fig.5 The block diagram of dynamic path planning program for picking robot

采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃具體流程為：

1)準(zhǔn)備。采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃的準(zhǔn)備內(nèi)容為設(shè)定各種參數(shù)：①人工勢(shì)場(chǎng)引力系數(shù)Ka；②斥力系數(shù)為Kr；③采摘機(jī)器人與障礙物之間的安全距離為ρ0；④斥力作用距離為ρm；⑤設(shè)定采摘機(jī)器人最終應(yīng)該到達(dá)的目標(biāo)點(diǎn)。

2)計(jì)算引力。根據(jù)準(zhǔn)備過(guò)程中設(shè)定的參數(shù)值，利用式(12)計(jì)算采摘機(jī)器人與應(yīng)該到達(dá)位置之間的引力Fat1。

3)計(jì)算斥力。本系統(tǒng)要求采摘機(jī)器人在行駛過(guò)程中，不會(huì)和作業(yè)邊界及障礙物發(fā)生碰撞行為。因此，采摘機(jī)器人會(huì)收到邊界和障礙物兩方面的斥力。

(1)邊界的斥力。倘若采摘機(jī)器人和作業(yè)區(qū)域邊界之間的距離小于斥力作用距離ρm時(shí)，其會(huì)受到該方面的斥力。那么，在計(jì)算斥力過(guò)程中，要考慮邊界的斥力。

(2)障礙物的斥力。采摘機(jī)器人在移動(dòng)過(guò)程中，會(huì)通過(guò)圖像傳感器、超聲波傳感器及紅外傳感器采集作業(yè)環(huán)境信息，確定障礙物的位置，并根據(jù)障礙物的位置計(jì)算與采摘機(jī)器人之間的斥力。

4)斥力和引力的合成。將采摘機(jī)器人受到邊界和障礙物兩個(gè)斥力利用式(8)合成，計(jì)算采摘機(jī)器人在人工勢(shì)場(chǎng)受到的合力。

5)計(jì)算采摘機(jī)器人新坐標(biāo)及規(guī)劃路徑。根據(jù)流程(4)合成的力及采摘機(jī)器人的移動(dòng)速度，計(jì)算出采摘機(jī)器人當(dāng)前位置，并根據(jù)勢(shì)場(chǎng)力及障礙物位置信息，規(guī)劃采摘機(jī)器人動(dòng)態(tài)路徑。

6)判斷采摘機(jī)器人是否到達(dá)目標(biāo)點(diǎn)。計(jì)算出采摘機(jī)器人新坐標(biāo)后，首先判斷其是否已經(jīng)到達(dá)目標(biāo)位置。如果是，則其完成當(dāng)前任務(wù)；若不是，則應(yīng)該重復(fù)3)、4)、5)流程步驟，直至達(dá)到目標(biāo)位置。

4 仿真試驗(yàn)與結(jié)果分析

本文采用柵格法建立一個(gè)采摘機(jī)器人作業(yè)環(huán)境模型，以一個(gè)柵格大小為最小單位，模型中每個(gè)柵格都是采摘機(jī)器人的移動(dòng)點(diǎn)。采摘機(jī)器人在移動(dòng)過(guò)程中，只能向以自己為中心的周邊5個(gè)落腳點(diǎn)移動(dòng)，分為上下左右和其自身所處的位置。Actionselect=[0,1;0,-1;-1,0;1,0;0,0]，采摘機(jī)器人移動(dòng)方向只能是這5個(gè)中的1個(gè)。在環(huán)境模型中，障礙物采用紅色表示。采摘機(jī)器人在移動(dòng)過(guò)程中，根據(jù)周邊5個(gè)方向的勢(shì)力值，進(jìn)行路徑搜索。為了避免采摘機(jī)器人停留在上一位置時(shí)間過(guò)長(zhǎng)，設(shè)定采摘機(jī)器人當(dāng)前位置和周邊柵格點(diǎn)勢(shì)場(chǎng)值都為1 000。

為了驗(yàn)證基于人工勢(shì)場(chǎng)法的采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃系統(tǒng)是否能夠滿足設(shè)計(jì)要求，能夠使采摘機(jī)器人在路徑最優(yōu)情況下到達(dá)目標(biāo)位置，利用MatLab仿真軟件設(shè)計(jì)了3組仿真試驗(yàn)。圖6(a)設(shè)置比較簡(jiǎn)單的環(huán)境，圖6(b)設(shè)置有死角的U型環(huán)境，圖6(c)設(shè)置為動(dòng)態(tài)的復(fù)雜環(huán)境。

圖6 MATLAB仿真結(jié)果Fig.6 Simulation result of Matlab

圖6(a)環(huán)境相對(duì)簡(jiǎn)單，采摘機(jī)器人經(jīng)過(guò)簡(jiǎn)單調(diào)整就能避開(kāi)障礙物到達(dá)了終點(diǎn)位置；(圖6b)中模擬環(huán)境為一個(gè)U型死角，采摘機(jī)器人容易陷入局部最小值，但從最后采摘機(jī)器人路徑可以看出，其在凹形環(huán)境下也能避開(kāi)障礙物，到達(dá)終點(diǎn)位置；(圖6c)中有3個(gè)靜止的障礙物和3個(gè)運(yùn)動(dòng)的障礙物，但采摘機(jī)器人依然實(shí)現(xiàn)了最優(yōu)的路徑優(yōu)化，順利避障所有障礙物，到達(dá)了目的地。從路徑長(zhǎng)度和轉(zhuǎn)彎次數(shù)可以看出：該系統(tǒng)優(yōu)化效果明顯，具有很好的避障和路徑規(guī)劃能力，驗(yàn)證了系統(tǒng)的有效性和實(shí)時(shí)性。

5 結(jié)論

針對(duì)果園環(huán)境不確定性因素大及采摘機(jī)器人作業(yè)特性，利用環(huán)境建模和自主定位方法，設(shè)計(jì)和研究了一套基于人工勢(shì)場(chǎng)法的采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃系統(tǒng)，并基于MatLab仿真軟件進(jìn)行了一系列的仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：該系統(tǒng)優(yōu)化效果明顯，具有很好的避障和路徑規(guī)劃能力，驗(yàn)證了系統(tǒng)的有效性和實(shí)時(shí)性，對(duì)采摘機(jī)器人動(dòng)態(tài)路徑規(guī)劃具有重要現(xiàn)實(shí)意義。