基于改进RRT结合B样条的机械臂运动规划方法_李扬.pdf

第 卷第期计算机集成制造系统 年月 ：收稿日期：；修订日期：。；基金项目：国家自然科学基金资助项目（）；陕西省科技计划资助项目（）。：，（），（）基于改进 结合样条的机械臂运动规划方法李扬，张蕾，李鹏飞，王晓华，王文杰（西安工程大学 电子信息学院，陕西西安 ）摘要：为解决传统快速拓展随机树（）算法的随机性强，导向性差，规划时间长及寻迹平滑度差等问题，提出一种基于目标偏置策略结合自适应可变步长的改进型 算法（）。首先，在传统 算法基础上设置一个目标偏置阈值，同时引入局部扩展机制避免因改变采样结构而造成的局部最优问题；其次，结合自适应步长策略优化其搜索时间；最后，采用三次样条函数对所规划路径进行拟合优化。仿真实验中所提算法在保证机械臂成功避障且顺利抵达目标位置的同时，其各关节参数均波动较小且未发生突变，有效降低了机械臂在运动规划过程中的抖振情况。实验结果表明，所提算法较基本算法其平均路径搜索时间提高了 ，算法搜索效率及平滑性得到显著改善。关键词：工业机械臂；运动规划；快速拓展随机树算法；目标偏置策略；自适应可变步长；三次样条中图分类号：文献标识码：，（，）：，（），（），；，；：；第期李扬 等：基于改进 结合样条的机械臂运动规划方法引言伴随着全球工业与智能化制造的发展趋势以及“中国制造 ”国家行动纲领的实施，我国机器人技术正面临着从低端制造向智能化制造发展的阶段。工业机械臂作为智能机器人中的执行终端，逐渐应用于各工业领域，由于我国社会老龄化问题日益严重，将会导致未来中国劳动生产力逐步减少，必须提高平均劳动生产力，防止 增速减缓，而机械臂是代替人力的有效手段，因此对机械臂运动规划的研究愈发重要。在机械臂系统执行任务的工作区内通常会有一些物体，它们对机械臂的运动形成了阻碍空间。传统示教路径的方式在避障等操作上会增加很多负担，因此引入智能避障路径规划算法，节省作业时间的同时，也能够避免磨损，具有提高生产质量的现实意义。机械臂的运动规划是以碰撞检测为基础并规划出一条从初始位姿到最终位姿的路径，使机械臂能沿着这条路径无碰撞地完成工作任务。运动规划问题是机械臂规划过程中的基础。传统的运动规划方法主要分为两大类：基于搜索的运动规划方法以及基于采样的运动规划方法。基于搜索的运动规划方法多针对二维空间中机器人的运动规划问题，通常使用、等算法，虽然其搜索能力强但当目标为机械臂且其关节数增多、自由度较高时，这些算法复杂度会大幅提升，即并不适用于高维空间下的运动规划，而基于采样的运动规划方法则巧妙地解决了这些弊端。基于采样的运动规划方法主要针对高维空间下的机械臂运动规划问题。年，提出一种基于采样的运动规划方法即快速拓展随机树（，）算法。此类算法具有概率完整性、快速的探索速度，且无需将障碍物从任务空间映射到配置空间的优点。同时，传统的 算法也存在一些不足，例如在随机树搜索过程中的收敛速度较低，导向性不稳定，在复杂环境中寻迹速度慢且不必要节点过多以及障碍物约束使其随机采样生成的路径包含许多不必要的断点，从而导致路径不平滑且不连续等。因此，机械臂的运动规划通常是不稳定的。为了解决上述问题，本文提出一种融合目标偏置策略以及自适应步长策略的 （，）算法，通过目标偏置策略在采样过程中引导算法以一定概率向目标点快速拓展，实现尽快抵达目标点的任务以减少无用区域搜索，然后通过自适应步长策略搜集地图环境中的信息来加速算法收敛并达到探索空间的目的。最终在其规划出来的路径上采用三次样条函数优化其平滑性，来生成机械臂可跟踪的光滑路径。通过仿真实验分析，验证了所提算法的快速性和有效性。基本算法原理及模型简化 机械臂运动学分析及其模型建立本文研究对象为六轴机械臂，其 模型如图所示，其相应的 参数如表所示。表参数表连杆 表中各参数分别为：假定当前关节标号为，下一关节为，为连杆相对于连杆 的偏置；为连杆的长度；为连杆的扭角；为关节角。连杆相对于连杆坐标系变换通式为：计算机集成制造系统第 卷 。（）由式（）可知，相对于基坐标系的机械臂末端坐标系齐次变换矩阵为：。（）式中：，三个列向量表示机械臂姿态；，为三维空间内的基于右手定则的坐标表示；表示其三维坐标。机械臂连杆与障碍物空间占位关系简化描述避障路径规划的前提是要对机械臂本体以及障碍物建立数学模型以便于设计碰撞检测算法。一般工业中，障碍物具有随机性分布、形状不确定性的特点，难以对其精确建模，针对障碍物建模方法一般包括包络盒、包络球等 。障碍物包络球建模方法具有计算简单且在三维空间内旋转角度和位置不会影响碰撞检测性能等的优点，因此，本文采用包络球方法进行障碍物建模，其半径可表示为：（，）（）（）（）。（）式中：（，）为包络球的中心在 坐标系上对应的坐标；为包络球半径；（，）为障碍物表面任意一点的坐标。建模过程如图所示，其中灰色部分为障碍物，以为球心，以为半径的球面为包络面，考虑到需要为后续碰撞检测做基础，将包络球表面进行膨化处理，具体膨化距离约等于机械臂连杆半径，膨化后包络球半径为。其次，需要对简化后机械臂连杆与障碍物之间的空间占比关系进行描述。一般情况下，六自由度工业机械臂的连杆多为长方体或圆柱体，在碰撞检测过程中必须考虑其中心到边缘的最大包络半径。为简化碰撞检测设计过程，本文采用一种机械臂建模简化方法，如图所示。在已知末端位姿的情况下，根据工具箱中逆运动学求解函数求得一组最优解，随后求出正解即可得到连杆 的方程。设点到连杆 的最短距离为 ，根据连杆与包络球的位置关系分为以下几种情况：（），不发生碰撞，如图 所示。（），但垂足点位于连杆 的延长线上，且与包围球不相交，此时机械臂与障碍物不会发生碰撞，如图 所示。（），但与包围球交点有个交点，此时机械臂与障碍物发生碰撞，如图 所示。（），但垂足点位于连杆 的延长线上，且与包围球交点有一个交点，此时机械臂与障碍物发生碰撞，如图 所示。第期李扬 等：基于改进 结合样条的机械臂运动规划方法由图可见，图 和图 所示两种情况分别为机械臂连杆与障碍物未发生碰撞的两种表现形式，图 和图 所示分别表示机械臂连杆与障碍物发生两个交点、一个交点碰撞的表现形式，根据该原理可便于计算机械臂的碰撞检测问题。算法原理 基本原理简介快速拓展随机树（）算法是 和 提出的一种通过随机构建 实现对非凸高维空间快速搜索的算法。该算法可以很容易地处理包含障碍物和差分运动约束的场景，因而被广泛地应用在各种机器人的运动规划场景中 。该算法示意图如图所示。算法虽然在高维空间中很适用，但也存在一些弊端，比如它是一种纯粹的随机搜索算法，对环境类型不敏感，当搜索空间中存在大量障碍物时，该算法缺乏导向性从而会导致搜索速度慢、效率大幅下降以及该算法规划出来的路径尖点较多不平滑等，针对这类问题本文提出了一些优化方法。传统 算法优化（）基于概率的目标偏向寻优考虑到传统的 算法在复杂环境中存在搜索时间长、采样点随机性高等问题，本文采用基于概率的目标偏向寻优的 算法（），其核心思想主要是在迭代过程中引入目标偏置策略来优化采样过程，在其采样过程中对扩展函数引入目标偏置策略来优化采样点：，；（），其他。（）如式（）所示，假定一个目标偏置阈值 ，然后按均匀概率分布随机获取一个取值范围在内的概率值，若该值大于阈值，则树的扩展方向为向目标点 生长，若该值小于阈值，则会采用 函数产生的随机点作为采样节点。基于概率的目标偏置策略的 算法伪代码如下：，：（，）；（）；（，）；（，）；（，）；（，）（）；（）；（）；上述代码中：为地图环境，为起始位置，为目标位置，为概率函数。路径空间搜索过程从起点开始，先随机撒点 ；然后查找距离 最近的节点 并根据概率的值来选择树的生长方向；然后沿着 到 方向前进 的距离得到 ；（，）方法检测 （，）是否与地图环境中的障碍物有碰撞，若没有碰撞则可以完成一次成功的空间拓展搜索。局部扩展机制考虑到在 算法中引入目标偏执的函数，从而使采样的概率分布性发生变化，新节点总是选择离目标近的节点，为保证收敛性、完备性以及避免扩展树易陷入到局部极小值的问题，本文引入一种计算机集成制造系统第 卷局部扩展机制。其核心思想为：对于陷入局部最优的节点选择是以局部 为主的，剔除已遍历过的区域。若树中新的扩展节点 与其父节点的距离小于与树上其他节点的最近距离，即 （，）（，）时，保留该临时节点，否则删除。如图 所示，虚线连接并向外扩展的节点是可以进行下一步扩展的节点，而与实心节点圈在一起的节点为不满足约束而不被实例化的节点，图 中所有节点均为待扩展节点，即可以实例化的节点。算法实现过程如下：（）算法使陷入局部极值的节点通过 （）函数延展出一个局部扩展集合；（）对集合中各节点及边进行碰撞检测；（）对未发生碰撞的节点求它们与树中除父节点外所有节点的最短距离，若集合中某些节点与树中节点的最短距离大于与其父节点的距离，则保留这些节点，并将它们加入树中。若当前节点无法扩展，则令其父节点通过 （）函数扩展。（）树的扩展陷入局部极值时，算法会使陷入局部极值部分的各节点向外扩展一个步长，随后再对每个待扩展节点进行分析，直至完成一次成功扩展。自适应步长策略采用固定步长的 算法，其节点在扩展过程中，无法充分利用工作空间和障碍物的信息。本文采用自适应步长策略，利用扩展过程中收集到的信息来适应树中节点的扩展，使得随机树可以更快遍历障碍物较少的区域。其具体步骤为：在算法开始时，给定一个初始步长 和最短步长 ，初始步长为最短步长的倍。首先使用初始步长对随机树进行拓展，在每一次随机树拓展生成新节点 时，进行障碍物碰撞检测 （，），如无检测到障碍物则调整步长。本文所采用的步长调整策略：（）随机树拓展生成新节点 ，判断该新节点与其父节点之间的路径是否会存在障碍物，如不存在障碍物则将 加入到随机树中。（）若 在障碍物范围内，则将 与其父节点的连线平分成个线段，其中节点为，。（）遍历这些节点找到满足要求的两个节点，其中不在障碍物内而在障碍物内且必须满足。（）若存在这两个节点，则拓展后新的节点为，否则判断此次拓展失败。与其父节点的连线分段如图所示。路径平滑策略 算法得到的路径是由随机树中的节点依次连接的连续线段，并不是一条平滑的曲线，从而会影响机械手的稳定性，加剧磨损。因此，需要对计划中的连续线段进行平滑处理。准均匀样条函数具有连续性的特点，它可以传递起始位置和方向以及目标位置和方向。考虑到该函数的这些优点，对本文提出的改进 （）算法所得到的路径进行平滑优化处理。首先，阶样条函数表达式为：（），（）。（）式中：（，）表示控制点，（）表示次样条基函数，且有：，（），；，其他。（），（），（），（）。（）三次样条基函数：，（）（），（）（），（）（），（），。（）第期李扬 等：基于改进 结合样条的机械臂运动规划方法三次样条曲线段：，（），（），（），（），（），。（）当给定控制点的时候，利用式（）可以求出一系列满足三次样条的曲线点。图中，与 分别为三角形和的中线，样条的起点选取为 的处，方向水平于底边且长度为其，终点为 的 处，方向水平于底边，长度为其。若改变单个控制点并不会对整条曲线的性能造成影响，仅会改变其局部曲率，并且该曲线在各个端点处连续，通过这些所生成的分段曲线的组合，拼接出一条完整的样条曲线。仿真实验验证 寻迹本文在 台式电脑上使用 对本文提出的算法在三维环境下进行仿真验证。在本次仿真实验中，搜索步长设定为，迭代次数上限设定为 ，由于类似于 算法这样的基于采样的算法存在随机性，执行实验次数为 。路径规划起点设置为，目标点设置为，实验环境设置为 的三维空间，障碍物为随机布置。如图所示为传统 ，以 及 本 文 所 提 出 的 改 进 算法（）的寻迹结果。表各类 算法性能对比算法平均规划时间空间中平均采样节点数个平均有效节点数个平均路径长度 图中：蓝色的线是各种算法最终寻找到的路径，红色线条是拓展树的分支，绿点为三维空间中随机采样点。由图可见，本文所提出的 算法在空间中采样节点数明显少于其他种方法，且规划出来的路径曲折程度较低，结合表中具体数据可得，算法相较于传统 算法计算机集成制造系统