自适应双向快速密集树避碰运动规划算法

李华忠，但唐仁，唐强平

（深圳信息职业技术学院软件工程系,广东 深圳 518172 ）

自适应双向快速密集树避碰运动规划算法

李华忠，但唐仁，唐强平

（深圳信息职业技术学院软件工程系,广东 深圳 518172 ）

针对采样运动规划算法效率低，尤其在处理高维空间和复杂障碍环境等问题时，严重依赖于所选采样参数和碰撞检测距离等，提出了一种自适应双向快速密集树(ABiRDT)避碰运动规划方法。首先，深入研究了ABiRDT 算法的基础理论和实现方法，可适应调整碰撞检测距离参数和随机采样扩展步长；其次，重点研究了本算法所采用的C-空间加权均匀采样、最近邻位形查找和基于混合包围盒的并行离散碰撞检测等关键自适应策略；最后，通过三维可视化计算机仿真验证了本文提出算法的有效性。

运动规划；快速密集树；自适应算法；基于采样技术；离散碰撞检测；位形空间

自Lozano-Perez提出位形空间(Configuration Space，C-空间)概念，从而可将各类运动规划问题转化成在C-空间中寻找点的无碰路径问题以来,基于采样的规划技术已被广泛应用于高自由度智能机器人避碰和操控等领域[1-4]，成为国内外普遍关注的研究热点。该方法的显著优点是基于随机采样只需近似构造自由位形空间 ，从而避免了采用骨架网格和栅格分解[5]等方法需完整和精确构造全部有效C-空间模型，以寻找精确解析解所带来的PSPACE难题，规避了高自由度所引起的存储空间和计算量呈指数爆炸增长的NP问题[6]。最典型的两类基于采样的运动规划方法为：快速随机搜索树法（Rapidly exploring Random Tree：RRT）[7-8]和概率地图法（Probabilistic Roadmap：PRM）[9]。它们均利用采样技术在C-空间中构造树或图，然后迭代细化，直到找到连接初始位形和目标位形的无碰撞路径解。该算法的效率，尤其在处理高维空间和复杂障碍环境等复杂问题时，严重依赖于所选的采样参数和离散碰撞检测距离参数。为了解决这些问题，本文提出一种自适应双向快速密集树搜索算法，从而可通过自适应调节碰撞检测参数和扩展步长以寻找避碰运动规划的精确解。

1 相关研究

T.Sartini等学者提出了一种分辨率自适应RRT算法[10]。该算法利用分辨率自适应策略增加概率运动规划效率，进而提出自适应选取扩展步长大小的方法，从而提高了RRT规划器处理狭窄通道问题的效率。但该算法必须依赖于靠人工精心设置的初始和最后搜索步长等参数值。R.Bohlin和L.Kavraki提出了延迟PRM算法[11]。该算法最初先假设C-空间是完全避碰的，以便在预处理阶段可以快速地构建地图，将边的有效性验证转移到查询阶段，从而延迟对 表示的精化处理。该算法属于单次查询PRM法，通过减少采样阶段碰撞检测预处理过程，加快规划速度，其性能依赖于初始节点数的大小。L.Tapia等学者提出了构造PRM的智能自适应采样方法[12]。该方法避免了在基于机器学习的混合PRM[13]和特征敏感的运动规划框架[14]等方法中经常所需的手动参数调整。它演示了如何在规划过程中结合拓扑和采样自适应技术，以便学习构建地图的最佳采样策略。然而，该方法需要学习采样策略的训练集，因太复杂，必须人工设置基本路线图的几个参数。S.R.Lindemann和S.M.LaValle综述了应用于运动规划算法的启发式参数调节技术。I.Sucan 和L.Kavraki提出了基于采样的单查询运动规划实现方法。现有研究表明基于随机采样的运动规划方法，尽管在概率完备性、路径解的近似优化性、采样策略、查询方式和计算复杂度等方面具有显著优越特性，但仍存在可重复性差、参数自适应调整和算法局部性等许多有待解决的问题。例如，参数自适应调整问题，包括概率阈值的确定、总采样点数目的估计、随机步长的选择、离散碰撞检测距离参数等等，将可能影响规划的收敛速度、优化程度,甚至决定规划的成功与否[15-16]。

2 ABiRDT算法的基础理论

本文提出的自适应双向快速密集树算法ABiRDT (qs,qg)的流程图如图1所示。CBiRDT对象通过生长两棵快速密集树，一个从初始位形qs开始，另一个从目标位形qg开始，递增地构造每棵搜索树，自适应地逐步提高分辨率。

图1 ABiRDT算法流程图Fig.1 Flow chart of ABiRDT algorithm

其中，qs和qg分别表示机器人的初始位形和目标位形。CheckPathValid(Path )调用碰撞检测算法来验证路径的有效性，若发生碰撞会自适应调用SetColPara()方法将碰撞检测参数rcol减半，以便于成功通过狭窄通道或障碍物密集空间。该算法的核心在于自适应双向快速密集树动态扩展方法ABiRDTDExtend(qs,qg)的设计与实现，其算法流程如图2所示。在ABiRDTDExtend(qs,qg)算法的搜索循环中主要采用RDTExtend (T,qrand，qnew)和RDTConnect (T,q)方法扩展搜索树，通过局部自适应调整存储在搜索树每个节点的扩展参数rext，能够处理狭窄通道运动规划问题。rext初始值为|cg-cs|，若扩展失败，则该值减半；若扩展成功，则该值加倍，从而对环境中的障碍物做出条件反射，自适应调整扩大或减小扩展步长，在障碍物较少的空间加快扩展速度，在狭窄通道或障碍物较多的地方，采用较小扩展步长，确保不与障碍物发生碰撞。

图2 ABiRDTDExtend 算法流程图Fig.2 Flow chart of ABiRDTDExtend algorithm

其中，快速密集树扩展RDTExtend方法的基本思路是：首先，确定在边e上与随机采样位形qrand的最近邻位形qNN，然后通过考虑边e的局部扩展步长rext，将扩展生成的新位形qnew，限制在rext以内（如图3所示）。

图3 RDTExtend扩展方法示意图Fig.3 Schematic diagram of RDTExtend expansion method

图中蓝色qrand情况表示，当随机采样位形qrand位于扩展步长rext以内，则qnew=qrand；绿色qrand情况表示，当随机采样位形qrand位于扩展步长rext以外，则需要裁减为：。

图4 RDTExtend算法流程图Fig.4 flow chart of RDTExtend algorithm

图5 RDTConnect算法流程图Fig.5 Flow chart of RDTConnect algorithm

bool RDTExtend(T,qrand，qnew)算法流程图如图4所示。其中的快速密集树连接bool RDTConnect (T,q) 方法的流程图如图5所示。

图6 RDT Connect方法rext减半示意图Fig.6 Diagram of RDT Connect method rext to halve

该快速密集树连接方法RDTConnect (T,q)主要用于构建搜索树的新边。如果在位形之间的直线段路径是避碰的，则新边被添加到RDT中，且与边e对应的扩展步长rext加倍。否则，在从到q路径上最大有效位形的边将被添加到树中，且将扩展步长rext减半。该方法的扩展步长减半的示意图如图6所示。

3 关键自适应策略

在ABiRDT算法中采用的关键自适应策略包括：C-空间加权均匀采样、最近邻位形查找和离散碰撞检测等。

3.1 C-空间加权均匀采样

RandConfig()函数主要处理C-空间随机采样，本文采用加权采样方法，生成均匀采样，以消除C-空间不同维度在工作空间对机器人影响的差异性。为了在C-空间路径两个位形q1和q2之间进行加权均匀采样，其单位步长矢量计算公式如(1)式：

在(2)式中，权重系数wi可通过求解从机器人第i个关节开始的所有运动链K的最大距离。对运动链K，属于K所有关节k∈K 在工作空间的最大扩展是通过查找在相应表面Sk上两点qci和qcj之间最大距离和获得，其计算公式如(3)式：

因此，在两个位形q1和q2之间的路径上可均匀产生个中间采样位形qt，其计算公式如（4）式：

若将wi设置为机器人工作空间位移的最大上限，则简化计算，从而避免依赖人工设置归一化C-空间维度参数，提高C-空间采样的自适应性。

3.2 最近邻位形查找

自适应双向快速密集树（ABiRDT）规划器在自由C-空间Cfree中采用直线路径段构造搜索树，从而避免了采用基于RRT的运动规划所依赖的采样参数，其与基于RRT的方法相比较，不需要将中间采样添加到树结构中，且可能存在长边，从而使得本算法所构造的树比RRT的树要稀疏很多。通常查找最近邻位形NNeighborOnEdge的计算时间与顶点树呈线性关系。顶点数量的增加可提供运动规划求解质量，但也会增加计算时间。本文为补偿这种损失将顶点插入一个为搜索最近邻位形而建立的Kd-树数据结构中。P个位形点的Kd-树的构造过程如下。初始时，将位形点对x轴进行排序，则中值点将p∈P 分成两个集合，这取决于位形点处在通过点p的垂线的哪一侧；对其中的每一侧再根据y轴进行分类，选取中值点，通过该中值的水平线将位形点再次分割为上下两部分；在下一轮递归中，再次使用垂线、水平线，如此重复。通过循环进行n维坐标分割，相同的思路应用于Rn,将Kd-树扩展到运动规划的拓扑空间，构造P点Kd-树所需时间为O(nplg p)。设KDTree root为Kd-树对象，在Kd-树中寻找最近邻位形的Node NNeighborOnEdge ()算法流程如图7示。

图7 查找最近邻位形算法流程图Fig.7 Flow chart of NNeighborOnEdge algorithm

3.3 离散碰撞检测

由于连续碰撞检测(Continuous Collision Detection,CCD)算法难以满足运动规划的实时性要求，本文实现的碰撞检测算法isCollision()主要基于采样的离散碰撞检测(Discrete Collision Detection,DCD)思想[17-18]。DCD算法存在的两大主要缺陷：（一）存在遗漏路径碰撞的情况，特别是对狭窄通道和障碍物密集的环境，若采样步长太大，则DCD算法无法正确地检测出路径中存在的碰撞；（二）存在刺穿现象，当DCD步长过大时，机器人和障碍物之间已经发生了一定深度的刺穿才被检测到路径已发生碰撞。为了克服DCD存在的问题，ABiRDT中通过自适应调整碰撞检测参数rcol和扩展步长rext加以避免。针对运动规划对碰撞检测实时性和精确性的要求，本文采用基于混合包围盒的并行碰撞检测算法，利用方向包围盒(OBB)的紧密性和球状包围盒(Sphere)计算简单的优点来构造机器人和环境的混合包围体层次树结构，快速排除不相交的对象，并利用OpenMP模型并行遍历混合包围体层次树，从而加快碰撞检测速度。该算法的流程图如图8所示。本文研究的混合包围盒之间的相交检测主要包括：两球状包围盒间，两OBB间以及球状包围盒和OBB的相交测试。

图8 基于混合包围盒的并行碰撞检测算法流程图Fig.8 flow chart of collision detection algorithm based on Parallel Hybrid bounding box

3.3.1 两球状包围盒间的相交检测

只需要比较两球状包围盒球心之间的距离是否大于两球半径之和，其公式如(5)所示(采用平方形式)。

其中，o1和o2、1r和r2分别为两球球心和半径。若(5)式成立，则它们之间不相交，否则相交。

3.3.2两球状包围盒间的相交检测

如图9所示，假设两个OBB包围盒A和B，其边长的一半分别为ai和bi，单位轴向量分别为Ai和Bi(i=1,2,3)。A和B两中心间的距离为TAB，LSB是平行于分离轴的单位向量。rA和rB分别为A和B的三个半径在LSB方向上投影之合。只有比较|TAB·LSB|与(rA+rB)之和就能判断两OBB间是否相交，即是：

其中，LSB为15条分离轴中的一条。若前者大于后者，则两个OBB不相交；否则继续计算A和B在其他14个分离轴上的投影，并利用(6)式进行比较，直到15个分离轴的投影均不满足(6)式，则可判定A和B相交[8]。

3.3.3 两球状包围盒间的相交检测

设OBB包围盒为A，球状包围盒为B，可以将球状包围盒和OBB的相交检测看成两个OBB间相交检测的特例。用球状包围盒B的半径r代替bi，可得类似的判定公式如(7)式：

其中，LSB为OBB包围盒三个分离轴中的一条。若前者大于后者，则球状包围盒和OBB不相交；否则继续计算A和B在其他两个分离轴上的投影，并利用（7）式进行比较，直到三个分离轴的投影均不满足（7）式，则可判定A和B相交。

图9 两OBB间的相交检测Fig.9 two intersection judgment between two OBBs

4 可视化仿真

为了验证本文提出算法的有效性和正确性，作者进行了大量的仿真实验，所有仿真实验均在一台配置Intel奔腾双核G640处理器（主频为2.8GHz）、4GB内存，带3MB三级缓存的计算机上进行。编译工具为VS2008 C++。本文以Kuka机器人在复杂环境中避碰运动规划为例，对本文提出的算法进行了三维可视化仿真验证研究。首先，用三维造型绘制Kuka机器人、环境和障碍物等三维模型，并转存为Open Inventor支持的iv格式；然后，通过Visual C++导入数据模型；最后，利用本文提出的算法实现自适应双向快速密集树（ABiRDT）避碰运动规划。图10为运动规划三维可视化场景。本运动规划算法的平均执行时间为62.4秒，平均碰撞检测时间为56.4秒，平均DCD采样大小为8.4。

图10 基于 Open Inventor的机器人运动规划可视化场景Fig.10 Thr diagram of baseing on Open Inventor

5 结论

本文提出了一种自适应双向快速密集树避碰运动规划算法，利用C-空间加权均匀采样、最近邻位形查找和离散碰撞检测等策略，不必人工设置碰撞检测参数和随机采样扩展步长，而是根据碰撞检测情况，自适应调整相关参数，从而更有效地寻找到避碰运动路径，最后通过可视化仿真验证了该算法的有效性。

（References）

[1]S.Chakravorty and S.Kumar.Generalized Sampling-Based Motion Planners[J].IEEE TRANSACTIONS ON SYSTEMS,MAN,AND CYBERNETICS—PART B:CYBERNETICS,2011,41(3):855-865.

[2]I.Sucan and L.Kavraki.On the implementation of singlequery sampling-based motion planners[C].//2010 IEEE International Conference on Robotics and Automation (ICRA),may.2010:2005-2011.

[3]N.Vahrenkamp,M.Do,T.Asfour and et al.Integrated Grasp and Motion Planning[C].2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8,2010,Anchorage,Alaska,USA:2883-2888.

[4]肖国宝,严宣辉.一种动态不确定环境中机器人路径规划方法[J].计算机系统应用,2012,21(4):92-98.XIAO Guo-Bao,YAN Xuan-Hui.Path Planning of Mobile Robot in Dynamic Nondeterministic Environments[J].Computer Systems &Applications,2012,21(4):92-98 (in Chinese).

[5]LI Huazhong,LIANG Yongsheng,WANG Meini,Dan Tangren.Design and Implementation of Improved RRT Algorithm for Collision Free Motion Planning of High-Dimensional Robot in Complex Environment[C]// ICCSNT 2012,2nd Int.Conference on Computer Science and Network Technology,CHANGCHUN,CHINA:IEEE,2012 :1391-1397.

[6]Fu,Yujian;Drager,Steven.Modeling and Verification of Humanoid Robot Task Coordination[C]// High-Assurance Systems Engineering (HASE),2014 IEEE 15th International Symposium on.Miami Beach,FL,USA:IEEE,2014.

[7]S.M.LaValle,Planning Algorithms.Cambridge,U.K.:Cambridge University Press,2006,available at http:// planning.cs.uiuc.edu/.

[8]李华忠，梁永生，但唐仁等.基于RRT的机器人避碰运动规划算法研究[J].深圳信息职业技术学院学报,2012,10(3):20-27.LI Huazhong，LIANG Yongsheng,DAN Tangren,et al.Collision-free Motion Planning Algorithm Based on RRT for Robot[J].Journal of Shenzhen Institute of Information Technology,2012,10(3):20-27 (in Chinese).

[9]刘洋,章卫国,李广文.基于改进PRM算法的路径规划研究[J].计算机应用研究,2012,29(1):104-106.LIU Yang,ZHANG Wei-guo,LI Guang-wen.Study on path planning based on improved PRM method[J].Application Research of Computers,2012,29(1):104-106 (in Chinese).

[10]T.Sartini,M.Vendittelli,and G.Oriolo.A resolutionadaptive strategy for probabilistic motion planning[C].// Proceedings of the 5th Biannual World in Automation Congress,2002,(14):591-596.

[11]R.Bohlin and L.Kavraki.Path planning using lazyprm[C].IEEE International Conference on Robotics and Automation,San Francisco,2000,1:521-528.

[12]L.Tapia,S.Thomas,B.Boyd,and N.Amato.An unsupervised adaptive strategy for constructing probabilistic roadmaps[C].//2009.ICRA' 09 in Robotics and Automation.IEEE International Conference on,may 2009:4037-4044.

[13]H.Kurniawati and D.Hsu.Workspace-based connectivity oracle:An adaptive sampling strategy for prm planning[C].//Proceedings of the International Workshop on the Algorithmic Foundations of Robotics (WAFR).SpringerVerlag,2006.

[14]M.Morales,L.Tapia,R.Pearce,S.Rodriguez,and N.M.Amato.A machine learning approach for feature-sensitive motion planning[M].Algorithmic Foundations of Robotics VI,2004:361-376.

[15]S.R.Lindemann and S.M.LaValle.Current issues in sampling-based motion planning[C].//The Eleventh International Symposium in Robotics Research,P.Dario and R.Chatila,Eds.,2005:36-54.

[16]I.Sucan and L.Kavraki.On the implementation of singlequery sampling-based motion planners.//2010 IEEE International Conference on Robotics and Automation (ICRA),2010,pp:2005-2011.

[17]谢世富,马立元,刘鹏远等.虚拟环境下运动线缆碰撞检测算法研究与实现[J].系统仿真学报,2013,25(8):1865-1870.XIE Shi-fu,MA Li-yuan,LIU Peng-yuan,et al.Research and Realization of Collision Detection Algorithm for Dynamic Cable in Virtual Environment[J].Journal of System Simulation,2013,25(8):1865-1870(in Chinese).

[18]姜晓路,刘渊.基于混合包围盒的碰撞检测算法优化[J].计算机工程,2012,38(9):285-287.JIANG Xiao-lu,LIU Yuan.Optimization of Collision Detection Algorithm Based on Hybrid Bounding Box[J].Computer Engineering,2012,38(9):285-287(in Chinese).

Research on adaptive bi-directional rapidly exploring dense tree collision avoidance motion planning algorithm

LI Huazhong，DAN Tangren，TANG QiangPing
（Department of Software Engineering,Shenzhen Institute of Information Technology,Shenzhen 518172,P.R.China）

Due to algorithm efficiency of sampling-based motion planning is low,especially when dealing with highdimensional space and complex environmental obstacles,it is heavily dependent on the selected sampling parameters and collision detection distance,adaptive bidirectional balance rapidly exploring dense tree(ABiRDT) motion planning method has been proposed.First,basic theory and implementation method of the ABiRDT algorithm have been researched deeply,its including adaptive strategies include how to automatically adjust the collision detection distance parameter,random sampling expansion step;Secondly,key adaptive key adaptive strategies strategy used in the ABiRDT about C-space weighted uniform sampling,nearest neighbor configuration searching and hybrid bounding box based parallel discrete collision detection have been investigated strongly;Finally,effectiveness of the proposed algorithms have been verified by three-dimensional visualization computer simulation.

motion planning;rapidly exploring dense tree;adaptive algorithm;sampling-based techniques;discrete collision detection;configuration space

TP242.6

：A

1672-6332（2014）03-0024-07

【责任编辑：高潮】

2014-08-24

广东省自然基金（项目编号：S2013010013779）；深圳基础研究基金（项目编号：JC201006020820A，JCYJ20120615101640639）;广东省高职教育信息技术类课题（xxjs-2013-1019）；院科技创新团队项目（项目编号：CXTD2-002）；校级科研培育项目（项目编号：LG201405）

李华忠（1963-），男（汉），湖北洪湖人，副教授，博士。研究领域为机器人技术、人工智能、智能规划。E-mail：lihz@sziit.com.cn