串番茄采摘機械臂避障路徑規(guī)劃算法研究*

張?zhí)斐?，李奇林，裴天?/p>

（江蘇理工學院機械工程學院，江蘇常州 213001）

0 引言

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

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

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

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

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

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

1 人工勢場法的改進內容

1.1 結合RRT＊算法幫助機械臂逃離人工勢場法極值

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

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

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

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

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

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

圖1 處于斥力場內的目標點

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

2 改進后算法搜索空間與整體流程

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

改進算法的流程如圖2所示。

圖2 改進算法流程

3 基于Matlab的仿真實驗分析

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

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

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

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

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

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

3.2 逃離局部極值對比檢驗

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

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

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

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

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

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

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

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

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

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

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

圖9 改進算法關節(jié)角度變化趨勢

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

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

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

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

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

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

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

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

圖13 改進算法關節(jié)角度變化趨勢

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

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

3.4 魯棒性檢驗

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

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

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

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

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

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

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

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

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

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

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

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

4 結束語

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

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

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

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

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