基于AGD-RRT算法的機械臂路徑規(guī)劃

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

（湖南工業(yè)大學(xué) 機械工程學(xué)院，湖南 株洲 412007）

1 研究背景

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

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

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

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

2 機械臂建模與碰撞檢測

2.1 機械臂運動學(xué)模型

本文以圖1a所示四自由度串聯(lián)機械臂為研究對象，其結(jié)構(gòu)模型如圖1b所示，相關(guān)參數(shù)如表1所示。

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

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

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

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

2.2 機械臂碰撞檢測

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

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

3 AGD-RRT算法

3.1 RRT算法基本原理

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

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

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

3.2 自適應(yīng)目標(biāo)導(dǎo)向

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

引力場函數(shù)為

改進的斥力場函數(shù)為

引力為

改進的斥力為

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

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

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

式中P為一個0～1 的隨機數(shù)。

3.3 貪婪收斂策略

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

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

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

3.4 路徑優(yōu)化處理

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

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

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

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

3.5 AGD-RRT算法

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

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

4 算法仿真與分析

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

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

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

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

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

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

圖7所示5 種算法進行路徑規(guī)劃的結(jié)果圖中，藍色和紫色線段為算法搜索過的路徑，紅色實線為最終路徑。對比圖7 中的5 個圖可知，本文所提算法生成的無效節(jié)點最少，路徑更光滑。表2 為二維空間中5 種算法的路徑規(guī)劃實驗結(jié)果。由表2 可知，在二維空間的路徑規(guī)劃對比實驗中，相較于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算法的結(jié)果均優(yōu)于其他4 種算法，這是由于采用了自適應(yīng)的目標(biāo)導(dǎo)向策略，引導(dǎo)隨機樹向目標(biāo)方向快速拓展，而貪婪收斂策略防止了隨機樹在目標(biāo)周圍時的盲目擴張。

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

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

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

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

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

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

5 機械臂路徑規(guī)劃實驗

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

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

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

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

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

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

6 結(jié)語

針對機械臂路徑規(guī)劃，本文以標(biāo)準(zhǔn)RRT算法為基礎(chǔ)，提出了一種AGD-RRT算法，解決了RRT算法搜索效率較低、適應(yīng)性較差、路徑質(zhì)量較差的問題，算法改進如下：

1）提出一種動態(tài)的目標(biāo)偏向概率函數(shù)，以實時調(diào)整對目標(biāo)點進行采樣的概率，達到自適應(yīng)目標(biāo)導(dǎo)向的效果，減少了無用節(jié)點的生成，提高了系統(tǒng)的收斂速度。

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

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

4）使用三次B 樣條曲線對路徑進行優(yōu)化，使得機械臂各關(guān)節(jié)的運動平穩(wěn)。

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