張?zhí)斐桑钇媪?，裴天?/p>

（江蘇理工學(xué)院機(jī)械工程學(xué)院，江蘇常州 213001）

0 引言

如今，伴隨著我國科學(xué)技術(shù)水平和高端制造業(yè)能力的不斷提高，機(jī)器人技術(shù)已經(jīng)應(yīng)用到生活的各方各面[1]。尤其是在農(nóng)業(yè)領(lǐng)域，機(jī)器人正在逐漸展現(xiàn)出其巨大的作用[2]。其中，果蔬采摘農(nóng)業(yè)機(jī)器人在進(jìn)入21 世紀(jì)以來發(fā)展趨勢尤為迅速，當(dāng)中具有代表性的實(shí)例有中國農(nóng)業(yè)大學(xué)的黃瓜采摘機(jī)器人、草莓收獲機(jī)器人以及陸華明團(tuán)隊(duì)的林木球果大型采摘機(jī)器人等。這一種類的機(jī)器人憑借視覺或其他傳感器輸入作業(yè)環(huán)境各項(xiàng)參數(shù)，再由機(jī)器人根據(jù)輸入?yún)?shù)去進(jìn)行機(jī)械臂的運(yùn)動規(guī)劃[3-4]完成采摘。值得注意的是，當(dāng)前串番茄作為近年來最為流行的果蔬之一，其相關(guān)的果蔬采摘機(jī)器人研究卻遠(yuǎn)遠(yuǎn)不如黃瓜、茄子之類的同樣熱度高的蔬菜。

果蔬機(jī)器人實(shí)現(xiàn)采摘的重要前提之一是機(jī)械臂的運(yùn)動規(guī)劃任務(wù)，而運(yùn)動規(guī)劃并不是簡單的起點(diǎn)和終點(diǎn)間的距離，而是一條復(fù)雜的路徑[5]。尤其是在環(huán)境中存在各種障礙物的情況下，針對采摘機(jī)械臂進(jìn)行避障礙路徑規(guī)劃到達(dá)采摘位置對于機(jī)器人能否實(shí)現(xiàn)果實(shí)采摘格外重要。在諸多學(xué)者的研究下，現(xiàn)在常用的適用于蔬果采摘機(jī)械臂的路徑規(guī)劃算法有A*算法、柵格法、RRT 系列算法和人工勢場法[5]。其中，人工勢場法作為局部路徑規(guī)劃算法，具備結(jié)構(gòu)簡單、計(jì)算量小的特點(diǎn)，在路徑規(guī)劃任務(wù)中應(yīng)用比較廣泛。人工勢場法是在1986 年由著名學(xué)者O Khatib 在提出[6]，其主要思想是在機(jī)械臂的作業(yè)空間內(nèi)構(gòu)造一種人為設(shè)置的勢場。將機(jī)械臂起始點(diǎn)作為勢場內(nèi)的起點(diǎn)，路徑規(guī)劃的終點(diǎn)作為勢場內(nèi)的對機(jī)械臂的引力場，障礙物則作為勢場內(nèi)作用于機(jī)械臂的斥力場，在引力場和斥力場的合力作用下控制機(jī)械臂從起點(diǎn)至終點(diǎn)的無碰撞運(yùn)動。

傳統(tǒng)的人工勢場法[7]的引力勢場表達(dá)式如式（1）所示：

傳統(tǒng)的人工勢場法的斥力勢場表達(dá)如式（2）所示：

其中，ka、kr為斥力勢場的系數(shù)；X=（x，y，z）為機(jī)械臂末端點(diǎn)在笛卡爾空間的坐標(biāo)向量；X0=（x0，y0，z0）為障礙物在笛卡爾空間的坐標(biāo)向量；Xg=（xg，yg，zg）為目標(biāo)點(diǎn)在笛卡爾空間的坐標(biāo)向量；d為障礙物產(chǎn)生的斥力場范圍的半徑值大小。于是有機(jī)械臂在斥力勢場和引力勢場的共同作用下往目標(biāo)點(diǎn)方向移動。

值得注意的是，人工勢場法屬于局部路徑規(guī)劃算法，缺乏對應(yīng)用空間環(huán)境因素的全局認(rèn)識，所以采摘機(jī)械臂在人工勢場法的引導(dǎo)下，會因?yàn)槌饬εc引力相抵消而陷入勢場極值的狀態(tài)，此時機(jī)械臂的運(yùn)動狀態(tài)會處于一個空間來回振蕩。所以，在使用人工勢場法對機(jī)械臂進(jìn)行路徑規(guī)劃時，需要針對人工勢場法的不足進(jìn)行算法的改進(jìn)。姬偉團(tuán)隊(duì)[8]通過人工勢場法引導(dǎo)蘋果采摘機(jī)械臂路徑規(guī)劃過程中，面對極值問題，他們通過提出設(shè)立虛擬目標(biāo)點(diǎn)來增大機(jī)械臂從局部極值逃脫的概率。這種做法為本文對人工勢場法的改進(jìn)提供了理論依據(jù)。于是本文鑒于當(dāng)前串番茄果實(shí)采摘人口數(shù)量不足，相關(guān)的機(jī)械采摘研究也很少見，以串番茄采摘機(jī)械臂為應(yīng)用背景，機(jī)械臂避障路徑規(guī)劃算法為研究對象，就探討如何解決采摘機(jī)械臂避開環(huán)境障礙物到達(dá)果實(shí)采摘位置這個機(jī)械采值問題，提出了一種基于機(jī)械臂避障路徑規(guī)劃的將人工勢場法進(jìn)行改進(jìn)，然后與RRT*算法相結(jié)合的改進(jìn)算法。

1 人工勢場法的改進(jìn)內(nèi)容

1.1 結(jié)合RRT＊算法幫助機(jī)械臂逃離人工勢場法極值

假設(shè)機(jī)械臂連桿A 的末端關(guān)節(jié)在迭代i次后的位置為Ai，在迭代到i第n次時構(gòu)型的關(guān)節(jié)位置An。當(dāng)|Ai-An|的值與差值μ滿足|Ai-An|≤μ的關(guān)系時，說明在迭代n次后，機(jī)械臂關(guān)節(jié)角的變化不大，且Ai與期望位置Agoal間也不滿足 |Agoal-Ai|≤δ，這可以表明機(jī)械臂陷入到了極值點(diǎn)。其中參數(shù)μ為定義的差值，下文設(shè)為0.3°，參數(shù)δ表示Agoal附近有效范圍的大小。為了讓機(jī)械臂以最快速度擺脫局部極小值的束縛，所以通過設(shè)立一個臨時目標(biāo)點(diǎn)讓機(jī)械臂在RRT*算法的引導(dǎo)下運(yùn)動到該點(diǎn)去逃離極值。臨時目標(biāo)點(diǎn)的設(shè)立過程如下所示。

假設(shè)采摘機(jī)械臂在迭代i次后的關(guān)節(jié)組合為Ai。通過將期望關(guān)節(jié)組合與當(dāng)前關(guān)節(jié)組合相減，即Agoal-Ai得出一個方向向量α。得出相減的方向向量α后，用α去乘以一個步長λ，可以得到一個基礎(chǔ)增量β1，并判斷位姿（Ai+β1）時采摘機(jī)械臂是否會與障礙物發(fā)生碰撞。如果發(fā)生碰撞，則定義位姿（Ai+β1）失敗，取消基礎(chǔ)增量β1。接著用方向向量α去乘以兩個步長λ得到基礎(chǔ)增量β2，判斷位姿（Ai+β2）時是否采摘機(jī)械臂會與障礙物發(fā)生碰撞。如果不發(fā)生碰撞，則保留該關(guān)節(jié)組合。按如上方法計(jì)算n個基礎(chǔ)增量βn。

計(jì)算上一節(jié)得到的不與障礙物發(fā)生碰撞的關(guān)節(jié)組合（Ai+βn）的總勢能，并找出最小勢能的關(guān)節(jié)組合。在找出最小總勢能的關(guān)節(jié)組合后，將其同搜尋路徑中鄰近的關(guān)節(jié)組進(jìn)行比較，如果與鄰近關(guān)節(jié)組角度大小變化不超過0.3°，則表示兩個關(guān)節(jié)組相同，以此來判斷找出的最小總勢能關(guān)節(jié)組合是否為局部最小點(diǎn)，如果不是，則將其關(guān)節(jié)組合作為逃出極值的臨時目標(biāo)點(diǎn)。然后通過RRT*算法控制機(jī)械臂運(yùn)動到該目標(biāo)構(gòu)型去擺脫陷入極值的狀況。除此之外，如果上述方法得不出臨時目標(biāo)點(diǎn)，則將路徑規(guī)劃任務(wù)期望構(gòu)型作為RRT*算法的臨時目標(biāo)點(diǎn)，使用RRT*算法控制機(jī)械臂運(yùn)動到該目標(biāo)構(gòu)型。

在上述方法的指引下，機(jī)械臂能夠在運(yùn)行過程中及時發(fā)現(xiàn)潛在的極值。當(dāng)判斷極值產(chǎn)生時，能夠自適應(yīng)的計(jì)算臨時目標(biāo)點(diǎn)的位置，并控制機(jī)械臂運(yùn)動到該構(gòu)型以擺脫陷入極值的狀況。此方法可在串番茄采摘環(huán)境中降低極值對于機(jī)械臂路徑規(guī)劃的影響，解決了機(jī)械臂受人工勢場法引導(dǎo)而陷入極值無法完成避障路徑規(guī)劃的難題。

1.2 改進(jìn)人工勢場法斥力函數(shù)

傳統(tǒng)的人工勢場法規(guī)劃的路徑中，如果期望目標(biāo)周圍存在障礙物，可能會造成Ai無法進(jìn)入Agoal的有效范圍內(nèi)，導(dǎo)致人工勢場法的可靠性將大幅降低。其原因如圖1 所示。其中，Ai表示機(jī)械臂連桿A 的末端關(guān)節(jié)在迭代i次后的位置，Aobs表示空間障礙物，D為障礙物斥力場的半徑范圍，Agoal為期望目標(biāo)。如1所示，Agoal處于障礙物Aobs的斥力場半徑范圍D內(nèi)。此時，Ai受引力朝著Agoal的方向前進(jìn)，隨著 |Agoal-Ai|的值減小，Agoal引力場的場強(qiáng)也在逐漸減小，但隨著Ai進(jìn)入Agoal的斥力場范圍，其所受到的斥力場的場強(qiáng)逐漸變強(qiáng)。在某個位置，Ai所受的引力場和斥力場剛好平衡，此時機(jī)械臂將會停止運(yùn)動或者在等勢圈上震蕩，無法逃離。而此時Ai與期望目標(biāo)Agoal且無法抵達(dá)不同，導(dǎo)致路徑規(guī)劃算法失敗。

圖1 處于斥力場內(nèi)的目標(biāo)點(diǎn)

基于上述討論，為了防止機(jī)械臂停止運(yùn)動或者在等勢圈上震蕩，需要對傳統(tǒng)勢場的斥力函數(shù)或者引力函數(shù)加以改進(jìn)。通過傳統(tǒng)斥力場表達(dá)公式可知，傳統(tǒng)斥力場的場強(qiáng)公式缺乏對Agoal和Ai差值大小的考慮，這使得不管機(jī)械臂在空間何處，斥力場場強(qiáng)都由 |Aobs-Ai|的大小來決定。所以在對傳統(tǒng)勢力場原有的表達(dá)式改進(jìn)中加入了距離因素，改進(jìn)結(jié)果如式（3）、（4）、（5）所示。

2 改進(jìn)后算法搜索空間與整體流程

傳統(tǒng)規(guī)劃算法的搜索可以在笛卡爾空間或者關(guān)節(jié)空間中進(jìn)行。在笛卡爾空間中的路徑規(guī)劃比較直觀，考慮的影響因素相比關(guān)節(jié)空間較少。但在笛卡爾空間對機(jī)械臂進(jìn)行路徑規(guī)劃時會增加逆運(yùn)動學(xué)求解的次數(shù)[9]，可能會造成關(guān)節(jié)軸的變化不連續(xù)、出現(xiàn)奇異解和計(jì)算時間過長等問題，最終可能會導(dǎo)致機(jī)械臂采摘失敗。所以本文決定在關(guān)節(jié)空間進(jìn)行搜索。關(guān)節(jié)空間內(nèi)搜索到的點(diǎn)對于機(jī)械臂都是可以到達(dá)的，這樣減少了逆運(yùn)動學(xué)出現(xiàn)無解的情況，增加了算法的計(jì)算效率，再通過正逆運(yùn)動學(xué)的數(shù)學(xué)關(guān)系實(shí)現(xiàn)機(jī)械臂在笛卡爾空間和關(guān)節(jié)空間坐標(biāo)的轉(zhuǎn)換。

改進(jìn)算法的流程如圖2所示。

圖2 改進(jìn)算法流程

3 基于Matlab的仿真實(shí)驗(yàn)分析

本節(jié)通過Matlab軟件進(jìn)行改進(jìn)算法在串番茄采摘環(huán)境下仿真實(shí)驗(yàn)，驗(yàn)證了改進(jìn)算法在串番茄采摘環(huán)境下的魯棒性，并對比改進(jìn)前人工勢場法，驗(yàn)證了改進(jìn)算法的優(yōu)越性。

3.1 仿真環(huán)境建立

仿真實(shí)驗(yàn)在搭載64位Windows 10系統(tǒng)的PC機(jī)上進(jìn)行，PC機(jī)主要硬件配置為AMD Ryzen 7 4800H CPU @2.90 GHz 16 GB DDR4 RAM。本章通過運(yùn)用Matlab 2019a 上Robitic toolbox 插件檢驗(yàn)改進(jìn)算法在模擬串番茄采摘環(huán)境下相比于傳統(tǒng)人工勢場法的優(yōu)越性。

為了真實(shí)模擬串番茄采摘環(huán)境，以甘肅省靜遠(yuǎn)鄉(xiāng)溫室大棚串番茄種植地為虛擬樣機(jī)模擬環(huán)境，取采摘環(huán)境如圖3所示。

圖3 串番茄采摘環(huán)境障礙物

串番茄的采摘環(huán)境的障礙物主要由串番茄植株莖稈、植株枝條和非本次規(guī)劃采摘的其他串番茄果實(shí)串組成，在仿真過程中選擇AABB 包圍盒技術(shù)[10]對障礙物以不同形狀的圓柱體進(jìn)行包絡(luò)，以此來代替不規(guī)則形狀的障礙物進(jìn)行避障仿真實(shí)驗(yàn)。

3.2 逃離局部極值對比檢驗(yàn)

在本節(jié)中，將從算法的逃離極值方面的改進(jìn)效果方面進(jìn)行仿真實(shí)驗(yàn)，對比驗(yàn)證改進(jìn)算法相對于傳統(tǒng)人工勢場法的優(yōu)越性。在環(huán)境中各項(xiàng)輸入?yún)?shù)如表1所示。

表1 不同環(huán)境參數(shù)

避障環(huán)境下機(jī)械臂在傳統(tǒng)人工勢場算法控制下和改進(jìn)算法控制下的運(yùn)動軌跡仿真結(jié)果分別如圖4、5 所示。如圖4 所示，在避障環(huán)境下，機(jī)械臂在傳統(tǒng)人工勢場算法的引導(dǎo)下從起始關(guān)節(jié)組合（10，0，30，40，50，60）出發(fā)，未能成功到達(dá)期望關(guān)節(jié)組合有效范圍內(nèi)，導(dǎo)致采摘路徑規(guī)劃失敗。如圖5 所示，在避障環(huán)境下，采摘機(jī)械臂在改進(jìn)算法的引導(dǎo)下成功抵達(dá)目標(biāo)。機(jī)械臂從起始關(guān)節(jié)組合出發(fā)，在改進(jìn)的人工勢場法的引導(dǎo)成功運(yùn)動到期望關(guān)節(jié)組合。

圖4 APF機(jī)械臂路徑規(guī)劃軌跡

圖5 改進(jìn)算法機(jī)械臂路徑規(guī)劃軌跡

為了進(jìn)一步分析采摘機(jī)械臂路徑規(guī)劃的信息，機(jī)械臂在人工勢場法和改進(jìn)算法下的各關(guān)節(jié)所受總勢能隨步數(shù)變化趨勢圖如圖6、圖7所示。

圖6 APF路徑規(guī)劃總勢能變化

圖7 改進(jìn)算法路徑規(guī)劃總勢能變化

由圖6 可以發(fā)現(xiàn)采摘機(jī)械臂在傳統(tǒng)人工勢場法的引導(dǎo)下，在X=4步后總勢能就呈現(xiàn)出振蕩的現(xiàn)象，直接證明了此時采摘機(jī)械臂已經(jīng)陷入極值。而如圖7 所示，當(dāng)采摘機(jī)械臂在改進(jìn)算法的引導(dǎo)下，在運(yùn)動到X=47 步時，機(jī)械臂陷入了極值，并在X=78步數(shù)時，總勢能達(dá)到了路徑規(guī)劃過程中的最高值。這是因?yàn)椴烧獧C(jī)械臂連桿在進(jìn)入了障礙物斥力場的作用范圍，隨著機(jī)械臂連桿與障礙物的距離變小，導(dǎo)致總斥力勢能也隨之增大，最終出現(xiàn)斥力大于引力的情況，這就導(dǎo)致機(jī)械臂當(dāng)前關(guān)節(jié)組合會遠(yuǎn)離期望的關(guān)節(jié)組合，回到上一個構(gòu)型。但當(dāng)機(jī)械臂處于上一個構(gòu)型時，所受引力又大于斥力，機(jī)械臂繼續(xù)運(yùn)動到之前處于的構(gòu)型，如此反復(fù)，形成震蕩。此時停止改進(jìn)人工勢場法對機(jī)械臂的引導(dǎo)，該用自適應(yīng)的方法去尋找臨時可以逃離極值的關(guān)節(jié)組合，然后運(yùn)用RRT*算法去控制機(jī)械臂運(yùn)動到此構(gòu)型逃離極值。

機(jī)械臂在人工勢場法引導(dǎo)下和改進(jìn)算法引導(dǎo)下關(guān)節(jié)角度隨步數(shù)變化趨勢如圖8、9 所示。由圖8可以明顯看出，在傳統(tǒng)人工勢場法的引導(dǎo)下，關(guān)節(jié)1在避障環(huán)境中的路徑規(guī)劃的第4步處于振蕩狀態(tài)，并在隨后的過程中角度一直在1°～4°之間振蕩，進(jìn)一步證明了采摘機(jī)械臂已經(jīng)陷入極值。并經(jīng)過計(jì)算，當(dāng)前位置并不處于期望關(guān)節(jié)組合有效范圍內(nèi)，路徑規(guī)劃失敗。由圖9可以看出，采摘機(jī)械臂關(guān)節(jié)組合最終到達(dá)（140，29，30，2，30，0），到達(dá)期望關(guān)節(jié)組合（140，30，20，0，30，0）有效范圍內(nèi)（＜3°），路徑規(guī)劃成功。

圖8 人工勢場法關(guān)節(jié)角度變化趨勢

圖9 改進(jìn)算法關(guān)節(jié)角度變化趨勢

通過上述對比實(shí)驗(yàn)可看出本文改進(jìn)算法在處理局部極值問題時具備可靠性，在仿真實(shí)驗(yàn)中陷入極值次數(shù)超過10 次，但都能成功引導(dǎo)機(jī)械臂逃離極值繼續(xù)完成采摘機(jī)械臂的避障路徑規(guī)劃。

3.3 勢場函數(shù)對比檢驗(yàn)

在避障環(huán)境中各項(xiàng)輸入?yún)?shù)如表2所示。

表2 避障環(huán)境輸入?yún)?shù)

在上述環(huán)境下，分別用傳統(tǒng)人工勢場法和改進(jìn)算法對串番茄采摘機(jī)械臂進(jìn)行避障路徑規(guī)劃仿真，其仿真運(yùn)動軌跡如圖10、圖11 所示。其中，采摘機(jī)械臂在傳統(tǒng)人工勢場法和改進(jìn)算法引導(dǎo)下的各關(guān)節(jié)角度隨步數(shù)變化趨勢如圖12、圖13 所示。

圖10 APF機(jī)械臂路徑規(guī)劃軌跡

圖11 改進(jìn)算法機(jī)械臂路徑規(guī)劃軌跡

圖12 APF關(guān)節(jié)角度變化趨勢

圖13 改進(jìn)算法關(guān)節(jié)角度變化趨勢

如圖10～11 所示，可以看出采摘機(jī)械臂在傳統(tǒng)人工勢場法引導(dǎo)下并未到達(dá)期望目標(biāo)關(guān)節(jié)有效范圍內(nèi)。結(jié)合圖12、圖13所示，發(fā)現(xiàn)采摘機(jī)械臂在人工勢場法的引導(dǎo)下，于X=60 步后各關(guān)節(jié)停止轉(zhuǎn)動，出現(xiàn)此情況是因?yàn)椴烧獧C(jī)械臂在該位置附近受到的疊加勢場剛好平衡，即此時采摘機(jī)械臂受到障礙物對其斥力和期望坐標(biāo)對其引力大小相等，方向相反，達(dá)到了一個平衡位置，使得機(jī)械臂停滯不前，造成目標(biāo)不可達(dá)的結(jié)果，導(dǎo)致路徑規(guī)劃的失敗。而采摘機(jī)械臂在改進(jìn)算法的引導(dǎo)下，于X=78 步靠近密集障礙物區(qū)域，該位置附近受到的疊加勢場平衡，使得機(jī)械臂停滯不前，雖然機(jī)械臂可以通過RRT*算法來進(jìn)行引導(dǎo)，但由于目標(biāo)坐標(biāo)處于障礙物斥力范圍內(nèi)，所以機(jī)械臂無論如何逃離，都不會接近到目標(biāo)坐標(biāo)（除非將虛擬目標(biāo)構(gòu)型定為期望關(guān)節(jié)構(gòu)型，在接下來的規(guī)劃中全程使用RRT*算法去引導(dǎo)采摘機(jī)械臂，但這回導(dǎo)致計(jì)算量陡然上升，并且機(jī)械臂采摘效率會嚴(yán)重下降）。在對勢場函數(shù)進(jìn)行改進(jìn)后，有效抑制隨著機(jī)械臂靠近障礙物而斥力勢能無限增大的趨勢，使其能在引力勢能下順利運(yùn)動到目標(biāo)坐標(biāo)，此時斥力勢能和引力勢能都將為0。如上所分析，采摘機(jī)械臂在改進(jìn)算法引導(dǎo)下有效逃離平衡勢場影響，并在X=123 步時到達(dá)關(guān)節(jié)組合（142，15，20，0.3，30，0），處于期望關(guān)節(jié)組合（140，15，20，0，30，0）（＜3°）有效范圍內(nèi)，表明路徑規(guī)劃成功。

上述實(shí)驗(yàn)結(jié)果表明，在算法的斥力勢函數(shù)中加入關(guān)節(jié)空間中當(dāng)前采摘機(jī)械臂構(gòu)型與期望構(gòu)型的距離影響因素和當(dāng)前采摘機(jī)械臂構(gòu)型與會受到障礙物斥力影響的構(gòu)型的距離影響因素，可以有效的約束斥力場的增長，從而逃離平衡勢場，使得機(jī)械臂路徑規(guī)劃成功。

3.4 魯棒性檢驗(yàn)

通過上節(jié)的對比實(shí)驗(yàn)驗(yàn)證了本文改進(jìn)算法的優(yōu)越性，考慮到上文實(shí)驗(yàn)只在三障礙物障礙的環(huán)境下進(jìn)行了實(shí)驗(yàn)，不足以證明改進(jìn)算法的魯棒性。于是，本節(jié)將改進(jìn)算法用于不同障礙物數(shù)量的作業(yè)環(huán)境，以更好地證明改進(jìn)算法的魯棒性。

在本節(jié)中，將額外構(gòu)造兩種障礙物數(shù)量不同的機(jī)械臂采摘作業(yè)空間，障礙物的數(shù)量為1 個和5 個。在環(huán)境中各項(xiàng)輸入?yún)?shù)如表3所示。

表3 避障環(huán)境輸入?yún)?shù)

作業(yè)環(huán)境1 中有一個障礙物。采摘機(jī)械臂在作業(yè)環(huán)境1 中受改進(jìn)算法引導(dǎo)下的路徑規(guī)劃效果如圖14 所示，關(guān)節(jié)組合及末端坐標(biāo)的變化如圖15所示。

圖14 作業(yè)環(huán)境1規(guī)劃效果

圖15 作業(yè)環(huán)境1關(guān)節(jié)組合變化

由圖14、15可知，采摘機(jī)械臂在作業(yè)環(huán)境1下，受改進(jìn)算法引導(dǎo)在X=88 步到達(dá)關(guān)節(jié)組合（138，16，20，0，30，1），成功到達(dá)期望關(guān)節(jié)組合（140，15，20，0，30，0）有效范圍內(nèi)（＜3°），路徑規(guī)劃成功。

在作業(yè)環(huán)境2 有5 個障礙物。采摘機(jī)械臂在作業(yè)環(huán)境2 中受改進(jìn)算法引導(dǎo)下的路徑規(guī)劃效果如圖16 所示，關(guān)節(jié)組合及末端坐標(biāo)的變化如圖17所示。

圖16 作業(yè)環(huán)境4規(guī)劃效果

圖17 作業(yè)環(huán)境2 關(guān)節(jié)組合及末端坐標(biāo)的變化

由圖17可知，采摘機(jī)械臂在作業(yè)環(huán)境2下，受改進(jìn)算法引導(dǎo)在X=163步到達(dá)關(guān)節(jié)組合（139，16，19，1，30，0），成功到達(dá)期望關(guān)節(jié)組合（140，15，20，0，30，0）有效范圍內(nèi)（＜3°），路徑規(guī)劃成功。

如上文所述，采摘機(jī)械臂在3 種不同的作業(yè)場景中（包括對比驗(yàn)證的三障礙物環(huán)境），改進(jìn)算法都可以引導(dǎo)采摘機(jī)械臂做出有效的避障路徑規(guī)劃，進(jìn)一步驗(yàn)證了改進(jìn)的人工勢場法與RRT*算法結(jié)合算法在應(yīng)用于穿番茄采摘機(jī)械臂在面對不同采摘環(huán)境進(jìn)行路徑規(guī)劃的魯棒性。

4 結(jié)束語

本文通過提出將人工勢場法進(jìn)行改進(jìn)，然后與RRT*算法結(jié)合的路徑規(guī)劃算法，解決了如何去引導(dǎo)在串番茄采摘環(huán)境下機(jī)械臂在串番茄采摘環(huán)境下進(jìn)行避障路徑規(guī)劃的機(jī)械采摘問題。改進(jìn)結(jié)果如下。

（1）在逃離局部極值對比檢驗(yàn)中，人工勢場法引導(dǎo)下的機(jī)械臂在步數(shù)為4 步時陷入極值狀況，關(guān)節(jié)1 角度在1°～4°之間來回振蕩，路徑規(guī)劃失敗。機(jī)械臂在改進(jìn)算法的引導(dǎo)下，機(jī)械臂在步數(shù)為47 步時陷入極值，并由RRT*算法引導(dǎo)機(jī)械臂運(yùn)動到臨時目標(biāo)構(gòu)型擺脫極值困境，該實(shí)驗(yàn)中機(jī)械臂前后一共超過10次陷入極值狀態(tài)，但都成功逃離。

（2）在勢場函數(shù)對比檢驗(yàn)中，在傳統(tǒng)人工勢場法的引導(dǎo)下，機(jī)械臂在步數(shù)為60 步時各關(guān)節(jié)停止運(yùn)動，導(dǎo)致目標(biāo)不可達(dá)現(xiàn)象，是的機(jī)械臂路徑規(guī)劃失?。辉诟倪M(jìn)算法的引導(dǎo)下，機(jī)械臂在步數(shù)為123 步時成功抵達(dá)期望構(gòu)型，完成路徑規(guī)劃任務(wù)。以上驗(yàn)證了改進(jìn)算法相對于人工勢場法的優(yōu)越性。

（3）在魯棒性檢驗(yàn)中，當(dāng)環(huán)境內(nèi)障礙物為一個時，機(jī)械臂于步數(shù)為88 步時順利抵達(dá)期望構(gòu)型；當(dāng)環(huán)境內(nèi)障礙物為五個時，機(jī)械臂于步數(shù)為163 步時順利抵達(dá)期望構(gòu)型且不發(fā)生碰撞，證明了改進(jìn)算法在不同環(huán)境下的魯棒性。

以上仿真的結(jié)果為改進(jìn)算法應(yīng)用到實(shí)際串番茄采摘機(jī)器人避障路徑規(guī)劃中提供了理論依據(jù)。