[发明专利]一种基于RRT算法的双机械臂解耦运动规划方法及系统

权利要求书

1.一种基于RRT算法的双机械臂解耦运动规划方法，其特征在于，所述方法包括：

根据每个机械臂对应的起始构型信息和目标构型信息，利用双向快速搜索随机树RRT算法分别对每个机械臂进行针对环境静态障碍物的无碰撞路径规划，以确定每个机械臂的第一运行路径节点序列和所述第一运行路径节点序列中每个节点对应的时间信息；

将每个机械臂的第一运行路径节点序列根据每个节点对应的时间信息进行一一对应，并根据对应的结果进行碰撞检测，获取碰撞检测结果；

当所述碰撞检测结果指示发生碰撞时，将作为主臂的第一机械臂的第一运行路径节点序列作为动态障碍物，利用单向RRT算法和双向RRT算法重新对作为从臂的第二机械臂进行针对动态障碍物的无碰撞路径规划，以确定所述第二机械臂的最终运行路径节点序列和所述最终运行路径节点序列中每个节点对应的时间信息；

根据所述第一机械臂的第一运行路径节点序列、所述第一机械臂的第一运行路径节点序列中每个节点对应的时间信息、所述第二机械臂的最终运行路径节点序列和所述最终运行路径节点序列中每个节点对应的时间信息确定双机械臂解耦运动规划方案。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

在利用单向RRT算法和双向RRT算法重新对作为从臂的第二机械臂进行针对动态障碍物的无碰撞路径规划时，利用动态扩展随机树的思想，在当单向RRT算法遇到瓶颈时，采用双向RRT算法进行加速运算，通过迭代的方式计算第二机械臂的第二路径节点序列，直到第二机械臂的第二路径节点序列不与第一机械臂的第一路径节点序列发生碰撞时，确定当前的第二路径节点序列为所述第二机械臂的最终运行路径节点序列。

3.根据权利要求2所述的方法，其特征在于，所述方法利用动态扩展随机树的思想，在当单向RRT算法遇到瓶颈时，采用双向RRT算法进行加速运算，通过迭代的方式计算第二机械臂的第二路径节点序列，直到第二机械臂的第二路径节点序列不与第一机械臂的第一路径节点序列发生碰撞时，确定当前的第二路径节点序列为所述第二机械臂的最终运行路径节点序列，包括：

当利用单向RRT算法规划的所述第二机械臂的第一随机树的节点数小于等于预设节点数阈值时采用单向目标偏置RRT算法进行规划，以获取第一子节点序列；

当所述第一随机树的节点数大于预设节点数阈值时，采用双向RRT算法从所述第二机械臂对应的目标构型信息对应的目标节点开始进行第二棵随机树的扩展，以获取第二子节点序列；

根据所述第一子节点序列和第二子节点序列确定当前的第二运行路径节点序列，为当前的第二运行路径节点序列中的每个节点赋予时间信息，根据当前的第二运行路径节点序列、当前的第二运行路径节点序列中每个节点的时间信息、所述第一机械臂的第一运行路径节点序列和所述第一机械臂的第一运行路径节点序列中每个节点的节点信息进行碰撞检测，确定第一次发生碰撞的碰撞节点，将当前的第二运行路径节点序列中的在所述碰撞节点之后的节点去除，保留没有发生碰撞的节点；

将所述第一机械臂的第一运行路径节点序列中从所述碰撞节点开始的运行路径节点序列作为动态障碍物，重新进行路径规划，直至所述第二机械臂第二路径节点序列各节点与所述第一机械臂第一路径节点序列对应时刻的节点不再发生碰撞时停止，直接将当前的第二运行路径节点序列和当前的第二运行路径节点序列中每个节点的时间信息作为所述第二机械臂的最终运行路径节点序列和所述最终运行路径节点序列中每个节点对应的时间信息。