一种改进双臂机器人RRT*轴孔装配路径规划方法

本发明涉及机器人运动规划、路径规划领域，具体涉及一种基于改进rrt算法与结合动态目标偏置策略、自适应邻域半径策略的轴孔装配路径规划方法。

背景技术：

1、随着自动化技术的持续演进，工业机器人已成为智能制造生产线的核心支撑，在更加复杂装配作业环境下，作业效率、人力成本等，成为我们需要重点关注的问题。然而，单臂工业机器人在多工序协同、高精度定位对接等复杂场景中，存在运动自由度不足、作业覆盖范围有限的固有局限，往往需依赖额外辅助工装才能完成任务，这在一定程度上制约了生产系统的柔性与整体效能。

2、随着工业机器人在复杂装配场景中应用日益广泛，高效的双臂路径规划策略对提升系统作业性能起到关键作用。karaman等提出了rrt*算法，通过重新计算代价函数以降低路径代价，然而该算法在动态复杂场景中收敛速率较慢，路径优化效率有待提升。lei等提出改进的rrt*算法，引入目标偏置策略引导随机树向目标点生长，加快了算法运行速度，但当路径通道存在局部障碍物遮挡时，易在障碍物附近产生无效分支，无法探索其他可行路径区域，破坏了算法的概率完备性。郭利进等针对rrt*算法收敛速率慢、不适用于动态场景等问题，通过整合目标点偏置与冗余节点删除策略，并优化采样点处理方式，实现了算法适配性与运行效率的同步提升，也有效改善了路径质量，但当目标点与起始点间存在大型障碍物时，仍可能导致路径搜索效率下降。

3、当前基于rrt*及其衍生算法的双臂路径规划方法，普遍存在采样效率与协同同步性不匹配、固定邻域半径无法适配双臂动态耦合关系、路径规划未充分考虑双臂时间同步约束等问题，导致计算量大、规划精度不足等问题。因此需要设计一种更为全面的算法。

技术实现思路

1、本发明的目的是克服上述背景技术中的不足，提供一种改进双臂机器人rrt*轴孔装配路径规划方法，以实现双机械臂机器人无碰撞路径的实时生成，同时满足路径低成本、高规划效率及平滑性。

2、本发明的技术方案是：

3、一种改进双臂机器人rrt*轴孔装配路径规划方法，包括以下步骤：

4、s1：设定双臂机器人运动学模型、障碍物信息、起始状态、目标状态、rrt*算法参数；

5、s2：根据动态目标偏置策略进行采样，随机数小于目标偏置概率时，在目标点附近采样，否则随机采样；

6、s3：在随机树中找到最近邻节点；

7、s4：从最近邻节点向采样点方向扩展生成新节点；

8、s5：若新节点发生碰撞，返回步骤s2重新采样，否则将新节点加入随机树tree；

9、s6：在自适应邻域半径内查找潜在最近邻节点，若潜在最近邻节点到新节点的路径无碰撞且成本更低，将其作为新节点的父节点；

10、s7：若新节点不在目标区域，判断是否达到最大迭代次数；若达到最大迭代次数，路径规划失败，否则，返回步骤s2重新规划；

11、s8：从目标点反向追溯父节点，提取从起始到目标状态的路径；

12、s9：采用双臂路径协同策略生成同步路径，若同步路径上各节点无碰撞且无干涉，得到无碰撞路径，路径规划结束，否则返回步骤s2重新规划。

13、所述步骤s1中：双机械臂运动学模型包括dh参数与基座位置；所述障碍物信息包括障碍物位置、障碍物形状与障碍物模型。

14、所述步骤s2中：

15、动态目标偏置策略的公式为：

16、目标偏置概率的公式为：

17、其中：为采样区内的随机点；为目标点；为自由空间；为自由空间内的随机采样；为0~1之间的随机数；为目标偏置概率常数；为第次迭代时的目标偏置概率；为初始偏置概率；为当前迭代次数；为最大迭代次数；为最小偏置概率。

18、所述步骤s6中：

19、自适应邻域半径的计算公式为：

20、其中：为第个节点生成时的领域半径；为控制邻域整体尺度系数；为节点密度系数；为当前搜索节点的总数；为空间维度；为自适应领域衰减因子。

21、所述步骤s7的新节点在目标区域的判断规则为位置误差小于目标半径且姿态误差小于阈值。

22、所述步骤s9的双臂路径协同策略包括：

23、定义路径规划迭代序号以及决策变量；

24、定义规则如下：

25、

26、其中：为第次迭代时应规划的机械臂编号；

27、设主臂路径包括个节点，从臂路径包括个节点；

28、定义同步后的路径长度为；基于路径长度，确定参考路径和待同步路径，当大于时，等于，等于，否则相反；

29、引入归一化时间变量，为0时对应运动起点，为1时对应运动结束点；

30、参考路径时间点序列为：

31、

32、将待同步路径根据参考路径的时间轴进行重采样与插值；

33、确定插值区间，对于每个目标时间点等于，寻找待同步路径原始时间序列中满足以下条件的索引：

34、

35、待同步路径的原始时间序列为：

36、

37、根据目标时间点与区间端点的相对位置计算线性插值系数：

38、

39、根据线性插值系数对待同步路径点的关节角度进行线性插值：

40、

41、其中：表示待同步路径在第个节点处的关节角度向量。

42、本发明的有益效果是：

43、通过动态目标偏置策略，改变传统rrt算法随机采样的盲目性，使采样点自适应向目标位姿方向聚集，有效加快随机树的收敛速度，减少无效探索节点，大幅提升双机械臂路径规划的效率，满足实时性作业需求；

44、通过自适应邻域半径策略，根据双机械臂作业环境的障碍分布与空间约束自适应调整邻域搜索范围，在狭窄复杂环境中缩小半径以提升避碰精度，避免机械臂自身碰撞及环境碰撞，在开阔环境中扩大半径以加快路径拓展，兼顾避碰安全性与规划效率；

45、通过两种策略协同作用，可有效精简路径节点、缩短路径长度，降低机械臂运动成本，同时优化路径形态。

1.一种改进双臂机器人rrt*轴孔装配路径规划方法，包括以下步骤：

2.根据权利要求1所述的改进双臂机器人rrt*轴孔装配路径规划方法，其特征在于：所述步骤s1中：双机械臂运动学模型包括dh参数与基座位置；所述障碍物信息包括障碍物位置、障碍物形状与障碍物模型。

3.根据权利要求1所述的改进双臂机器人rrt*轴孔装配路径规划方法，其特征在于：所述步骤s2中：

4.根据权利要求1所述的改进双臂机器人rrt*轴孔装配路径规划方法，其特征在于：所述步骤s6中：

5.根据权利要求1所述的改进双臂机器人rrt*轴孔装配路径规划方法，其特征在于：所述步骤s7的新节点在目标区域的判断规则为位置误差小于目标半径且姿态误差小于阈值。

6.根据权利要求1所述的改进双臂机器人rrt*轴孔装配路径规划方法，其特征在于：所述步骤s9的双臂路径协同策略包括：