[发明专利]一种双机械臂协同装配作业路径规划方法

说明书

一种双机械臂协同装配作业路径规划方法，建立双机械臂作业的运动学模型与动力学模型；设定初始参数，进行初始采样，计算树节点距离采样点的距离，找到最近的树节点作为父节点；如果不发生碰撞则加入候选采样点；计算采样点的势能，根据势能的倒数进行轮盘赌选择得到采样点；将代价小于r的点加入重连接点中，计算采样点与各重连接点连接而组成的路径的代价，选择最短代价的连接方式确定采样点的连接父节点；选择代价最低的连接方式重新确立节点的父子关系；当采样点与目标点的势能差小于h时用目标点替换采样点，完成随机树；根据随机树的节点父子关系确定规划路径。本发明可以规划出末端运动距离短、作业时间短、机械臂能耗低的高质量路径。

技术领域

本发明属于机器人控制与自动化装配领域，适用于工业机器人自动化装配领域。具体地说，涉及一种基于快速势能探索随机树算法(Rapidly potential energyexploring random trees，以下简称RPT)的双机械臂协同装配作业路径规划方法。

背景技术

工业机器人是柔性自动化的核心设备。在生产中的应用，工业机器人对提高劳动生产率，提高产品质量，改善劳动条件，提高企业的竞争能力和应变能力，促进新产业的建立和发展，改变劳动结构，以及促进相关学科的技术进步，均发挥了重大的社会效益和经济效益。在自动化装配领域，单个机械臂往往难以满足日益复杂的装配内容与装配效率要求。因此，为了提高自动化装配的效率，有必要对双机械臂协同作业方法进行研究。

对于机械臂路径规划，现有的方法有人工势场法、A*算法和RRT算法等。在自动化装配的过程中，为了保证零件不发生损坏，使零件之间能平稳地进行配合，需要对零件移动的速度进行约束，在保证零件能够平稳装配的情况下规划出一条效率高的合适路径。但是由于人工势场法容易陷入局部最优，A*算法难以在动态路网中发挥作用，RRT算法路径质量差等问题，导致在双机械臂协同装配作业方面难以规划出质量高的路径。

发明内容

为了克服现有的规划算法对双机械臂协同装配路径进行规划时遇到的路径质量低的问题，并满足装配作业要求的机械臂末端速度约束，本发明提供一种基于RPT算法的双机械臂协同装配作业路径规划方法，可以规划出末端运动距离短、装配作业时间短、机械臂能耗低的高质量路径，也为双机械臂协同控制提供了基础。

本发明解决其技术问题所采用的技术方案是：

一种双机械臂协同装配作业规划方法，所述方法包括以下步骤：

步骤1：建立双机械臂作业的运动学模型与动力学模型：

式中：Q为关节位置，为机械臂m的第i关节在基础坐标系下的坐标，为机械臂m的第i关节在机械臂m的基坐标系下的坐标，P_m为机械臂m的基坐标系到基础坐标系的转换矩阵，R_i为关节i的位姿矩阵，T_Hi为关节i到基坐标的转换矩阵，A_i为关节i与关节i-1的坐标系转换矩阵，c_i，s_i，∝_i，d_i为机械臂的D-H参数，q为机器人关节位置向量，为关节速度向量，为关节加速度向量，M(q)为机器人的质量矩阵，为关节离心力和科式力，τ为关节控制力矩，τ_fric为关节摩擦力矩，τ_motor为关节电机的惯性力矩；

步骤2：设定初始参数，包括起始位姿q_start，目标位姿q_goal，初始随机树tree，采样概率p，采样上限k，采样步长d_q，单次候选采样点数n，重连接代价r，碰撞距离d_lim，最终位置势能误差h；