[发明专利]一种对机械臂路径冗余节点的反向拆分计算的方法及系统

说明书

本发明公开了一种对机械臂路径冗余节点的反向拆分计算的方法及系统，属于机器人运动规划技术领域。本发明方法包括：对机械臂R进行运动规划，确定运动路径，作为动态障碍物，并确定无碰撞运动路径；对机械臂R去除冗余节点，记录运动轨迹中对应的时间，对机无碰撞运动路径去除冗余节点，记录对应的时间；对运动路径做反向拆分操作，确定无碰撞路径；做碰撞检测，确定首次碰撞时间，并确定发生碰撞时刻的关节构型；根据关节构型，确定最优节点轨迹；重复机械臂R及机械臂L运动路径的反向拆分操作，获取机械臂R及机械臂L最优节点轨迹的路径，直到机械臂R及机械臂L优化后的路径不再发生碰撞，确定反向拆分路径。

技术领域

本发明涉及机器人运动规划技术领域，并且更具体地，涉及一种对机械臂路径冗余节点的反向拆分计算的方法及系统。

背景技术

双臂机器人在技术要求较高的领域，如装配作业，救援抢险，医疗服务等领域，需要机器人可以有更好的协调与灵活性，具有单臂机器人所无法比拟的优势。

双机械臂运动规划算法可以分为集中式与解耦式两类，其中集中式规划算法将双臂看作有更多自由度的单臂，从而可以直接将单臂的运动规划算法用于双臂，如此通常可以保留规划算法的完整性和最优性，但由于双臂的自由度明显高于单臂，导致规划的效率较低。

解耦式的运动规划算法则将两条机械臂分为主臂和从臂，先对主臂进行运动规划，再将规划出的主臂路径作为从臂的动态障碍物来规划从臂的运动。解耦式的运动规划算法通常无法保证规划的完整性和最优性，但在简单的环境中可以快速的规划出双臂的运动轨迹。

基于采样的路径规划算法如RRT、PRM算法在双机械臂集中式和解耦式运动规划中都得到了广泛的应用。此类方法具有不受构型空间维度的限制、简单场景下规划速度快的优点，但采样运动规划算法计算得到的运动路径往往转折过多，非常不平滑，因而很难直接作为控制器的参考轨迹，需要在保证不发生碰撞的条件下去掉冗余的节点。

发明内容

本发明针对用采样运动规划算法进行双机械臂解耦运动规划得到的运动轨迹转折过多、不平滑的问题，而提出了1、一种对机械臂路径冗余节点的反向拆分计算的方法，所述方法包括：

对机械臂R进行运动规划，确定机械臂R的运动路径，将机械臂R的运动路径作为机械臂L的动态障碍物，确定机械臂L的无碰撞运动路径；

使用预设算法对机械臂R的运动路径去除冗余节点，记录机械臂R的关节节点在运动轨迹中对应的时间，对机械臂L的无碰撞运动路径去除冗余节点，记录机械臂L无碰撞运动路径的关节节点在运动轨迹中对应的时间；

对机械臂R及机械臂L的运动路径做反向拆分操作，确定机械臂R的运动路径及机械臂L的无碰撞路径；

将机械臂R及机械臂L的运动路径时间，进行一一对应碰撞检测，确定机械臂R及机械臂L首次碰撞的时刻t_p，并确定机械臂R及机械臂L的运动路径发生碰撞时刻的关节构型；

根据机械臂R及机械臂L的无碰撞路径和运动路径发生碰撞时刻的关节构型，确定机械臂R及机械臂L运动路径的最优节点轨迹；

重复机械臂R及机械臂L运动路径的反向拆分操作，获取机械臂R及机械臂L最优节点轨迹的路径，直到机械臂R及机械臂L优化后的路径不再发生碰撞，确定反向拆分路径。

可选的，预设算法为贪婪算法，包括：

步骤一：将运动路径的第一个节点p₁和最后一节节点p_n直线连接，并将节点p₁和节点p_n加入优化后的路径节点集合中，判断机械臂位于直线上的路径点时是否与环境障碍物发生碰撞，若未发生碰撞则优化后的路径仅包含p₁和p_n，输出优化后的{p₁，p_n}；