一种基于改进rrtstar的六轴机械臂避障路径规划方法
技术领域
1.本发明属于机械臂路径规划技术领域，具体涉及一种基于改进rrtstar的六轴机械臂避障路径规划方法。
背景技术：
2.机器人避障路径规划是机器人控制的一个重要领域，它涉及到如何在复杂的环境中，让机器人自主避开各种障碍物。常见的机器人避障路径规划算法包括基于图搜索的算法和基于采样的算法。基于图搜索的算法如dijkstra算法和astar算法，通过建立环境地图，将机器人和障碍物抽象为图中的节点，利用图搜索算法搜索最短路径。这种算法计算效率高，能够找到最短路径，但在高维空间和复杂约束环境下，搜索空间会变得非常大，从而增加了计算复杂性，导致计算时间变长。
3.基于采样的算法如rrt(rapidly-exploring random tree)算法和prm(probabilistic roadmap)算法，则通过随机采样机器人姿态和环境数据，建立机器人运动空间的采样图，利用图搜索算法搜索可行路径。这种算法具有快速探索能力和随机性质，能够在复杂环境中获得较好的表现。因此，在机械臂路径规划中，采用基于采样的算法，可以帮助机械臂找到可行的、高效的路径，并避开障碍物。
4.快速随机扩展树算法是一种常用的机械臂路径规划算法，通过随机采样策略使得能够在大范围内搜索可行解，从而更有可能找到全局最优解。但是可能会存在采样点分布不均匀的情况，从而导致规划出的路径不够平滑，机械臂在运动时可能会出现震动等问题。但是随机采样可能会存在采样点分布不均匀的情况，从而导致规划出的路径不够平滑，生成的路径质量较低，使得机械臂在运动时可能会出现震动等问题。
技术实现要素：
5.针对现有技术的不足，提供一种搜索时间少、路径短且平滑的机械臂避障路径规划方法。
6.为解决上述技术问题，本发明的技术方案是：基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于，该方法包含以下步骤：
7.步骤1、获取机械臂工作空间障碍物分布情况，建立机械臂碰撞模型和运动学模型，初始化两颗随机树：tree1和tree2，包括起点x_start和终点x_goal,阈值thr,最大迭代次数maxiterations，步长stepsize，搜索范围searchsize，地图环境。
8.步骤2、随机树tree1和tree2通过目标偏置策略交替采样，生成随机点x_rand；
9.步骤3、搜索近邻节点x_near，然后x_near沿着与x_rand连线方向以步长stepsize进行扩展，生成新节点newpoint，扩展完进行碰撞检测，若不发生碰撞，则将新节点添加到随机树tree1和tree2中,若发生碰撞，则重新采样；
10.步骤4、将扩展的新节点newpoint作为圆心，搜索已有树中距离newpoint距离小于半径r的近邻节点集q_near，将其中距离最小的节点x_min作为newpoint新的父节点，之后
进行剪枝rewire操作，新节点newpoint重新连接到q_near中除了x_min之外路径最短的节点。
11.步骤5、计算新节点newpoint与随机树tree1和tree2中已有节点之间的距离，并将其与距离阈值d进行比较；如果新节点与已有节点的距离大于等于阈值d，则保留新节点，如果小于阈值d，则去除随机树中与newpoint重合的节点，距离函数如下：
[0012][0013]
其中，a和b为节点坐标，a(1)、a(2)、a(3)分别代表三维空间中节点a的xyz坐标，b(1)、b(2)、b(3)分别代表三维空间中节点b的xyz坐标；
[0014]
步骤6、随机树tree1和tree2连接成功后，继续进行交替采样，如达到最大迭代次数maxiterations，则输出路径，按照路径缩短策略缩短路径，最后用三次b样条曲线平滑路径。
[0015]
进一步地，所述步骤1中，根据机械臂实际工作场景和机械臂相关参数确定机械臂工作需要的起点x_start、终点x_goal、还有搜索范围searchsize、地图环境，地图环境包含障碍物分布情况。
[0016]
进一步地，所述步骤2中目标偏置策略，通过引入目标偏置概率p1来考虑目标点的影响，使生成新随机点时有一定概率朝向目标点。随机树tree1和tree2生成新随机点x_rand1和x_rand2的采样方式为：
[0017][0018][0019]
其中，p是随机生成的(0,1)范围内的随机数，对于随机树tree1，如果随机数p大于目标偏置概率p1，那么x_rand1将朝着目标点x_goal的方向生成一个新的随机点；如果随机数p小于等于目标偏置概率p1，那么x_rand1将是一个随机采样的点sample。对于随机树tree2，如果p大于目标偏置概率p1，那么x_rand2将朝着起点x_start的方向生成一个新的随机点；如果随机数p小于等于目标偏置概率p1，那么x_rand2将是一个随机采样的点sample。
[0020]
进一步地，所述步骤4中的剪枝rewire操作，针对新节点newpoint，找到距离它小于等于r的所有节点的集合q_near。
[0021]
对于q_near中的每个节点xi，检查是否存在一条从newpoint到xi的路径，使得路径的代价小于xi当前的代价值。如果存在这样的路径，则将xi的父节点设置为newpoint，以及更新xi的代价值和xi所有子节点的代价值。
[0022]
进一步地，所述步骤6中的随机树tree1和tree2连接成功的条件是：新加入的节点newpoint(从tree1和tree2扩展的节点)与另一个树的节点距离小于阈值thr且通过碰撞检测。
[0023]
进一步地，所述步骤6中的路径缩短策略是对达到最大迭代次数时的路径进行缩短，基本思想是检查路径中连续的三个点，判断是否存在碰撞和需要删除的点。然后用首末两个点之间的直线段来代替原先这三个点组成的路径，并将首尾两个点添加至路径中。
[0024]
进一步地，所述步骤6中的三次b样条曲线对路径缩短策略得到的路径进行平滑，
采用拟合的方式，其中涉及的公式如下：
[0025]b0,3
(t)＝(1-t)3/6
[0026]b1,3
(t)＝1/6(3t
3-6t2 4)
[0027]b2,3
(t)＝1/6(-3t3 3t2 3t 1)
[0028]b3,3
(t)＝t3/6
[0029]
其中，t为节点向量，表示在曲线上的位置，取值范围是[0,1],区间内的实数b
0,3
(t)、b
1,3
(t)、b
2,3
(t)和b
3,3
(t)都是b样条基函数，用于构造三次b样条曲线。
[0030]
三次b样条曲线方程为上述四个基函数的线性组合，方程如下：
[0031]
p(t)＝p0b
0,3
(t) p1b
1,3
(t) p2b
2,3
(t) p3b
3,3
(t)
[0032]
其中，p(t)为总的曲线方程，p0,p1,p2,p3是四个控制点的坐标值。
[0033]
将平滑后的最终路径点传输给机械臂，机械臂根据获取到的路径点来执行工作任务。
[0034]
本发明的有益效果是：本发明所提供的一种基于改进rrtstar的六轴机械臂避障路径规划方法，可以有效减少搜索时间，降低搜索复杂度，缩短路径同时实现路径平滑化，使机械臂末端在工作过程中顺利避障，实现高效安全的路径规划。
附图说明
[0035]
图1是本发明的改进rrtstar算法流程图；
[0036]
图2是本发明的路径缩短策略原理图；
[0037]
图3是本发明实施例的三维空间中的改进rrtstar算法路径搜索过程图；
[0038]
图4是本发明实施例的未经优化的初始避障路径图；
[0039]
图5是本发明实施例的经过b样条平滑后的避障路径图；
[0040]
图6是本发明实施例的初始避障路径和平滑后的避障路径效果对比图。
具体实施方式
[0041]
下面结合附图和具体实施例对本发明做进一步的说明：
[0042]
实施例1：如图1至图6所示，本发明提供的一种基于改进rrtstar的六轴机械臂避障路径规划方法，包括以下步骤：
[0043]
步骤1、获取机械臂工作空间障碍物分布情况，建立机械臂碰撞模型和运动学模型，初始化两颗随机树：tree1和tree2，包括起点x_start和终点x_goal,阈值thr,最大迭代次数maxiterations，步长stepsize，搜索范围searchsize，地图环境。
[0044]
在本步骤中，根据机械臂实际工作场景和机械臂相关参数确定机械臂工作需要的起点x_start、终点x_goal、还有搜索范围searchsize、地图环境，地图环境包含障碍物分布情况。
[0045]
在本实施例中，本发明的方法在型号为hw700六轴机械臂进行使用，机械臂的有效负载3kg。
[0046]
首先初始化机械臂，然后移动机械臂末端执行器至终点，通过传感器获取机械臂工作路径末端点的起点和终点以及空间中障碍物分布情况，同时构建三维空间地图模型，标出起点x_start和终点x_goal，然后进行初始化两颗随机树将起点x_start和终点x_
goal,阈值thr,最大迭代次数maxiterations，步长stepsize，搜索范围searchsize，地图环境传递给改进rrtstar算法。
[0047]
步骤2、随机树tree1和tree2通过目标偏置策略交替采样，生成随机点x_rand
[0048]
在本步骤中的目标偏置策略，是通过引入目标偏置概率p1来考虑目标点的影响，使生成新随机点时有一定概率朝向目标点。随机树tree1和tree2生成新随机点x_rand1和x_rand2的采样方式为：
[0049][0050][0051]
其中，p是随机生成的(0,1)范围内的随机数，对于随机树tree1，如果随机数p大于目标偏置概率p1，那么x_rand1将朝着目标点x_goal的方向生成一个新的随机点；如果随机数p小于等于目标偏置概率p1，那么x_rand1将是一个随机采样的点sample。对于随机树tree2，如果p大于目标偏置概率p1，那么x_rand2将朝着起点x_start的方向生成一个新的随机点；如果随机数p小于等于目标偏置概率p1，那么x_rand2将是一个随机采样的点sample。
[0052]
本步骤中，生成随机点之前，要先判断是否超过最大迭代次数maxiterations，如果是，则结束采样，输出结果，如果未超过，则迭代次数加一，继续采样生成新随机点x_rand。
[0053]
步骤3、搜索近邻节点x_near，然后x_near沿着与x_rand连线方向以步长stepsize进行扩展，生成新节点newpoint，扩展完进行碰撞检测，若不发生碰撞，则将新节点添加到随机树中,若发生碰撞，则重新采样。
[0054]
步骤4、将扩展的新节点newpoint作为圆心，搜索已有树中距离newpoint距离小于半径r的近邻节点集q_near，将其中距离最小的节点x_min作为newpoint新的父节点，之后进行剪枝rewire操作，新节点newpoint重新连接到q_near中除了x_min之外路径最短的节点。
[0055]
本步骤中的剪枝rewire操作，针对新节点newpoint，找到距离它小于等于r的所有节点的集合q_near。
[0056]
对于q_near中的每个节点xi，检查是否存在一条从newpoint到xi的路径，使得路径的代价小于xi当前的代价值。如果存在这样的路径，则将xi的父节点设置为newpoint，以及更新xi的代价值和xi所有子节点的代价值。
[0057]
步骤5、计算新节点与随机树中已有节点之间的距离，并将其与距离阈值d进行比较；如果新节点与最近节点的距离小于阈值d，则去除随机树中与newpoint重合的节点，距离函数如下：
[0058][0059]
其中，x和y为两个节点的坐标。
[0060]
步骤6、如达到最大迭代次数maxiterations，则输出路径，按照路径缩短策略缩短路径，最后用三次b样条曲线平滑路径。
[0061]
本步骤中随机树tree1和tree2连接成功的条件是：当前树中新加入的节点
newpoint(从tree1和tree2扩展的节点)与另一个树的节点距离小于阈值thr且通过碰撞检测。
[0062]
本步骤中的路径缩短策略是对达到最大迭代次数时的路径进行缩短，基本思想是检查路径中连续的三个点，判断是否存在碰撞和需要删除的点。然后用首末两个点之间的直线段来代替原先这三个点组成的路径，并将首尾两个点添加至路径中。
[0063]
本步骤中的三次b样条曲线对路径缩短策略得到的路径进行平滑，采用拟合的方式，其中涉及的公式如下：
[0064]b0,3
(t)＝(1-t)3/6
[0065]b1,3
(t)＝1/6(3t
3-6t2 4)
[0066]b2,3
(t)＝1/6(-3t3 3t2 3t 1)
[0067]b3,3
(t)＝t3/6
[0068]
其中，t为节点向量，表示在曲线上的位置，取值范围是[0,1],区间内的实数b
0,3
(t)、b
1,3
(t)、b
2,3
(t)和b
3,3
(t)都是b样条基函数，用于构造三次b样条曲线。
[0069]
三次b样条曲线方程为上述四个基函数的线性组合，方程如下：
[0070]
p(t)＝p0b
0,3
(t) p1b
1,3
(t) p2b
2,3
(t) p3b
3,3
(t)
[0071]
其中，p(t)为总的曲线方程，p0,p1,p2,p3是四个控制点的坐标值
[0072]
将平滑后的最终路径点传输给机械臂，机械臂根据获取到的路径点来执行工作任务。
[0073]
本次实验仿真在matlab2022b进行，在设置有五个球形障碍物的三维空间地图进行仿真，起点x_start坐标为(15,15,15)，终点x_goal坐标为(160,160,160)，球形障碍物球心的坐标分别为：(100,100,105)、(50,54,50)、(100,47,64)、(143,100,108)、(55,137,53)；半径均为14.5；搜索范围searchsize大小为[250 250 250]，步长stepsize为13，阈值thr为10。
[0074]
图3为三维空间中的改进rrtstar算法路径搜索过程图，图中是加入了目标偏置策略、双树交替扩展、路径缩短策略、剪枝rewire操作和去除冗余节点后的搜索效果图。
[0075]
图4与图5进行对比可以看出，明显减少了路径的弯曲度和长度，提高了路径的平滑度，路径中不存在折角，规划出来的路径更加简洁，并且平滑化之后的路径也是无碰撞路径，不会导致机械臂与工作空间中的障碍物发生碰撞。
[0076]
图6中，实线表示改进rrtstar算法规划出的原始避障路径经过三次b样条平滑后的拟合优化路径，带star的线表示初始路径，由此图可以看出，本发明中的算法规划出来的路径更短且更加平滑。
[0077]
在本领域的技术人员看来，本发明公开的实施例旨在帮助读者理解本发明的原理和实现方式，而不是限定本发明的保护范围。因此，本领域的技术人员可以根据本发明公开的技术启示进行各种变形和组合，只要这些变形和组合不脱离本发明的实质，仍然在本发明的保护范围内。