技术特征：
1.一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于，包括以下步骤：步骤1、获取机械臂工作空间障碍物分布情况，建立机械臂碰撞模型和运动学模型，初始化两颗随机树：tree1和tree2，包括起点x_start和终点x_goal,阈值thr,最大迭代次数maxiterations，步长stepsize，搜索范围searchsize，地图环境；步骤2、随机树tree1和tree2通过目标偏置策略交替采样，生成随机点x_rand；步骤3、搜索近邻节点x_near，然后x_near沿着与x_rand连线方向以步长stepsize进行扩展，生成新节点newpoint，扩展完进行碰撞检测，若不发生碰撞，则将新节点添加到随机树tree1和tree2中,若发生碰撞，则重新采样；步骤4、将扩展的新节点newpoint作为圆心，搜索已有树中距离newpoint距离小于半径r的近邻节点集q_near，将其中距离最小的节点x_min作为newpoint新的父节点，之后进行剪枝rewire操作，新节点newpoint重新连接到q_near中除了x_min之外路径最短的节点；步骤5、计算新节点newpoint与随机树tree1和tree2中已有节点之间的距离，并将其与距离阈值d进行比较；如果新节点与已有节点的距离大于等于阈值d，则保留新节点，如果小于阈值d，则去除随机树中与newpoint重合的节点，距离函数如下：其中，a和b为节点坐标，a(1)、a(2)、a(3)分别代表三维空间中节点a的xyz坐标，b(1)、b(2)、b(3)分别代表三维空间中节点b的xyz坐标；步骤6、随机树tree1和tree2连接成功后，继续进行交替采样，如达到最大迭代次数maxiterations，则输出路径，按照路径缩短策略缩短路径，最后用三次b样条曲线平滑路径。2.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤1中，根据机械臂实际工作场景和机械臂相关参数确定机械臂工作需要的起点x_start、终点x_goal、搜索范围searchsize、地图环境，地图环境包含障碍物分布情况。3.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤2中的目标偏置策略，通过引入目标偏置概率p1来考虑目标点的影响，使生成新随机点时有一定概率朝向目标点，随机树tree1和tree2生成新随机点x_rand1和x_rand2的采样方式为：rand2的采样方式为：其中，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。
4.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤4中的剪枝rewire操作，针对新节点newpoint，找到距离它小于等于r的所有节点的集合q_near；对于q_near中的每个节点xi，检查是否存在一条从newpoint到xi的路径，使得路径的代价小于xi当前的代价值，如果存在这样的路径，则将xi的父节点设置为newpoint，以及更新xi的代价值和xi所有子节点的代价值。5.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤6中的随机树tree1和tree2连接成功的条件是：新加入的节点newpoint与另一个树的节点距离小于阈值thr且通过碰撞检测。6.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤6中的路径缩短策略是对达到最大迭代次数时的路径进行缩短，检查路径中连续的三个点，判断是否存在碰撞和需要删除的点，然后用首末两个点之间的直线段来代替原先这三个点组成的路径，并将首尾两个点添加至路径中。7.根据权利要求1所述的一种基于改进rrtstar的六轴机械臂避障路径规划方法，其特征在于：所述步骤6中的用三次b样条曲线对路径缩短策略得到的路径进行平滑，采用拟合的方式，其中涉及的公式如下：b
0，3
(t)＝(1-t)3/6b
1，3
(t)＝1/6(3t3-6t2 4)b
2，3
(t)＝1/6(-3t3 3t2 3t 1)b
3，3
(t)＝t3/6其中，t为节点向量，表示在曲线上的位置，取值范围是[0，1]，区间内的实数b0，3(t)、b1，3(t)、b2，3(t)和b3，3(t)都是b样条基函数，用于构造三次b样条曲线；三次b样条曲线方程为上述四个基函数的线性组合，方程如下：p(t)＝p0b0，3(t) p1b1，3(t) p2b2，3(t) p3b3，3(t)其中，p(t)为总的曲线方程，p0，p1，p2，p3是四个控制点的坐标值；将平滑后的最终路径点传输给机械臂，机械臂根据获取到的路径点来执行工作任务。
技术总结
本发明公开了一种基于改进rrtstar的六轴机械臂避障路径规划方法，属于机械臂路径规划技术领域。本发明针对rrtstar算法搜索空间大，搜索时间长，计算复杂程度高，且生成的路径折角过多不平滑不利于机械臂工作这些缺点，首先引入目标偏置策略，并结合双树扩展策略从起点和目标点同时扩展，加入缩短路径策略和去除冗余节点策略，最后对搜索路径进行b样条拟合优化，通过此方法可以有效减少搜索时间，降低搜索复杂度，缩短路径同时实现路径平滑化，使机械臂末端在工作过程中顺利避障，实现高效安全的路径规划。的路径规划。的路径规划。
技术研发人员：李丁 张宇
受保护的技术使用者：昆明理工大学
技术研发日：2023.07.21
技术公布日：2023/10/10