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

陳波芝 陸 亮 雷新宇 趙 萍

合肥工業大學機械工程學院，合肥，230009

0 引言

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

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

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

1 機械臂雙臂運動學建模

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

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

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

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

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

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

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

2 改進RRT算法的描述

2.1 RRT算法的基本原理

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

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

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

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

2.2.1 利用障礙物函數建立避障環境

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

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

2.2.2 extendTreeRobot函數

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

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

本文建立的罰函數如下：

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

用相鄰兩個位姿各個關節角的變化程度表示機械臂運動平穩性的優劣，定義相鄰兩個位姿的關節角變化程度：

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

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

該子函數流程如下：

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

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

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

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

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

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

（7）建立代價函數。

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

（9）進行快速收斂。

（10）調用Robotics Toolbox中的函數使得角度一一對應6個關節的位置。

（11）調用collisionrobot避障函數避免主臂與障礙物發生碰撞。

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

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

該子函數表示如下：

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和一個空節點P while|Pstart-Pgoal|＞L do Srand=隨機節點（Sfree）Prand=ForwardKinematics（Srand）；end if optimal

2.2.3 碰撞檢測

考慮機械臂與障礙物、機械臂與機械臂是否發生碰撞時，只需考慮機械臂與包絡障礙物的球體的球心位置即可。考慮本文的機械臂與障礙物的實際大小、機械臂本體的大小，將障礙物進行膨脹，使障礙物的半徑變為1.5倍障礙物的半徑，以確保不會發生碰撞。

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

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

該子函數表示如下：

（x1，x2，x3，…，xn）表示n個關節的位置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雙臂機器人環境模型

如圖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的球體。主臂（左臂）初始狀態時，關節1～6的6個關節角均為0°，主臂的末端位置坐標為（-545 mm，0，-145 mm）。從臂（右臂）初始狀態時，關節1～6的6個關節角均為0°，從臂的末端位置坐標為（-545 mm，0，445 mm）。

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

3.2 MATLAB數值仿真

利用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為對應的俯視圖。可以看出，雙臂機器人的兩個機械臂從指定的初始位姿協調無碰的運動到達指定的目標位姿，從而驗證了算法具有有效性以及可行性。

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

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

3.3 ADAMS樣機驗證

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

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

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

4 結論

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

［1］ 劉宏，蔣再男，劉業超.空間機械臂技術發展綜述［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］ 王恒升，陳偉鋒，王思遠.基于末端桿件螺旋運動的鑿巖機械臂孔序規劃［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］ 計時鳴，黃希歡.工業機器人技術的發展與應用綜述［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］ 王美玲.面向救援任務的雙臂機器人協作運動規劃與控制方法研究［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］ 胡雨濱.移動機器人雙六自由度機械臂運動規劃［D］.哈爾濱：哈爾濱工業的大學，2009.HU Yubin.The Motion Planning of Dual 6-DOF Arms of a Mobile Robot［D］.Harbin：Harbin Institute of Technology，2009.

［6］ 康浩博.基于改進蟻群算法的機器人雙臂無碰撞路徑規劃［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機械臂避障達點運動規劃［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］ 王道威，朱明富，劉慧.動態步長的RRT路徑規劃算 法［J］.計 算 機 技 術 與 發 展 ，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］ 杜明博，梅濤，陳佳佳，等.復雜環境下基于RRT的智能車輛運動規劃算法［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機器人動態路徑規劃［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］ 關英姿，宋春林，董惠娟.空間自由漂浮機器人對運動目標抓捕的路徑規劃［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*算法的空間多自由度機械臂路徑規劃方法［J］.合肥工業大學學報（自然科學版），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.