CN112809665A

CN112809665A - 一种基于改进rrt算法的机械臂运动规划方法

Info

Publication number: CN112809665A
Application number: CN202011487613.4A
Authority: CN
Inventors: 徐向荣; 陈肇星; 张卉; 查文斌
Original assignee: Anhui University of Technology AHUT
Current assignee: Anhui University of Technology AHUT
Priority date: 2020-12-16
Filing date: 2020-12-16
Publication date: 2021-05-18
Anticipated expiration: 2040-12-16
Also published as: CN112809665B

Abstract

本发明公开了一种基于改进RRT算法的机械臂运动规划方法。该路径规划方法主要是在RRT算法的基础上进行改进的，改进的方向是在快速扩展随机树算法中的三个过程，一个是针对随机点采样的过程，另一个是新节点的扩展过程，最后是对得到路径的路径优化过程。改进后的算法能够在较短的时间内完成路径规划，路径的代价会更小，且构架保持了原有简洁的特点，在高维空间中仍旧表现良好；该算法应用在串联机器人的关节空间中，使串联机器人能够在工作空间中有障碍物的情况下得到一条安全的路径；最后，可以将机器人的关节路径进行B样条曲线的拟合，使得机器人的运行更加平稳柔顺。

Description

一种基于改进RRT算法的机械臂运动规划方法

技术领域

本发明涉及的主要内容是机械臂的运动规划方面，主要包含了一种基于改进RRT算法的机械臂运动规划方法。

背景技术

机器人运动规划时机器人研究领域的常见问题，同时也是基础问题。其目的是为机器人规划出一条由起始位置到目标位置的安全路径，该路径要符合其在工作空间中的一些既定的约束条件。早期的机器人路径规划在规划过程中，很多时候是采用了机器人示教控制的方法。而许多早期的路径规划方法是借鉴于移动机器人的规划算法，是将机器人的末端当成质点进行规划。但是由于机械臂在空间不规则的特点，这样是难以实现的。

针对机械臂的路径规划，已经有许多的算法被提出和应用，例如人工势场法、蚁群算法、A*算法和遗传算法等，同时，这些算法都有一些在理论上的缺陷。例如人工势场法会遇到局部最小值问题，A*算法在较大空间中的运算量会非常大，蚁群算法在求解过程中天然会占据较大的计算量。

基于快速搜索随机树算法进行路径规划，是通过对状态空间中的采样点进行碰撞检测，对空间中障碍物不需要过于准确地建模，这样有利于机械臂在路径规划中的高维空间和复杂约束下的运动规划。

传统的RRT算法的工作流程比较简单。以自由空间中一个初始点为根节点，采用随机采样的方法进行扩展，生成无碰撞的随机扩展树。当扩展树不停生长，在自由空间中逐步覆盖，最终会得到足够靠近终点的节点，从而生成从初始到目标的路径。整体思路是简洁有效的，但由于可能会对额外区域进行过度搜索，算法在一定程度上的浪费。

中国发明专利文献(CN 108444489A)于2018年8月24日公开的一种改进RRT算法的路径规划方法中，随机树在扩展过程中当遇到障碍物进行随机扩展；当没有遇到障碍物时，引入目标引力策略修正随机树的扩展方向；引入双向扩展方法，分别从起始点和目标点进行扩展。该方法存在不足之处：在随机树的扩展过程中没有考虑到多次重复到同一个路点的问题，大大降低了路径扩展的效率。

因此，设计一种基于改进RRT的机械臂运动规划方法具有一定的价值意义。

发明内容

本发明针对现有技术的不足，提出了一种基于改进RRT算法的机械臂运动规划方法。本方法采用改进的RRT算法，可以有效减小规划的路径代价，为机械臂规划出一条优化的路径，并提高机械臂的平稳运行。

为达到上述目标，本发明采用如下技术方案：

一种基于改进RRT算法的机械臂运动规划方法的具体步骤包含如下：

步骤一：确认机器人的起始位姿和目标位姿，利用机器人的逆运动学分析，求得对应的关节值；

步骤二：初始化机器人的工作环境，确定环境中障碍物的基本信息；

步骤三：在传统RRT算法的基础上，引入了生成随机采样点组，和目标点引力势场的方法对算法的扩展进行处理；

步骤四：在得到了相对应的路径之后，利用适用于商旅问题的弹性带算法对得到的路径进行进一步的优化。在机器人的关节空间运用改进算法进行路径规划，得到了其对应的由起始点到目标点的安全路径；

步骤五：为了使机器人在运行过程中更加柔顺，在这里将会对得到的路径用B样条曲线函数进行拟合，并确保拟合后的曲线经过关键路径点。

所描述的步骤一分析如下：对于选取的串联机器人，要构建好其运动学模型。在确定了起始位姿和终点位姿之后利用构建的机器人模型，求出与之对应的关节角。起始状态的关节角可以设为xstart，目标状态的关节角可以设为xgoal。

所描述的步骤二分析如下：要对工作环境中可能会与机器人发生碰撞的一些障碍物进行位置和体型上的确定。这是由于在进行机器人路径规划的过程中，每个新节点的产生都需要映射到机器人的工作空间，判断在新节点的环境下是否会与环境发生碰撞。

所描述的步骤三具体如下：该步骤是是本方法的核心内容，就是在传统RRT算法的基础上进行改进，其原理如下：在确定起始位置xstart和终点位置xgoal之后，在起始位置的基础上构建随机树。在机器人的关节空间中选取一组随机采样点，并由与目标点的距离由近到远开始进行排序，依次选取xrandi，在随机树中选取距离最近的节点xnearrest,然后开始确定新节点延伸的方向。新节点的延伸方向是依据xrandi和xgoal共同参与下决定的，两者对方向的影响力会根据设置参数kp进行控制。确定方向之后，以xnearrest开始向xrandi方向按照既定的步长进行新节点的延伸，得到xnew。此时要对生成的xnew与xnearrest之间的路径进行碰撞检测，如果新生成的节点通过了碰撞检测，就可以将新节点添加到随机树中。接下来就可以对随机树不停地进行扩展，最终找到目标的节点。具体的步骤分析如下所示：

(1)对随机树T进行扩展，将已确定的xstart作为起始点进行扩展，即将其设为根节点。自由空间中选取一组随机采样点。随机采样点的选取是独立且随机的，但是要选取一组随机采样点。将这一组随机采样点计算求得距离目标点的距离，并依据得到的距离值从近到远进行排序。然后在随机树中找到距离第一个随机采样点最近的节点，记为xnearrest。

(2)确定新节点的延伸方向。在本方法中，新节点的延伸方向是由目标点和选取的随机采样点共同决定的。

(3)对新节点以及新节点的相应路径进行碰撞检测。如果检测未发现碰撞，则可以将新节点添加到随机树中，继续进行。如果发现产生了碰撞，首先判断随机采样点数组是否有剩余选项，如果有，那么将进行两处参数的修改：一是将选取的随机采样点按照要求后移，二是将上一步骤中的参数kp进行减少；如果没有，就需要从选取随机采样点数组开始进行重新地计算。

(4)判断新节点是否到达了目标节点。如果到达了，就结束随机树的生长。如果没有，就从第一步开始继续对随机树进行扩展。

(5)根据得到的随机树，从目标点开始找到完整的路径。

所描述的步骤四分析系如下：由于对路径的生成过程是随机的，因此得到的路径在局部上还是充满了曲折。在这里需要对路径进行优化。本方法中将利用弹性带原理，将得到的路径点看成弹性带中的一个个节点。在不会与障碍物发生碰撞的基础之上，将得到的路径进行拉伸，使路径在局部上更加符合最优路径的方式。

所描述的步骤五具体如下：为了使机器人的关节运动曲线在高阶导数上仍是连续的，可以利用B样条曲线函数进行拟合。此时我们已经知道了路径点，此时可以利用反求控制点的方法，使拟合后的曲线仍旧可以顺利通过既定的路径点。

与现有技术相比，本发明的有效增益包括：

首先本方法中在传统RRT算法中改进了新节点的生长方式，具体的方法是利用改变随机点的采样方式以及调整新节点的生长方向进行实现的；之后则是对已生成的路径利用弹性带方法进行优化，使部分路径更加平滑；最后是对于应用到机械臂上面时，将B样条曲线函数与得到的路径进行拟合，实现机械臂关节路径的高阶可导。

附图说明

图1为改进RRT算法应用与机械臂的运动规划方法流程图。

图2为改进RRT算法中随机点的生成选取示意图。

图3为改进RRT算法中新节点的生长示意图。

图4为改进RRT算法中利用弹性带算法对已存在路径进行优化示意图。

图5为改进RRT算法在平面环境中的应用示意图。

图6为改进RRT算法在七自由度机械臂运动规划中应用示意图。

具体实施方式

下面结合附图及实施例进一步详细介绍本发明的具体结构、工作原理的内容。

为了使本方法的目的、技术方案及优点更加清楚明晰，以下结合附图，对本发明进行进一步地详细说明。应当理解，此处所描述的具体实施实例仅用以解释，并不用于限定本发明。

如图1所示，一种基于改进RRT算法的机械臂运动规划方法，该方法的基本工作流程可以描述为：首先在基于MATLAB平台的Robotic System Toolbox中搭建仿真所需的环境；确定机械臂末端的起始位姿与目标位姿，并利用工具箱求取其运动学逆解；同时，对机械臂所工作的环境进行分析，判断工作环境中的障碍物等信息，为运动规划中的约束做好准备；利用改进的RRT算法对机械臂的运动路径进行规划；利用B样条曲线对机器人的运动关节进行拟合，得到高阶可导的关节路径。具体包括以下步骤：

步骤一：给定机械臂目标位置，通过逆运动学的方法获得目标位置的逆解；具体如下：搭建好七自由度机械臂模型，如图6所示，给定目标位置，通过求逆解求出其目标位置的各关节角，判断逆解是否存在，若存在，设定为目标状态xgoal；若不存在，重新设置目标点位置，直至求出可达的目标状态为止。

步骤二：获取环境中所有障碍物的大小、位置信息；具体如下：检测环境中所有障碍物，获取环境中所有障碍物的大小、位置信息，为步骤三中生成无碰撞路径提供碰撞信息。

步骤三：融合传统RRT算法，引入采用随机采样点组的方式构成随机采样点；以及引入目标点的虚拟引力势场影响新节点的生成，以实现改进RRT算法中随机树的生长；

步骤四：在步骤三中得到安全路径的基础上，将得到的路径利用弹性带原理对路劲进行优化。此步骤结合步骤三得到了完整的改进RRT算法，利用改进后的算法可以生成一条由起始点到终点的安全路径。

对步骤三与步骤四进行具体过程进行分析，描述如下：

(1)在算法的工作空间中确定随机树的起始点xinit，即随机树的根节点，同时，将xgoal作为目标点。

(2)在空间中生成一组随机采样点xrandm，个数为m个。同时计算该组随机采样点距离目标的距离，并根据距离，有小到大重新确定随机点组的排列顺序。并确定第一个随机采样点xrand1，计算随机树的节点距离该采样点的距离，并确定距离最近的节点xnearest.

(3)记录随机点中的采样点的次数m，确认得到的xrandi,以便后续的确认工作。

(4)在目标点设置虚拟引力，对新节点的生成产生引力作用。确认最近点到随机点的方向为F1＝xrandi-xnearest，最近点到目标点的方向为F2＝xgoal-xnearest。此时在两者的共同作用下，分析新节点的生产方向为F＝F1+kp*F2

(5)在得到了新节点的生长方向之后，得到新节点，xnew＝xnearest+F*λ，在得到新节点之后需要对新节点进行碰撞检测。若不发生碰撞，则可以将新节点添加到随机树中；如果发生碰撞，则将采样点次数m增加1，且将目标点引力系数减少返回到步骤(3)。若此时m的值超过了随机点数组的个数，则需要返回到步骤(2)。

(6)当随机树的新节点与目标点的距离小于设定的误差值时，就可以判断随机树到达目标点，并将xgoal加入到随机树中。若未到达目标点，将回到步骤(2)继续进行。

(7)在最终到达了目标点之后，寻找到可行路径。对于得到的路径，如图所示，路径点收到了弹性力的作用f，对路径点进行弹性带处理。最终可以得到了一个优化的路径，其中弹性力计算如下，q_n为获得的路径点：

步骤五：对机械臂进行路径规划后，已经得到了对应的路径点，此时结合B样条曲线函数，拟合后就可以得到期待的，具备高阶可导特性的路径。机械臂就可以按照得到的安全路径进行运动，并可以轻易得到符合要求的速度和加速度等信息，方便处理。其中，B样条曲线函数为：

在已知机械臂运动路径点，q_n以及机械臂运动的起始和终点状态，可以求出对应的B样条曲线控制点，从而得到拟合后且通过关键点的曲线函数。

(1)确定机械臂运动边界条件，初始速度v₀,初始加速度a₀，终点速度v_e，终点加速度a_e；

(2)建立条件方程组：

Ad＝p

其中的d是所求的样条曲线控制顶点，p是已知的机械臂路径点，A是相关参数。

d＝[d₀,d₁,d₂,...,d_n+k-1]^T

p＝[p₀,p₁,...,p_n,v_o,v_e,a_o,a_e]

(3)求解方程组，得到控制点进而获得拟合后的机械臂运动轨迹。

本方法在基于改进RRT算法的基础之上，对机械臂进行了路径规划。并且后续的路径优化和柔顺处理对机器人的安全工作有着积极的作用。符合预期。

Claims

1.一种基于改进RRT算法的机械臂运动规划方法，其特征在于，根据串联机器人的路径规划的要求和特点，有以下步骤：

2.根据权利要求1中所述的基于改进RRT算法的机械臂运动规划方法，其特征在于，所描述的步骤一如下：对于选取的串联机器人，要构建好其运动学模型；在确定了起始位姿和终点位姿之后利用构建的机器人模型，求出与之对应的关节角；起始状态的关节角可以设为xstart，目标状态的关节角可以设为xgoal。

3.根据权利要求1中所述的基于改进RRT算法的机械臂运动规划方法，其特征在于，所描述的步骤二具体如下：要对工作环境中可能会与机器人发生碰撞的一些障碍物进行位置和体型上的确定；这是由于在进行机器人路径规划的过程中，每个新节点的产生都需要映射到机器人的工作空间，判断在新节点的环境下是否会与环境发生碰撞。

4.根据权利要求1中所述的基于改进RRT算法的机械臂运动规划方法，其特征在于，所描述的步骤三具体如下：该步骤是是本方法的核心内容，就是在传统RRT算法的基础上进行改进，其原理如下：在确定起始位置xstart和终点位置xgoal之后，在起始位置的基础上构建随机树；在机器人的关节空间中选取一组随机采样点，并由与目标点的距离由近到远开始进行排序，依次选取xrandi，在随机树中选取距离最近的节点xnearrest,然后开始确定新节点延伸的方向；新节点的延伸方向是依据xrandi和xgoal共同参与下决定的，两者对方向的影响力会根据设置参数kp进行控制；确定方向之后，以xnearrest开始向xrandi方向按照既定的步长进行新节点的延伸，得到xnew；此时要对生成的xnew与xnearrest之间的路径进行碰撞检测，如果新生成的节点通过了碰撞检测，就可以将新节点添加到随机树中；接下来就可以对随机树不停地进行扩展，最终找到目标的节点，具体的步骤分析如下所示：

(1)对随机树T进行扩展，将已确定的xstart作为起始点进行扩展，即将其设为根节点；自由空间中选取一组随机采样点，随机采样点的选取是独立且随机的，但是要选取一组随机采样点；将这一组随机采样点计算求得距离目标点的距离，并依据得到的距离值从近到远进行排序；然后在随机树中找到距离第一个随机采样点最近的节点，记为xnearrest；

(2)确定新节点的延伸方向；在本方法中，新节点的延伸方向是由目标点和选取的随机采样点共同决定的；

(3)对新节点以及新节点的相应路径进行碰撞检测；如果检测未发现碰撞，则可以将新节点添加到随机树中，继续进行；如果发现产生了碰撞，首先判断随机采样点数组是否有剩余选项，如果有，那么将进行两处参数的修改：一是将选取的随机采样点按照要求后移，二是将上一步骤中的参数kp进行减少；如果没有，就需要从选取随机采样点数组开始进行重新地计算；

(4)判断新节点是否到达了目标节点；如果到达了，就结束随机树的生长；如果没有，就从第一步开始继续对随机树进行扩展；

(5)根据得到的随机树，从目标点开始找到完整的路径。

5.根据权利要求1中所述的基于改进RRT算法的机械臂运动规划方法，其特征在于，所描述的步骤四具体如下：由于对路径的生成过程是随机的，因此得到的路径在局部上还是充满了曲折；在这里需要对路径进行优化；本方法中将利用弹性带原理，将得到的路径点看成弹性带中的一个个节点；在不会与障碍物发生碰撞的基础之上，将得到的路径进行拉伸，使路径在局部上更加符合最优路径的方式。

6.根据权利要求1中所述的基于改进RRT算法的机械臂运动规划方法，其特征在于，所描述的步骤五具体如下：为了使机器人的关节运动曲线在高阶导数上仍是连续的，可以利用B样条曲线函数进行拟合；此时我们已经知道了路径点，此时可以利用反求控制点的方法，使拟合后的曲线仍旧可以顺利通过既定的路径点。