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

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

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

中圖分類號：TP241.2"" 文獻標(biāo)志碼：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 引言

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

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

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

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

1 標(biāo)準(zhǔn)RRT*算法

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

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

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

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

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

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

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

2.1 基于人工勢場的路徑預(yù)規(guī)劃

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

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

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

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

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

2.2 改進RRT*算法

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

2.2.1 目標(biāo)引導(dǎo)的隨機樹擴展機制

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

Xsample=XgoalP≤PthreshXrandelse

（5）

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

2.2.2 搜索參數(shù)的自適應(yīng)

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

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

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

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

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

（6）

搜索參數(shù)的自適應(yīng)策略根據(jù)目標(biāo)零件的擺放位姿與框內(nèi)零件的分布情況計算合適的擴展步長與目標(biāo)引導(dǎo)概率閾值，從而提高算法在面向不同場景時的魯棒性與靈活性。

2.2.3 采樣節(jié)點拒絕機制的引入

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

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

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

2.3 路徑剪枝與平滑

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

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

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

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

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

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

采用準(zhǔn)均勻三次B樣條曲線對路徑進行擬合能夠?qū)崿F(xiàn)對路徑點位置的平滑處理。將去冗余后的路徑節(jié)點作為三次B樣條曲線的控制點，曲線上每一個點的位置通過式（7）計算。

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

（7）

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

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

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

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

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

2.4 算法流程

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

3 實驗與分析

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

3.1 算法對比實驗

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

通過三維相機感知得到料框點云以及零件堆點云，根據(jù)人工勢場的定義并結(jié)合該點云以及設(shè)定的目標(biāo)節(jié)點實現(xiàn)虛擬力場的構(gòu)建，進而完成對拾取路徑的預(yù)規(guī)劃，隨后尋找中間路徑節(jié)點從而確定改進RRT*算法的規(guī)劃目標(biāo)節(jié)點。預(yù)規(guī)劃路徑以及目標(biāo)節(jié)點選擇結(jié)果如圖10所示。

各算法在兩種場景下的搜索過程以及搜索結(jié)果如圖11、12所示?？紤]到采樣算法存在隨機性，對兩組場景下的每種算法實驗重復(fù)執(zhí)行100次，所得的平均指標(biāo)數(shù)據(jù)如表1、2所示。

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

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

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

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

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

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

3.2 消融實驗

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

將不加入人工勢場、無自適應(yīng)搜索參數(shù)策略、無節(jié)點拒絕機制的本文算法與完整算法在相同環(huán)境中重復(fù)規(guī)劃100次，統(tǒng)計得到的平均指標(biāo)如表4所示。

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

3.3 路徑優(yōu)化實驗分析

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

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

3.4 機械臂仿真與分揀測試

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

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

4 結(jié)束語

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

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

參考文獻：

［1］翟敬梅，黃樂.機器人無序分揀技術(shù)研究［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］姚宇，張秋菊，陳宵燕，等.復(fù)雜空間曲面焊接機器人自動編程系統(tǒng)［J］.焊接學(xué)報，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］王育文，明偉偉，張執(zhí)南.基于雙臂共軸桁架機器人的鈑金件分揀順序規(guī)劃［J］.計算機集成制造系統(tǒng)，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］劉毅，易旺民，姚建濤，等.狹長空間內(nèi)重載調(diào)姿裝配機器人的設(shè)計與研究［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］呂丞干，陳海軍，樊虹岐，等.基于云端部署的多機器人視覺協(xié)同工作系統(tǒng)［J］.組合機床與自動化加工技術(shù)，2023（12）：50-55.（Lyu Chenggan，Chen Haijun，F(xiàn)an 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算法的平滑路徑規(guī)劃方法［J］.北京航空航天大學(xué)學(xué)報，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］宋勇，張蕾，田榮，等.復(fù)雜多場景下機械臂避障運動規(guī)劃方法研究［J］.西北工業(yè)大學(xué)學(xué)報，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，F(xiàn)razzoli 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機械臂路徑規(guī)劃［J］.計算機工程與應(yīng)用，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算法的機械臂路徑規(guī)劃［J］.組合機床與自動化加工技術(shù)，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*算法的機械臂路徑規(guī)劃［J］.計算機應(yīng)用，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算法的機械臂避障路徑規(guī)劃［J］.計算機集成制造系統(tǒng)，2024，30（3）：1149-1160.（Shang Deyong，Wang Junjie，F(xiàn)an 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算法的冗余機械臂末端路徑規(guī)劃［J］.計算機應(yīng)用研究，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*的全局路徑規(guī)劃算法［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］龍樟，李顯濤，帥濤，等.工業(yè)機器人軌跡規(guī)劃研究現(xiàn)狀綜述［J］.機械科學(xué)與技術(shù)，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.）