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

2022-10-09 08:37:18張天成李奇林裴天朔

機電工程技術 2022年9期

張天成，李奇林，裴天朔

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

0 引言

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

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

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

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

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

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

1 人工勢場法的改進內容

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

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

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

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

1.2 改進人工勢場法斥力函數

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

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

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

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

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

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

圖2 改進算法流程

3 基于Matlab的仿真實驗分析

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

3.1 仿真環境建立

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

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

圖3 串番茄采摘環境障礙物

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

3.2 逃離局部極值對比檢驗

在本節中，將從算法的逃離極值方面的改進效果方面進行仿真實驗，對比驗證改進算法相對于傳統人工勢場法的優越性。在環境中各項輸入參數如表1所示。

表1 不同環境參數

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

圖4 APF機械臂路徑規劃軌跡

圖5 改進算法機械臂路徑規劃軌跡

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

圖6 APF路徑規劃總勢能變化

圖7 改進算法路徑規劃總勢能變化

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

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

圖8 人工勢場法關節角度變化趨勢

圖9 改進算法關節角度變化趨勢

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

3.3 勢場函數對比檢驗

在避障環境中各項輸入參數如表2所示。

表2 避障環境輸入參數

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

圖10 APF機械臂路徑規劃軌跡

圖11 改進算法機械臂路徑規劃軌跡

圖12 APF關節角度變化趨勢

圖13 改進算法關節角度變化趨勢

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

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

3.4 魯棒性檢驗

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

在本節中，將額外構造兩種障礙物數量不同的機械臂采摘作業空間，障礙物的數量為1 個和5 個。在環境中各項輸入參數如表3所示。

表3 避障環境輸入參數

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

圖14 作業環境1規劃效果

圖15 作業環境1關節組合變化

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

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

圖16 作業環境4規劃效果

圖17 作業環境2 關節組合及末端坐標的變化

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

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

4 結束語

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

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

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

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

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