(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210671495.5 (22)申请日 2022.08.18 (71)申请人 安徽大学 地址 230601 安徽省合肥市经济技 术开发 区九龙路1 11号 (72)发明人 董翔　叶蒙蒙　黄泽霞　 (74)专利代理 机构 合肥国和专利代理事务所 (普通合伙) 34131 专利代理师 侯婷　张祥骞 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于改进RRT算法的机械臂路径 规划方 法 (57)摘要 本发明公开了一种基于改进RRT算法的机械 臂路径规划方法， 首先以合向量的形式生成节 点， 有利于快速找到路径； 其次使用动态步长一 定程度上使得算法避免了在复杂障碍物环境下 难以找到路径的问题， 防随机树逆生长避免了后 期节点过多而导致的大量计算； 进一步， 基于三 角剪枝策略在初始路径的基础上有效减少了路 径代价， 并使用三次B样条曲线对剪枝后的路径 光滑处理， 降低了机械臂的抖动， 减少机械臂运 动过程中而产生的磨损。 通过改进RRT算法降低 了路径距离且优化了路径质量， 避免盲目采样的 同时提高了规划效率。 权利要求书2页 说明书9页 附图5页 CN 115056222 A 2022.09.16 CN 115056222 A 1.一种基于改进R RT算法的机 械臂路径规划方法， 其特 征在于， 所述方法包括： S1， 初始化树T， 给定起始点xinit、 目标点xgoal、 扩展步长eps， 最大迭代次数max、 空间障 碍物信息obs、 目标点阈值m； S2， 采用概率偏置生成随机点xrand； 当rand＜bias 时， 采样点xrand向目标点xgoal采样； 否 则， 采样点xrand随机采样； S3， 在树T中寻找距离随机点最近的近邻点xnear， 判断xnear是否进入目标点阈值范围内， 若进入则直接缩短步长为ep s1， 其中， ep s1<m， 反之采用原步长； 沿着 与 合向量的方向以当前步长生成新节点xnew； S4， 当生成新节点xnew后， 判断xnew节点是否进入 目标点邻域， 若进入 目标点邻域， 则直 接连线xnew与xgoal， 生成初始路径， 退出循环， 进行步骤S5； 反之， 以该点xnew为圆心， R为半 径， 计算圆内所有节点到xnew节点的距离， 并选择距离xnew节点最近的节点作为新生成节点 的父节点， 并将xnew节点加入到树T中， 再次返回步骤S2， 直至找到初始路径或者达到最大迭 代次数退出循环； S5， 对初始路径点使用三角剪枝策略， 采用三角剪枝对初始路径处理， 记输出的初始路 径点为[T1， T2， T3， ...,TN]， 从目标点开始依次向上回溯节点， 若两节点之间[Ti,Tm]满足碰 撞检测， 则将该两点之间的其余节点全部剔除； 若发生碰撞， 则以不满 足碰撞检测的节点Ti 作为起点， 再依次向上回溯， 重复上述操作， 直至遍历完路径序列； S6， 基于三次B样条平 滑路径对剪枝后的路径 平滑处理， 获得最终路径。 2.根据权利要求1所述的基于改进RRT算法的机械臂路径规划方法， 其特征在于， 步骤 S3之后， 以及步骤S4之前还 包括： S31， 判断向量 与向量 间的夹角 θ 是否在 范围内， 若满足则进行 步骤S32， 不满足则返回步骤S2； S32， 连线 判断连线是否与空间障碍物发生碰撞； 若产生碰撞， 则缩短步长 eps， 并返回步骤S2， 当再次进入到步骤S3时， 反之采用原 步长则更改为采用缩短后的步长； 若不发生碰撞， 进行步骤S4。 3.根据权利要求2所述的基于改进RRT算法的机械臂路径规划方法， 其特征在于， 步骤 S3， 还包括： 设定向量P1为 其控制参数为动态步长λ， 向量P2为 权重系数为τ； 在 向量P1与P2的共同作用下， 沿着P1与P2的合向量P3的方向生成新节点xnew； 其中， 当生成的新节点与障碍物发生碰撞， 则缩短步长； 当没有发生碰撞时， 恢复为原 始步长。 4.根据权利 要求3所述的基于改进RRT算法的机械臂路径规划方法， 其特征在于， S6， 基 于三次B样条平 滑路径对剪枝后的路径 平滑处理， 获得最终路径， 包括：权　利　要　求　书 1/2 页 2 CN 115056222 A 2设置B样条的控制点表示为Mj， j＝0,1,2,3...， B 样条的基函数表示为Nj,k(u)， 则B样条的 数学描述函数记为： 其中Nj,0(u)的取值满足以下关系： 三次B样条基函数为： 代入B样条的数 学描述函数： M0,3(u)＝M0×N0,3(u)+M1×N1,3(u)+M2×N2,3(u)+M3×N3,3(u)u∈[0,1] 给定控制点Mj， 利用上式求出满足要求的曲线点； B样条的起始点为线段M1M1″的三分之 一处， 终点 位于M2M2″线段的三分之一处； 其中， 起始点处的切线与M1″M2平行， 终点处的切线平行于M2″M3； M1M1″与M2M2″分别为三 角形M0M1M2和三角形M1M2M3的中线； 将分段曲线结合 起来， 形成一条完整且光滑的B样条曲线， 作为 最终路径。 S6， 基于三次B样条平 滑路径对剪枝后的路径 平滑处理， 获得最终路径。 5.一种电子装置， 所述电子装置包括： 一个或多个处理器， 存储器， 所述存储器用于存 储一个或多个计算机程序； 其特征在于， 所述计算机程序被配置成由所述一个或多个处理 器执行， 所述程序包括用于执行如权利要求 1‑4任一项所述的基于改进RRT算法的机械臂路 径规划方法步骤。 6.一种存储介质， 所述存储介质存储有计算机程序； 所述程序由处理器加载并执行以 实现如权利要求1 ‑4任一项所述的基于改进R RT算法的机 械臂路径规划方法步骤。权　利　要　求　书 2/2 页 3 CN 115056222 A 3