基于AGD-RRT算法的機械臂路徑規劃

周楓林，趙釩地，陳騰飛，龍厚云，王瑾元，李 光

（湖南工業大學 機械工程學院，湖南 株洲 412007）

1 研究背景

隨著人工智能技術的飛速發展，工業機器人已經被廣泛地應用于汽車、船舶、航空等生產領域，面對日趨復雜多樣的作業環境，傳統人工示教編程的方式已經無法滿足當前作業生產的需求，因此，如何利用智能算法使機器人能夠快速、自主規劃一條可行的運動軌跡，靈活高效地進行作業，成為當前工業機器人的一個研究熱點。

工業機器人傳統的路徑規劃方法有基于搜索和基于采樣兩類。A*算法[1]是基于搜索路徑規劃方法的常用算法之一，其搜索能力較強，但是不適合用于高維環境下的路徑規劃；RRT（rapidly-exploring random tree）算法[2]是基于采樣的常用算法，其搜索能力較強，且無需將搜索空間柵格化，適用于不同維度環境下的路徑規劃。但是RRT算法存在采樣隨機、搜索時間較長、冗余節點較多、路徑質量較差等問題。

鑒于RRT算法在路徑規劃過程中存在的問題，許多學者對此展開了大量的研究，并提出了各種改進策略[3-4]。如顧子侶等[5]提出了一種啟發式擴展改進策略，因該策略在選擇鄰近節點時考慮了隨機點到目標點的距離，故在一定程度上提高了算法的收斂速度，但是其并沒有解決算法對復雜環境的適應性差的問題；劉紫燕等[6]提出了一種目標偏向策略，其隨機樹在拓展時以一定的概率向目標點方向拓展，因而該策略提高了算法的目標導向性，縮短了算法的搜索時間，但是概率閾值調試工作量較大；朱冰等[7]采用重選父節點與剪枝策略規劃智能汽車路徑，隨機樹在擴展時根據路徑代價給新節點重新選擇父節點，并且剪斷原先的連接，使路徑在搜索過程中達到漸進最優，但是這也增加了算法的搜索時間；辛鵬等[8]引入人工勢場法，根據當前位置的目標引力以及障礙物斥力的合力方向來決定隨機樹的生長方向，這一方法雖然提高了算法的收斂速度，但是對復雜環境的探索能力不足；鐘華庚等[9]通過約束隨機樹枝條的轉角，重選父節點剔除冗余路段，該方法雖然提高了路徑的質量，但是沒能解決算法收斂速度慢等問題；張振等[10]通過約束采樣方式，引入稀疏節點產生機制和動態采樣域策略，避免了算法對已探索區域的重復探索，從而減少了無用節點的產生，但是該算法缺乏導向性，搜索時間較長。王坤等[11]提出了一種RRTconnect 算法，該算法在起點和終點處各生成一棵隨機樹，兩棵隨機樹同時拓展直至相遇。該算法減少了算法的搜索時間，但是沒能解決算法適應能力低和路徑質量差等問題。

針對以上改進的不足，本文提出一種AGD-RRT算法，該算法采用啟發式采樣，引入了人工虛擬勢場思想[12]，以提高算法的目標導向性，減少算法的搜索時間。且在搜索結束后，對軌跡進行平滑處理，以提高路徑質量。

2 機械臂建模與碰撞檢測

2.1 機械臂運動學模型

本文以圖1a所示四自由度串聯機械臂為研究對象，其結構模型如圖1b所示，相關參數如表1所示。

表1 機械臂D-H 參數Table 1 D-H parameters of the robotic arm

圖1 機械臂運動模型與樣機Fig.1 Robotic arm motion model and prototype

根據表1所示參數及機器人相鄰兩連桿間的齊次變換公式[13]，得到如下機械臂末端執行裝置坐標系相對于基坐標系的齊次變換矩陣：

式（1）中：n、o、a3 個列向量和px、py、pz分別表示機械臂末端執行器坐標系在世界坐標系中的位置和姿態。

2.2 機械臂碰撞檢測

碰撞檢測是機械臂進行路徑規劃的重要環節之一，選取一個合適的碰撞檢測方法可以有效提高機械臂路徑規劃效率。機械臂的碰撞檢測包括如下兩個部分：一個部分是機械臂與空間障礙物間的碰撞檢測，另一個部分是機械臂工作時自身連桿的碰撞檢測。為了能夠快速準確地檢測出機械臂的碰撞結果，本研究采用包圍盒法[14]簡化碰撞檢測過程，將障礙物、機械臂關節分別用球體包圍簡化，機械臂連桿用圓柱體包圍簡化，簡化后機械臂與障礙物的碰撞檢測就變成了包絡體中心線與球心的距離判斷，如圖2a所示，若球心到中心線的空間距離d1小于包圍球半徑與圓柱體包圍盒半徑之和，則機械臂連桿與障礙物有碰撞，否則沒有碰撞。而機械臂自身連桿的碰撞檢測則只需要檢測非相鄰兩連桿中心線的空間距離d2，如圖2b所示，比較d2與圓柱體包圍盒的尺寸即可判斷是否有碰撞。

圖2 機械臂碰撞檢測原理示意圖Fig.2 Schematic diagram of collision detection principle of the manipulators

3 AGD-RRT算法

3.1 RRT算法基本原理

RRT算法的概率完備、拓展能力較強，常被用于高維空間和復雜約束下的路徑規劃中。RRT算法的基本原理如下：將起始點xstart加入隨機樹tree，作為隨機樹tree的母節點，再在地圖空間M中生成一個隨機點xrand，然后遍歷隨機樹tree，找到與隨機點最近的節點xnearest；從節點xnearest開始往隨機點方向以固定步長step生成一個點xnew，檢測點xnew與節點xnearest之間是否存在障礙物，如果存在障礙物，則重新生成隨機點xrand，若檢測兩節點間不存在障礙物，則將點xnew加入隨機樹tree中。重復以上步驟，直至隨機樹中存在某個子節點到目標點xgoal的距離小于所設置的權值r。

隨機樹的隨機拓展，保證了算法的探索能力，最終總能搜索到一條可行的路徑，但存在搜索效率低、導向性差、路徑不平滑等問題。RRT算法的詳細流程如圖3所示。

圖3 RRT算法流程圖Fig.3 RRT algorithm flow chart

3.2 自適應目標導向

標準RRT算法的采樣方式為全局均勻采樣，該方式雖然保證了算法的探索能力，但是缺乏目標導向性，算法拓展隨機，收斂速度較慢。劉奧博等[15]采用目標偏向策略提高RRT算法的導向性，該策略雖然讓采樣有了一定的偏向性，但是概率閾值依靠人工設置，需要進行反復調試，且不同地圖所對應的最優概率閾值也不同，同時也不能根據隨機樹拓展邊界的位置實時調整概率閾值。針對這一問題，本研究擬引入人工勢場法生成自適應概率閾值。依據模擬人工勢場法的虛擬勢場思想，引入目標引力函數和目標方向的障礙物斥力函數。在斥力場中，僅考慮目標方向的最近障礙物對當前位置的影響，并且不限制斥力場范圍。

引力場函數為

改進的斥力場函數為

引力為

改進的斥力為

在隨機樹拓展過程中，若引力大于斥力，則說明可以繼續向目標點方向進行拓展，此時的概率閾值增大，采樣的目標趨向性增強，而隨機性減弱；若斥力大于引力，則說明不宜繼續向目標點方向且進行拓展，此時的概率閾值減小，采樣的隨機性增強，而趨向性減弱。由此可以得到如式（6）所示的概率閾值函數表達式。

為了防止概率閾值較大而目標點方向又無法拓展導致隨機樹局部鎖死現象的產生，此處引入一種重置機制，即當遇到在目標點方向拓展失敗的情況時，將概率閾值P0重置為0，隨機樹隨機拓展，逃離鎖死位置。

在得到概率閾值P0后，以概率P0向目標點方向采樣，即以概率(1-P0)在地圖范圍內隨機采樣，隨機樹的采樣方式如下：

式中P為一個0～1 的隨機數。

3.3 貪婪收斂策略

標準RRT算法的收斂條件是，當最新添加的節點xnew與目標點xgoal之間的距離小于所設置的權值時，判定已經找到了目標點xgoal，路徑規劃完成。RRT算法概率完備，總能找到一條可行的路徑，但是權值設置是否合理對算法有著重要影響：當權值設置過小時，算法的收斂速度會顯著變慢；而當權值設置過大時，由于判斷算法是否收斂時沒有進行碰撞檢測，難以保證規劃路徑的有效性，且當目標點處于空曠區域，隨機樹中已經生成了與目標點之間沒有障礙物的節點時，隨機樹仍然以固定步長進行隨機搜索擴展，因而嚴重減緩了算法的收斂速度?？紤]到標準RRT算法收斂條件存在的不足，本文對算法的收斂條件進行了改進。貪婪收斂策略的詳細步驟如流程圖4所示。

圖4 貪婪收斂策略流程Fig.4 Greedy convergence strategy flow chart

本文對算法收斂條件的改進主要是在隨機樹每添加一個新的節點xnew后，對新節點xnew與目標點xgoal之間進行碰撞檢測，如果有碰撞，則繼續進行隨機拓展，若檢測沒有碰撞，則將目標點加入隨機樹，路徑規劃完成。

3.4 路徑優化處理

標準RRT算法規劃出的路徑冗余節點多、路線彎折、路徑代價大，最終形成的路徑質量較差。本文采用節點剔除的方式去除冗余節點，其原理如圖5所示，從初始點n(1)開始，由近及遠逐個對節點與起始點進行障礙檢測，若點n(1)與點n(i)之間無障礙物（i為整數），點n(1)與點n(i+1)之間無障礙物，則連接起點n(1)與點n(i)，將點n(i)當作新的起始點，重復上述步驟，直至連接到終點。

圖5 冗余節點剔除原理示意圖Fig.5 Redundant node removal principle diagram

經過剔除冗余節點處理后的路徑存在較多的拐點，拐點的存在會影響機械臂運動的穩定性和精度，因此需要把簡化后的路徑在拐點處進行平滑處理。本文采用三次B 樣條曲線對簡化后的路徑進行平滑處理，三次B 樣條曲線函數如下：

式中：bi,3為三次B 樣條曲線的基函數；ci為曲線的控制點，i=0,1,2,3。

3.5 AGD-RRT算法

結合以上方法得到一種AGD-RRT算法，算法流程如圖6所示。

圖6 AGD-RRT算法流程Fig.6 AGD-RRT algorithm flow chart

4 算法仿真與分析

為了驗證所提算法的有效性和優越性，設計了兩組對比試驗，分別在二維空間和三維空間中進行對比仿真驗證。在進行三維仿真實驗時，將機械臂看作一個質點，不考慮機械臂自身連桿的碰撞以及機械臂與空間障礙物的碰撞。

實驗方案如下：首先在matlab 軟件中建立仿真地圖，根據起始點與目標點的坐標，在圖中繪出其位置，然后分別使用5 種算法進行避障路徑規劃。各算法分別進行50 次仿真實驗，取其平均值作為最終結果。

4.1 二維空間仿真實驗與分析

仿真實驗所得5 種算法在二維空間的路徑規劃結果如圖7所示。

圖7 5 種算法在二維空間的路徑規劃結果Fig.7 Path planning results of five algorithms in 2D space

如圖7所示，仿真地圖大小為624 mm×438 mm，紫紅色圓點和綠色圓點分別代表起點和目標點，其坐標分別為(40，400)、(560，60)，引力系數λ1和斥力系數λ2分別取0.1,10 000，為了保證標準RRT算法和GB-RRT算法能夠順利通過地圖中的狹窄位置，完成路徑規劃，經過多次實驗，確定固定步長step取20 較為適合，仿真實驗所用計算機配置為64位Windows 10 操作系統以及AMD Ryzen 5 2600X 處理器。

圖7所示5 種算法進行路徑規劃的結果圖中，藍色和紫色線段為算法搜索過的路徑，紅色實線為最終路徑。對比圖7 中的5 個圖可知，本文所提算法生成的無效節點最少，路徑更光滑。表2 為二維空間中5 種算法的路徑規劃實驗結果。由表2 可知，在二維空間的路徑規劃對比實驗中，相較于RRT算法、RRT-connect 算法、文獻[5]算法和文獻[6]算法，AGD-RRT算法在搜索時間上分別減少了80.1%,29.6%,51.3%,45.7%，路徑長度分別減少了25.1%,23.7%,1.7%,17.8%。AGD-RRT算法的結果均優于其他4 種算法，這是由于采用了自適應的目標導向策略，引導隨機樹向目標方向快速拓展，而貪婪收斂策略防止了隨機樹在目標周圍時的盲目擴張。

表2 二維空間中5 種算法的實驗結果Table 2 Experimental results of five algorithms in 2D space

4.2 三維空間仿真實驗與分析

如圖8所示5 種算法進行路徑規劃的結果圖中，地圖尺寸為550 mm×550 mm×550 mm，球狀物體為障礙物，綠色圓點和藍色圓點分別代表起點和終點，其坐標分別為(50，50，50)、( 500，500，500)，固定步長step取30，其余參數與之前保持一致。圖8 中紅色實線為算法規劃出的最終路徑，根據對比可知，AGD-RRT算法的無效拓展相對較少，路徑較為平滑，質量較好。表3 為三維空間中5 種算法的路徑規劃實驗結果。

表3 三維空間中5 種算法的實驗結果Table 3 Experimental results of five algorithms in 3D space

圖8 5 種算法在三維空間的路徑規劃結果Fig.8 Path planning results of five algorithms in 3D space

由表3 可知，在三維空間的路徑規劃對比實驗中，相較于RRT算法、RRT-connect 算法、文獻[5]算法和文獻[6]算法，AGD-RRT算法在搜索時間上分別減少了86.0%,29.8%,56.6%,36.5%，路徑長度分別減少了26.5%,28.9%,4.7%,9.3%。從這個結果可以得知，本文所提出的改進RRT算法的性能相對于其它幾種算法更優。

5 機械臂路徑規劃實驗

機械臂的路徑規劃空間分為笛卡爾空間和關節空間。其中笛卡爾空間的路徑規劃需要對路徑點求逆解，其逆解可能存在多組情況，機械臂碰撞檢測與路徑選取較為復雜。而關節空間的每個路徑點映射到三維空間時，機械臂的位姿是確定的，其碰撞檢測較為直觀。因此，本文在機械臂的關節空間中進行路徑規劃。

考慮到機械臂的工作空間限制，僅在機械臂的前方設置了一個長方體形狀的障礙物，讓機械臂執行器的末端從起點運動到終點位置，起點和終點分別位于障礙物的兩側。起點和終點的坐標分別為（-45°,-60°,75°,5°）、(50°,-60°,80°,0°)，步長設置為15°，其余參數和以上實驗一致。

為進一步驗證所提路徑規劃方法在實際應用中有效性和可行性，通過實驗驗證，實驗方案見圖9。

圖9 機械臂路徑規劃實驗方案Fig.9 Experimental scheme of robotic arm path planning

采用標準RRT算法和改進RRT算法為機械臂規劃起始位姿到目標位姿的避障路徑，將規劃所得的路徑數據導入機械臂示教器，機械臂對路徑進行跟蹤。圖10所示為機械臂的一次避障路徑規劃過程。從圖中可以看到機械臂能夠準確避開障礙物，從起始點到達終點。重復上述實驗10 次，兩種算法均能準確地避開障礙物，但標準RRT算法的平均搜索耗時為17.74 s，平均路徑長度為237.5°，而改進RRT算法的平均搜索耗時僅為4.27 s，平均路徑長度為190.6°，這里的路徑長度是指4 個關節角度變化量之和。從這一實驗結果可知，改進RRT算法在耗時和路徑長度方面比標準RRT算法更優，可見本文所提改進RRT算法對機械臂進行路徑規劃有效可行。

圖10 機械臂避障過程圖Fig.10 Robotic arm obstacle avoidance process

6 結語

針對機械臂路徑規劃，本文以標準RRT算法為基礎，提出了一種AGD-RRT算法，解決了RRT算法搜索效率較低、適應性較差、路徑質量較差的問題，算法改進如下：

1）提出一種動態的目標偏向概率函數，以實時調整對目標點進行采樣的概率，達到自適應目標導向的效果，減少了無用節點的生成，提高了系統的收斂速度。

2）采用貪婪收斂策略，防止了隨機樹在目標周圍時的盲目擴張。

3）使用了一種簡單有效的節點拒絕方法與本文算法結合，避免了對重復區域的探索，提高了算法的收斂效率。

4）使用三次B 樣條曲線對路徑進行優化，使得機械臂各關節的運動平穩。

通過多組仿真實驗對比，驗證了AGD-RRT算法的有效性和可靠性。在機械臂的路徑規劃實驗中，相較于標準RRT算法，AGD-RRT算法的平均搜索耗時減少了13.47 s，平均路徑長度減少了46.9°。