[发明专利]一种用于双机器人协作的运动规划方法及系统

权利要求书

1.一种用于双机器人协作的运动规划方法，其特征在于，包括以下步骤：

在ROS系统中，建立双机器人及工作环境的模型描述文件以及双机器人的MoveIt配置文件；

进行初始化设置，并创建操作对象，将操作对象作为参数创建双臂指令，编排双机器人执行任务的动作；

逐条解析双臂指令，并调用MoveIt函数对该指令进行运动规划，并将规划结果进行保存与发布；若存在规划有误的指令则停止解析与规划；

其中，一项任务由多条双臂指令组成，每条双臂指令由两条单臂指令组成，每台机器人各执行其中的一条单臂指令；

其中，具体包括以下步骤：

步骤1：在SolidWorks中建立双机器人及其工作环境的装配图，在ROS中生成该双机器人的MoveIt配置文件；

步骤2：在ROS中加载urdf文件和配置文件，然后依次启动：规划结果处理单元、MoveIt的主要节点move_group、rviz节点、人机交互单元；在人机交互单元中设置双机器人的初始点；

步骤3：用户在人机交互单元中创建若干个操作对象，所述操作对象包括待操作物体和标记点；

步骤4：创建若干条双臂指令形成双机器人执行任务的动作过程，在每条双臂指令中，将调用一个操作对象；

步骤5：点击人机交互界面上的“执行”按钮，人机交互单元将双臂操作指令逐条解析，再逐条调用MoveIt的函数接口；MoveIt通过move_group节点进行运动规划并将运动规划结果封装为FollowJointTrajectory消息发往规划结果处理单元中；若某一条指令的运动规划有误，则停止解析与规划并在人机交互界面上显示错误，否则进入下一步；

步骤6：规划结果处理单元接收FollowJointTrajectory消息，将其拆分为左右臂的运动轨迹，并写入文本文件中保存，同时使用关节消息发布器将运动规划结果转化为JointState类型的消息发布在ROS上；rviz订阅该消息后，将显示双机器人的运动过程；人机交互单元订阅该消息后，将在界面中更新双机器人的位姿信息；

步骤7：若双臂指令全部解析完毕，则本次任务执行完成，否则回到步骤5解析下一条指令。

2.根据权利要求1所述的一种用于双机器人协作的运动规划方法，其特征在于，所述初始化设置具体为：在ROS系统中，将双机器人及其工作环境的模型进行显示，并设置双机器人的初始点。

3.根据权利要求1所述的一种用于双机器人协作的运动规划方法，其特征在于，所述逐条解析双臂指令，并调用MoveIt函数对该指令进行运动规划，并将规划结果进行保存与发布，若存在规划有误的指令则停止解析与规划具体包括以下步骤：

步骤S1：判断当前双臂指令的规划结果是否超出预设的运动范围或者是否产生碰撞，若是，则判断该条指令的规划有误，结束解析与规划，否则进入步骤S2；

步骤S2：保存并发布当前双臂指令的规划结果，并判断是否已经解析完所有的双臂指令，若是，则结束解析与规划，否则返回步骤S1。

4.根据权利要求3所述的一种用于双机器人协作的运动规划方法，其特征在于，所述保存并发布当前双臂指令的规划结果具体为：实时接收运动规划结果和发布双机器人的位姿消息，同时将运动规划结果写入文件中保存；双机器人每次执行任务，都生成一份保存运动规划结果的文件。

5.根据权利要求1所述的一种用于双机器人协作的运动规划方法，其特征在于，所述操作对象的类型包括待操作物体及标记点，所述待操作物体是双机器人在执行任务的过程中所取用的包括零件、辅件、工具在内的物体进行简化后的简单几何体；所述标记点是双机器人运动过程中必须经过或到达的特定点；创建待操作物体时，输入的参数为几何体尺寸以及物体位姿；创建标记点时，输入的参数为点的位姿；对生成的待操作物体和标记点分别进行编号。