CN112677153B - 一种改进的rrt算法及工业机器人路径避障规划方法 - Google Patents

Abstract

本发明是一种改进RRT的工业机器人路径避障规划算法，针对传统RRT算法在机械臂的运动规划上缺乏导向性，收敛速度慢等问题，文中在传统RRT的基础上，提出了一种扩展点选择策略和自适应步长策略，并且在算法陷入局部极小值时，采用避免回归机制，快速脱离极小值。然后结合Dijkstra算法对改进算法产生的路径进行优化，得到一条优化后的路径。最后，得到的机械臂末端有效路径再通过本文的机械臂规划模块，转化为一条机械臂最优位姿路径。将该改进算法与其他算法在Matlab和ROS中进行仿真实验，实验结果表明，该算法能有效指导RRT树的生长方向，避免陷入极小值，并且提高算法的收敛速度，并且提高了机械臂在仿真中运动规划效率。

Description

一种改进的RRT算法及工业机器人路径避障规划方法

技术领域

本发明是一种的改进RRT算法及工业机器人路径避障规划方法,尤其涉及智能机器人机械运动中避障的规划算法，属于智能机器人领域。

背景技术

随着硬件技术和计算机技术的飞速发展，机器人也逐渐受到各个领域的人们的关注。在工业现场，常见到许多多关节串联机械臂在一起快速，灵巧的完成各种任务，而其中机械臂的路径规划技术则是指导机械臂运动的关键。由于机械臂的工作环境越来越复杂，路径规划的作用也越来越重要。

机械臂的路径规划是在给定起始点和目标点的情况下，搜索得到一组合理的关节角度集合，利用这一组关节角度集合，可以驱动机械臂无碰撞的从初始位姿运动到目标位姿，以引导机械臂完成作业。传统的路径规划算法有A*算法，人工势场法，蚁群算法，遗传算法等。其中，A*算法需要大量空间来存储环境信息，计算效率低；人工势场法容易陷入局部极小值中；蚁群优化和遗传算法等适用于多目标问题求解。这些方法在低维空间具有一定的优势，但当机械臂关节数增多，自由度较高时，这些算法的复杂性将会大幅度增加。

为了解决在高维空间中机械臂的运动规划问题，1998年，Steven M.La Valle提出了快速扩展随机树(RRT)算法。RRT算法是一种基于采样的路径规划算法，这类基于采样的算法不用对障碍物进行建模，而是对每个采样点进行碰撞检测，因此在高维空间的路径规划中应用较广。当然RRT算法同样存在一些不足，如随机树扩展导向性差，算法收敛速度慢，在狭窄路段中搜索效率低等问题，针对这些不足，国内外学者们提出了许多改进方案。现有技术中提出了一种基子目标搜索策略的RRT算法，该算法针对三种不同的环境提出了不同的应对策略，但是该算法主要是针对二维环境中的路径规划，对于高维环境规划该算法搜索缓慢；现有技术中提出了一种RRT-GD模型用于机械臂的路径规划，该模型通过控制扩展方向来减少扩展时间，使其能迅速抵达目标点，但是如果有一个大的障碍物在起始点和目标点之间时，这个障碍物可能会限制该模型的所有扩展方向而无法得到一条可行路径。传统RRT算法还有一个缺陷，那就是它不利用扩展时收集到的空间和障碍物的信息，比如扩展成功时，说明这个方向上大概率是不存在障碍物的。自适应步长策略解决了这个问题，它能够利用探索过程中收集到的信息来适应树的扩展，以使树更快地覆盖较少受阻的空间区域。它的基本思想是：使用两个变量e1，e2(初始都为1)分别代表随机方向和引导方向的扩展长度因子，当给定节点在上面两种方向扩展成功后，增加这个方向上的e值，并用新e值乘上步长扩展新的节点，若该方向上扩展失败，即遭遇障碍物，则将该e值初始化。

扩展点选择策略与自适应步长策略相结合，能够非常快速的探索未知空间，并将扩展树引导向最终目标点，但同时也会令扩展树陷入局部极小值的问题中；针对空间中存在大量的狭窄路径的问题提出了一种解决方案，但是当环境中障碍物较多时，算法效率会大大降低；现有技术中提出了一种快速扩展策略，可以快速探索空间，但是当遇到局部极小值时因为只有8个固定扩展方向，可能会难以跳出局部极小值；另外现有技术中提出了RRT*算法，该算法引入了对新生成节点相邻节点的搜索，进而可以选择较低代价的节点，可以得到一条渐进最优的路径，但是当运用到多自由度机械臂上时，每次搜索应该结合机械臂结构特点和关节约束等限制，实现复杂并且搜索缓慢。为了解决这些问题，本文对算法做了以下的改进。

发明内容

为解决现有技术中RRT算法搜索缓慢以算法缺乏导向性，不利用扩展时收集到的空间和障碍物的信息，收敛速度慢，而且当障碍物较多时，算法容易陷入局部极小值的问题，本发明提出一种的改进RRT算法，算法的方案如下：

一种改进的RRT算法，结合Dijkstra算法使用贪心策略计算从起始点到目标点的最短路径，得到改进的RRT算法方程，其算法步骤为：

步骤A，初始化变量；

步骤B，随机采样生成节点qnew，并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；

步骤C，判断生成的qnew节点与qgoal是否满足提前给定的约束，若满足，则转步骤G，否则转步骤D；

步骤D，根据生成的qnew情况，调用自适应步长策略，调整步长因子；

步骤E，根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点qnew然后转步骤C判断，否则转到步骤F；

步骤F，此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成qnew然后转步骤C判断，若没陷入极小值转步骤D；

步骤G，此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

步骤H，返回生成的RRT树，构建过程结束。

为解决机械臂运动规划上缺乏导向性，收敛速度慢的问题，提出一种工业机器人路径避障规划方法，方案如下：其方法步骤如下：

步骤一，对机械臂进行建模，利于建模过程计算处正逆运动学算法；

步骤二，利用机械臂所处环境，给定目标位姿和障碍物环境；

步骤三，建立改进的RRT算法方程，通过步骤二参数给定三维任务空间约束，并基于约束，利用改进的RRT算法在机械臂的笛卡尔空间搜索路径，使其即不与障碍物碰撞，也不发生自碰撞；

步骤四，利用步骤三获得的路径，进行贪心优化，并利用约束对优化后的路径上的插值点，确定出该点处的机械臂最优位姿；

步骤五，通过控制机械臂的行径，整合出最优位姿集合，使机械臂无碰撞的达到目标位姿；

步骤六，根据上述步骤得到的结果进行验证。

进一步地，步骤一中所述的机械臂是指六自由度的串联机械臂。

进一步地，在步骤三或四中，使用改进的RRT算法在笛卡尔空间中规划出一条机械臂末端的有效路径，对于机械臂末端在走这条有效路径时，其他关节连杆的位姿的确定方式如下：

关于确定目标点处机械臂最优位姿对应的关节角：

(a)若机械臂末端位姿已经确定，则首先根据该末端位姿能够逆解出8组解，关于8组解中的每一组解，首先通过正运动学得出路径中每个连杆的坐标位置的插值点，其次运用包络法，选出解中满足关节限位和符合约束条件的解，最后通过计算确定一个最优目标位姿的关节角；

当机械臂末端位姿已经确定，由逆运动学得到8组逆解，其中4组解是不满足关节限位的约束的，则从另外的4组逆解中，求出最小路径的代价；

(b)当末端姿态不固定时，首先计算出根据机械臂末端的位置坐标从全局Global_System()中计算其姿态：考虑奇异点的同时要满足关节限位和避障，雅可比矩阵条件数的非线性约束条件，最终确定最优目标位姿对应的关节角。

进一步地，在确定了机械臂在目标点处的最优位姿后，继续确定机械臂从起始点运动到目标点的路径上的最优位姿，具体过程为：

首先利用改进后的RRT得到(a)中的路径及其上的插值点；若在某一插值点不存在有效关节角时，则以当前插值点为起始点qinit，重新规划一条到终点qgoal的扩展树，并计算插值点处的有效解，直到找到符合条件的一条路径或者达到迭代上限为止。

进一步地，在步骤六中，通过控制机械臂的行径，整合出最优位姿集合的过程，具体验证步骤细化为：

步骤六一，利用实验平台进行运行，以点状机器人和六自由度的串联机械臂作为实验对象验证改进算法，点状机器人仿真在Matlab R2019a仿真平台运行，六自由度机械臂在ROS仿真平台进行；

步骤六二，进行上述算法验证及结果对照；

步骤六三，最终完成点状机器人仿真，实现改进RRT的工业机器人路径避障规划算法，仿真实验中，算法的搜索步长均为50，最大迭代次数为10000，实验次数为500次；二维实验中将避障环境设置为一个大小为1000*1000的平面，在其中添加障碍物进行测验。

本发明的有益效果体现在：

本发明以RRT算法为基础，针对原始算法在机械臂运动规划上缺乏导向性，收敛速度慢等问题，提出了一种扩展点选择策略和自适应步长策略，并且在算法陷入局部极小值时，采用避免回归机制，最后结合Dijkstra算法对改进算法产生的路径进行优化。通过对改进的算法在Matlab和ROS中的仿真实验，其结果表明改进后的算法在算法收敛速度上，路径代价上和避障方面相比传统RRT算法具有明显的优势，而且在机械臂仿真中表现良好，可以快速地对机械臂进行运动规划，具有较大的发展与应用空间。

RRT算法可以通过人为加入约束，在搜索过程中尽可能地向目标点收敛，最终通过在目标点设置严格位置约束或者附加姿态约束完成搜索过程。如果目标距离当前存在至少一条可达路径，那么当搜索时间足够长时都会到达目标位置。

本发明主要针对RRT算法在机械臂的路径规划上缺乏导向性提出了一种扩展点选择策略和势场函数，通过该策略引导算法尽快向目标点处扩展，尽快达到目标，针对算法收敛速度慢的问题提出了自适应步长策略，通过该策略将探索过程中收集到的环境信息利用起来，达到更快探索空间的特点。针对算法容易陷入局部极小值等问题提出一种逃脱策略，然后将得到的路径结合Dijkstra算法对改进算法产生的路径进行优化。最后根据规划出的机械臂末端点的一条路径，通过一系列运算和约束得到机械臂的相对最优位姿集合。通过仿真实验分析，改进后的算法能够有效提高机械臂运动规划的效率。

本发明的改进RRT在三维各种情景中仍然表现良好，搜索时间，生成的有效路径长度都比传统的RRT与RRT*要短，显著地提高了效率。

附图说明

图1为一种的改进RRT算法及工业机器人路径避障规划方法流程图；

图2为六轴机械臂及障碍物环境示意图；

图3为六自由度机械臂模型及DH坐标系示意图；

其中(a)部分为六自由度机械臂模型展示，(b)部分为及DH坐标系；

图4为RRT算法流程图；

图5为障碍物碰撞检测示意图；

图6为确定目标点处机械臂最优位姿对应的关节角代码流程图；

图7为基于三维空间约束的机械臂避障运动规划路径与路径优化示意图；

图8为随机树扩展示意图；

图9为改进后的随机树扩展示意图。

具体实施方式

具体实施方式一：本发明的一种的改进RRT算法及工业机器人路径避障规划方法，该实施例以一个六自由度的串联机械臂为例进行实施说明，如图1所示。该机械臂的各个连杆坐标系如图3所示，相应的D-H参数如表1所示，则坐标系i相对于坐标系i-1的变换通式为：

表1 D-H参数

连杆i-1两端分别是轴线i-1与轴线i。那么，di就是连杆i-1与连杆i的相对位置之间的距离，ai-1为轴线i-1与轴线i之间的距离，αi-1为两轴线在同一平面内的夹角，θi是两连杆公垂线的夹角。

根据(1)式可以得到机械臂末端坐标系相对于基座坐标系的齐次变换矩阵

为

(2)式中，n,o,a 3个列向量表示机械臂的姿态，p表示其三维坐标。利用蒙特卡罗法，将机械臂的所有关节在各自的关节限位内随机取值，再利用(2)式的正运动学公式将其映射成机械臂末端相对于基坐标系的集合，即可得到机械臂大致的工作空间。

碰撞检测

机械臂的碰撞检测是其运动规划过程中的关键环节，每当在随机扩展树中添加一个新的节点时，都需要进行大量的碰撞检测，所以该模块的效率直接影响到机械臂避障运动的效率。

考虑到在机械臂运动过程中，大多数都是刚体之间的碰撞检测，所以本发明采用包络法来简化机械臂与障碍物之间的碰撞检测

该包络法将机械臂的各个连杆用圆柱体包络起来，将关节处用球体包络起来，将障碍物同样用球体和柱体包络起来，由此将碰撞检测问题转换成球体与球体，柱体与球体，柱体与柱体之间的位置关系问题，计算量得到大大简化，算法效率较高。

球体与球体指的是关节处与障碍物之间的位置关系，如图5所示之间的位置关系经过简单的计算就可以得出：

ri代表的是关节的包络半径，rj代表障碍物的包络半径。根据(2)式可知，

1)若|AB|≥r_i+r_j，不发生碰撞；

2)若|AB|＜r_i+r_j，发生碰撞；

关于球体与柱体指连杆与障碍物之间的位置关系，如图5所示。

连杆的碰撞检测问题转化为柱体与球之间是否发生碰撞。这里柱体与球之间的碰撞检测可以近似的看成判断线段与球的位置关系。在已知末端位姿的情况下，由逆运动学先求得一组最优解，再正解即可得到线段MN的方程。

设点B到直线MN的最短距离为dmin，根据柱体与球的位置关系如下几种情况：

1)d_min≥r_i+r_j，不发生碰撞

2)d_min＜r_i+r_j，分以下几种情况讨论

a)若

则发生碰撞

b)若|BM|＞r_i+r_j∧|BN|＞r_i+r_j，则分以下两种情况；

i.当点M、N分布在B的两侧时，

发生碰撞

ii.当点M、N分布在B的同一侧时，

不发生碰撞

关于柱体与柱体，设点Q为机械臂连杆C1上的一个插值点，点M，N为柱体障碍物C2上的相邻插值点，P点为线段MN中点。容易得到，当Q在MN上的投影恰好为P点时，Q点到M和N点的距离达到最大值。考虑两个柱体碰撞的极限情况，当Q点所在切面与C2相切时，距离最近的大小为||PQ||＝R+r。为了方便计算，规定只要C1上所有的插值点与C2上所有的插值点之间的距离大于(R+r)时，即认为两者不发生碰撞。

具体实施方式二：结合实施方式一中的算法模型构建，建立RRT算法，其中RRT算法是一种基于随机采样的搜索算法，随机树扩展过程如图8所示。它的目标是：求得一条从起始点qinit到目标点qgoal的无碰撞路径。算法基本原理是：

(1)首先初始化树T，T中当前只包含一个起始点qinit；

(2)随机采样生成采样点qrand；

(3)在树T中搜索得到距离qrand最近的叶节点qnear；

(4)以固定步长沿向量

的方向生成qnew；

(5)对节点qnew和树枝qnear—qnew进行碰撞检测，若发生碰撞，则舍弃节点qnew并转到步骤(2)，否则保留qnew，并将qnew加入树T中；

(6)判断qnew是否已达目标点qgoal或满足约束，若是则结束算法，否则继续；

(7)重复(2)到(6)的步骤直到达到迭代上限或找到目标点为止。

虽然传统的RRT算法适用于高维空间的机械臂运动规划，但是它同样存在不足之处，比如算法缺乏导向性，收敛速度慢，而且当障碍物较多时，算法容易陷入局部极小值等。本发明针对这些不足，提出了一些改进方案。

由于传统RRT扩展叶节点的随机性大，导致产生的大量叶节点都是盲目的，其扩展速度可能是缓慢的，所以本发明提出了一种扩展点选择策略。其核心思想是，模拟人工势场法中目标点对机械臂末端的引力作用，将RRT算法中的随机采样函数加以改进，使算法可以引导扩展树的生长方向，从而尽快的向目标探索，使扩展树生长更有倾向性，而不作大量的无效扩展，缩短了路径规划的时间，节约了资源。

新叶子节点由随机节点qrand和目标点qgoal共同决定,以此来引导树向目标点扩展。定义一个常量Pthreshold，当随机值temp小于Pthreshold时，使用上述方法引导树扩展，否则仍是朝着随机方向扩展。这样既保留了扩展树的随机性，又能够更快的向目标扩展。

式(4)中krep为斥力系数，ρ为机器人与障碍物之间的距离，ρ0为斥力场的作用范围。根据斥力场势函数Urep的概念，为所有的障碍物都建立一个斥力场R，如式(5)。其中，kr为斥力场系数，与地图的大小正相关，s为障碍物与目标点之间的距离，s0是提前约定好的常数，与目标点周围障碍物的数量正相关。因此，对于远离目标点的障碍物，其斥力场较大。规定在斥力场中的qnew点不能加入到扩展树T中，从而压缩了搜索空间，使算法尽快向目标扩展。

当算法生成新的节点qnew时，首先判断其是否在障碍物的斥力场范围内，若在，则舍弃此点，进入下一次采样迭代；若不在，则进行碰撞检测。倘若通过碰撞检测，那么将此点加入到树T中，并检查其是否满足||qnew-qgoal||<d约束，若满足约束，则下次迭代尝试直接向目标点扩展。

传统RRT算法还有一个缺陷，那就是它不利用扩展时收集到的空间和障碍物的信息，比如扩展成功时，说明这个方向上大概率是不存在障碍物的。自适应步长策略解决了这个问题，它能够利用探索过程中收集到的信息来适应树的扩展，以使树更快地覆盖较少受阻的空间区域。它的基本思想是：使用两个变量e1，e2(初始都为1)分别代表随机方向和引导方向的扩展长度因子，当给定节点在上面两种方向扩展成功后，增加这个方向上的e值，并用新e值乘上步长扩展新的节点，若该方向上扩展失败，即遭遇障碍物，则将该e值初始化。

扩展点选择策略与自适应步长策略相结合，能够非常快速的探索未知空间，并将扩展树引导向最终目标点，但同时也会令扩展树陷入局部极小值的问题中，为了解决这一问题，本发明对算法做了以下的改进。

关于局部逃脱机制，由于在RRT算法中融合了导向目标的函数，新的叶节点总是选择离目标近的节点，这就使扩展树容易陷入到局部极小值的问题中，本发明采用一种逃脱机制可以快速脱离极小值。其核心思想是以局部RRT为基础，并且不重复探索以前达到过的节点的周边区域。例如，若新的待扩展节点qnew与其父节点qparent的距离小于qnew与随机扩展树上其他节点的最近距离，即distance(qnew,qparent)<distance(qnew,qnear)，则此临时节点将被保留；否则，不保留该节点[23]。每当扩展树陷入局部极小值时，算法会使扩展树上陷入极小值的部分的每一个节点向外扩展一个步长，然后对每个待扩展节点进行判断，

局部逃脱机制的部分伪代码如下：

算法首先从陷入极小值的节点qparent通过Extend_Esc-ape_Node()函数扩展出一个逃脱节点集合q，然后对集合中的每个节点和边都进行碰撞检测，然后对未发生碰撞的每一个节点求它们与树T中除了父节点qparent外所有节点的最近距离，若集合中某些节点与树中节点的最近距离大于它们与其父节点的距离，则保留这些节点q，并将它们加入树T中。如果当前节点无法扩展，则令其父节点通过Extend_Esc-ape_Node()函数扩展，直到完成一次扩展。

Dijkstra算法使用贪心策略计算从起始点到目标点的最短路径。将Dijkstra与RRT算法相结合，是在RRT算法建立完随机扩展树的基础上，运用Dijkstra算法对随机扩展树的节点矩阵进行遍历搜索，若路径代价最小，则更新路径。

改进的RRT算法的流程图如图4所示，则具体步骤如下：

(a)首先初始化变量，如起始点qinit,目标点qgoal，路径path，树T等；

(b)随机采样生成节点qnew，并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；

(c)判断生成的qnew节点与qgoal是否满足提前给定的约束，若满足，则转(g)，否则转(d)；

(d)根据生成的qnew情况，调用自适应步长策略，调整步长因子；

(e)根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点qnew然后转(c)判断，否则转到(f)；

(f)此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成qnew然后转(c)判断，若没陷入极小值转(d)；

(g)此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

(h)返回生成的RRT树，构建过程结束。

具体实施方式三：使用改进的RRT算法在笛卡尔空间中规划出一条机械臂末端(TCP)的有效路径，对于机械臂末端在走这条有效路径时，其他关节连杆的位姿的确定方式如下：

确定目标点处机械臂最优位姿对应的关节角：

a.末端位姿已确定

若机械臂末端位姿已经确定，则首先根据该末端位姿能够逆解出8组解，为了保证在每个随机扩展树节点取得最优的关节角度值，确保前后关节角度变化的连续性，本发明将机械臂的路径代价定义为关节角度变化的范数。并且考虑到机械臂各关节对其位姿的影响不同，给每个关节定义一个权重系数，以此为依据来计算路径代价。

其中，ωi指每个关节的权重，Δθi指关节变化量。

对于这8组解中的每一组解，首先通过正运动学得出每个连杆的坐标位置，其次运用第二节中的包络法，选出这些解中满足关节限位和其他约束条件(如避障)的解，最后在选出的这些解中，通过(6)式确定一个最优目标位姿的关节角。

当机械臂末端位姿已经确定，由逆运动学可以得到8组逆解，其中4组解是不满足关节限位等约束的，从剩下的4组逆解中，考虑奇异点，同时要满足关节限位，避障等非线性约束条件，再由(6)式求出最小路径代价即可。

当末端姿态不固定时，算法首先计算出将根据机械臂末端的位置坐标从全局Global_System()中计算其姿态：考虑奇异点，同时要满足关节限位，避障，雅可比矩阵条件数等非线性约束条件，然后利用(6)式确定最优目标位姿对应的关节角。

两部分伪代码如下：

在上一步中，确定了机械臂在目标点处的最优位姿，接下来需要就确定机械臂从起始点运动到目标点的路径上的最优位姿。

首先利用改进后的RRT得到(a)中的参考路径及其上的插值点。若在某一插值点不存在有效关节角时，则以当前插值点为起始点qinit，重新规划一条到终点qgoal的扩展树，并插值计算插值点初的有效解，直到找到这样的一条路径或者达到迭代上限为止。

该部分伪代码如下：

算法首先用插值函数Interpolation()对从改进RRT算法得到的机械臂末端从起始点到目标点的参考路径Path进行插值处理，得到一系列插值点集合interpolation_point，并且设置一个标志位effective；然后对于每一个插值点在全局中通过Inverse_Kinematics()函数进行逆解，得到4组有效关节角，再根据(4)式寻找机械臂的最优姿态，同时考虑关节限位和避障约束；若插值点存在有效解，则机械臂按照得到的一系列关节角运动；若某一插值点不存在有效解，则置标志位为0，输出当前插值点坐标coordinate，并且机械臂从该插值点coordinate处重新进行RRT末端路径规划，然后重复进行确定目标点最优位姿和插值点最优位姿，直到找到有效解或达到迭代次数上限。

具体实施方式四：根据上述实施例的构建，本发明的实验验证部分过程可细化为：

(1)实验平台

本发明以点状机器人和六自由度的串联机械臂作为实验对象验证改进算法。实验在Intel(R)Core(TM)i5-7500 CPU@3.40GHz 3.41GHz 8.00GB内存，WIN10操作系统的电脑上运行。点状机器人仿真在Matlab R2019a仿真平台运行，六自由度机械臂在ROS仿真平台进行。

(2)算法验证及结果讨论

考虑到机械臂各关节对位姿影响的程度不同，对每个关节定义一个权重系数，如表2。

表2机械臂关节权重

(3)点状机器人仿真

本实施例仿真实验中，算法的搜索步长均为50，最大迭代次数为10000，实验次数为500次。二维实验中将避障环境设置为一个大小为1000*1000的平面，然后在其中添加了一些障碍物。

表3二维无障碍实验结果平均值

表4二维少量障碍实验结果平均值

表5二维大量障碍实验结果平均值

表3、表4、表5显示的是实验成功的前提下的平均值。从表3，表4，表5可以看出，虽然RRT*通过对父节点的重新搜索，能够得到比传统RRT代价更低的路径，但是本发明的改进RRT算法在无障碍、少量障碍、大量障碍环境中都比以上两种算法在寻路所需时间要少很多，并且最终生成的有效路径要短，代价更小，大大减少了算法规划时间。

关于三维实验中的两种算法如下：

把实验环境大小设置为100*100*100的三维空间，并在其中添加一些障碍物，根据文中之前所述的包络法，将障碍物分别用球体包络起来。传统RRT，RRT*和改进RRT的某些仿真结果如下：

表6三维无障碍实验结果平均值

表7三维少量障碍实验结果平均值

表8三维大量障碍实验结果平均值

在一些三维环境中的仿真结果中，分别展示了Traditional-RRT在无障碍、少量障碍、大量障碍环境下所得的仿真结果，另一部分分别为RRT*在无障碍、少量障碍、大量障碍环境下所得的仿真结果，其他部分分别为Improved-RRT在无障碍、少量障碍、大量障碍环境下所得的仿真结果。在三维少量障碍和大量障碍环境中，当实验次数分别为500次时，Traditional-RRT和RRT*都有部分实验在达到最大迭代次数时(或者设置的最大搜索时间内)，仍然没有搜索到一条有效路径，而Improved-RRT在每次实验中都可以快速成功的得到一条有效路径。RRT*虽然找到了相对较小代价的路径，但是花费的时间太多了，尤其是当维度变高以后。表6、表7、表8显示的是实验成功的前提下的平均值。从表中可以看出实验效果。

已经通过Matlab实现了点状机器人的仿真，由此得到了一条有效路径，接下来本发明将在Ubuntu16.04系统中，通过ROS Kinetic里的Gazebo仿真平台对改进算法进行验证。将改进的RRT算法加入到OMPL库中，并通过MoveIt对其进行调用。

接下来，将障碍物添加到环境中。这里仍然运用前面讲述的包络法，将障碍物用球体和柱体包络起来，图中灰色的为障碍物，白色为机械臂，经过改进RRT算法后，可以规划出一条无碰撞的有效路径。

在机械臂在相同的初始位置和目标位置，分别以传统RRT，RRT*，改进RRT算法运动规划后的机械臂运动时的关节变化数据。机械臂以传统RRT和RRT*算法规划出的路径，机械臂关节通常变化范围比较大，而使用改进RRT算法通常可以得到一条代价更小的关节集合，最终对比Traditional-RRT、RRT*和Improved-RRT三种算法的结果如表9所示：

表9 Traditional-RRT、RRT*和Improved-RRT结果对比表

以上所述，仅为本发明的具体实施方式，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内；因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (6)

1.一种改进的RRT算法，其特征在于：结合Dijkstra算法使用贪心策略计算从起始点到目标点的最短路径，得到改进的RRT算法方程，其算法步骤为：

步骤A，初始化变量；

步骤B，随机采样生成节点qnew，并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；采用包络法来简化机械臂与障碍物之间的碰撞检测，包络法将机械臂的各个连杆用圆柱体包络起来，将关节处用球体包络起来，将障碍物同样用球体和柱体包络起来，由此将碰撞检测问题转换成球体与球体，柱体与球体，柱体与柱体之间的位置关系问题；

球体与球体指关节处与障碍物之间的位置关系，计算得出：

(3)

ri代表的是关节的包络半径，rj代表障碍物的包络半径；

1) 若

，不发生碰撞；

2) 若

，发生碰撞；

柱体与球体指连杆与障碍物之间的位置关系：设点B到直线MN的最短距离为dmin，根据柱体与球的位置关系如下几种情况：

1)

，不发生碰撞

2)

，分以下几种情况讨论

a) 若

，则发生碰撞；

b) 若

，则分以下两种情况；

i. 当点M、N分布在B的两侧时，

，发生碰撞；

ii. 当点M、N分布在B的同一侧时，

，不发生碰撞；

柱体与柱体之间的位置关系：设点Q为机械臂连杆C1上的一个插值点，点M，N为柱体障碍物C2上的相邻插值点，P点为线段MN中点，得到，当Q在MN上的投影为P点时，Q点到M和N点的距离达到最大值，考虑两个柱体碰撞的极限情况，当Q点所在切面与C2相切时，距离最近的大小为

，规定只要C1上所有的插值点与C2上所有的插值点之间的距离大于(R+r)时，即认为两者不发生碰撞；

步骤C，判断生成的qnew节点与qgoal是否满足提前给定的约束，若满足，则转步骤G，否则转步骤D；

步骤D，根据生成的qnew情况，调用自适应步长策略，调整步长因子；

步骤E，根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点qnew然后转步骤C判断，否则转到步骤F；

步骤F，此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成qnew然后转步骤C判断，若没陷入极小值转步骤D；

步骤G，此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

步骤H，返回生成的RRT树，构建过程结束。

2.工业机器人路径避障规划方法，其特征在于：其方法步骤如下：

步骤一，对机械臂进行建模，利于建模过程计算处正逆运动学算法；

步骤二，利用机械臂所处环境，给定目标位姿和障碍物环境；

步骤三，通过步骤二参数给定三维任务空间约束，并基于约束，利用权利要求1所述一种改进的RRT算法在机械臂的笛卡尔空间搜索路径，使其即不与障碍物碰撞，也不发生自碰撞；

步骤四，利用步骤三获得的路径，进行贪心优化，并利用约束对优化后的路径上的插值点，确定出该点处的机械臂最优位姿；

步骤五，通过控制机械臂的行径，整合出最优位姿集合，使机械臂无碰撞的达到目标位姿；

步骤六，根据上述步骤得到的结果进行验证。

3.根据权利要求2所述的工业机器人路径避障规划方法，其特征在于：步骤一中所述的机械臂是指六自由度的串联机械臂。

4.根据权利要求2所述的工业机器人路径避障规划方法，其特征在于：在步骤三或四中，使用改进的RRT算法在笛卡尔空间中规划出一条机械臂末端的有效路径，对于机械臂末端在走这条有效路径时，其他关节连杆的位姿的确定方式如下：

关于确定目标点处机械臂最优位姿对应的关节角：

（a）若机械臂末端位姿已经确定，则首先根据该末端位姿能够逆解出8组解，关于8组解中的每一组解，首先通过正运动学得出路径中每个连杆的坐标位置的插值点，其次运用包络法，选出解中满足关节限位和符合约束条件的解，最后通过计算确定一个最优目标位姿的关节角；

当机械臂末端位姿已经确定，由逆运动学得到8组逆解，其中4组解是不满足关节限位的约束的，则从另外的4组逆解中，求出最小路径的代价；

（b）当末端姿态不固定时，首先计算出根据机械臂末端的位置坐标从全局Global_System()中计算其姿态：考虑奇异点的同时要满足关节限位和避障，雅可比矩阵条件数的非线性约束条件，最终确定最优目标位姿对应的关节角。

5.根据权利要求4所述的工业机器人路径避障规划方法，其特征在于：在确定了机械臂在目标点处的最优位姿后，继续确定机械臂从起始点运动到目标点的路径上的最优位姿，具体过程为：

首先利用改进后的RRT得到(a)中的路径及其上的插值点；若在某一插值点不存在有效关节角时，则以当前插值点为起始点qinit，重新规划一条到终点qgoal的扩展树，并计算插值点处的有效解，直到找到符合条件的一条路径或者达到迭代上限为止。

6.根据权利要求2所述的工业机器人路径避障规划方法，其特征在于：

在步骤六中，通过控制机械臂的行径，整合出最优位姿集合的过程，具体验证步骤细化为：

步骤六一，利用实验平台进行运行，以点状机器人和六自由度的串联机械臂作为实验对象验证改进算法，点状机器人仿真在Matlab R2019a仿真平台运行，六自由度机械臂在ROS仿真平台进行；

步骤六二，进行上述算法验证及结果对照；

步骤六三，最终完成点状机器人仿真，实现改进RRT的工业机器人路径避障规划算法，仿真实验中，算法的搜索步长均为50，最大迭代次数为10000，实验次数为500次；二维实验中将避障环境设置为一个大小为1000*1000的平面，在其中添加障碍物进行测验。

Images