基于改進雙樹RRT*算法的冗余機械臂末端路徑規劃

吳劍雄 畢卓然 李宗道 李清都

收稿日期：2023-06-01；修回日期：2023-07-26? 基金項目：國家自然科學基金資助項目（92048205）

作者簡介：吳劍雄（1999—），男（回族），江蘇鎮江人，碩士研究生，主要研究方向為機械臂的運動控制；畢卓然（1996—），男，黑龍江哈爾濱人，碩士研究生，主要研究方向為機械臂末端結構設計；李宗道（1994—），男，河南鄭州人，博士研究生，主要研究方向為冗余機械臂的雙臂協作；李清都（1980—），男（通信作者），湖北襄陽人，教授，博士，主要研究方向為仿生機器人理論與技術、復雜系統的動力學與控制等（liqd@usst.edu.cn）.

摘? 要：針對冗余機械臂的冗余特性與相關RRT*算法在規劃機械臂末端路徑的應用中存在的搜索效率較低、收斂性不穩定以及沒有充分考慮到機械臂末端幾何構型與自身運動特性對路徑規劃影響的問題，提出一種改進策略。首先，引入一種基于根尾節點連線夾角的采樣點選擇方式，并設置目標逼近區域。根據連續采樣成功次數動態選擇改進采樣與隨機采樣。接著，將雙樹擴展策略與上述方法相結合。最后，將初始可行路徑進行二次重連得到最終的優化路徑。通過驗證，改進雙樹RRT*方法能夠有效地提升搜索效率、收斂性以及路徑的優越性。虛擬碰撞體與膠囊碰撞體的引入也能較好地應對機械臂末端結構與運動特性帶來的影響。使用Mujoco物理仿真引擎進行機械臂運動驗證，證明該策略可以為冗余機械臂末端規劃出一條較優的可行路徑。

關鍵詞：冗余機械臂；RRT*；末端路徑；根尾節點；目標逼近區域；雙樹擴展；虛擬碰撞體；膠囊碰撞體；Mujoco

中圖分類號：TP241??? 文獻標志碼：A

文章編號：1001-3695（2024）02-021-0459-07

doi：10.19734/j.issn.1001-3695.2023.06.0275

Redundant manipulator end-effector path planning based onimproved bidirectional RRT* algorithm

Wu Jianxionga，b，Bi Zhuorana，b ，Li Zongdaoa，b，Li Qingdua

（a.Institute of Machine Intelligence，b.School of Medical Instrument & Food Engineering，University of Shanghai for Science & Technology，Shanghai 200093，China）

Abstract：Aiming at the redundancy characteristics of the redundant manipulator and the relevant RRT* algorithm had issues with low search efficiency，unstable convergence and not fully considering the influence of the geometric configuration and self-motion characteristics of the redundant manipulators end effector on path planning in applications for planning the end-effector path，this paper proposed an improved strategy.Firstly，this paper introduced a sampling point selection method based on the angle between the root and end node connection，along with the establishment of a target proximity region.This method dynamically switched between improved sampling methods and random sampling methods based on the number of successful continuous samples.Subsequently，this paper combined the bidirectional tree expansion strategy with the aforementioned method.Finally，it reconnected the initial feasible path twice to obtain the optimized path.Through verification，it improved that the bidirectional tree RRT* method could effectively enhance search efficiency，convergence，and path optimality.The introduction of virtual collision bodies and capsule collision bodies could also effectively respond to the impact of the structure and motion characteristics of the end effector of the redundant manipulator.This paper used the Mujoco physics simulation engine to validate the movement of the redundant manipulator.This paper demonstrates that this strategy can plan a more optimal feasible end-effector path for the redundant manipulator.

Key words：redundant manipulator；RRT*；end-effector path；root and end node；target proximity region；bidirectional tree expansion；virtual collision bodies；capsule collision bodies；Mujoco

0? 引言

在眾多類型的機器人研究項目中，機械臂因其廣泛的應用場景、較強的環境適應能力，成為該領域的研究熱點。例如人形機器人、移動操作機器人以及一般的工業場景都有機械臂的身影。近年來，冗余機械臂因其靈活的操作性，也為機械臂的運動學算法提供了新的思路。

機械臂的末端路徑規劃算法一直是機械臂應用領域中的關鍵問題。常用的路徑規劃算法有D*算法以及A*算法。但是D*算法過于依賴初始路徑的選擇，且容易受到維度的懲罰。A*算法依賴于啟發函數的選擇且較難適應障礙物較多的場景。而RRT類算法憑借全局搜索性、對高維空間的兼容性以及較優的避障性能，更適合用于機械臂末端的路徑規劃。

代偉等人［1］引入基于sobol序列的采樣點生成方式來代替傳統隨機采樣，雖然采樣點分布更均勻，但不利于在較為復雜的障礙物環境下搜索路徑。周益邦等人［2］根據擴展節點至目標點的距離為代價對節點擴展進行取舍，并優化了重布線過程，改良了雙向RRT*-Connect算法，在效率與生成的路徑長度方面得到了提升，但在復雜環境下有可能會找不到路徑。劉建宇等人［3］引入基于概率的目標偏置采樣，結合前后采樣點夾角約束提出了一種改進雙向的RRT*方法，但是實驗環境是基于二維地圖的，沒有拓展到三維空間。孫立新等人［4］提出了根據采樣點距離樹中最近節點的距離動態調節擴展步長，可以有效地降低節點的密集程度，但是該方法容易錯過一些可行路徑。Karaman等人［5］提出了在隨機樹擴展過程中加入重選父節點與重布線的過程，所得路徑得到了明顯優化，但是算法效率較低。Tahir等人［6］采用啟發的方式生成樹的擴展點。Klemm等人［7］提出了RRT*-Connect算法，結合了RRT*與RRT-Connect算法。Zhang等人［8］引入了節點自調節機制，減少了對搜索空間的無效搜索。Wang等人［9］提出了NC-RRT算法，該方法盡量縮小了采樣空間，避免獲得較為曲折的路線，但是縮小采樣空間的同時限制了算法隨機性，障礙物較多時易出現難以收斂的情況。

本文提出了一種根據連續采樣成功次數動態切換基于根尾節點連線夾角的采樣點選擇策略與隨機采樣策略的方法。該方式可以在障礙物密集的情況下，通過在采樣區域的純隨機采樣繞過障礙物，避免了局部收斂問題。在障礙物較少的情況下連續采樣成功后，切換至基于根尾節點連線夾角的采樣點，選擇策略較快的靠近目標點，并設定目標點逼近區域，當有節點進入，將采樣區域設置為逼近區域。采用起點與終點的雙樹擴展方式，每棵樹按以上方式擴展，極大地提升了算法的收斂速度。對所得的初始路徑進行二次重連，得到一條冗余節點較少的執行路徑。在機械臂路徑規劃過程中，引入膠囊碰撞體與虛擬碰撞體，對機械臂末端路徑進行合理約束。

1? 冗余機械臂末端路徑規劃注意點

1.1? 采樣空間

移動機器人路徑規劃一般是基于二維平面的。相比之下，機械臂的末端路徑是在三維空間中進行規劃。應用于二維平面中的RRT類方法的采樣點區域一般是選擇能包絡工作平面的矩形區域（每個節點與采樣點用二維坐標（x，y）的形式給出）。應用于三維機械臂運動空間中的RRT類方法的采樣點區域，是選擇能包絡工作空間的立方體區域（每個節點與采樣點用三維坐標（x，y，z）的形式給出）。

1.2? 針對冗余機械臂的路徑規劃

機械臂的路徑規劃分為機械臂末端與機械臂連桿的規劃。對于非冗余機械臂，可以將機械臂連桿的規劃與末端規劃相結合，在算法擴展末端路徑點時的障礙物判別條件中加入連桿的碰撞判斷［4］，或是通過粒子群等優化算法直接搜索出一系列滿足末端路徑與連桿無碰撞的關節序列［10，11］。這些方式效率較低且會出現無法收斂的情況。冗余機械臂因其冗余度使其每一個末端位姿對應的機械臂構型有無數種，利用零空間無窮的性質在執行末端路徑時，通過零空間自運動實現連桿避障［12］。這時將末端路徑規劃與連桿路徑規劃分離，要求末端路徑生成算法的速度快、路徑較優的收斂性好（不抑制算法隨機性），以及能盡量避開奇異位置，且能在末端路徑規劃過程中考慮末端執行器與末端連桿幾何構型的碰撞，從而減少連桿路徑規劃的碰撞檢測計算過程。

1.3? 虛擬碰撞體應對不可達與奇異末端位姿

當機械臂末端距離障礙物較近且靠近臂身時，算法為機械臂末端規劃出的路徑，有時會出現距離機械臂臂身過近，導致奇異位姿或超出其工作范圍的情況［13］。在規劃過程中，通過創建一個尺寸合適的虛擬障礙物，可以有效地引導路徑在符合機械臂運動特性的范圍內延伸。冗余機械臂的末端工作形狀大致是環形區域，所以加入以機械臂基坐標系原點為底面圓心的圓柱碰撞體是較為適合的。如圖1所示，如果按圖1（a）所示的末端路徑進行運動，機械臂極易出現奇異位姿或超出工作范圍，從而導致規劃失敗。通過加入虛擬碰撞體，引導路徑向更合理的方向擴展，提高規劃成功率。

1.4? 以路徑線為軸心的膠囊狀碰撞體包絡末端

由算法得到的機械臂末端路徑是機械臂末端坐標系原點的路徑。末端在笛卡爾空間中的運動需要通過逆運動學分析將其映射到關節空間，并且機械臂末端規劃還包含末端的姿態。這時，受機械臂末端連桿與末端執行器構型的影響，很有可能出現末端坐標系原點軌跡無碰撞，但是末端的幾何結構與障礙物發生接觸的問題。所以在規劃機械臂末端路徑時，光做直線與障礙物的碰撞檢測是不夠的，需要為路徑直線也加上合適的碰撞體。本文選用以路徑為軸的膠囊狀碰撞體包絡路徑線。膠囊碰撞體能更好地契合路徑的拐點，與路徑線的擬合度較高。如圖2所示，本文添加了長度為0.143的robotiq2f85型號末端夾爪，根據機械臂末端連桿長度為0.15，所以選擇膠囊碰撞體的球半徑為0.15。本文通過調用FCL庫實現膠囊碰撞體與障礙物的碰撞檢測，其中需要確定的是膠囊碰撞體的位姿問題。如圖2（b）所示，在world坐標系中，表示capsual1中心位置需由式（1）計算。

capsual1_center=p0+（p1-p0）2（1）

由于膠囊由圓柱體與兩個半球體組成，屬于對稱幾何體，只需要保證其固連坐標系的z軸與路徑線方向平行即可，x、y軸可通過向量叉乘的方式得到在world中的表達。以capsual2舉例。

首先設置任意一個單位向量v1，并將z軸方向定義為路徑線朝向且單位化。通過z軸與向量v1叉乘得到必垂直于z軸的向量，再對其單位化得到x軸。此時圓柱體坐標系的x、z軸相對于world的表達已得到，直接將z軸叉乘x軸即可得到y軸。將x、y、z進行列排即可得到對應的姿態變換矩陣。兩個半球體可以膨脹為兩個完整的球體與圓柱體，組合為膠囊體，只需將路徑節點作為球體中心位置即可，無須考慮球體姿態。圓柱體在world中的姿態可以由式（2）算出，并作為參數傳入FCL函數。

v1=［0，1，0］

z=p2-p1‖（p2-p1）‖

x=z×v1‖z×v1‖（x⊥z且x為單位向量）

y=z×x‖z×x‖（y⊥x⊥z且x、y、z均為單位向量）（2）

1.5? 最后與終點的連線

RRT和RRT*算法最后到達終點的判斷條件是，若節點至終點的歐氏距離小于一定范圍，則直接相連。這個范圍如果選擇很小，則可能導致算法難以收斂，如果選擇過大，則會出現最后的路徑與障礙物發生碰撞的情況。通過將判斷條件改為節點與終點是否能無碰撞連接作為算法結束條件，能避免上述問題。雙樹擴展也可以規避這個問題。

2? 算法介紹

2.1 ?RRT*

RRT*算法是在基本RRT算法的基礎上加入了重選父節點和重布線的過程。大致思路是先將路徑起點作為搜索樹上的根節點，在給定的搜索區域中隨機選取一個采樣點xrandom，并找到當前隨機樹上與采樣點xrandom之間的歐氏距離最小的節點xnear，然后以xnear為起點，按一定的步長沿著xnear→xrandom方向延伸得到xextension，最后對xextension與xnear的連線進行碰撞檢測，如果沒有發生碰撞，則將xextension加入隨機搜索樹中，并設置xnear為xextension的父節點。搜索樹中加入xextension后，以 xextension為中心按照一定范圍找到與之鄰近的節點集xnearSet，以路徑代價為評判標準，分別對xextension和xnearSet重選父節點與重布線。具體流程如下：

a）將起點xinit加入搜索樹。

b）在采樣區域內隨機選取一個采樣點xrandom 。

c）在搜索樹中找到與隨機采樣點之間歐氏距離最小的節點xnear。

d）按設定的步長以xnear為起點并沿著xnear→xrandom的方向進行擴展，得到擴展點xextension。

e）判斷xextension與xnear的連線有沒有觸碰到障礙物。如果碰到障礙物則舍棄xextension并返回步驟b）。如果沒有，則將xextension加入搜索樹，并設置其父節點為xnear。

f）在以xextension為中心的給定范圍內，找到目前搜索樹上所有在這個范圍內的節點形成xnearSet集合。分別計算存在于xnearSet的節點以及當前xnear回歸至起點xinit的路徑歐氏距離和，并分別加上xextension與xnearSet的各個節點和xnear之間的歐氏距離。找到歐氏距離和最小的節點xmin，將xextension的父節點重選為xmin，達到重選父節點的效果。

g）若xnear不是xmin，則在重布線過程中的xnearSet集合中不考慮xmin這個節點。分別將xnearSet中的其他節點與xextension之間的歐氏距離與xextension回歸至起點xinit的歐氏距離和相加。如果該歐氏距離和小于對應xnearSet中的節點回歸至起點xinit的歐氏距離和，那么將對應xnearSet中節點的父節點重新選為xextension，達到重布線的效果。

h）判斷xextension與終點xend的歐氏距離。如果小于給定值，就直接連接xextension與xend，搜索結束；如果大于或等于給定值，則返回步驟b）繼續擴展采樣（本文將節點與終點是否能無碰撞連接作為算法結束條件）。

圖3、4為重選父節點與重布線示意圖，路徑上的數字為節點間的歐氏距離。

2.2? 改進雙樹RRT*算法

2.2.1? 采樣點的條件選擇策略

一般將目標點按一定概率作為采樣點的偏置采樣策略，或者在目標區域內按目標點擴展的方式，在障礙物不密集的情況下效率的確有所提升，但是在障礙物較為密集的情況下，該方法極有可能導致局部極小的出現。

本文在搜索過程中定義一個累加器ACC并初始化為0。在隨機樹的擴展過程中先采用隨機采樣策略，當采樣點擴展成功后，將累加器ACC進行累加。如果連續采樣成功，則ACC也連續累加。直到一次采樣不成功，直接將ACC重置為0。當ACC超過給定的閾值后，則代表目前區域的障礙物密集程度不高。這時就切換至根據采樣點xrandom與xinit的連線和xinit與xend的連線角度選擇采樣點的生成方式，如式（3）所示。

SampleMethod=RandSampleACC≤β

ImporvedSampleACC>β（3）

范圍偏置采樣如圖5所示（三角為采樣點）。

如圖5（a），當采樣點在區域外時，直接設xend為采樣點進行擴展。如圖5（b），當采樣點在區域（二維空間中為扇形，三維空間中區域為圓錐）內時，則按原采樣點擴展。該方式可以讓隨機樹偏向目標點擴展的同時減小目標點的偏置影響，增加隨機樹擴展的可能性。在障礙物較多與較少的情況下均有不錯的適應性。如圖5（c），隨機樹起點處的一個較大障礙物導致剛開始擴展時的成功率較低，累加器會低于閾值。此時采用隨機采樣繞過障礙物后，障礙物較為稀疏，采樣成功率提高，此時會切換至上述非隨機采樣策略向目標點靠近，如式（4）（5）所示。

θ=arcos（（xend-xinit）·（xrand-xinit）‖（xend-xinit）‖·‖（xrand-xinit）‖）（4）

xrandom=xrandθ≤α

xendθ>α（5）

目標區域采樣如圖6所示（三角為采樣點。虛線框為以xend為中心的逼近區域）。在隨機樹擴展的初期，xrandom在給定區域中隨機選取。以xend為中心，構建一個逼近區域。當最新的xextension進入了該區域，則之后的xrandom均在這個逼近區域內選取，在逼近區域中，扇形或圓錐體區域占比更高，從而可以引導路徑以更快且不失隨機性的特性到達xend。如圖所示，當框內左上角有節點擴展進入時，采樣區域將被縮小至該區域內，并繼續按上文的采樣策略采樣。

改進采樣后的單樹擴展算法可以表示為如下偽代碼形式：

由于RRT*算法的重選父節點與重布線過程需要計算節點回歸至起點的歐氏距離和。本文將節點封裝為第二行的結構體形式，每次擴展后更新當前節點的三維位置、父節點在樹中的索引以及當前節點與其父節點的歐氏距離。在重選父節點以及重布線過程中動態更新當前節點與范圍內節點的結構體數據。在回歸過程中只需進行加和運算即可，無須重復進行開根運算，提升算法速度。

1? ACC←0；α←value1；β←value2；O2S←xend-xinit；dstep=value3；isclose=0；

2? Tree={}；Tree.add（xinit，distance，father）；

//節點樹結構體（三維位置，與父節點歐氏距離，父節點編號）

3? while（1）

4? if isclose==0

5??? xrandom=rand（GobalArea）；

6? else

7??? xrandom=rand（NearArea）；

8? if ACC>β：

9??? if arcosO2S·（xrandom -xinit ）‖O2S‖·‖（xrandom-xinit）‖ > α

11????? xrandom=xend

12? find min（‖（xrandom-xi）‖）（xi∈Tree.x）；xnear=xi；xextension=xnear+（xrandom-xnear）·dstep

13? if collision（line（xextension-xnear），collisionobj）==false

14??? ACC←ACC+1；Tree.add（xextension，distance（xextension，xnear），xnearID in Tree）；

15??? Repet_Select_Father（xextension，Tree）；

//在重布線與重選父節點函數中

16??? Repet_Select_Line（xextension，Tree）；

//根據重連結果更新相應節點的distance與father

17??? if （xextension in NearArea and isclose==0）

18????? isclose=1；

19??? if（collision（line（xextension-xend），collisionobj）==false）

20??? break；

21? else

22??? ACC←0；

23? end

2.2.2? 雙樹擴展

分別定義起點為xinit與xend的兩棵隨機樹Tree1、Tree2，并定義一個切換變量switch。Tree1以xend為終點，Tree2則以xinit為終點。先對Tree1中的節點進行擴展，如果發生碰撞，則重新嘗試擴展，直到擴展成功后就取反switch切換至Tree2進行擴展。當Tree1和Tree2最新擴展的節點能無碰撞聯通后，就獲得了一條初始可行路徑，如圖7所示（三角為Tree1的采樣點，五角為Tree2的采樣點）。

2.2.3? 二次重連

該過程是在算法得到一條可行路徑后，對路徑的二次優化。將獲得的可行路徑節點存入Road1，并將Road1的第一個節點存入空路徑Road2中，定義兩個指針（pointinit、pointend）分別指向Road1的第一個節點與最后一個節點，判斷pointinit指向的節點與pointend指向的節點之間是否可以無碰撞聯通。若是，則將pointend當前指向的節點加入空樹Road2，接著將pointinit指向pointend所指向的節點。若發生碰撞，則將pointend指向當前指向節點的上一節點繼續碰撞檢測，直到pointend指向了pointinit當前指向節點的下一個節點。如此往復，當pointinit指向了Road1的最后一個節點時，得到二次重連后的路徑Road2，如圖8所示。將得到的初始可行路徑重連后得到了一條更優越的路徑。

2.2.4? 結合后的改進雙樹RRT*算法

將2.2.1～2.2.3節的方法結合至RRT*方法后的三維空間路徑搜索，具體偽代碼如下所示。

1? ACC1←0；ACC2←0；α←value1；β←value2；switch←1；

2? O2S←xend-xinit；dstep=value3；isclose1=0；isclose2=0；

3? Tree={}；TreeS={}；TreeE={}；

4? TreeS.add（xinit，distance，father）；TreeE.add（xend，distance，father）；

5? while（1）

6??? if switch==1：

7??? if isclose1==0：

8????? xrandom=rand（GobalArea）；

9??? else

10????? xrandom=rand（NearArea1）；

11??? Tree=TreeS；

12??? if ACC1>β

13????? if arcos（O2S·（xrandom -xinit ）‖O2S‖·‖（xrandom -xinit ）‖）>α

14??????? xrandom=xend

15? else

16??? if isclose2==0：

17????? xrandom=rand（GobalArea）；

18??? else

19????? xrandom=rand（NearArea1）；

20??? Tree=TreeE；

21??? if ACC2>β

22??? if arcos（-O2S·（xrandom -xend ）‖-O2S‖·‖（xrandom -xend ）‖） > α

23??? xrandom=xinit

24? find min（‖（xrandom-xi）‖）（xi∈Tree.x）；xnear=xi；xextension=xnear+（xrandom-xnear）·dstep

25? if collision（line（xextension-xnear），collisionobj）==false：

26??? if switch==1：ACC1←ACC1+1；else：ACC2←ACC2+1；

27??? Tree.add（xextension，distance（xextension，xnear），xnear ID in Tree）；

28??? Repet_Select_Father（xextension，Tree）；

//在重布線與重選父節點函數中

29??? Repet_Select_Line（xextension，Tree）；

//根據重連結果更新相應節點的distance與father

30??? if switch==1：if （xextension in NearArea1 and isclose1==0）：isclose1=1；

31??? else

if（xextension in NearArea2 and isclose2==0）：isclose2=1；

32??? switch=-switch

33??? if（collision（line（TreeE.xextension-TreeS.xextension）， collisionobj）==false）

34????? break；

35? else

36??? if switch==1：ACC1←0；else：ACC2←0；

37? SecondaryReconnection（PathSplicing（TreeS，TreeE））；

38? end

3? 改進算法對比實驗

對比實驗部分基于ubuntu20.04版本的Python3.8.10環境，算法對比實驗部分的球形碰撞檢測算法實現參考文獻［11］。

3.1? 改進采樣策略的有效性

為驗證改進采樣策略在隨機樹搜索過程中是否有效，設置采樣步長為0.1，θ設為π/12，起始點為［200 -50 300］，目標點為［200 2000 700］，累加器閾值設為5，逼近區域設置為以隨機樹終點為中心的半邊長為200 mm的正方體區域。如圖9（三角為在夾角小于θ的區域內的采樣點，紅色虛線為采樣擴展延長線，綠點為樹中的非路徑點，參見電子版）所示，起點處障礙物較密集，這時算法主要以采樣空間隨機采樣為主。待越過障礙物后，采樣成功率提高，累加器超過閾值后，便開始向目標點或三角采樣點擴展。待再次遇到障礙物后，繼續隨機采樣，越過障礙物后與目標點無障礙連通。

3.2? 加入改進采樣策略的單樹RRT*法與其他算法對比

RRT*為方法1，按0.5的概率直接將目標點偏置為采樣點的RRT*算法為方法2，RRT*-connect算法［7］為方法3，改進雙向RRT*-connect算法［2］為方法4，改進雙向RRT*-connect算法［3］為方法5，加入改進采樣的RRT*算法為方法6。采樣空間為0≤x≤600，-200≤y≤700，0≤z≤650的立方體。擴展步長均設置為0.3。重布線與重選父節點的區域為以xextension為球心，半徑為800 mm的球體。累加器給定閾值為5。θ設為π/8。逼近區域設置為以隨機樹終點為中心的棱長為200 mm的正方體。場景中共有19個半徑為100 mm的球形障礙物。路徑起點為［0 0 0］，終點為［350 700 700］。運用這三種方法分別搜索100條可行路徑。表1為方法1～6的對比。圖10為各方法的路徑與采樣點分布圖。可以看出，加入改進采樣方式的單樹RRT*方法在采樣點個數、搜索時間以及路徑長度上表現良好。

3.3? 改進雙樹RRT*法

改進雙樹RRT*算法為方法7，運用改進雙樹RRT*算法在相同的條件下獲取100組可行路徑。其與方法1～6的對比如表2所示（其中綠色三角為擴展Tree1的采樣點，藍色五角為擴展Tree2的采樣點，參見電子版）。表2為方法1～7的對比。圖11為方法7獲取的一組路徑與采樣點分布圖（黑色節點為初始可行路徑的冗余節點，綠色細線為冗余路徑）。可以看出，加入雙樹擴展與二次重連之后的改進法在采樣點個數、搜索時間以及路徑長度上有更好的表現。

4? Diana7 Med機械臂末端運動仿真

Diana7 Med是一款由思靈公司自主研發的具有關節偏置的高性能七自由度冗余機械臂。仿真環境基于ubuntu20.04版本的Python3.8.10、ros-noetic、python-fcl、orocos-kdl 1.5.0、PyKDL、mujoco2.3.1。

4.1? 機械臂運動學分析

由于該機械臂關節與關節之間存在相對于x、y軸的偏置較多，故采用DH參數法建模會導致構建的末端坐標系與機械臂實際末端有偏差，所以本文通過將該機械臂的URDF文件導入至KDL庫建立運動鏈，完成對機械臂的運動學建模。將URDF文件導入至rviz可視化后，如圖12所示。

在實驗中運用了KDL庫作逆運動學解算，以及笛卡爾空間中的直線位置插值。Mujoco環境中的機械臂位置控制采用PD控制器，輸入關節位置，輸出關節扭矩。由于歐拉角插值會發生突變［14］，軸角插值過程中軸具有不可控性［15］，所以實驗中選用了四元數球面插值法進行姿態的平滑插值。如式（6）所示，其中t∈［0，1］，θ=arcos（p·q），p為初始姿態的單位四元數表達，q為目標姿態的單位四元數表達。通過離散t進行姿態插值。

Slerp（p，q，t）=sin［（1-t）θ］p+sin（tθ）q

sin θ（6）

4.2? 實驗場景構建

在實驗中共選用了兩個球形障礙物與五個圓柱體障礙物（其中有一個為1.3節所述的虛擬障礙物）。

Mujoco中的仿真環境如圖13所示，其中夾爪型號為robotiq2f85。虛擬碰撞體為半高0.6 m、底面半徑為0.3 m的圓柱體。

4.3? 機械臂末端路徑規劃

規劃過程中在碰撞檢測環節為路徑添加膠囊狀碰撞體，其膠囊高度為路徑長度，兩端球半徑為0.15 m。碰撞體檢測環節調用FCL碰撞檢測包的多碰撞檢測接口實現。采樣空間為0≤x≤0.6，-0.68≤y≤0.6，0.15≤z≤1.13的立方體空間。累加器給定閾值5，擴展步長為0.8，θ設置為π/10。逼近區域設置是以終點為中心的棱長為0.2 m的正方體。起點為［0.3698 -0.5987 0.1298］，終點為［-0.0827 0.7892 0.3678］。姿態由四元數來表示，起始姿態為［-0.1546 -0.2748 -0.4653 0.8271］，目標姿態為［0.2959 -0.2677 0.6800 0.6152］。規劃出的路徑如圖14所示。

基于Mujoco環境下的機械臂運動仿真路徑如圖15所示，可以看出，機械臂末端按圖14規劃出的路徑進行末端運動。

5? 結束語

考慮到冗余機械臂相比于非冗余機械臂有著零空間無窮的特性，使得其可以通過靈活的零空間自運動［11］進行連桿避障，從而使得末端路徑規劃與連桿路徑規劃分離。本文基于RRT*算法，結合基于根尾節點連線夾角的采樣方式，目標逼近區域的設置，雙樹擴展的機制以及可行路徑的后處理提出了一種改進雙樹RRT*方法。經實驗證明，該方法在采樣點個數、搜索時間與所得路徑長度上相比于相關改進算法有不錯的表現。

通過為末端執行器與末端連桿加入膠囊包絡體，可以使得冗余機械臂在末端路徑規劃過程中處理掉一部分碰撞體，從而減少連桿在零空間中避障時的碰撞檢測環節，且能得到相對更安全的末端路徑。根據場景與機械臂自身運動特性構建合適的虛擬碰撞體，可以使路徑向更符合機械臂運動特性的方向延伸。

下一步將為該算法獲得的末端路徑加入基于樣條插值的平滑處理，并結合相關獲取冗余機械臂零空間的方法（如幾何臂角法、QP迭代法等）形成一整套機械臂運動避障策略。算法調試穩定后，將遷移至結合三維激光雷達與實物機械臂的實物平臺進行進一步驗證。

參考文獻：

［1］代偉，李創業，楊春雨，等.基于低差異序列與快速擴展隨機樹融合算法的機械臂路徑規劃［J］.控制理論與應用，2022，39（1）：130-144.（Dai Wei，Li Chuangye，Yang Chunyu，et al.Robot arm path planning based on low difference sequence and fast extended random tree fusion algorithm［J］.Control Theory and Application，2022，39（1）：130-144.）

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

［3］劉建宇，范平清.基于改進的RRT～*-Connect算法機械臂路徑規劃［J］.計算機工程與應用，2021，57（6）：274-278.（Liu Jianyu，Fan Pingqing.Robot arm path planning based on improved RRT～*-connect algorithm［J］.Computer Engineering and Applications，2021，57（6）：274-278.）

［4］孫立新，高建明，高春艷，等.基于改進RRT*算法的七自由度機械臂避障路徑規劃［J］.控制工程，2023，30（9）：1592-1597，1615.（Sun Lixin，Gao Jianming，Gao Chunyan，et al.Obstacle avoidance path planning of seven-degree-of-freedom manipulator based on improved RRT* algorithm［J］.Control Engineering，2023，30（9）：1592-1597，1615.）

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

［6］Tahir Z，Qureshi A H，Ayaz Y，et al.Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments［J］.Robotics and Autonomous Systems，2018，108：13-27.

［7］Klemm S，Oberlnder J，Hermann A，et al.RRT*-Connect：faster，asymptotically optimal motion planning［C］//Proc of IEEE International Conference on Robotics and Biomimetics.Piscataway，NJ：IEEE Press，2015：1670-1677.

［8］Zhang Haojian，Wang Yunkuan，Zheng Jun，et al.Path planning of industrial robot based on improved RRT algorithm in complex environments［J］.IEEE Access，2018，6：53296-53306.

［9］Wang Xinda，Luo Xiao，Han Baoling，et al.Collision-free path planning method for robots based on an improved rapidly-exploring random tree algorithm［J］.Applied Sciences，2020，10（4）：1381.

［10］雷超帆.基于改進蟻群算法的六自由度機械臂避障路徑規劃［D］.鄭州：鄭州大學，2019.（Lei Chaofan.Obstacle avoidance path planning of six-degree-of-freedom manipulator based on improved ant colony algorithm［D］.Zhengzhou：Zhengzhou University，2019.）

［11］朱戰霞，靖颯，仲劍飛，等.基于碰撞檢測的空間冗余機械臂避障路徑規劃［J］.西北工業大學學報，2020，38（1）：183-190.（Zhu Zhanxia，Jing Sa，Zhong Jianfei，et al.Obstacle avoidance path planning of space redundant manipulator based on collision detection［J］.Journal of Northwest Polytechnical University，2020，38（1）：183-190.）

［12］周送鋼.基于零空間自運動的七軸冗余機械臂避障算法研究［D］.合肥：合肥工業大學，2022.（Zhou Songgang.Research on obstacle avoidance algorithm of seven-axis redundant manipulator based on zero-space self-motion［D］.Hefei：Hefei University of Technology，2022.）

［13］史士財，尹斌，蔣再男.一種七自由度冗余機械臂的奇異構形特征分析［J］.機械與電子，2014（10）：67-70.（Shi Shicai，Yin Bin，Jiang Zainan.Singular configuration analysis of a seven-degree-of-freedom redundant manipulator［J］.Mechanical and Electronics，2014（10）：67-70.）

［14］普亞松，張文斌，藺小軍，等.Hopf映射在機械臂四元數姿態規劃中的應用研究［J］.機電工程，2020，37（11）：1387-1392.（Pu Yasong，Zhang Wenbin，Lin Xiaojun，et al.Application of Hopf mapping in quaternion attitude planning of manipulator［J］.Electromechanical Engineering，2020，37（11）：1387-1392.）

［15］孫斌，常曉明，段晉軍.基于四元數的機械臂平滑姿態規劃與仿真［J］.機械科學與技術，2015，34（1）：56-59.（Sun Bin，Chang Xiaoming，Duan Jinjun.Smooth attitude planning and simulation of manipulator based on quaternion［J］.Mechanical Science and Technology，2015，34（1）：56-59.）

［16］張蘭勇，韓宇.基于改進的RRT*算法的AUV集群路徑規劃［J］.中國艦船研究，2023，18（1）：43-51.（Zhang Lanyong，Han Yu.AUV swarm path planning based on improved RRT* algorithm［J］.China Ship Research，2023，18（1）：43-51.）

［17］王紅莉，向國菲，朱雨琪，等.基于PS-RRT算法的機械臂避障路徑規劃［J］.計算機應用研究，2023，40（1）：42-46.（Wang Hongli，Xiang Guofei，Zhu Yuqi，et al.Obstacle avoidance path planning of manipulator based on PS-RRT algorithm［J］.Application Research of Computers，2023，40（1）：42-46.）

［18］張建冬，王東，馬立東，等.基于改進RRT算法的移動機械臂路徑規劃［J］.電子測量技術，2021，44（23）：48-53.（Zhang Jiandong，Wang Dong，Ma Lidong，et al.Mobile manipulator path planning based on improved RRT algorithm［J］.Electronic Measurement Techno-logy，2021，44（23）：48-53.）

［19］賈焦予，袁杰，李中華，等.葡萄采摘機器人終端作業路徑規劃方法研究［J］.計算機工程與應用，2023，59（14）：285-292.（Jia Jiaoyu，Yuan Jie，Li Zhonghua，et al.Research on terminal operation path planning method of grape picking robot［J］.Computer Engineering and Application，2023，59（14）：285-292.）

［20］張一帆，史國友，徐家晨.基于人工勢場法引導的Bi-RRT的水面無人艇路徑規劃算法［J］.上海海事大學學報，2022，43（4）：16-22.（Zhang Yifan，Shi Guoyou，Xu Jiachen.An unmanned surface vehicle path planning algorithm based on Bi-RRT guided by artificial potential field method［J］.Journal of Shanghai Maritime University，2022，43（4）：16-22.）

［21］Sun Yuxi，Zhang Chengrui，Sun Pengcheng，et al.Safe and smooth motion planning for mecanum wheeled robot using improved RRT and cubic spline［J］.Arabian Journal for Science and Engineering，2020，45（4）：3075-3090.

［22］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.