姚國(guó)林，王合闖

(1.河南農(nóng)業(yè)職業(yè)學(xué)院機(jī)電工程學(xué)院，河南鄭州 451450；2.華北水利水電大學(xué)信息工程學(xué)院，河南鄭州 450045)

0 前言

采摘機(jī)器人一直是智能農(nóng)業(yè)裝備領(lǐng)域的研究熱點(diǎn)，由于機(jī)械手的可操作性和精度較高，一般采用機(jī)械手作為采摘機(jī)器人的主要執(zhí)行部件[1]。采用機(jī)械手作為采摘機(jī)器人的主要執(zhí)行部件，首先需要解決機(jī)械手的運(yùn)動(dòng)規(guī)劃問題。運(yùn)動(dòng)規(guī)劃是指機(jī)械手從初始構(gòu)型移動(dòng)到目標(biāo)構(gòu)型的過程中，各個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡規(guī)劃。在這個(gè)過程中，必須保證機(jī)械手不與工作空間內(nèi)的任何障礙物或本身發(fā)生碰撞。

國(guó)內(nèi)外研究學(xué)者對(duì)機(jī)器人的運(yùn)動(dòng)規(guī)劃問題開展了大量研究，現(xiàn)有的運(yùn)動(dòng)規(guī)劃算法包括快速隨機(jī)搜索樹(Rapidly Random Trees，RRT)[2]、概率路線圖法(Probabilistic Roadmap Method，PRM)[3]、A*算法[4]、神經(jīng)網(wǎng)絡(luò)模型[5]、人工勢(shì)場(chǎng)(Artificial Potential Field，APF)[6]。由于采摘機(jī)器人工作的環(huán)境是非結(jié)構(gòu)化的、非常復(fù)雜的、障礙物很多的環(huán)境，機(jī)械手的C空間構(gòu)造將花費(fèi)極長(zhǎng)的時(shí)間。從現(xiàn)有文獻(xiàn)來看，RRT、PRM和A*算法需要構(gòu)建C空間，因此無法滿足拾取機(jī)器人的實(shí)時(shí)性要求。而神經(jīng)網(wǎng)絡(luò)算法不僅需要長(zhǎng)時(shí)間的自我訓(xùn)練，對(duì)硬件的要求也更高，因此也無法用于拾取機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃。

APF算法通過笛卡爾空間建立虛擬勢(shì)場(chǎng)來規(guī)劃運(yùn)動(dòng)，其中運(yùn)動(dòng)物體在從初始位置移動(dòng)到目標(biāo)位置的過程中會(huì)受到引力和斥力兩種力的影響，并避免碰撞障礙物。與RRT、PRM等全局規(guī)劃算法不同，APF是一種算法復(fù)雜度低、對(duì)計(jì)算機(jī)資源依賴性低、實(shí)時(shí)性強(qiáng)的局部運(yùn)動(dòng)規(guī)劃算法。但是，APF存在局部最小缺陷(Local Minimum Trap，LMT)和目標(biāo)不可達(dá)問題(Target Unreachable Problem，TNRP)等問題[7]。MATOUI 等[8]對(duì)APF算法進(jìn)行了改進(jìn)，設(shè)計(jì)了一種非最小速度算法，改進(jìn)后的APF能有效避免出現(xiàn)LMT現(xiàn)象。彭鵬等人[9]提出一種改進(jìn)的APF和RRT結(jié)合的機(jī)械臂避碰路徑規(guī)劃算法，該算法有效避免了機(jī)械臂運(yùn)動(dòng)規(guī)劃陷入LMT問題。李卓等人[10]在傳統(tǒng)的智能車運(yùn)動(dòng)規(guī)劃方面，將速度和加速度等因素導(dǎo)入了引力勢(shì)場(chǎng)，通過仿真表明該算法能有效避免TNRP問題。由于采摘機(jī)器人對(duì)運(yùn)動(dòng)規(guī)劃算法的實(shí)時(shí)性要求較高，上述研究均耗時(shí)較長(zhǎng)，無法滿足采摘機(jī)器人的實(shí)時(shí)性要求。

在上述研究的基礎(chǔ)上，本文作者分別對(duì)APF的勢(shì)場(chǎng)和力的計(jì)算模型進(jìn)行了改進(jìn)，在規(guī)劃?rùn)C(jī)械手運(yùn)動(dòng)時(shí)直接計(jì)算機(jī)械手關(guān)節(jié)空間的吸引力矩，提出了改進(jìn)的人工勢(shì)場(chǎng) (IAPF)優(yōu)化算法，有效地減少機(jī)械手運(yùn)動(dòng)規(guī)劃的計(jì)算時(shí)間，同時(shí)避免LMT和TPNR問題。最后通過實(shí)驗(yàn)結(jié)果，驗(yàn)證了所提優(yōu)化算法在采摘機(jī)械手無碰撞運(yùn)動(dòng)規(guī)劃中的實(shí)時(shí)性和準(zhǔn)確性。

1 采摘機(jī)器人的結(jié)構(gòu)模型

圖1為本文作者所研究的智能采摘機(jī)器人的功能模型，主要由3個(gè)部件組成：負(fù)責(zé)果園內(nèi)運(yùn)動(dòng)的自動(dòng)移動(dòng)車輛，并作為搭載所有其他部件的平臺(tái)；用于采摘水果的六連桿機(jī)械手；安裝在車輛上用于放置機(jī)器人控制柜的框架結(jié)構(gòu)，包括水果貯藏箱、移動(dòng)電源以及用于檢測(cè)和定位水果的立體攝像機(jī)視覺系統(tǒng)。

圖1 智能采摘機(jī)器人實(shí)物模型

2 采摘機(jī)械手的運(yùn)動(dòng)規(guī)劃模型

2.1 采摘機(jī)械手的運(yùn)動(dòng)學(xué)模型

機(jī)械手運(yùn)動(dòng)學(xué)模型的建立是運(yùn)動(dòng)規(guī)劃的前提工作，本文作者采用初等變換序列對(duì)六連桿機(jī)械手進(jìn)行運(yùn)動(dòng)學(xué)建模[11]，如圖2所示，di(i=1，2，…，6)表示連桿偏移量，αi(i=1，2，…，6)表示連桿扭轉(zhuǎn)角，ai(i=1，2，…，6)表示連桿長(zhǎng)度，θi(i=1，2，…，6)表示關(guān)節(jié)角度。從初等變換序列模型中，可得到機(jī)械手上每一點(diǎn)的雅可比矩陣。

圖2 六連桿機(jī)械手的初等變換序列模型

圖3所示為采摘水果的3個(gè)步驟：

圖3 采摘機(jī)械手運(yùn)動(dòng)規(guī)劃流程

步驟1，采摘機(jī)械手從初始位置移動(dòng)到采摘位置，目標(biāo)水果由末端執(zhí)行器采摘分離。在規(guī)劃運(yùn)動(dòng)時(shí)，同時(shí)設(shè)計(jì)和計(jì)算采摘構(gòu)型。

步驟2，采摘機(jī)械手從采摘構(gòu)型移動(dòng)到釋放構(gòu)型，將水果放入存儲(chǔ)箱中。

步驟3，采摘機(jī)械手從釋放構(gòu)型移動(dòng)到初始構(gòu)型，等待下一個(gè)采摘周期。在采摘實(shí)驗(yàn)中，初始構(gòu)型和釋放構(gòu)型都是預(yù)先設(shè)定的，以進(jìn)一步提高實(shí)時(shí)性。

2.2 機(jī)械手運(yùn)動(dòng)規(guī)劃算法

圖4所示為六連桿機(jī)械手的運(yùn)動(dòng)規(guī)劃概念圖。

圖4 六連桿機(jī)械手的運(yùn)動(dòng)規(guī)劃概念圖

APF算法是通過建立虛擬勢(shì)能場(chǎng)來規(guī)劃運(yùn)動(dòng)軌跡，以避免與工作空間中的任何障礙物發(fā)生碰撞[12]。引力勢(shì)場(chǎng)和引力的計(jì)算公式為

(1)

Fa=-?Ua=kada?da

(2)

式中：Ua∈R為引力勢(shì)場(chǎng)；Fa為引力；ka為引力系數(shù)；da為目標(biāo)距離。斥力勢(shì)場(chǎng)和斥力計(jì)算公式為

(3)

(4)

式中：Ur∈R為斥力勢(shì)場(chǎng)；Fr為斥力；kr為斥力系數(shù)；dr為障礙物距離；d0為距離的閾值。合力勢(shì)場(chǎng)Us和合力Fs計(jì)算公式為

Us=Ua+Ur

(5)

Fs=Fa+Fr

(6)

與移動(dòng)車輛不同，APF的應(yīng)用和機(jī)械手的無碰撞運(yùn)動(dòng)規(guī)劃更加復(fù)雜。移動(dòng)車輛可以看作是工作空間中的一個(gè)點(diǎn)進(jìn)行運(yùn)動(dòng)規(guī)劃，但對(duì)于機(jī)械手來說，需要考慮如何對(duì)整個(gè)機(jī)械手進(jìn)行無碰撞運(yùn)動(dòng)規(guī)劃，而不是將其作為單個(gè)點(diǎn)。機(jī)械手的路徑規(guī)劃可以在其工作空間或C空間中進(jìn)行。由于C空間的計(jì)算量大，構(gòu)建耗時(shí)長(zhǎng)，所以通常采用工作空間中的路徑規(guī)劃。由于機(jī)械手上每個(gè)點(diǎn)的運(yùn)動(dòng)不能單獨(dú)規(guī)劃，因此必須在整體機(jī)械臂結(jié)構(gòu)的背景下考慮機(jī)械手上的點(diǎn)。在此研究中，引力作用于所有初等變換序列坐標(biāo)系的原點(diǎn)，斥力作用于每個(gè)初等變換序列線段上距離障礙物最近的點(diǎn)，如圖5所示。通過引力作用，機(jī)械手可以從初始構(gòu)型變?yōu)槟繕?biāo)構(gòu)型。通過斥力作用，可以避免在工作空間中移動(dòng)時(shí)撞到任何障礙物。

圖5 施加在機(jī)械手上的IAPF力示意

當(dāng)工作空間中有多個(gè)障礙物時(shí)，合力勢(shì)場(chǎng)的合力可以計(jì)算為

(7)

(8)

為了控制機(jī)械手，通過雅可比矩陣的轉(zhuǎn)換，最終將合力映射到關(guān)節(jié)力矩上，計(jì)算關(guān)節(jié)力矩的公式為

(9)

2.3 運(yùn)動(dòng)規(guī)劃優(yōu)化策略

APF中由于存在局部最小缺陷(LMT)、目標(biāo)位置不可達(dá)(TNRP)等問題，將直接導(dǎo)致兩個(gè)問題，即Fa隨著da的減小而線性減小，當(dāng)da=0時(shí)，F(xiàn)a減小到零，但當(dāng)dr趨于零時(shí)Fr趨于增加，當(dāng)dr=0時(shí)，F(xiàn)r增大到無窮大。首先，如果在目標(biāo)位置附近有障礙物，其斥力會(huì)遠(yuǎn)遠(yuǎn)大于引力。當(dāng)物體接近目標(biāo)位置時(shí)，會(huì)不斷被斥力反彈，導(dǎo)致路徑振蕩甚至失效，這就是TNRP問題。其次，機(jī)械手關(guān)節(jié)所能承受的力矩有限，在運(yùn)動(dòng)規(guī)劃過程中無法實(shí)現(xiàn)無限的斥力。為了解決上述兩個(gè)問題，本文作者對(duì)勢(shì)場(chǎng)和力模型進(jìn)行了優(yōu)化。

為了避免TNRP問題，需要在接近目標(biāo)位置時(shí)仍然有較大的梯度，以確保Fa足夠大，能夠克服Fr的影響，連續(xù)引導(dǎo)物體到達(dá)目標(biāo)位置。同時(shí)，為了使路徑規(guī)劃算法收斂，當(dāng)da=0時(shí)，F(xiàn)a也應(yīng)該趨近于0。為此，IAPF算法在APF算法原有的引力勢(shì)場(chǎng)計(jì)算模型的基礎(chǔ)上，通過加入Sigmoid函數(shù)分量以提升算法的收斂性[13]，即IAPF算法的引力勢(shì)場(chǎng)和引力計(jì)算為

(10)

(11)

式中：ka1和ka2為Sigmoid函數(shù)的控制因子，為避免Fr隨dr減小而不可控的問題，IAPF算法改變了原有的斥力勢(shì)場(chǎng)模型。APF算法的斥力勢(shì)場(chǎng)模型是分段函數(shù)，不僅需要在分段接點(diǎn)處連續(xù)可微，而且當(dāng)dr=0時(shí)還會(huì)發(fā)生梯度爆炸。為確保IAPF算法的斥力不會(huì)無限增加，并且隨著dr的減小仍然是連續(xù)可微的，本文作者將IAPF算法的斥力變?yōu)橐粋€(gè)反S的Sigmoid函數(shù)[14]，IAPF算法的斥力函數(shù)不僅在遠(yuǎn)離障礙物時(shí)趨近于零，而且隨著dr的減小而增大，仍然是連續(xù)可微的。IAPF算法的Fr會(huì)逐漸增大，并趨于最大斥力，不會(huì)出現(xiàn)斥力爆炸的問題。IAPF算法的斥力勢(shì)場(chǎng)和斥力計(jì)算為

(12)

(13)

式中：kr1和kr2為反S型Sigmoid函數(shù)的控制因子；kr3為Ur的補(bǔ)償系數(shù)。為了消除LMT帶來的影響，IAPF算法選擇在合力勢(shì)場(chǎng)梯度消失時(shí)，增加臨時(shí)力Ft，打破斥力和引力的平衡。Ft不僅決定了IAPF算法結(jié)果的好壞，也決定了IAPF算法是否會(huì)再次陷入LMT。通過仿真實(shí)驗(yàn)，F(xiàn)t同時(shí)垂直于Fa和Fr時(shí)效果較好。在文中，當(dāng)IAPF算法即將進(jìn)入LMT時(shí)，將出現(xiàn)Ft，其值與Fa相等，其方向如圖6所示。

圖6 避免LMT的臨時(shí)力示意

通過雅可比矩陣的轉(zhuǎn)換，將工作空間中的力直接映射到關(guān)節(jié)空間中的力矩，仍然需要大量的計(jì)算，因此為了進(jìn)一步優(yōu)化，IAPF算法直接計(jì)算關(guān)節(jié)空間中的引力矩，計(jì)算公式為

(14)

(15)

3 采摘機(jī)械手運(yùn)動(dòng)規(guī)劃的仿真對(duì)比

文中的仿真都是在Windows 10操作系統(tǒng)下通過Python 3在Visual Studio Code中實(shí)現(xiàn)的。

3.1 勢(shì)場(chǎng)面與力曲線的仿真

APF和IAPF算法的勢(shì)場(chǎng)面和力曲線分別如圖7和圖8所示，表1給出了APF和IAPF算法的勢(shì)場(chǎng)模型和力模型的具體參數(shù)，其中Pg為目標(biāo)位置，Pc為當(dāng)前位置，P0為障礙物位置。

表1 第3.1節(jié)仿真使用的具體參數(shù)

圖7 APF和IAPF的勢(shì)場(chǎng)面

圖8 APF和IAPF的力曲線

可以看出：APF的引力勢(shì)場(chǎng)梯度在目標(biāo)位置附近逐漸消失，而IAPF算法的梯度并沒有消失而是增加。因此，與APF相比，IAPF對(duì)TNRP的抵抗力更強(qiáng)，收斂速度更快。由于APF斥力勢(shì)場(chǎng)中存在梯度爆炸現(xiàn)象，當(dāng)APF靠近障礙物時(shí)，斥力勢(shì)場(chǎng)的梯度遠(yuǎn)大于引力勢(shì)場(chǎng)的坡度，導(dǎo)致合成勢(shì)場(chǎng)的引力勢(shì)場(chǎng)表面缺乏特征。然而，IAPF算法可以有效地克服以上問題，其合成勢(shì)場(chǎng)表面具有引力勢(shì)場(chǎng)面和斥力勢(shì)場(chǎng)面的共同特征，這使得IAPF算法不僅具有更快的收斂速度，還具有較強(qiáng)的無碰撞能力。

3.2 驗(yàn)證TNRP的仿真對(duì)比

當(dāng)目標(biāo)位置附近有障礙物時(shí)，APF會(huì)出現(xiàn)TNRP問題。因此，為了驗(yàn)證IAPF算法能夠有效避免TNRP問題，進(jìn)行了下面的仿真實(shí)驗(yàn)。仿真區(qū)域?yàn)?0×10矩形，具體參數(shù)如表2所示，各障礙物的動(dòng)作閾值相等。仿真結(jié)果如圖9所示，仿真曲線如圖10所示。

表2 第3.2節(jié)仿真使用的具體參數(shù)

圖9 TNRP的仿真結(jié)果

圖10 TNRP的仿真曲線

從圖9可以看出：當(dāng)目標(biāo)位置附近有障礙物時(shí)，機(jī)械手不斷靠近目標(biāo)位置，但被障礙物的斥力推開，來回?cái)[動(dòng)，無法通過APF到達(dá)目標(biāo)位置，因此APF無法收斂。從圖10可以看出：IAPF算法在第20次迭代附近接近目標(biāo)位置，獲得了增強(qiáng)的引力，避免了TNRP的發(fā)生。仿真結(jié)果表明，IAPF算法可以有效避免TNRP的發(fā)生，保證路徑規(guī)劃的成功率。

3.3 驗(yàn)證LMT的仿真對(duì)比

為了驗(yàn)證IAPF算法能夠避免陷入LMT問題，設(shè)計(jì)了如下仿真實(shí)驗(yàn)：當(dāng)目標(biāo)與初始位置之間的直線上存在障礙物時(shí)，容易陷入LMT，因此在這種情況下，APF和IAPF算法同時(shí)進(jìn)行路徑規(guī)劃。模擬區(qū)域?yàn)橐粋€(gè)10×10的矩形，具體參數(shù)見表3，各障礙物的動(dòng)作閾值相等，仿真結(jié)果如圖11所示，仿真曲線如圖12所示。

表3 第3.3節(jié)仿真使用的具體參數(shù)

圖11 LMT的仿真結(jié)果

圖12 LMT的仿真曲線

從仿真結(jié)果可以看出：APF迅速陷入LMT，并一直保持這種狀態(tài)，直到路徑規(guī)劃失敗；IAPF算法沒有落入LMT，并迅速到達(dá)目標(biāo)位置；IAPF算法在第22次迭代附近到達(dá)目標(biāo)位置，而APF仍然停留在LMT中。仿真結(jié)果表明，IAPF算法能夠有效克服LMT問題，保證路徑規(guī)劃的成功率。

3.4 機(jī)械手運(yùn)動(dòng)規(guī)劃的仿真對(duì)比

采用APF和IAPF算法對(duì)機(jī)械臂進(jìn)行路徑規(guī)劃。實(shí)驗(yàn)參數(shù)如表4所示，其中初始構(gòu)型為Θi，目標(biāo)構(gòu)型為Θg，障礙物坐標(biāo)為POi，障礙物半徑均為120 mm。仿真結(jié)果如表5所示，機(jī)械手各初等變換序列線段與障礙物的關(guān)節(jié)角曲線和距離曲線如圖13所示。

表4 第3.4節(jié)仿真使用的具體參數(shù)

表5 IAPF和APF算法的路徑規(guī)劃結(jié)果

圖13 采摘機(jī)械手運(yùn)動(dòng)規(guī)劃的實(shí)驗(yàn)結(jié)果

仿真結(jié)果表明：IAPF算法優(yōu)于APF算法。IAPF算法的關(guān)節(jié)角曲線更平滑，變化率幾乎相同；APF算法的關(guān)節(jié)角曲線變化比較明顯，變化率相差較大，關(guān)節(jié)角θ3經(jīng)過一個(gè)反向過程后開始收斂，而關(guān)節(jié)角θ2則出現(xiàn)波峰。APF算法的收斂速度非常慢，IAPF算法在第238次直接到達(dá)目標(biāo)構(gòu)型，而APF在第549次才收斂。與APF算法相比，IAPF算法在運(yùn)行時(shí)間上減少了55.12%，在總關(guān)節(jié)角誤差上減少了46.21%。

4 運(yùn)動(dòng)規(guī)劃的實(shí)驗(yàn)驗(yàn)證

為進(jìn)一步驗(yàn)證IAPF算法的有效性和可行性，對(duì)智能采摘機(jī)器人進(jìn)行了運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表6所示。目標(biāo)果樹及其簡(jiǎn)化模型如圖14所示，首先用部分球面包絡(luò)(PSE)方法對(duì)目標(biāo)果樹進(jìn)行建模，該方法類似于一個(gè)包圍球算法[15-16]。PSE方法僅包含球面和線段模型，可以大大減少碰撞檢測(cè)過程中的計(jì)算量，進(jìn)一步保證IAPF算法能夠滿足智能采摘機(jī)器人的實(shí)時(shí)性要求。PSE通過球體包絡(luò)果樹的所有非采摘部分，通過直線分段對(duì)當(dāng)前采摘部分的分支進(jìn)行建模，智能采摘機(jī)器人的采摘模擬和軌跡曲線如圖15所示，應(yīng)用于智能采摘機(jī)器人的IAPF算法實(shí)驗(yàn)結(jié)果如圖16所示。

表6 IAPF應(yīng)用于智能采摘機(jī)器人的模擬實(shí)驗(yàn)結(jié)果

圖14 目標(biāo)果樹(a)及其PSE模型(b)

圖15 智能采摘機(jī)器人采摘仿真時(shí)的軌跡曲線

圖16 應(yīng)用于智能采摘機(jī)器人的IAPF實(shí)驗(yàn)結(jié)果

IAPF算法在智能采摘機(jī)器人采摘目標(biāo)果實(shí)的路徑規(guī)劃過程中分別運(yùn)動(dòng)0.38、1.08、1.11 s，在采摘、釋放和初始化階段的關(guān)節(jié)總誤差分別為1.29°、1.08°和1.21°。智能采摘機(jī)器人規(guī)劃果實(shí)采摘路徑耗時(shí)約2.56 s，由于機(jī)械手運(yùn)動(dòng)所花費(fèi)的時(shí)間比路徑規(guī)劃所花費(fèi)的時(shí)間要長(zhǎng)得多，因此IAPF算法可以在智能采摘機(jī)器人采摘過程中充分規(guī)劃下一個(gè)果實(shí)采摘路徑。從實(shí)驗(yàn)結(jié)果可以得出結(jié)論：IAPF算法具有較快的求解速度和精度，能夠滿足智能采摘機(jī)器人的實(shí)時(shí)性要求。

5 結(jié)論

針對(duì)六連桿采摘機(jī)械手的無碰撞運(yùn)動(dòng)規(guī)劃，提出了改進(jìn)的人工勢(shì)場(chǎng)(IAPF)優(yōu)化算法，得出的主要結(jié)論如下：

(1)采用初等變換序列方法對(duì)機(jī)械手進(jìn)行運(yùn)動(dòng)學(xué)建模，該模型可方便地得到機(jī)械手上各點(diǎn)相對(duì)于基底坐標(biāo)系的雅可比矩陣。

(2)IAPF算法在APF算法原有的引力勢(shì)場(chǎng)計(jì)算模型的基礎(chǔ)上加入了Sigmoid函數(shù)，使得路徑規(guī)劃算法收斂；在斥力勢(shì)場(chǎng)中引入了反S的Sigmoid函數(shù)，防止出現(xiàn)斥力爆炸現(xiàn)象，有效地避免了局部最小缺陷問題和目標(biāo)位置不可達(dá)問題。

(3)與傳統(tǒng)的APF算法相比，IAPF算法的計(jì)算時(shí)間和關(guān)節(jié)總誤差分別減少了55.12%和46.21%。最后通過實(shí)驗(yàn)結(jié)果，驗(yàn)證了所提優(yōu)化算法在采摘機(jī)械手無碰撞運(yùn)動(dòng)規(guī)劃中的實(shí)時(shí)性和準(zhǔn)確性。