基于PS-RRT算法的機械臂避障路徑規劃

摘要：針對應用快速搜索隨機樹（RRT）算法進行機械臂路徑規劃時，存在采樣區域大、有效區域小、路徑冗余節點多、剪枝時間長等問題，提出一種基于分區動態采樣策略和重復區域節點拒絕機制的高效RRT路徑規劃算法PS-RRT（partitioned sampling RRT）。首先，通過PS-RRT快速規劃機械臂末端初始路徑；其次，分段檢測機械臂跟隨該路徑時的連桿碰撞情況，對碰撞路段進行帶臂形約束的第二次規劃；最后，將初始路徑和第二次規劃的路徑拼接后進行路徑裁剪。將所提方法在多種場景中進行仿真驗證，結果表明：基于PS-RRT算法的機械臂避障路徑規劃策略使得無效節點數大幅減少，可高效規劃出機械臂的無碰路徑，驗證了算法的可行性。

關鍵詞：機械臂；路徑規劃；快速搜索隨機樹；分區動態采樣；節點拒絕；路徑裁剪

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

文章編號：1001-3695（2023）01-006-0042-05

doi：10.19734/j.issn.1001-3695.2022.06.0287

Obstacle avoidance path planning of manipulator based on PS-RRT algorithm

Wang Hongli，Xiang Guofei，Zhu Yuqi，Dian Songyi

（College of Electrical Engineering，Sichuan University，Chengdu 610065，China）

Abstract：Aiming at the problems of large sampling area，small effective area，many redundant nodes and long pruning time when applying rapidly exploring random trees（RRT） algorithm to manipulator path planning，this paper proposed an efficient RRT path planning algorithm based on partitioned dynamic sampling strategy and repeated area node rejection mechanism.Firstly，this paper used PS-RRT to quickly plan the initial path of the manipulator end.Secondly，it detected the collision of the links when the manipulator followed the path in sections，and carried out the secondary planning of the collision section with arm constraints.Finally，it spliced the initial path and the second planned path，and then pruned the path.The simulation results show that the obstacle avoidance path planning strategy of the manipulator based on PS-RRT algorithm greatly reduces the number of invalid nodes and can efficiently plan the collision free path of the manipulator，which verifies the feasibility of the algorithm.

Key words：manipulator；path planning；rapidly exploring random trees；partition dynamic sampling；node rejection；path pruning

0引言

多關節串聯機械臂因具備強大的關節靈活性，已廣泛應用于集成電路、汽車裝配、物流搬運等自動化生產線領域。在這些任務中，找到一條機械臂可以從起始位置安全無碰撞地運動到目標位置的可行路徑是至關重要的。但由于機械臂連桿之間的耦合性，無法將其視為質點，規劃時需檢測每個位置的臂形碰撞情況，規劃難度較高，規劃時間較長。所以，為了減少機械臂的運動能耗與磨損，適應更多對速度有要求的應用場景，研究如何減少機械臂路徑規劃的時間成本與機械臂運動的路徑代價具有重要意義。

目前國內外學者對機械臂路徑規劃的方法和策略已經做了大量研究。文獻［1］將笛卡爾空間中的障礙物映射到C空間（configuration space）［2］，在維度較低時，該方法可快速劃分關節的自由空間與障礙空間，但隨著機械臂自由度的增加，C空間的求解復雜度呈指數級增長，將占用較多的計算時間［3］，該缺陷嚴重限制了C空間映射自由空間方法的發展。近年來，更多的學者選擇將臂形映射到笛卡爾空間中完成機械臂的路徑規劃。在笛卡爾空間中常用的路徑規劃方法有人工勢場法［4］、蟻群算法［5］、A*算法［6］等。其中，人工勢場法為局部路徑規劃方法，易陷入局部極小值，蟻群算法收斂速度慢，A*算法得到的路徑為最優，但其規劃時間較長，這些方法更多地適用于二維地圖中移動機器人的路徑規劃，在機械臂的高維空間中運算復雜，耗時增加。為解決高維空間中的路徑規劃問題，LaValle等人[7]首先提出快速搜索隨機樹算法（rapidly-exploring random trees，RRT）。該算法由于其隨機采樣機制，可以快速地在高維度的空間中采樣，無須建模，且具備探索方向概率的完整性［8］，但其擴展方向可能不在目標方向上，所以收斂速度慢。

為了降低RRT路徑規劃時采樣的隨機性，減少冗余節點數量，提高規劃效率，文獻［9，10］應用Bias-RRT算法，在RRT算法中引入目標偏置策略，搜索樹探索過程中設置一定概率P將目標點定義為隨機點，使搜索樹朝目標生長。文獻［11，12］引入權重系數，目標點和隨機點各占一定權重，搜索樹生長過程中取隨機點與目標點方向的矢量和，使規劃的路徑始終向目標點收斂。上述方法僅在目標方向上對路徑規劃有引導作用，但未有效利用障礙物信息，對如何避開障礙物，引導性不強，當環境復雜，障礙物較多時，可能陷入局部極小值，無法快速引導避開障礙物。文獻［13］根據節點與障礙物碰撞情況自適應調整偏置概率P，通過柵格儲存樹節點，結合柵格信息快速查找鄰近節點，但未充分利用柵格信息減小其采樣范圍，會產生較多無效隨機點。文獻［14］提出NC-RRT算法，利用障礙物信息將采樣范圍限制在包含目標點的大小可變的圓形區域內，該區域為整個采樣空間的子集，減小了采樣空間，避免規劃的路徑過于曲折，但該方法待設參數較多，難以獲得最佳規劃效果。文獻［15］提出Improved-RRT算法，引入回歸機制控制節點擴展方向，當新節點與父節點距離較遠，與其他節點較近時，認為搜索樹此處分支過于稠密，則放棄該新節點，避免對配置空間的過度搜索，但需要檢測每個臨時新節點與搜索樹中所有節點的距離才可判斷該區域是否稠密，耗時增加。

此外，基于采樣的路徑規劃的一個常見缺點是規劃的路徑通常由步長連接而成，冗余節點較多，會導致機械臂產生不必要的姿態變化［16］。Ratliff等人［17］使用協變哈密頓優化（CHOMP），將碰撞函數和優化函數相結合，使用梯度下降法優化路徑。Kalakrishnan等人［18］使用隨機優化方法（STOMP），即使不能計算代價函數的梯度，該方法也可以完成優化。Schulman等人［19］引入運動規劃的軌跡優化（TrajOpt），通過將軌跡優化問題表述為序列二次規劃來解決復雜的CHOMP問題。然而，這些方法通常需要很長時間才能獲得優化路徑。另外一個解決此問題的常用方法是對RRT規劃的路徑執行后處理，使用路徑貪心裁剪［20，21］以減少不必要的節點，縮短路徑長度，但路徑裁剪過程中機械臂的碰撞檢測時間占比高達90%以上［22］，裁剪速度慢，因此還需要在規劃出路徑后考慮如何快速地縮短路徑。

本文將關節空間中的高維臂形映射到三維笛卡爾空間，規劃機械臂末端路徑。首先在傳統RRT算法中加入直連碰撞檢測，避免在目標點附近振蕩錯過目標點，使該方法能在障礙物較少的環境下快速獲得無碰撞路徑；其次，加入分區采樣策略與重復區域節點拒絕機制，迭代更新采樣區域，保證采樣區域始終與規劃路徑的方向一致，該方法能在保持RRT算法隨機性的同時大幅縮小采樣范圍，即使在障礙物較多的復雜環境下也可快速獲得末端無碰撞路徑，通過貪心裁剪去除冗余節點獲得初始路徑，此時不考慮機械臂臂形；最后，檢測機械臂跟隨每段路徑時連桿是否發生碰撞，若碰撞，則以發生碰撞的上一位置為起點，以碰撞路段的末端為終點，帶入臂形約束進行第二次規劃，若無碰撞則檢測下一段路徑，直到檢測完所有路段。將初始路徑中的無碰路段與第二次規劃的路徑進行拼接，獲得機械臂從起點至終點連桿無碰撞的可行路徑，減少路徑中的節點數量，縮短路徑裁剪時碰撞檢測的時間。

1問題描述、機械臂模型與RRT算法原理

1.1問題描述

在多軸串聯機械臂路徑規劃領域，規劃維度較高，大多數規劃方法都是基于隨機抽樣的，由于算法本身的隨機性，保證了路徑探索的全局性，但將導致規劃的路徑復雜，收斂速度慢，無效采樣多，生成的路徑中無效節點也多，時間成本高，規劃后的路徑曲折，路徑代價高，不利于機械臂的直接跟隨。因此，本文將針對機械臂路徑規劃過程中，RRT方法的隨機性和路徑冗余節點較多導致的耗時高、效率低的問題進行研究。本文中時間成本主要包括采樣、碰撞檢測、路徑查找、路徑裁剪四部分耗時，路徑代價主要考慮機械臂末端行走的長度。

1.2機械臂模型

本文中六自由度機械臂由六根連桿組成，包含六個旋轉關節。利用標準D-H參數法建立坐標系，可構建機械臂運動學模型，機械臂標準D-H參數如表1所示。為了降低碰撞檢測難度，簡化計算，將六根連桿簡化為六個圓柱體，將障礙物近似為規則的長方體，將規劃的末端路徑離散成有限個路徑點，在每個路徑點處檢測六根連桿的碰撞情況。

1.3RRT算法原理

RRT算法是一種基于均勻隨機采樣的搜索樹結構的搜索算法。該算法基于一棵方向可隨機生長的構造樹，尋找一條從起點到目標點的無碰撞路徑。RRT算法在配置空間中構造了一棵搜索樹，可探索空間中的任意點，其原理如圖1所示。搜索樹從起始位置Qstart開始，將此位置作為起始節點；然后，在配置空間中生成一個隨機點Qrand，搜索樹中所有節點到該隨機點Qrand的距離，得到最靠近Qrand的節點，并將該節點命名為Qnear。將Qnear在Qrand方向延伸一個步長delta，得到一個新節點Qnew。當該新節點Qnew與障礙物無碰撞時，將該節點加入搜索樹中，搜索樹變大，若Qnew與障礙物發生碰撞，則放棄該節點，重新生成隨機點Qrand，重復上述過程，對搜索樹中的每個新節點Qnew都需考慮其與目標點之間的距離，當該距離小于或等于預設閾值后，認為已搜索到目標點，停止搜索，可反向追溯出該無碰撞路徑。若搜索次數超過設置的迭代次數，則認為路徑規劃失敗。RRT算法基本過程如算法1所示，其中T為搜索樹集合，k為迭代次數，distance為新節點Qnew與目標點goal的距離。

2PS-RRT算法

本文提出的PS-RRT算法引入動態分區域采樣機制來快速填充父節點附近區域，防止對配置空間的過度搜索。主體思路如下：a）當障礙物設置復雜時，利用目標偏置的思想將搜索樹的鄰近區域視為有效區域，將有效區域視為臨時偏置目標，使搜索樹僅在鄰近區域內采樣；b）將采樣數過多的區域標記為重復區域，控制搜索樹不在重復區域中采樣，避免搜索樹在障礙物附近陷入振蕩或局部最優；c）在障礙物簡單時，利用下文提到的直連碰撞檢測能快速找到目標位置。該方法可高效地在有效區域內采樣，規劃出可行路徑，大幅減少無效采樣點，從根源減少規劃時間，提高效率。

算法2PS-RRT算法

T←tree（Qstart，Qgoal，Obs）

for k=1 to N do

Qrand←Sample（k）；

if Activespace（Qrand） then

if！Repeatspace（Qrand） then

Qnear←Nearest（T，Qrand）

Qnew←Steer（Qnear，Qrand，delta）

if！Obstaclefree（Qnew，Qgoal） then

if Obstaclefree（Qnew，Qnear） then

T←InsertNode（T，Qnew）

end if

if ‖Qnew-Qgoal‖lt;distance then

return T

end if

end for

T′=PathPruning（T）

PathSmoth（T′）

2.1分區動態采樣

PS-RRT方法將配置空間橫軸0～300，縱軸0～1 000，豎軸0～700分為3×10×2的小立方體，共60份，根據節點坐標位置將其歸類到對應小立方體中計數，每迭代50次更新一次計數值。將沿目標方向生長的搜索樹子葉非零區域與距離其最近的零區域定義為有效區域C1，將有效區域C1沿零節點區域擴展對齊獲得有效區域C2，C1和C2合集定義為有效區域C。為了便于理解，圖2以二維網格劃分圖展示分區效果，有效區域為紫色區域（見電子版），區域編號V-1、VI-2和VI-3為子葉非零區域，VI-1、VII-2和VII-3為最近的零節點區域，兩部分區域組成有效區域C1，VII-1為擴展區域C2。搜索樹在生長時，僅認為區域C中的采樣點有效，將該區域采樣點送入下一條判斷規則，如該點是否位于重復區域。該方法的核心思想在于根據搜索樹的生長情況，動態更新有效采樣區域，使隨機點可用的概率大幅提高；同時保留RRT方法本身探索方向的完備性，使搜索樹快速朝目標點方向擴展。分區動態采樣過程如算法3所示，其中xi、yj、zh表示縱軸方向上首個節點數為零的小立方體的橫、縱、豎軸位置編號；ymax表示yj中的最大值。

算法3分區動態采樣過程

Activespace（Qrand）

if （mod（k，50）==0）

if count（T）==0

active_xi=［0，300］

active_yj=［yj-2，ymax］

active_zh=zh

end if

active_space=［active_xi；active_yj；active_zh］

if （Qrand∈active_space）

return Qrand

end if

end if

2.2重復區域節點拒絕

當上述有效區域的單個小立方體內節點數大于閾值時，則認為該區域路徑代價過高，將之定義為重復區域，該區域內的隨機點認為是無效點，不再進行碰撞檢測，避免規劃出的路徑代價過高、冗余節點過多。其核心思想在于放棄采樣節點過于稠密的區域，該區域已有過多無效節點，即使繼續采樣也極可能仍是無效的。重復區域如圖3中藍色區域VI-2所示，實際采樣范圍為紫色區域。本文中步長為30，小立方體對角線長度約337，要使搜索樹能覆蓋小立方體至少需要13個步長，因此閾值設置為13。對采樣區域進行分區處理后，路徑規劃過程如圖4所示，該圖中，藍色圓點是起始點，綠色圓點為目標點，綠色方塊為障礙物，紅色小點表示隨機采樣的點，藍色短線為搜索樹（見電子版）。可以發現，整個配置空間都有采樣點分布，因此，PS-RRT方法保留了RRT算法的隨機性，可以探索任意方向。當探索右上方地圖，節點數超出閾值后，開始僅搜索左邊有效區域，由紅色隨機點分布位置可以看出，當前時刻，區域VI-1、VI-2、V-1為有效區域，采樣數量最多，區域V-2為重復區域，搜索樹在該區域節點數超出閾值，此后該區域采樣點無效（見電子版）。

重復區域節點拒絕過程如算法4所示，其中，i2、j2、h2表示小立方體中節點數大于閾值13的橫、縱、豎軸位置編號。

算法4重復區域節點拒絕過程

Repeatspace（Qrand）

if （count（T，i2，j2，h2）gt;13）

space_x=［i2-1，i2］

space_y=［j2-1，j2］

space_z=［h2-1，h2］

end if

repeat_space=［space_x；space_y；space_z］

if （Qrandrepeat_space）

return Qrand

end if

2.3直連檢測

每當搜索樹中多一個節點Qnew，將該節點與目標點直連，判斷該條線段是否會與障礙物發生碰撞，若無碰撞，則將目標點列為搜索樹的最后一個節點，路徑規劃完畢；若發生碰撞，則放棄直連線段，繼續隨機采樣，重復算法2流程。當搜索樹中節點Qnew與目標節點距離distance小于步長delta時，認為搜索樹已擴展至目標點，將該節點與目標點直連，路徑規劃成功。最后將所得路徑進行不帶臂形約束的貪心裁剪，裁剪后的路徑節點記為關鍵點keypoint，關鍵點數量一定程度上反映了環境復雜度。

2.4局部第二次規劃

當初始路徑冗余節點較多時，路線曲折，代價高，路徑裁剪前后如圖5、6所示。假設路徑中有n個節點，則大概需碰撞檢測（n-1）×m次，m為每段路徑離散點數。若帶入機械臂關節約束對該路徑進行裁剪，耗時長，效率低。

通過上文中PS-RRT算法快速獲得末端無碰撞路徑后，逐段檢測機械臂跟隨每段路徑時連桿是否發生碰撞，若碰撞，則帶入臂形碰撞檢測，以碰撞位置的上一位置為臨時起點，該碰撞路段的末端點為臨時終點，進行局部第二次規劃，如圖7所示；若無碰撞，則檢測下一段路徑，直到獲得末端和連桿均無碰撞的待裁剪路徑。圖7相比于圖5，有效降低了路徑中的節點數量，可減少路徑裁剪時機械臂碰撞檢測的時間。將無碰撞的初始路段與二次規劃的路徑進行拼接，經過裁剪后如圖8所示，關鍵點數量與圖6相同，路徑代價接近。

3仿真結果與分析

為了驗證本文方法在機械臂路徑規劃過程中對速度的提升，本文在不同復雜度環境下對規劃算法進行比較，主要對比方法有傳統RRT［7］、Bias-RRT［10］、NC-RRT［14］、Improved-RRT［15］。該算法使用64位版本的MATLAB 2019a實現，硬件情況為：Intel CoreTM i7-10700 CPU @2.9 GHz，內存為16 GB。該操作系統是64位的Windows 10系統。

3.1PS-RRT算法有效性

為了驗證PS-RRT算法的有效性，考慮其采樣的隨機性，在每種環境地圖中均實驗30次。RRT、Bias-RRT、NC-RRT、Improved-RRT、PS-RRT五種方法在同一地圖中的規劃效果如圖9所示，圖中深藍色折線為裁剪后的路徑。可以看到紅色采樣點數大幅減小（見電子版），藍色搜索樹中節點明顯減少，將規劃時長及其他指標都進行均值處理，最后將指標對比數據整理成表2（見電子版）。可以看出，本文方法耗時僅為RRT方法的7.82%，無效節點為RRT方法的2.58%。其中，文獻［15］采用變步長擴展的方法，使搜索樹能快速向目標點生長，且生成的路徑節點較少，但需要對每個步長內部點進行碰撞檢測，增加了耗時。

為了驗證PS-RRT算法的通用性，使用PS-RRT在四種不同的環境地圖map1、map2、map3、map4中進行實驗，規劃結果如圖10所示，五種方法規劃耗時對比如表3所示。可以看出，PS-RRT方法在一般復雜度環境下，耗時較短，在復雜度較高的環境下，時間優勢明顯，效率更高。

3.2局部二次規劃方法有效性

為了驗證局部二次規劃方法能有效減少路徑節點，縮短路徑裁剪時碰撞檢測的時間，將末端路徑進行局部二次規劃，效果如圖11所示。若在獲取初始路徑后直接對其進行帶臂形碰撞檢測的路徑裁剪，效果如圖12所示。圖中，除起止點外，紫色圓點為路徑節點，可以看出，圖11中路徑節點數遠少于圖12，深藍色折線為貪心裁剪后路徑（見電子版）。RRT、Bias-RRT、PS-RRT三種方法規劃的路徑直接裁剪，與PS-RRT規劃的路徑局部第二次規劃后再裁剪需處理的路徑節點數與裁剪過程中碰撞檢測耗時如表4所示。

3.3機械臂路徑規劃測試

為了測試本文算法在機械臂上的有效性，在MATLAB建立的六自由度機械臂模型中，通過對機器人的逆運動學分析，將起始點與目標點坐標轉換為機械臂的起始與目標位姿。如果起始點和目標點在機械臂關節空間中是可達的，則使用PS-RRT算法規劃機器臂末端路徑，貪心裁剪后逐段檢測連桿碰撞情況，對碰撞段進行帶臂形碰撞檢測的局部第二次規劃，完成機械臂避障路徑規劃，機械臂運動過程如圖13所示，圖中綠色圓點為起始點，藍色圓點為目標點，綠色方塊為障礙物，深藍色折線為機械臂末端路徑（見電子版）。將上文所述的五種方法應用于圖13環境中進行10次仿真測試，路徑規劃的平均時間成本如表5所示。仿真結果表明，本文方法在機械臂的避障路徑規劃中，節點更少，速度更快。

4結束語

本文提出了機械臂路徑規劃的一種改進RRT算法，該算法具有如下四個特點：a）對配置空間進行分區域節點統計，僅在搜索樹附近采樣，提高有效隨機點概率；b）重復區域節點拒絕，避免對重復區域內的點進行多次判斷，減少耗時；c）對搜索樹中的每個節點進行直連檢測，對路徑進行目標引導，避免在目標點附近振蕩，甚至錯過目標點；d）首次裁剪末端路徑時不考慮臂形，最終路徑由初始無碰路段與第二次規劃的路徑拼接而成，大幅減少裁剪路徑時機械臂碰撞檢測的次數，提高路徑規劃效率。在多種障礙物的復雜環境下，與傳統Bias-RRT和RRT算法相比，PS-RRT算法結合局部二次規劃方法尋找可行路徑速度更快，驗證了該算法的有效性和通用性。

參考文獻：

［1］祁若龍，周維佳，王鐵軍.一種基于遺傳算法的空間機械臂避障軌跡規劃方法［J］.機器人，2014，36（3）：263-270.（Qi Ruolong，Zhou Weijia，Wang Tiejun.An obstacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm［J］.Robot，2014，36（3）：263-270.）

［2］Lozano P.Spatial planning：a configuration space approach［J］.IEEE Trans on Computers，2006，32（2）：108-120.

［3］黃獻龍，梁斌，吳宏鑫.機器人避碰規劃綜述［J］.航天控制，2002（1）：34-40，46.（Huang Xianlong，Liang Bin，Wu Hongxin.A survey on robotics collision avoidance planning［J］.Aerospace Control，2002（1）：34-40，46.）

［4］Zhou Haibo，Zhou Shun，Yu Jia，et al.Improved artificial potential field method applied for AUV path planning［J］.Mathematical Problems in Engineering，2020（1）：1-21.

［5］Liu Jianhua，Yang Jianguo，Liu Huaping，et al.An improved ant colony algorithm for robot path planning［J］.Soft Computing，2017，21（19）：5829-5839.

［6］Dolgov D，Thrun S，Montemerlo M，et al.Path planning for autonomous vehicles in unknown semi-structured environments［J］.The International Journal of Robotics Research，2010，29（5）：485-501.

［7］LaValle S M，Kuffner J J.Randomized kinodynamic planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，1999：473-479.

［8］Hsu D，Latombe J C，Kurniawati H，et al.On the probabilistic foundations of probabilistic roadmap planning［J］.International Journal of Robotics Research，2006，25：627-643.

［9］Yuan Chengren，Zhang Wenqun，Liu Guifeng，et al.A heuristic rapidly-exploring random trees method for manipulator motion planning［J］.IEEE Access，2020，8：900-910.

［10］徐娜，陳雄，孔慶生，等.非完整約束下的機器人運動規劃算法［J］.機器人，2011，33（6）：666-672.（Xu Na，Chen Xiong，Kong Qingsheng，et al.Motion planning for robot with nonholonomic constraints［J］.Robot，2011，33（6）：666-672.）

［11］李洋，徐達.基于引力自適應步長RRT的雙臂機器人協同路徑規劃［J］.機器人，2020，42（5）：606-616.（Li Yang，Xu Da.Cooperative path planning of dual-arm robot based on attractive force self-adaptive step size RRT［J］.Robot，2020，42（5）：606-616.）

［12］李季，史晨發，邵磊，等.基于改進RRT算法的6-DOF機器人路徑規劃［J］.計算機應用與軟件，2020，37（9）：221-226.（Li Ji，Shi Chenfa，Shao Lei，et al.Path planning of 6-DOF robot based on improved RRT algorithm［J］.Computer Applications and Software，2020，37（9）：221-226.）

［13］張立彬，林后凱，譚大鵬.基于柵格空間的自適應GB_RRT機械臂路徑規劃［J］.計算機集成制造系統，2022，28（6）：1638-1649.（Zhang Libin，Lin Houkai，Tan Dapeng.Manipulator path planning based on grid space-based adaptive goal bias rapidly exploring random tree star［J］.Computer Integrated Manufacturing Systems，2022，28（6）：1638-1649.）

［14］Wang Xinda，Han Baoling，Chen Yuhan，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.

［15］劉亞秋，趙漢琛，劉勛，等.一種基于改進的快速擴展隨機樹的工業機器人路徑避障規劃算法［J］.信息與控制，2021，50（2）：235-246，256.（Liu Yaqiu，Zhao Hanchen，Liu Xun，et al.An improve RRT based obstacle avoidance oath olanning algorithm for industrial robot［J］.Information and Control，2021，50（2）：235-246，256.）

［16］Guernane R，Achour N.An algorithm for generating safe and execution-optimized paths［C］//Proc of the 5th International Conference on Autonomic and Autonomous Systems.Piscataway，NJ：IEEE Press，2009：16-21.

［17］Ratliff N，Zucker M，Bagnell J A，et al.CHOMP：gradient optimization techniques for efficient motion planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2009：489-494.

［18］Kalakrishnan M，Chitta S，Theodorou E，et al.STOMP：stochastic tra-jectory optimization for motion planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：4569-4574.

［19］Schulman J，Duan Yan，Ho J，et al.Motion planning with sequential convex optimization and convex collision checking［J］.The International Journal of Robotics Research，2014，33（9）：1251-1270.

［20］王雨，劉延俊，賈華，等.基于強化RRT算法的機械臂路徑規劃［J］.山東大學學報：工學版，2022，52（4）：1-9.（Wang Yu，Liu Yanjun，Jia Hua，et al.Path planning of mechanical arm based on intensified RRT algorithm［J］.Journal of Shandong University：Engineering Science，2022，52（4）：1-9.）

［21］仲健寧，向國菲，佃松宜.針對包含狹窄通道復雜環境的高效RRT*路徑規劃算法［J］.計算機應用研究，2021，38（8）：2308-2314.（Zhong Jianning，Xiang Guofei，Dian Songyi.Efficient RRT* path planning algorithm for complex environments with narrow passages［J］.Application Research of Computers，2021，38（8）：2308-2314.）

［22］李耀仲，王書亭，蔣立泉，等.基于稀疏節點快速擴展隨機樹的移動機械臂運動規劃［J］.中國機械工程，2021，32（12）：1462-1470.（Li Yaozhong，Wang Shuting，Jiang Liquan，et al.Motion planning of mobile manipulator based on RRT with spares nodes［J］.China Mechanical Engineering，2021，32（12）：1462-1470.）

收稿日期：2022-06-01；修回日期：2022-08-03

作者簡介：王紅莉（1997-），女，四川成都人，碩士研究生，主要研究方向為機械臂路徑規劃；向國菲（1990-），男，四川巴中人，助理研究員，博士，主要研究方向為機器人智能與進化、多機器人協調、多傳感信息融合；朱雨琪（1996-），男，江蘇南通人，博士研究生，主要研究方向為機器人控制；佃松宜（1972-），男（通信作者），湖北松滋人，教授，博士，主要研究方向為先進控制、感知與人工智能算法（scudiansy@scu.edu.cn）．