融合人工勢場的改進RRT*機械臂料框分揀路徑規劃

摘 要：為使機械臂在料框分揀應用中快速規劃出較優的拾取路徑，提出一種融合人工勢場的改進RRT*（rapidly-exploring random tree*）機械臂路徑規劃方法。首先，利用人工勢場進行預規劃，在預規劃路徑上找到能夠與目標節點無碰撞直連的路徑節點，并將其作為RRT*的規劃目標節點，避免對空白區域的無用搜索。其次，在RRT*算法基礎上加入目標引導采樣以及搜索參數自適應計算策略，提高算法的指向性以及魯棒性。引入一種基于機械臂末端姿態約束的采樣節點拒絕機制，降低有效性檢查次數，提高規劃效率。最后，對生成的原始路徑進行剪枝優化，降低路徑代價與轉角數量，隨后利用準均勻三次B樣條結合四元數球面姿態插值進行平滑優化，提高路徑質量。實驗結果表明，所提出的改進算法與RRT*算法相比，規劃成功率提高了12.66%，規劃時間與路徑成本分別降低了79.05%以及34.80%。通過消融實驗證明了各部分改進的有效性。在硬件平臺上進行分揀測試，驗證了該方法的實用性。

關鍵詞：機械臂路徑規劃；RRT*算法；人工勢場；料框分揀

中圖分類號：TP241.2"" 文獻標志碼：A"" 文章編號：1001-3695（2025）03-021-0804-08

doi：10.19734/j.issn.1001-3695.2024.08.0310

Path planning of robotic arm bin-picking based on improved RRT* combined with artificial potential field

Luo Xuan，Chen Xindu，Wu Lei，Liu Yuesheng，Chen Yubing，Mai Zhanhao，Lu Xingyu

（School of Electromechanical Engineering，Guangdong University of Technology，Guangzhou 510006，China）

Abstract：To enable the robotic arm to quickly plan an optimal picking path in the bin-picking application，this paper proposed an improved RRT*（rapidly-exploring random tree*）path planning method that integrated artificial potential field.Firstly，this paper utilized an artificial potential field for pre-planning，found a path node on the pre-planned path that could connect directly to the target node without collisions，and used it as the planning target node for RRT*，avoiding unnecessary search in the blank area.Next，this paper incorporated target-guided sampling and adaptive search parameter computation strategies into the RRT* algorithm，enhancing the algorithm’s directionality and robustness.It introduced a sampling node rejection mechanism based on the end-effector pose constraints of the robotic arm，reducing the number of validity checks and improving planning efficiency.Finally，the algorithm performed pruning optimization on the generated raw path to reduce path cost and the number of turns.Subsequently，it utilized quasi-uniform cubic B-splines combined with quaternion spherical interpolation for smoothing optimization，improving the path quality.The experimental results indicate that the proposed improved algorithm increases the planning success rate by 12.66% compared to the RRT* algorithm，while reducing planning time and path cost by 79.05% and 34.80%，respectively.Ablation experiments have demonstrated the effectiveness of each improvement component.Picking tests conducted on a hardware platform verified the practicality of this method.

Key words：robotic arm path planning;RRT* algorithm;artificial potential field;bin-picking

0 引言

隨著科學技術的發展以及工業機器人的普及，以工業機器人為主導來完成繁重復雜的任務是實現制造業自動化、智能化與現代化的主流趨勢［1］。在零部件焊接、裝配生產線上，常借助工業機械臂完成上下料以及分揀等工序［2～5］，機械臂對料框內零件進行拾取前需要完成末端的路徑規劃，從而避免取出過程中料框壁以及框內其余零件對其產生干涉。

工業機器人路徑規劃的目的在于根據作業任務和運行環境設定機器人的工作路徑。對于低維空間的機器人運動規劃問題，常采用基于搜索的運動規劃方法，如A*算法和Dijkstra算法［6］，但當對象是高自由度的多軸機械臂時，基于采樣的運動規劃方法更具優勢［7］。LaValle［8］提出的快速搜索隨機樹算法（rapidly-exploring random tree，RRT）是典型的基于采樣的運動規劃算法，該算法在解決高維狀態空間內的路徑規劃問題時，具有計算復雜度低、泛用性好等特點，因此被廣泛應用于機械臂路徑規劃的研究當中，但RRT算法的隨機采樣導致搜索過程產生過多的無效路徑，存在規劃效率低、路徑非最優等問題。

為解決RRT算法所存在的問題，國內外學者針對RRT算法提出多種改進。Karaman等人［9］提出一種具備路徑優化能力的快速搜索隨機樹算法（rapidly-exploring random tree*，RRT*），通過在原始RRT算法中添加父節點重選以及重布線的步驟，保證在有限的時間內搜索到最優解或者接近最優解，但搜索效率低、采樣范圍大的問題仍未解決；Gammell等人［10］提出了Informed-RRT*算法，通過將采樣限制在橢圓區域內，避免了對非必要區域的探索，提高了規劃的效率，但當起始節點與目標節點相距較遠時，算法仍舊存在較多的冗余搜索；龍厚云等人［11］提出了一種融合A*策略的改進RRT算法，通過生成采樣點序列的方式降低了搜索的采樣次數，但搜索路徑仍然存在大量冗余節點。欒慶磊等人［12］提出了一種能夠自適應設置搜索步長的改進RRT算法，對RRT算法在不同環境下的適應能力進行優化，但在三維空間規劃問題上算法仍然存在較多的無用搜索。周益邦等人［13］提出了一種基于目標采樣和局部路徑優化的改進雙向RRT*算法，降低了路徑的曲折程度，但存在算法魯棒性不足的問題；商德勇等人［14］提出了一種基于動態區域采樣的改進RRT*算法，在找到初始路徑之后通過動態區域采樣對初始路徑進行優化，提高了算法在不同環境下的適應能力，但算法的效果高度依賴于初始路徑的質量。吳劍雄等人［15］提出了一種基于根尾節點連線夾角采樣和目標逼近區域搜索的改進雙樹RRT*算法，有效提高了搜索效率與收斂性，但路徑的平滑性還有待優化。

上述研究從不同角度對RRT算法進行改進，并取得一定進展，但冗余節點多、規劃效率低、路徑質量差等問題仍未得到有效解決。針對上述問題，本文提出一種將人工勢場與改進RRT*相結合的機械臂路徑規劃方法，用于解決機械臂在料框分揀應用中的路徑規劃問題。首先，通過人工勢場進行路徑預規劃，在預測路徑中找到能夠與目標節點無碰撞直連的路徑節點，并將RRT*的規劃目標節點更新為該路徑節點，避免了RRT*在料框上方無障礙區域的隨機搜索，同時縮小隨機樹的擴展規模。其次，對RRT*算法進行改進，通過加入目標引導策略，降低RRT*在搜索過程的盲目性，利用起始節點與場景中的障礙物的分布情況自適應計算搜索步長與目標引導概率，增強RRT*面對不同場景的適應能力。同時引入一種采樣節點拒絕機制，減少算法的計算量，提高規劃效率。最后，對生成的路徑進行剪枝以及平滑優化，從而提高規劃路徑的質量。經仿真及實機實驗證明了該改進算法的有效性與實用性。

1 標準RRT*算法

RRT算法原理如圖1所示，確定起始節點Xstart與目標節點Xgoal之后，在規劃空間中隨機采樣一點Xrand，并搜索隨機樹中距離采樣點Xrand最近的節點Xnearest。若采樣點Xrand與節點Xnearest之間的距離小于步長Lstep，則將Xrand作為新的節點Xnew，若距離大于步長Lstep，則以Xnearest為起點，沿著Xnearest與Xrand連線方向擴展步長Lstep得到新的節點Xnew，隨后判斷新節點Xnew以及路徑XnearestXnew的合法性。若合法，則將節點Xnew加入到隨機樹中，否則重新進行采樣。最后計算新加入的節點Xnew與目標節點Xgoal的歐氏距離dist，若距離小于閾值，則認為已經找到連接起點和目標點的路徑，反之重復以上過程。

針對RRT算法不考慮規劃所得路徑是否最優的問題，RRT*算法在其基礎上加入父節點重選與重布線步驟，隨著迭代次數的增加，可以逐漸找到最優路徑，在保證快速探索特性的同時，提高了路徑的質量。

如圖2所示，各節點標號表示節點加入隨機樹的順序，路徑連線上的數字代表該路徑的代價。節點11為當前新加入隨機樹的節點，其父節點默認為節點6。父節點重選是指在隨機樹中找到節點1～11的最小代價路徑，如圖2（a）所示，以節點11為中心，將一定范圍內的節點作為候選節點，即節點7、9以及10。計算節點11與各候選節點連接后的代價，并將最小代價對應的候選節點作為節點11的新父節點。經計算，以節點6作為父節點時路徑代價為14，而以節點7作為父節點時路徑代價降為11，因此將節點7作為當前新節點的父節點，父節點重選結果如圖2（b）所示。

重布線過程可以進一步降低路徑代價，如圖2（c）所示，以節點11為中心，計算以節點11為父節點，指定范圍內的候選節點作為其子節點時的代價是否會降低，若降低，則將該候選節點作為節點11的子節點。當節點11作為節點10的父節點時，節點10的代價由原來的22降低為14；當節點11作為節點9的父節點時，代價由原來的18降低為16，因此將節點11作為節點10與9的父節點。重布線結果如圖2（d）所示。

標準RRT*算法在RRT基礎上引入優化機制和路徑調整策略，一定程度上提升了生成路徑的質量，但仍然存在以下缺陷：a）隨機樹冗余節點多，隨機樹擴張缺乏目標性，導致在空白區域造成大量無用搜索；b）規劃效率低下，大量的冗余節點增加了計算成本，帶來算力的浪費;c）規劃路徑質量差，由于隨機采樣的限制，規劃的路徑存在較多轉折。

2 融合人工勢場的改進RRT*路徑規劃方法

標準RRT*算法在進行路徑規劃時盲目性強，產生大量冗余節點，導致其難以滿足機械臂料框分揀應用場景中高效率與高質量的要求。為了避免RRT*算法在空白區域的無用搜索，保證算法在多障礙空間中精確規劃出較短的無碰撞分揀路徑，同時提高算法的搜索效率，本文提出一種笛卡爾空間下的人工勢場與改進RRT*相結合的機械臂料框分揀路徑規劃算法。

2.1 基于人工勢場的路徑預規劃

相較于直接采用RRT*搜索可行的拾取路徑解，本文方法在改進RRT*搜索前加入基于人工勢場的路徑預規劃。

人工勢場法是一種被廣泛應用于機器人路徑規劃的局部動態規劃算法，該算法的基本思想是將機器人的工作空間抽象為虛擬的力場，其中包括目標吸引力與障礙物排斥力。目標吸引力是指機器人在當前位置所受到的指向目標的引力，引導機器人向目標點運動，吸引力的大小與機器人當前位置到目標點位置的距離成正相關；障礙物排斥力指機器人在障礙物附近時所受到的斥力，引導機器人原理障礙物，排斥力大小與機器人到障礙物的距離成負相關。通過力場的作用從而實現機器人沿預定路徑的運動。

根據人工勢場的定義，利用目標節點與掃描點云構建人工勢場，如式（1）（2）所示，其中ξ與η為尺度因子，ρ（q，qgoal）表示當前點q與目標點qgoal之間的歐氏距離，ρ（q，qobs）表示當前點q與障礙點qobs的歐氏距離，P0表示斥力場的作用范圍。對引力場Uatt與斥力場Urep進行求導可以得到引力Att與斥力Rep的計算公式，分別如式（3）（4）所示。根據公式計算當前點所受到的引力與斥力在x、y、z三個方向上的大小，同時將各方向合力乘以步長即可得到各方向的坐標變化量，從起始節點出發，不斷對當前點疊加坐標變化量，從而生成一條連接起始節點與目標節點的預測路徑。基于人工勢場預測拾取路徑的流程如圖3所示。

如圖4所示，為了避免改進RRT*算法在料框上方的空白區域進行隨機搜索，通過節點直連的方式在預測路徑中找到最接近起始節點并且能夠與目標節點進行無碰撞直連的中間路徑節點，隨后將該路徑節點作為改進RRT*搜索的目標節點，并記錄該節點與目標節點的直連路徑pathdirect。查找中間路徑節點的流程如圖5所示，基于起始節點與目標節點的位姿信息，通過線性插值得到每個預測路徑點對應的位姿，隨后從起始節點開始遍歷每個路徑節點，生成該路徑節點與目標節點的直連路徑，并判斷機械臂末端沿該路徑運動時的碰撞情況，若機械臂不發生碰撞，則將當前的路徑節點作為RRT*的規劃目標節點。

通過將滿足直連條件的預測路徑中間節點作為改進RRT*的規劃目標節點，進一步縮短了起始節點到目標節點的距離，使得隨機樹的擴展規模縮小，從而降低規劃所需的計算量，加快搜索到可行路徑解的速度，同時避免了改進RRT*算法在料框上方空白區域的隨機搜索，提高了規劃的效率。

2.2 改進RRT*算法

標準RRT*算法用于機械臂料框分揀路徑規劃時，隨機樹擴展的盲目性強，導致生成大量冗余節點，使得規劃過程中對采樣點的有效性檢查次數過多，嚴重影響路徑規劃效率。同時，隨機樹的路徑搜索效果依賴于步長的設置，步長設定過大或過小會對規劃成功率和效率造成影響。為了減少隨機樹的無用搜索，降低規劃過程中的計算量，提高算法對不同場景的適應性，提出一種改進的RRT*算法。

2.2.1 目標引導的隨機樹擴展機制

針對標準RRT*算法在搜索時存在過分強調探索性而忽略目的性的問題，一種常用的改進方法是在RRT*算法的節點采樣階段使用目標引導的采樣策略［16］，使得改進RRT*算法在采樣時有一定概率將目標節點Xgoal直接作為采樣節點Xsample，如式（5）所示。

Xsample=XgoalP≤PthreshXrandelse

（5）

其中：Pthresh為引導概率閾值，取值為0～1;P為0～1服從均勻概率分布的隨機值。當P≤Pthresh時，算法將目標節點Xgoal直接作為采樣節點Xsample，此時隨機樹將朝著目標節點方向進行擴展，從而實現對目標節點的快速接近；當Pgt;Pthresh，算法在狀態空間中進行隨機采樣得到隨機節點Xrand，使隨機樹按照該方向進行生長，從而繞開障礙區域，避免陷入局部極小，實現對無障礙空間的探索。通過疊加目標引導的擴展機制，使得隨機樹在擴張過程中實現探索性和目的性的平衡。

2.2.2 搜索參數的自適應

改進RRT*算法的隨機樹搜索參數包括擴展步長stepsize與目標引導概率閾值Pthresh。對于擴展步長來說，若設定值過大會降低隨機樹在復雜障礙區域的探索能力，過小則會影響搜索的效率；對于目標引導概率閾值，過小會導致隨機樹的探索性增強，降低搜索效率，過大則使得隨機樹擴展更具目的性，難以在復雜障礙環境中找到可行路徑解。在機械臂料框分揀應用中，零件在框內的擺放位姿隨機，固定的擴展步長與目標偏置閾值難以滿足不同約束情況下的高效路徑搜索，因此本文提出一種搜索參數自適應策略，利用零件起始位姿與框內零件的分布情況計算合適的隨機樹擴展步長與目標引導概率閾值，從而提高算法在面對不同場景時的適應能力。

搜索參數自適應策略根據分揀場景中零件的分布情況來調整搜索步長，通過計算將零件從起始節點朝目標節點取出時機械臂末端及零件恰巧發生碰撞的最大步長，從而得到改進RRT*算法的搜索步長。當起始節點與目標節點的連線存在較少障礙物時，搜索步長相應增大，從而提高搜索效率；相反，當連線處于障礙物數量較多的復雜障礙區域中，搜索步長相應縮小，從而保證搜索到可行解的成功率。搜索步長的具體計算方法如下：

從n=2開始對起始節點與目標節點進行位姿線性插值計算得到n等分點中的第一個點所對應的位姿，檢查機械臂末端以及目標零件在該位姿下的碰撞情況，當發生碰撞時，記錄n的值，計算起始節點與目標節點之間的距離dist，并將dist/n作為隨機樹的擴展步長；若不發生碰撞，則令n=n+1并重復上述步驟，直至找到滿足條件的n等分點。

規劃任務中的起始節點表示為待拾取零件在料框中的位姿。當目標零件的擺放傾斜程度較小時，通過提高目標引導概率閾值，可以加強隨機樹采樣的目的性，使機械臂能夠快速將零件轉移到目標節點狀態；而當目標零件的傾斜程度較大時，需要考慮零件取出時與料框壁以及框內其余零件的干涉情況，因此通過降低目標引導概率閾值，從而增加隨機樹在復雜障礙區域擴展時的探索性，提高搜索的成功率。基于此，提出一種基于零件初始姿態的目標引導概率閾值自適應計算方法，該方法通過式（6）來實現，其中θ表示零件起始姿態的z軸方向與豎直方向的夾角。當夾角接近0時，通過式（6）計算得到的引導概率閾值接近于1，從而增強隨機樹搜索的目的性，使得隨機樹快速向目標節點擴展；當夾角大于π/6時，目標引導概率閾值保持在0.05附近，隨機樹表現出較強的探索性，提高了規劃成功率。

Pthresh=0.95e-11.44θ+0.05，θ∈［0，π/2］

（6）

搜索參數的自適應策略根據目標零件的擺放位姿與框內零件的分布情況計算合適的擴展步長與目標引導概率閾值，從而提高算法在面向不同場景時的魯棒性與靈活性。

2.2.3 采樣節點拒絕機制的引入

由于本文提出的改進RRT*算法在笛卡爾空間下進行規劃，規劃任務中的起始節點Xstart、目標節點Xgoal，以及隨機采樣節點的Xrand均表示為笛卡爾空間中的位姿。只有通過有效性檢查的采樣節點才能加入到隨機樹中，節點有效性檢查包含兩部分內容，一部分是判斷當前節點是否存在對應的機械臂逆解，另一部分則是判斷機械臂末端工具以及被拾取物體在當前位姿下是否與其余物體發生碰撞。

通過Modified-DH參數法建立機械臂的運動學模型，并利用解析法對新采樣節點進行逆解計算。當逆解中至少存在一組滿足關節行程限制的關節角度時，進行下一步的碰撞檢測。傳統的碰撞檢測使用包圍盒法對檢測對象進行簡化，由于存在較大的實體誤差，無法滿足機械臂分揀應用中的精確避碰要求。本文利用開源的碰撞檢測庫FCL（flexible collision library）［17］來完成改進RRT*算法在規劃中的碰撞檢測。碰撞檢測流程為：根據機械臂末端以及目標零件的STL格式模型構造FCL庫中的BVH模型，利用BVH模型將物體劃分至最小圖元并生成碰撞對象，根據采樣節點更新碰撞對象的位姿，檢測該位姿下的機械臂末端工具以及被拾取物體是否發生碰撞，并將無碰撞的采樣節點加入到隨機樹中。

在料框分揀應用中，當機械臂末端的傾斜程度越小，與料框發生碰撞的概率也越低，結合該特征，改進RRT*算法在節點有效性檢查之前引入基于末端位姿的采樣節點拒絕機制，具體為：記錄隨機樹起始節點的位姿法向量Zstart與世界坐標系豎直向量Zworld之間的角度anglestart，在每次進行有效性檢查之前，計算當前節點的位姿法向量Znew與Zworld之間的夾角angle，若anglegt;anglestart+902，則舍棄該節點并重新進行采樣，反之對該節點執行后續的有效性判斷。該節點拒絕機制可以在不影響規劃成功率的同時，有效減少逆解計算以及碰撞檢測的次數，從而降低計算成本，縮短整體規劃時間。

2.3 路徑剪枝與平滑

改進RRT*算法依次連接隨機樹上的節點從而生成連續線段，但由于冗余節點的存在，路徑結果未達到最優，所以需要對路徑進行剪枝處理。具體步驟如下：

a）初始化剪枝路徑結果pathpruned，并將原始路徑結果path的起始節點Pstart加入到pathpruned中，令Pend為path中Pstart的下一個節點。

b）對Pstart與Pend之間的路徑進行有效性檢查：在Pstart與Pend兩者之間，以擴展步長stepsize為間隔進行插值得到一系列中間節點，接著對每一個中間節點進行碰撞檢測。

c）若Pstart與Pend之間的每一個節點都不發生碰撞，則令原始路徑中的下一個節點作為Pend，執行步驟b）;若Pstart與Pend之間存在碰撞，記錄當前節點Pend在原始路徑中的前一個節點Pend-1，Pstart與Pend-1之間的節點皆為可去除的冗余節點，將節點Pend-1加入到Pathpruned中，隨后將起始節點Pstart更新為Pend-1，繼續向后尋找路徑關鍵點。

重復上述步驟，遍歷整個path，即可得到路徑代價更小的優化路徑pathpruned。圖6為路徑剪枝處理。

路徑去冗余后，仍然存在轉折，不利于機械臂的平穩運行，因此需要對剪枝路徑進行平滑優化，使得機械臂末端的拾取軌跡連續并且平滑［18］。路徑的平滑分為位置平滑與姿態平滑兩部分，具體如下：

采用準均勻三次B樣條曲線對路徑進行擬合能夠實現對路徑點位置的平滑處理。將去冗余后的路徑節點作為三次B樣條曲線的控制點，曲線上每一個點的位置通過式（7）計算。

C（t）=∑n-1i=0Bi，deg（t）·Pi

（7）

其中：Pi為控制點;Bi，deg（t）為B樣條曲線的基函數，由Coxde-Boor遞推關系公式得到。連接各點即可生成位置平滑后的路徑。

驅動機械臂進行分揀時需要嚴格限制末端位姿以避免發生碰撞，同時要求路徑點之間的位姿平滑變化，因此通過以下步驟求取平滑路徑點的姿態信息：

a）尋找平滑曲線上的關鍵位姿點：遍歷B樣條曲線的控制點，在B樣條曲線上找到離該控制點最近的路徑節點，并將控制點的姿態作為該路徑節點的姿態，從而得到曲線上的關鍵位姿點。

b）通過已確定姿態的關鍵點將平滑路徑分為多段，利用每一段路徑的起點姿態與終點姿態作為插值起點與終點，使用四元數球面姿態插值法對起點與終點之間的節點進行姿態插值計算。

至此已完成對改進RRT*算法規劃路徑結果的剪枝與平滑處理，將該路徑優化結果與預規劃階段記錄的直連路徑pathdirect合并，最終得到連接起始節點與目標節點的無碰撞平滑拾取路徑。

2.4 算法流程

融合以上方法得到機械臂料框分揀路徑規劃算法，流程如圖7所示。算法通過人工勢場進行路徑預規劃，縮小了隨機樹的擴展規模，避免RRT*算法在料框上方空白區域進行無用搜索；在標準RRT*算法基礎上疊加目標引導采樣策略實現探索性與目的性的平衡，同時加入隨機樹搜索參數的自適應計算方法，提高了算法在不同場景下的魯棒性與適應性；此外，引入采樣節點拒絕機制降低算法計算量，提高了整體規劃效率；最后，對路徑結果進行剪枝優化與平滑處理，提高路徑質量。

3 實驗與分析

通過仿真實驗與實機實驗對算法的有效性以及實用性進行驗證，實驗所使用的計算機配置為12th Gen Intel CoreTM i5-12600K CPU，主頻3.7 GHz，運行內存32 GB，仿真實驗主要包括：算法對比實驗、消融實驗、路徑優化實驗以及機械臂仿真實驗；實機實驗為針對料框內隨機擺放零件的機械臂分揀操作，通過圖8所示的硬件平臺完成。

3.1 算法對比實驗

為驗證算法的有效性與優越性，在零件隨機擺放的兩種拾取場景中，分別與原始RRT*和Informed-RRT*進行對比實驗，在兩種場景下的待分揀零件起始狀態以及設定的目標狀態如圖9所示。參數設置：RRT*與Informed-RRT*的搜索步長stepsize取值為3，每種算法的迭代次數上限取值為15 000次。

通過三維相機感知得到料框點云以及零件堆點云，根據人工勢場的定義并結合該點云以及設定的目標節點實現虛擬力場的構建，進而完成對拾取路徑的預規劃，隨后尋找中間路徑節點從而確定改進RRT*算法的規劃目標節點。預規劃路徑以及目標節點選擇結果如圖10所示。

各算法在兩種場景下的搜索過程以及搜索結果如圖11、12所示。考慮到采樣算法存在隨機性，對兩組場景下的每種算法實驗重復執行100次，所得的平均指標數據如表1、2所示。

從各算法在不同場景中的搜索過程可以看出，由于原始RRT*算法是完全隨機采樣，隨機樹節點廣泛分布在規劃空間的空白區域，造成過多的無用搜索，使得節點數量大幅增加，影響路徑規劃的效率。Informed-RRT*算法在找到一條初始可行路徑之后，采樣空間從全局收縮到高維的橢球區域內，隨著路徑規劃結果的長度不斷縮短，橢球區域也相應縮小，從而收縮了隨機樹的采樣范圍，使得搜索集中在可行路徑附近，優化了規劃的成功率、效率以及路徑質量。

本文算法利用人工勢場完成路徑預規劃并找到中間路徑點與目標節點的無碰撞直連路徑，隨后基于改進RRT*算法在局部多障礙區域附近進行路徑搜索，即可完成對無碰撞分揀路徑的規劃，有效避免了改進RRT*算法在料框上方的隨機搜索，使得冗余節點數量大幅度降低，有助于提高規劃效率。路徑直連的方式減少了拐角的數量，使整體路徑成本得到進一步降低。

分析各算法在兩種場景下的成功率可以得出，由于原始RRT*算法在搜索過程中存在較強的盲目性，其規劃成功率在三者中處于最低水平，而Informed-RRT*在隨機樹的構建過程中通過限制采樣區域，使得隨機樹在最優區域周邊進行集中搜索，因此更容易找到可行路徑，規劃成功率相較RRT*有所提高。本文的改進RRT*算法將搜索區域進一步收縮至目標零件附近的多障礙區域，減少隨機樹的無效擴展，避免搜索次數的浪費，從而更有可能在有限的迭代次數中找到可行路徑。在規劃耗時與路徑成本方面，本文算法明顯優于RRT*和Informed-RRT*。相較于RRT*，本文融合人工勢場的改進RRT*算法在規劃耗時上平均縮短了79.05%，路徑成本平均降低了34.80%。盡管Informed-RRT*對采樣區域進行限制，一定程度上優化了規劃效率與路徑成本，但本文方法的平均規劃時間比Informed-RRT*平均縮短38.26%，路徑成本平均下降了23.55%。

為了全面地分析本文算法的性能，使用目前較先進的改進雙樹RRT*路徑規劃算法［15］在場景一下進行對比，對比算法的搜索過程與搜索結果如圖13所示，表3為重復100次實驗后得到的平均指標。

結合表1、3的數據可以分析得出，改進雙樹RRT*算法引入了雙樹擴展策略，使得整體的路徑搜索效率更高，在規劃耗時方面少于本文算法。由于本文算法通過直連的方式生成連接中間路徑節點與目標節點的路徑，避免了不必要的彎折，所以在降低路徑成本方面本文算法的表現優于對比的改進雙樹RRT*算法。

通過上述實驗結果對比可知，RRT*與Informed-RRT*應用于機械臂料框分揀時存在采樣范圍大、無效搜索多的問題，引起冗余節點數量的大幅增加，從而影響了路徑規劃的效率，同時隨機樹的盲目搜索導致路徑彎折過多，增加了路徑成本。本文算法通過人工勢場預規劃并確定中間路徑節點，針對性地避免了RRT*算法在料框上方空白區域的隨機搜索，縮小了隨機樹的擴展規模，使得隨機樹的冗余節點數量大幅降低，提高了路徑規劃的效率。同時，從中間路徑節點到目標節點的無碰撞路徑通過直連的方式生成，有效減少了不必要的路徑轉折，優化了路徑的質量。通過加入目標引導的采樣策略，加強了隨機樹擴展的目標導向性，提高了算法的規劃效率與成功率。由于RRT*與Informed-RRT*使用固定的搜索參數，當場景發生變換時，規劃的效率以及成功率容易受到影響，難以滿足不同規劃任務的需求，而本文方法的搜索參數基于零件起始位姿信息以及框內零件的分布情況計算得到，在不同環境復雜度的規劃任務中都能保持較高的規劃成功率，算法的魯棒性與自適應性得到進一步提升。

3.2 消融實驗

為了驗證融合人工勢場的改進RRT*算法各改進部分的有效性與必要性，保持算法參數不變，在另一拾取場景下對算法進行消融實驗，如圖14所示。

將不加入人工勢場、無自適應搜索參數策略、無節點拒絕機制的本文算法與完整算法在相同環境中重復規劃100次，統計得到的平均指標如表4所示。

通過實驗結果可以得出，缺少人工勢場預規劃的算法在進行路徑搜索過程中，在空白區域存在大量的無用搜索，造成冗余節點數量的大幅增加以及搜索次數的浪費，使得算法在有限搜索次數中找到可行解的成功率從98%下降到93%，相較于完整改進算法，平均規劃耗時與平均路徑成本分別增加到原來的2.401倍以及1.431倍。而沒有加入搜索參數自適應策略的算法在規劃場景發生變化后由于無法根據當前規劃任務的特征而相應調整搜索參數，導致路徑搜索的性能未達到最優，在平均規劃耗時方面相較完整改進算法增加了48.96%。由于缺少節點拒絕機制的算法對每一個采樣點都進行了有效性檢查，所以算法的計算量有一定程度的增加，平均規劃時間比完整改進算法增加了11.37%。綜上所述，利用人工勢場進行路徑預規劃的策略對算法性能的提升作出較大的貢獻，其對于規劃時間與路徑成本的降低發揮較為關鍵的作用，而自適應搜索參數策略在提高算法魯棒性方面以及節點拒絕機制在加快算法效率方面都是有效的，由此可以驗證各改進部分的有效性與必要性。

3.3 路徑優化實驗分析

為驗證路徑優化方法的有效性，以場景二為例，對融合人工勢場的改進RRT*算法生成的原始路徑進行優化實驗，基于原始路徑得到的剪枝路徑以及平滑路徑如圖15所示。計算去冗余后的路徑節點數量以及路徑長度，并與原始路徑進行比較，數據結果如表5所示。

分析圖15可以得出原始路徑經過剪枝優化后，僅保留了維持路徑特征的關鍵點，隨后在此基礎上對剪枝路徑使用準均勻三次B樣條擬合以及四元數球面姿態插值，得到位姿平滑過渡的路徑結果，進而保證了機械臂沿該路徑的平穩運動。根據表5數據，經過剪枝優化后，路徑節點數量減少了65.71%，路徑成本降低了4.36%，有效剔除了路徑中不必要的彎折。綜上，通過剪枝與平滑優化能夠有效生成較短的平滑拾取路徑，有助于提升機械臂的分揀效率以及運動平穩度。

3.4 機械臂仿真與分揀測試

為了進一步驗證本文改進算法在機械臂料框分揀應用中的有效性，在MATLAB軟件中導入機械臂分揀場景，并根據Modified-DH參數法建立機械臂簡化模型，通過Robotic System Toolbox進行可視化。圖16（a）為機械臂按照平滑路徑運動時所對應的各關節位置，圖16（b）為機械臂各個關節角度的變化曲線。分析機械臂沿平滑路徑從料筐內取出目標零件的過程可以得出，機械臂一開始主要執行向上提拉的動作，旨在最大程度降低料筐以及框內其余零件對目標產生的干涉和碰撞，隨著機械臂末端周圍的空間約束逐漸減弱，機械臂沿直連路徑快速運動至目標狀態，從而完成分揀流程。在整個分揀過程中，機械臂的各個關節沒有發生突變或回轉，機械臂整體運動平穩。

為驗證本文算法的實用性，在搭建的硬件平臺進行實機實驗。機械臂選用UR3，電腦作為上位機與機械臂建立通信，將運動信息轉換為機械臂腳本語言并控制機械臂執行相應運動。圖17為機械臂分揀過程，機械臂沿改進算法生成的路徑可以安全地將零件從隨機擺放的位姿轉移到目標位姿，避免了末端拾取的零件受到嚴重干涉而導致分揀失敗，整個分揀過程耗時5.184 s，基本滿足工業應用需求，驗證了本文方法的實用性。

4 結束語

針對機械臂料框分揀應用的路徑規劃問題，本文在RRT*算法的基礎上進行改進，通過引入基于人工勢場的路徑預規劃、目標引導的節點采樣策略、隨機樹搜索參數自適應計算方法、基于位姿約束的采樣節點拒絕機制以及可行路徑的剪枝與平滑優化，提出了一種融合人工勢場的改進RRT*路徑規劃算法。經過在不同場景下進行多組重復實驗，對比證明了該算法在規劃成功率、算法效率以及路徑質量方面相較于相關改進RRT*算法有更優的表現。同時，在消融實驗中驗證了算法各改進部分的有效性與必要性。在機械臂分揀的仿真實驗以及實機實驗中，機械臂能夠沿平滑路徑安全且快速地取出料筐內的目標零件，基本滿足工業應用的要求，體現了本文方法的實用性。

下一步將結合機械臂拾取過程中的運動特性，通過設置虛擬碰撞體來規避機械臂位姿奇異與超出工作范圍的情況。同時，為了使機械臂在工作中具備均衡的性能，還應考慮運行效率、能量消耗以及抖動沖擊等因素，對規劃所得的路徑進行進一步優化。

參考文獻：

［1］翟敬梅，黃樂.機器人無序分揀技術研究［J］.包裝工程，2022，43（8）：66-75，16.（Zhai Jingmei，Huang Le.Review of unordered pic-king technology for robots［J］.Packaging Engineering，2022，43（8）：66-75，16.）

［2］姚宇，張秋菊，陳宵燕，等.復雜空間曲面焊接機器人自動編程系統［J］.焊接學報，2023，44（5）：122-128，136.（Yao Yu，Zhang Qiuju，Chen Xiaoyan，et al.Automatic programming system of complex space trajectory welding robot［J］.Transactions of the China Wel-ding Institution，2023，44（5）：122-128，136.）

［3］王育文，明偉偉，張執南.基于雙臂共軸桁架機器人的鈑金件分揀順序規劃［J］.計算機集成制造系統，2024，30（5）：1770-1778.（Wang Yuwen，Ming Weiwei，Zhang Zhinan.Sheet metal part sorting sequence planning based on dual-arm co-axial truss robot［J］.Computer Integrated Manufacturing Systems，2024，30（5）：1770-1778.）

［4］劉毅，易旺民，姚建濤，等.狹長空間內重載調姿裝配機器人的設計與研究［J］.中國機械工程，2024，35（2）：324-336.（Liu Yi，Yi Wangmin，Yao Jiantao，et al.Design and research of heavy-duty posture-adjusting assembly robots in narrow space［J］.China Mechanical Engineering，2024，35（2）：324-336.）

［5］呂丞干，陳海軍，樊虹岐，等.基于云端部署的多機器人視覺協同工作系統［J］.組合機床與自動化加工技術，2023（12）：50-55.（Lyu Chenggan，Chen Haijun，Fan Hongqi，et al.Research on multi-robot vision cooperative work system based on cloud deployment［J］.Modular Machine Tool amp; Automatic Manufacturing Technique，2023（12）：50-55.）

［6］鞏慧，倪翠，王朋，等.基于Dijkstra算法的平滑路徑規劃方法［J］.北京航空航天大學學報，2024，50（2）：535-541.（Gong Hui，Ni Cui，Wang Peng，et al.A smooth path planning method based on Dijkstra algorithm［J］.Journal of Beijing University of Aeronautics and Astronautics，2024，50（2）：535-541.）

［7］宋勇，張蕾，田榮，等.復雜多場景下機械臂避障運動規劃方法研究［J］.西北工業大學學報，2023，41（3）：500-509.（Song Yong，Zhang Lei，Tian Rong，et al.Research on obstacle avoidance motion planning method of manipulator in complex multi scene［J］.Journal of Northwestern Polytechnical University，2023，41（3）：500-509.）

［8］Lavalle S.Rapidly-exploring random trees：a new tool for path planning，Research Report 9811［R］.1998.

［9］Karaman S，Frazzoli E.Sampling-based algorithms for optimal motion planning［J］.The International Journal of Robotics Research，2011，30（7）：846-894.

［10］Gammell J D，Srinivasa S S，Barfoot T D.Informed RRT*：optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2014：2997-3004.

［11］龍厚云，李光，譚薪興，等.融合A*的改進RRT機械臂路徑規劃［J］.計算機工程與應用，2024，60（4）：366-374.（Long Houyun，Li Guang，Tan Xinxing，et al.Path planning of robotic arm based on improved RRT algorithm combined with A*［J］.Computer Enginee-ring and Applications，2024，60（4）：366-374.）

［12］欒慶磊，郭繼智，屈紫浩，等.改進RRT算法的機械臂路徑規劃［J］.組合機床與自動化加工技術，2024（5）：60-64，70.（Luan Qinglei，Guo Jizhi，Qu Zihao，et al.Improved RRT algorithm for manipulator path planning［J］.Modular Machine Tool amp; Automatic Manufacturing Technique，2024（5）：60-64，70.）

［13］周益邦，章蘭珠，徐海銘.基于改進雙向RRT*算法的機械臂路徑規劃［J］.計算機應用，2022，42（Z1）：342-346.（Zhou Yibang，Zhang Lanzhu，Xu Haiming.Path planning of manipulator based on improved two-way RRT* algorithm［J］.Journal of Computer Applications，2022，42（Z1）：342-346.）

［14］商德勇，汪俊杰，樊虎，等.基于RRT*-DR算法的機械臂避障路徑規劃［J］.計算機集成制造系統，2024，30（3）：1149-1160.（Shang Deyong，Wang Junjie，Fan Hu，et al.Obstacle avoidance path planning for manipulator based on RRT*-DR algorithm［J］.Computer Integrated Manufacturing Systems，2024，30（3）：1149-1160.）

［15］吳劍雄，畢卓然，李宗道，等.基于改進雙樹RRT算法的冗余機械臂末端路徑規劃［J］.計算機應用研究，2024，41（2）：459-465.（Wu Jianxiong，Bi Zhuoran，Li Zongdao，et al.Redundant manipulator end-effector path planning based on improved bidirectional RRT algorithm［J］.Application Research of Computers，2024，41（2）：459-465.）

［16］許萬，楊曄，余磊濤，等.一種基于改進RRT*的全局路徑規劃算法［J］.控制與決策，2022，37（4）：829-838.（Xu Wan，Yang Ye，Yu Leitao，et al.A global path planning algorithm based on improved RRT*［J］.Control and Decision，2022，37（4）：829-838.）

［17］Pan Jia，Chitta S，Manocha D.FCL：a general purpose library for collision and proximity queries［C］//Proc of IEEE International Confe-rence on Robotics and Automation.Piscataway，NJ：IEEE Press，2012：3859-3866.

［18］龍樟，李顯濤，帥濤，等.工業機器人軌跡規劃研究現狀綜述［J］.機械科學與技術，2021，40（6）：853-862.（Long Zhang，Li Xiantao，Shuai Tao，et al.Review of research state of trajectory planning for industrial robots［J］.Mechanical Science and Technology for Aerospace Engineering，2021，40（6）：853-862.）