基于改進快速擴展隨機樹算法的雙機械臂協(xié)同避障規(guī)劃方法

陳波芝 陸 亮 雷新宇 趙 萍

合肥工業(yè)大學機械工程學院，合肥，230009

0 引言

隨著科學技術的進步和現(xiàn)代工業(yè)的發(fā)展，各種類型的機械臂在各個行業(yè)得到了越來越多的應用［1-3］。對于很多實際生產(chǎn)需求而言，僅由一般的單機械臂來進行操作不足以完成相對復雜的任務。兩個機械臂相互協(xié)調(diào)、配合地完成作業(yè)的形式能夠適應工作任務中不斷提高的復雜性、智能性以及系統(tǒng)柔順性的要求，因此雙臂機器人成為研究和應用熱點［4］。雙臂機器人不同于兩個單臂機器人的簡單組合，它們在共同的工作空間內(nèi)運動時，機械臂與空間障礙物以及雙臂彼此之間可能發(fā)生干涉碰撞，導致故障，故需要對機械臂與環(huán)境之間的碰撞以及兩臂之間的碰撞進行一定程度上的配合協(xié)調(diào)處理。在雙臂機器人運動規(guī)劃的研究中，如何避免這兩類碰撞是關鍵問題之一［4-6］。

在機器人運動規(guī)劃領域中，KIM等［7］提出一種適用于裝配任務系統(tǒng)的改進快速擴展隨機樹（rapidly-exploring random tree，RRT）算法。該系統(tǒng)主要用于較困難的裝配，如非線性配合工件的裝配，既有兩個機械臂首次的抓取組裝策略，又有一個機械臂握持工件，另一個機械臂改變位置再抓取的組裝策略，采用該算法生成裝配路徑、相關路徑和再抓取路徑，可以滿足對復雜裝配的要求，但在求解相關路徑時，使用參數(shù)較多，計算十分復雜，較難應用于實際。LI等［8］提出一種將高斯分布用于RRT算法的方法，該方法將傳統(tǒng)RRT算法中的隨機采樣用高斯分布采樣代替，將目標位置設為期望值，將算法步長設為標準差，通過調(diào)整標準差的大小來調(diào)節(jié)算法的步長，進而調(diào)節(jié)算法向目標位置收斂的速度，但標準差設置不合適時，該算法會出現(xiàn)無解的情況。謝碧云等［9］提出基于雙向RRT算法的運動規(guī)劃技術，該技術主要用于7R機械臂的避障達點運動中，在雙向RRT算法給出目標位形樹時，結合末端姿態(tài)調(diào)整與7R機械臂的解析逆解來生成目標位形，但算法需要以目標位置為輸入，并且需要通過機械臂的逆運動學獲得多組目標的關節(jié)姿態(tài)，再建立雙向隨機擴展樹，計算量較大，而且不能適應于動態(tài)環(huán)境。王道威等［10］在對RRT算法進行改進的基礎上，提出了一種動態(tài)步長的RRT路徑規(guī)劃算法，該方法可以有效地降低算法的時間復雜度，但是該論文未給出動態(tài)步長的數(shù)學計算公式，其數(shù)值變化的確定主要靠經(jīng)驗。杜明博等［11］提出一種基于連續(xù)曲率RRT算法的智能車輛運動規(guī)劃算法，引入目標偏向采樣策略以及合理的度量函數(shù)以提高規(guī)劃速度，采用一種基于最大曲率約束的后處理方法來生成平滑且曲率連續(xù)的可執(zhí)行軌跡，但該方法在目標采樣策略中的閾值選擇較為繁瑣且僅限于對車輛進行運動規(guī)劃。劉成菊等［12］提出基于改進RRT算法的RoboCup機器人動態(tài)路徑規(guī)劃方法，該算法的主要特點是以一定概率選擇目標點，增加引力分量，對路徑進行平滑處理，并引入路徑緩存區(qū)生成動態(tài)擴展隨機樹，可以縮短路徑約20%的長度，但當動態(tài)障礙物距離規(guī)劃路徑的起始點和目標點較近時，算法很容易發(fā)生振蕩，導致機器人原地打轉。關英姿等［13］提出空間自由漂浮機器人抓捕運動目標的路徑規(guī)劃方法，對于視線遮擋約束問題，采用擴展卡爾曼濾波器來建立目標運動狀態(tài)估計器，在遮擋區(qū)域采用滾動RRT方法，在未遮擋區(qū)域采用比例導引算法使機械臂向目標位置不斷接近。該方法可以進行有效的運動狀態(tài)估計并快速可靠地實施抓捕，但在兩個算法切換時，容易因為目標估計的延遲而產(chǎn)生振動，導致算法失效。

本文以雙臂機器人為研究對象，在傳統(tǒng)RRT算法的基礎上，引入罰函數(shù)和碰撞檢測，通過將主機械臂的運動和位置轉化為從機械臂的動態(tài)障礙物的方法，進行雙臂機器人的協(xié)同避障運動規(guī)劃的研究，給定雙臂機器人的尺寸參數(shù)以及工作環(huán)境中的障礙信息，最終可規(guī)劃一條使得各關節(jié)角平穩(wěn)轉動的雙臂整體協(xié)同避障運動軌跡，且算法具有較快的收斂速度和較好的精度。

1 機械臂雙臂運動學建模

本文以一種六自由度雙臂機器人為例，根據(jù)D-H建模方法得到主從機械臂的關節(jié)示意圖（圖1）及其D-H參數(shù)（表1）。其中，從機械臂的關節(jié)示意圖與主機械臂的關節(jié)示意圖以機器人本體為鏡像對稱。主機械臂的D-H參數(shù)α、θ與從機械臂D-H參數(shù)互為相反數(shù)。

圖1 主機械臂關節(jié)示意圖Fig.1 Joint diagram of main robot arm

表1 主機械臂的D-H參數(shù)Tab.1 D-H parameter of main robot arm

由D-H法，我們已獲得2個機械臂的構型參數(shù)，接下來需要在算法中建立主從機械臂運動的對象。首先利用Robotics Toolbox中的函數(shù)link和robot建立雙臂的運動對象，然后建立六自由度機械臂的正運動學方程，得到機械臂6個關節(jié)的位置。

逆運動學求解主要有三個方面的問題：①逆解的存在性問題。在機械臂的工作空間內(nèi)必須存在期望位姿。②逆解的多解性問題。逆解的數(shù)目主要取決于機械臂的結構連桿參數(shù)中的連桿長度和偏距中非零項的個數(shù)。一般說來，六自由度雙臂機械人最多存在16組運動學逆解。③逆解的求解方法，包括數(shù)值解法和解析解法兩種。

對于雙機械臂機器人運動路徑規(guī)劃問題，一般都是根據(jù)其期望末端位姿進行逆解的。對多自由度雙機械臂進行逆解時，有兩個主要問題需要解決：①由于奇點位置的存在，有些位置無法進行逆解。②由于逆解的多解性，逆解不唯一的情況可能會導致機械臂有多個可行的解，而多個可行解會導致機械臂從上一個狀態(tài)到下一個狀態(tài)時，各關節(jié)角變化相差較大，使機械臂運動不穩(wěn)定。

考慮到以上逆解求解存在的問題，本文提出了一種基于改進RRT算法的雙臂機器人的運動規(guī)劃方法：將改進的RRT算法與機器人正運動學相結合來避免對逆運動學進行求解，在通過運動規(guī)劃算法求得符合目標位置精度的可行路徑之后，經(jīng)過一次末端位姿微調(diào)來消除目標誤差，使最終得到的運動路徑可以精確到達目標位置。

2 改進RRT算法的描述

2.1 RRT算法的基本原理

RRT算法［14］是一種基于隨機采樣的路徑規(guī)劃算法，在處理非完整約束的路徑規(guī)劃問題時具有相當大的優(yōu)勢。它可以將各種約束集成到算法之中，對環(huán)境要求較低，而且該算法在概率上具有完備性，理論上肯定能找到可行路徑，但其缺點是搜索過于平均，算法效率較低，且規(guī)劃出的路徑往往偏離最短路徑。針對RRT算法的缺點，本文提出了基于RRT的改進算法，用于雙臂機器人的無碰路徑規(guī)劃。

2.2 基于改進RRT算法的雙機械臂避障路徑規(guī)劃

首先，將兩條機械臂中的一條作為主臂，一條作為從臂，建立障礙環(huán)境，同時，將機器人本體也作為障礙；其次，利用路徑規(guī)劃算法，在障礙環(huán)境中規(guī)劃出主臂的一條可行路徑；再次，將主臂作為動態(tài)障礙，對從臂規(guī)劃出一條可行路徑。算法總體流程見圖2，其中，障礙物環(huán)境的建立、extendTreeRobot函數(shù)以及碰撞檢測是本算法的關鍵改進。

圖2 算法總體流程圖Fig.2 Algorithm overview flow chart

2.2.1 利用障礙物函數(shù)建立避障環(huán)境

球型障礙物的參數(shù)少（僅有球心坐標和半徑），可以降低算法的復雜度以提升其搜索效率，所以本算法采用球型包絡不規(guī)則障礙物。

主臂進行路徑規(guī)劃時，只需考慮靜態(tài)障礙物。從臂進行路徑規(guī)劃時，除了靜態(tài)障礙物，還需要將主臂考慮為動態(tài)障礙物，將主臂的各個桿件的首末端和中點作為球心，根據(jù)機械臂尺寸大小可得障礙物的半徑，建立動態(tài)障礙物。

2.2.2 extendTreeRobot函數(shù)

路徑規(guī)劃時，以障礙物空間中的機械臂的起始位姿節(jié)點為根位姿節(jié)點，通過罰函數(shù)采樣逐漸增加葉位姿節(jié)點的方式，生成一個隨機擴展樹。當隨機樹中的葉位姿節(jié)點包含機械臂目標位姿區(qū)域時，便可在隨機樹中找到一條從起始位姿到目標位姿的路徑。

本文對RRT算法的節(jié)點擴展進行了改進，主要通過對extendTreeRobot函數(shù)增加代價函數(shù)和快速收斂策略來實現(xiàn)。

本文建立的罰函數(shù)如下：

其中，G(Si,Srand)為算法已規(guī)劃的位姿Si到當前的隨機位姿Srand的路徑代價；D(Pi,Pgoal)為Si狀態(tài)下的機械臂末端的位置Pi到目標位置Pgoal的距離代價；Tnode為算法所生成的隨機樹，距離代價越小表示末端位置越優(yōu)；A(Si,Srand)為機械臂位姿Si到隨機位姿Srand的各個關節(jié)角的角度變化程度，越小表示機械臂運動越平穩(wěn)；Wd、Wa分別為末端位置的最優(yōu)性和機械臂運動的平穩(wěn)性的權重。

用相鄰兩個位姿各個關節(jié)角的變化程度表示機械臂運動平穩(wěn)性的優(yōu)劣，定義相鄰兩個位姿的關節(jié)角變化程度：

其中，機械臂位姿Si表示算法已經(jīng)規(guī)劃的位姿中的第i個位姿下，機械臂各個關節(jié)的一組關節(jié)角；隨機位姿Srand表示在隨機位姿下，機械臂各個關節(jié)的一組關節(jié)角。

由于雙臂機器人的尺寸和工作空間都較大，所以快速擴展隨機樹函數(shù)的搜索空間也很大，導致搜索效率下降。為提高算法的搜索效率，對所遍歷過的機械臂的位姿節(jié)點進行擇優(yōu)篩選。代價函數(shù)的存在使得機械臂從起始位姿到目標位姿的搜索過程可以看作是一個逐漸尋優(yōu)的過程，因而可以進行剪枝，剔除一些次優(yōu)解以便減小算法的遍歷空間，從而提高搜索效率。當機械臂使用本文算法搜索目標位姿時，先設定一個誤差范圍。當前位姿節(jié)點處機械臂的末端位置與隨機位姿節(jié)點處機械臂末端位置間的距離小于設定誤差時，就將該位姿節(jié)點加入算法的遍歷空間，反之就將其剔除。在通過運動規(guī)劃算法求得符合目標位置精度的可行路徑之后，利用一種避免碰撞的位姿調(diào)整策略［15］，對機械臂的目標末端位姿進行一次微調(diào)，消除對目標位置產(chǎn)生的誤差，使最終得到的機械臂運動路徑是可以精確到達目標位置的運動路徑。

該子函數(shù)流程如下：

（1）從機械臂6個關節(jié)中隨機選取一個關節(jié)，例如關節(jié)2，確定算法步長為5°。

（2）在-90°～90°的閉區(qū)間內(nèi)，每隔5°隨機選取一個角度，如果角度為正，就順時針轉5°，如果角度為負，就逆時針轉5°。假設隨機選取到的角度為正。

（3）讓關節(jié)2在初始角度1的基礎上順時針轉5°，把當前位置記為隨機樹子節(jié)點角度2，把初始角度1記為子節(jié)點的父節(jié)點。

（4）從6個關節(jié)中隨機選取一個關節(jié)，例如關節(jié)3。

（5）從-90°～90°的閉區(qū)間內(nèi)，每隔5°隨機選取一個角度，例如，隨機選取到30°。

（6）讓關節(jié)3在隨機樹子節(jié)點角度2的基礎上順時針轉5°，記為隨機角度1。

（7）建立代價函數(shù)。

（8）如果隨機樹子節(jié)點角度2的代價函數(shù)值最小，那么，就讓關節(jié)3在隨機樹子節(jié)點角度2的基礎上順時針轉5°，記為隨機樹子節(jié)點角度3。

（9）進行快速收斂。

（10）調(diào)用Robotics Toolbox中的函數(shù)使得角度一一對應6個關節(jié)的位置。

（11）調(diào)用collisionrobot避障函數(shù)避免主臂與障礙物發(fā)生碰撞。

（12）以此類推，直至找到的新節(jié)點與目標節(jié)點之間的距離小于給定誤差范圍時，函數(shù)結束循環(huán)。

（13）對最后一步末端位姿進行微調(diào)，到達精確目標位置。

該子函數(shù)表示如下：

Function1：extendTreeRobot Input：Sstart，Pstart，Pgoal，L，Ssatrt∈Sfree，Tnode，T Output：Spath，T

1.2.3.4.5.6.7.8.9.10.11.12.13.14.15.16.17.18.19.20.21.22.23.24.25.26.27.28.29.30.初始化一個空位姿S和一個空節(jié)點P while|Pstart-Pgoal|＞L do Srand=隨機節(jié)點（Sfree）Prand=ForwardKinematics（Srand）；end if optimal

2.2.3 碰撞檢測

考慮機械臂與障礙物、機械臂與機械臂是否發(fā)生碰撞時，只需考慮機械臂與包絡障礙物的球體的球心位置即可?？紤]本文的機械臂與障礙物的實際大小、機械臂本體的大小，將障礙物進行膨脹，使障礙物的半徑變?yōu)?.5倍障礙物的半徑，以確保不會發(fā)生碰撞。

主臂進行碰撞檢測時，需保證主臂的各個關節(jié)以及桿件中點與靜態(tài)障礙物球心的距離大于1.5倍靜態(tài)障礙物的半徑。

從臂進行碰撞檢測時，除了保證與靜態(tài)障礙物不發(fā)生碰撞，還需保證與動態(tài)障礙物不發(fā)生碰撞。所以從臂的各個關節(jié)以及桿件中點與靜態(tài)障礙物、動態(tài)障礙物球心的距離大于1.5倍障礙物的半徑。

該子函數(shù)表示如下：

（x1，x2，x3，…，xn）表示n個關節(jié)的位置R表示障礙物的半徑，C表示障礙物的球心O1=x1，O2=（x1+x2）/2，O3=x2 if‖O1-C‖<1.5…R and‖O2-C‖<1.5…R and‖O3-C‖<1.5…R then CollisionCheck（S）=1 end Function2：collision Input：（x1，x2，x3，…，xn），obstacle Output：CollisionCheck（S）1.2.3.4.5.6.7.

3 仿真實驗驗證及結果分析

3.1 MATLAB雙臂機器人環(huán)境模型

如圖3所示，代表機器人本體的球體障礙物是豎直排列以坐標（0，30 mm，150 mm）、（0，-224.2 mm，150 mm）、（0，-500 mm，150 mm）為球心、半徑為140 mm的球體，其余的2個障礙物是球心坐標（-200 mm，100 mm，300 mm）、半徑50 mm的球體，和球心坐標（-20 mm，100 mm，-300 mm）、半徑50 mm的球體。主臂（左臂）初始狀態(tài)時，關節(jié)1～6的6個關節(jié)角均為0°，主臂的末端位置坐標為（-545 mm，0，-145 mm）。從臂（右臂）初始狀態(tài)時，關節(jié)1～6的6個關節(jié)角均為0°，從臂的末端位置坐標為（-545 mm，0，445 mm）。

圖3 雙臂機器人模型Fig.3 Dual-arm robot model

3.2 MATLAB數(shù)值仿真

利用MATLAB和Robotic Toolbox模型對所提出的算法進行仿真實驗驗證。設置主臂目標點的位置坐標為（-300 mm，-250 mm，100 mm）。從臂目標點的位置為（-410 mm，-150 mm，150 mm），目標位置精度為25 mm，即當機械臂的末端位置到達離目標點的距離小于25 mm時，算法即判定機械臂已到達目標位置。

MATLAB仿真結果如圖4a、圖4c、圖4e所示，圖4b、圖4d、圖4f為對應的俯視圖?？梢钥闯觯p臂機器人的兩個機械臂從指定的初始位姿協(xié)調(diào)無碰的運動到達指定的目標位姿，從而驗證了算法具有有效性以及可行性。

圖4MATLAB仿真結果Fig.4 Simulation results of MATLAB

圖5 a所示為MTLAB仿真實驗中主臂6個關節(jié)角完成一組運動轉過的角度，圖5b所示為從臂6個關節(jié)角完成一組運動轉過的角度。

3.3 ADAMS樣機驗證

為了進一步驗證上述數(shù)值仿真的結果，在ADAMS仿真分析軟件上進行驗證。圖6a、圖6c、圖6e所示為在虛擬樣機下的一組實驗結果，圖6b、圖6d、圖6f為對應的俯視圖，確認其與障礙物不發(fā)生碰撞。

圖5 主從臂關節(jié)角變化Fig.5 Main and second arm joint angle changes

圖6 虛擬樣機仿真實驗結果Fig.6 Experimental results of virtual prototypes

4 結論

利用提出的改進RRT算法可以實現(xiàn)雙臂機器人動態(tài)協(xié)同避障的運動規(guī)劃，該算法在運動規(guī)劃過程中利用機器人正運動學求解機械臂的位置，避免了對逆運動學進行求解，并且在經(jīng)典RRT算法的基礎上加入代價函數(shù)，從而可以搜索出較優(yōu)的避障路徑。主從機械臂前后兩組動作有且僅有一組關節(jié)角發(fā)生變化（變化的值由算法的步長決定），相較于6個關節(jié)同時動作，該算法所得的運動軌跡較為平穩(wěn)。利用節(jié)點剪枝擇優(yōu)和設立目標區(qū)域的方式達到快速收斂，提高了算法的搜索效率。通過運動規(guī)劃算法求得的可行路徑符合目標位置精度后，經(jīng)過一次末端位姿微調(diào)消除目標誤差，使最終得到的機械臂運動路徑精確到達目標位置。將Solidworks中建好的雙臂機器人的模型導入ADAMS生成虛擬樣機進行仿真實驗，驗證了算法的有效性和可行性。

［1］ 劉宏，蔣再男，劉業(yè)超.空間機械臂技術發(fā)展綜述［J］.載人航天，2015，21（5）：435-443.LIU Hong，JIANG Zainan，LIU Yechao.Review of Space Manipulator Technology［J］.Manned Space Flight，2015，21（5）：435-443.

［2］ 王恒升，陳偉鋒，王思遠.基于末端桿件螺旋運動的鑿巖機械臂孔序規(guī)劃［J］.中國機械工程，2016，27（13）：1748-1754.WANG Hengsheng，CHEN Weifeng，WANG Siyuan.Drilling Sequence Planning Based on Screw Motion of End Lever of a Tunneling Rig Manipulator［J］.China Mechanical Engineering，2016，27（13）：1748-1754.

［3］ 計時鳴，黃希歡.工業(yè)機器人技術的發(fā)展與應用綜述［J］.機電工程，2015，32（1）：1-13.JI Shiming，HUANG Xihuan.Review of Development and Application of Industrial Robot Technology［J］.Journal of Mechanical ＆ Electrical Engineering，2015，32（1）：1-13.

［4］ 王美玲.面向救援任務的雙臂機器人協(xié)作運動規(guī)劃與控制方法研究［D］.合肥：中國科學技術大學，2015.WANG Meiling.Research on Cooperative Motion Plan?ning and Control of a Redundant Dual-arm Robot for Rescuing Tasks［D］.Heifei：University of Science and Technology of China，2015.

［5］ 胡雨濱.移動機器人雙六自由度機械臂運動規(guī)劃［D］.哈爾濱：哈爾濱工業(yè)的大學，2009.HU Yubin.The Motion Planning of Dual 6-DOF Arms of a Mobile Robot［D］.Harbin：Harbin Institute of Technology，2009.

［6］ 康浩博.基于改進蟻群算法的機器人雙臂無碰撞路徑規(guī)劃［D］.沈陽：東北大學，2010.KANG Haobo.Mechanical Dual-arms'Collision-free Path Planning of Robot Based on Improved Ant Colony Algorithms［D］.Shenyang：Northeastern University，2010.

［7］ KIM D H，LIM S J，LEE D H，et al.A RRT-based Mo?tion Planning of Dual-arm Robot for（Dis）assembly Tasks［C］//International Symposium on Robotics.Seoul，2014：1-6.

［8］ LI Y，SHAO J.A Revised Gaussian Distribution Sam?pling Scheme Based on RRT*Algorithms in Robot Mo?tion Planning［C］//International Conference on Con?trol，Automation and Robotics.Nagoya，2017：22-26.

［9］ 謝碧云，趙京，劉宇.基于快速擴展隨機樹的7R機械臂避障達點運動規(guī)劃［J］.機械工程學報，2012，48（3）：64-69.XIE Biyun，ZHAO Jing，LIU Yu.Motion Planning of Reaching Point Movements for 7R Robotic Manipula?tors in Obstacle Environment Based on Rapidly-explor?ing Random Tree Algorithm［J］.Journal of Mechanical Engineering，2012，48（3）：64-69.

［10］ 王道威，朱明富，劉慧.動態(tài)步長的RRT路徑規(guī)劃算 法［J］.計 算 機 技 術 與 發(fā) 展 ，2016，26（3）：105-107.WANG Daowei，ZHU Mingfu，LIU Hui.Rapidly-ex?ploring Random Tree Algorithm Based on Dynamic Step［J］.Computer Technology and Development，2016，26（3）：105-107.

［11］ 杜明博，梅濤，陳佳佳，等.復雜環(huán)境下基于RRT的智能車輛運動規(guī)劃算法［J］.機器人，2015，37（4）：443-450.DU Mingbo，MEI Tao，CHEN Jiajia，et al.RRT-based Motion Planning Algorithm for Intelligent Vehicle in Complex Environments［J］.Robot，2015，37（4）：443-450.

［12］ 劉成菊，韓俊強，安康.基于改進RRT算法的Robo?Cup機器人動態(tài)路徑規(guī)劃［J］.機器人，2017，39（1）：8-15.LIU Chengju，HAN Junqiang，AN Kang.Dynamic Path Planning Based on an Improved RRT Algorithm for RoboCup Robot［J］.Robot，2017，39（1）：8-15.

［13］ 關英姿，宋春林，董惠娟.空間自由漂浮機器人對運動目標抓捕的路徑規(guī)劃［J］.機器人，2017，39（6）：803-811.GUAN Yingzi，SONG Chunlin，DONG Huijuan.Path Planning of the Free-floating Manipulator for Captur?ing a Moving Target［J］.Robot，2017，39（6）：803-811.

［14］ LAVALLE S M，KUFFNER J J.Randomized Kinody?namic Motion Planning［J］.International Journal of Robotics Research，1999，20（5）：378-400.

［15］ 宗成星，陸亮，雷新宇，等.一種基于A*算法的空間多自由度機械臂路徑規(guī)劃方法［J］.合肥工業(yè)大學學報（自然科學版），2017，40（2）：164-168.ZONG Chengxing，LU liang，LEI Xinyu，et al.A Path Planning Approach for Multi-DOF Spatial Manipula?tor via A*Algorithm［J］.Journal of Hefei University of Technology（Natural Science Edition），2017，40（2）：164-168.