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

权利要求书

1.一种双机械臂协同装配作业路径规划方法，其特征在于，所述方法包括以下步骤：

步骤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为机械臂第i关节的D-H参数，q为机器人关节位置向量，为关节速度向量，为关节加速度向量，M(q)为机器人的质量矩阵，为关节离心力和科式力，τ为关节控制力矩，τ_fric为关节摩擦力矩，τ_motar为关节电机的惯性力矩；

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

步骤3：根据公式(7)进行初始采样：

式中：q_randj为关节j的采样角度，q_jmin为关节j的最小转角，q_jmax为关节j的最大转角，p_j为随机概率，p为采样概率，为目标位置关节j的转角；

步骤4：计算树节点距离采样点的距离，找到最近的树节点q_near作为父节点；

步骤5：根据公式(8)扩展节点得到子节点，判断子节点是否碰撞，如果不发生碰撞则加入候选采样点；

式中：q_j为得到的采样点的关节j角度，q_nearj为父节点的关节j角度；

步骤6：根据公式(9)-(12)计算采样点的势能：

u(i)＝F_att(i)+F_repout(i)+F_repin(i) (9)

F_att(i)＝ε(g_i-g_goal) (10)

式中：U(i)为节点i的势能，F_att(i)为节点i的引力势能，F_repout(i)为节点i与外界的斥力势能，F_repin(i)为节点i内部位姿产生的斥力势能，ε为引力比例系数，g_i为节点i的机械臂末端位置，g_goal为目标位置的机械臂末端位置，τ为斥力比例系数，为节点i的关节j的位置，map为外部障碍物的包围盒中心，d_map为外部障碍物距离关节j的距离，d_lim为最小碰撞距离，d_out为外部斥力边界，为机械臂m在节点i处关节j的坐标，d_robot为两个机械臂之间的最短距离，d_in为内部斥力边界；

步骤7：根据公式(13)修改采样步长，根据公式(14)修改采样概率；

式中：∝与β为步长与概率调整系数，用来调整自适应的采样步长与采样概率；

步骤8：重复采样过程，直到采样候选点数达到n；

步骤9：根据势能U(i)的倒数进行轮盘赌选择，得到采样点q_new；

步骤10：根据公式(15)-(19)计算树上各节点到q_new的代价，将代价小于r的点加入重连接点中，计算q_new与各重连接点连接而组成的路径的代价，选择最短代价的连接方式，确定q_new的连接父节点；

C(g_i，g_j)＝W_qC_q(q_i，q_j)+W_TC_T(T_i，T_j)+W_EC_E(E_i，E_j) (14)

式中：C代表总代价，C_q代表关节角变化代价，C_T代表末端速度代价，C_E代表能耗代价,W表示三者权重，costq_i,j代表从第i个节点向第j个节点拓展的关节角代价，用与原节点相应代价的比值来统一代价标准，q_i,j为从第i个节点向第j个节点拓展的关节角变化，costT_i,j代表从第i个节点向第j个节点拓展的速度变化代价，v_i,j代表从第i个节点向第j个节点拓展的速度变化值，C_E代表从第i个节点向第j个节点拓展的能耗代价，costE_i,j代表从第i个节点向第j个节点拓展的能耗值，k_τ代表电机的电流-转矩系数，τ代表电机转矩；

步骤11：去除重连接点之间的连接关系，计算重连接点之间所有的连接方式所需要的路径代价，选择代价最低的连接方式重新确立节点的父子关系；

步骤12：当q_new与q_goal的势能差小于h时，用q_goal替换q_new，完成随机树；

步骤13：根据随机树的节点父子关系，确定规划路径。