一种基于改进RRT算法的机械臂路径规划方法技术

本发明专利技术公开了一种基于改进RRT算法的机械臂路径规划方法，首先以合向量的形式生成节点，有利于快速找到路径；其次使用动态步长一定程度上使得算法避免了在复杂障碍物环境下难以找到路径的问题，防随机树逆生长避免了后期节点过多而导致的大量计算；进一步，基于三角剪枝策略在初始路径的基础上有效减少了路径代价，并使用三次B样条曲线对剪枝后的路径光滑处理，降低了机械臂的抖动，减少机械臂运动过程中而产生的磨损。通过改进RRT算法降低了路径距离且优化了路径质量，避免盲目采样的同时提高了规划效率。同时提高了规划效率。同时提高了规划效率。

【技术实现步骤摘要】
一种基于改进RRT算法的机械臂路径规划方法


[0001]本专利技术涉及路径规划术领域，具体而言，涉及一种基于改进RRT算法的机械臂路径规划方法。

技术介绍

[0002]路径规划技术于20世纪70年代被提出，至今仍是机器人领域的重要研究方向之一。经过50多年的快速发展，许多理论算法被相继提出，用于解决路径规划问题。
[0003]机械臂的避障规划目标是规划出一条满足各类指标的最优路径，针对这一问题，路径规划算法大致可以被归纳为以下几种：基于图论路径规划算法、人工势场法、基于生物启发路径规划算法、基于离散采样路径规划算法。其中，基于图论路径规划算法的复杂度依赖于网格分辨率，将其应用于三维空间时，计算复杂度高且耗时，不太适用于机械臂的工作环境。人工势场法寻找路径时经常陷入局部最小值，因此不能保证一定找到解。基于生物启发路径规划算法需要通过大量的训练获取算法参数，运算量大，耗时久、需要占用较大的内存空间。基于离散采样算法，参数少，结构简单，算法的复杂度不依赖于地图的复杂度，并且具备概率完备性。因此，离散采样算法比较适合应用于高维环境的路径规划。
[0004]而RRT路径规划算法作为离散采样算法的一种，不仅搜索能力强，且不需要对环境进行建模，易与其它算法结合解决高纬空间、复杂约束环境下的路径规划，不足之处在于算法的后期节点利用率偏低，计算量偏大，生成的路径不稳定。
[0005]现有技术优化后的RRT算法，例如专利CN 114115239 A虽加快了算法的搜索速度，但生成的路径具有较多的转折点，不满足机器人的运动学规律；专利CN 110497403 A采用双向RRT策略，没有考虑到路径平滑问题。纵观现有技术的优化后的RRT算法，只是单一地提升了某项性能，例如减少了规划时间、缩短了路径代价，并没有考虑到路径代价与规划时间两者之间的平衡；其次，规划出的路径常伴有较多的转折点，没有考虑到转折点处的路径点会对机械臂造成物理上的冲击，这种状况的出现将会加剧机械臂的磨损。

技术实现思路

[0006]有鉴于此，本专利技术实施例的目的在于提供一种基于改进RRT算法的机械臂路径规划方法，以提高机械臂路径规划效率的同时确保路径代价较小，且生成的路径光滑，满足机械臂的运动学规律。
[0007]本专利技术的第一方面提供了一种基于改进RRT算法的机械臂路径规划方法，所述方法包括：
[0008]S1，初始化树T，给定起始点x
init
、目标点x
goal
、扩展步长eps，最大迭代次数max、空间障碍物信息obs、目标点阈值m；
[0009]S2，采用概率偏置生成随机点x
rand
；当rand＜bias时，采样点x
rand
向目标点x
goal
采样；否则，采样点x
rand
随机采样；
[0010]S3，在树T中寻找距离随机点最近的近邻点x
near
，判断x
near
是否进入目标点阈值范
围内，若进入则直接缩短步长为eps1，其中，eps1<m，反之采用原步长；沿着与合向量的方向以当前步长生成新节点x
new
；
[0011]S4，当生成新节点x
new
后，判断x
new
节点是否进入目标点邻域，若进入目标点邻域，则直接连线x
new
与x
goal
，生成初始路径，退出循环，进行步骤S5；反之，以该点x
new
为圆心，R为半径，计算圆内所有节点到x
new
节点的距离，并选择距离x
new
节点最近的节点作为新生成节点的父节点，并将x
new
节点加入到树T中，再次返回步骤S2，直至找到初始路径或者达到最大迭代次数退出循环；
[0012]S5，对初始路径点使用三角剪枝策略，采用三角剪枝对初始路径处理，记输出的初始路径点为[T1，T2，T3，...,T
N
]，从目标点开始依次向上回溯节点，若两节点之间[T
i
,T
m
]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T
i
作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列；
[0013]S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。
[0014]进一步，步骤S3之后，以及步骤S4之前还包括：
[0015]S31，判断向量与向量间的夹角θ是否在范围内，若满足则进行步骤S32，不满足则返回步骤S2；
[0016]S32，连线判断连线是否与空间障碍物发生碰撞；若产生碰撞，则缩短步长eps，并返回步骤S2，当再次进入到步骤S3时，反之采用原步长则更改为采用缩短后的步长；若不发生碰撞，进行步骤S4。
[0017]进一步，步骤S3，还包括：
[0018]设定向量P1为其控制参数为动态步长λ，向量P2为权重系数为τ；在向量P1与P2的共同作用下，沿着P1与P2的合向量P3的方向生成新节点x
new
；
[0019][0020]其中，当生成的新节点与障碍物发生碰撞，则缩短步长；当没有发生碰撞时，恢复为原始步长。
[0021]进一步，S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径，包括：
[0022]设置B样条的控制点表示为M
j
，j＝0,1,2,3...，B样条的基函数表示为N
j,k(u)
，则B样条的数学描述函数记为：
[0023][0024]其中N
j,0(u)
的取值满足以下关系：
[0025][0026][0027]三次B样条基函数为：
[0028][0029]代入B样条的数学描述函数：
[0030]M
0,3
(u)＝M0×
N
0,3
(u)+M1×
N
1,3
(u)+M2×
N
2,3
(u)+M3×
N
3,3
(u)u∈[0,1][0031]给定控制点M
j
，利用上式求出满足要求的曲线点；B样条的起始点为线段M1M1″
的三分之一处，终点位于M2M2″
线段的三分之一处；
[0032]其中，起始点处的切线与M1″
M2平行，终点处的切线平行于M2″
M3；M1M1″
与M2M2″
分别为三角形M0M1M2和三角形M1M2M3的中线；
[0033]将分段曲线结合起来，形成一条完整且光滑的B样条曲线，作为最终路径。
[0034]此外，本专利技术的第二方面提供了一种电子装置，所述电子装置包括：一个或多个处理器，存储器，所述存储器用于存储一个或多个计算本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进RRT算法的机械臂路径规划方法，其特征在于，所述方法包括：S1，初始化树T，给定起始点x
init
、目标点x
goal
、扩展步长eps，最大迭代次数max、空间障碍物信息obs、目标点阈值m；S2，采用概率偏置生成随机点x
rand
；当rand＜bias时，采样点x
rand
向目标点x
goal
采样；否则，采样点x
rand
随机采样；S3，在树T中寻找距离随机点最近的近邻点x
near
，判断x
near
是否进入目标点阈值范围内，若进入则直接缩短步长为eps1，其中，eps1<m，反之采用原步长；沿着与合向量的方向以当前步长生成新节点x
new
；S4，当生成新节点x
new
后，判断x
new
节点是否进入目标点邻域，若进入目标点邻域，则直接连线x
new
与x
goal
，生成初始路径，退出循环，进行步骤S5；反之，以该点x
new
为圆心，R为半径，计算圆内所有节点到x
new
节点的距离，并选择距离x
new
节点最近的节点作为新生成节点的父节点，并将x
new
节点加入到树T中，再次返回步骤S2，直至找到初始路径或者达到最大迭代次数退出循环；S5，对初始路径点使用三角剪枝策略，采用三角剪枝对初始路径处理，记输出的初始路径点为[T1，T2，T3，...,T
N
]，从目标点开始依次向上回溯节点，若两节点之间[T
i
,T
m
]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T
i
作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列；S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。2.根据权利要求1所述的基于改进RRT算法的机械臂路径规划方法，其特征在于，步骤S3之后，以及步骤S4之前还包括：S31，判断向量与向量间的夹角θ是否在范围内，若满足则进行步骤S32，不满足则返回步骤S2；S32，连线判断连线是否与空间障碍物发生碰撞；若产生碰撞，则缩短步长eps，并返回步骤S2...

【专利技术属性】
技术研发人员：董翔，叶蒙蒙，黄泽霞，
申请(专利权)人：安徽大学，
类型：发明
国别省市：