一种双机械臂协同装配作业路径规划方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN110181515A(43)申请公布日2019.08.30(21)申请号201910496139.2(22)申请日2019.06.10(71)申请人浙江工业大学地址310014浙江省杭州市下城区朝晖六区潮王路18号(72)发明人张立彬林邦扬陈教料胥芳谭大鹏(74)专利代理机构杭州斯可睿专利事务所有限公司33241代理人王利强(51)Int.Cl.B25J9/16(2006.01)权利要求书2页说明书6页附图2页(54)发明名称一种双机械臂协同装配作业路径规划方法(57)摘要一种双机械臂协同装配作业路径规划方法，建立双机械臂作业的运动学模型与动力学模型；设定初始参数，进行初始采样，计算树节点距离采样点的距离，找到最近的树节点作为父节点；如果不发生碰撞则加入候选采样点；计算采样点的势能，根据势能的倒数进行轮盘赌选择得到采样点；将代价小于r的点加入重连接点中，计算采样点与各重连接点连接而组成的路径的代价，选择最短代价的连接方式确定采样点的连接父节点；选择代价最低的连接方式重新确立节点的父子关系；当采样点与目标点的势能差小于h时用目标点替换采样点，完成随机树；根据随机树的节点父子关系确定规划路径。本发明可以规划出末端运动距离短、作业时间短、机械臂能耗低的高质量路径。CN110181515ACN110181515A权利要求书1/2页1.一种双机械臂协同装配作业路径规划方法，其特征在于，所述方法包括以下步骤：步骤1：建立双机械臂作业的运动学模型与动力学模型：式中：Q为关节位置，为机械臂m的第i关节在双臂坐标系下的坐标，为机械臂m的第i关节在机械臂m的坐标系下的坐标，Pm为机械臂m的坐标系到双臂坐标系的转换矩阵，Ri为关节i的状态矩阵，THi为关节i到基坐标的转换矩阵，Ai为关节i与关节i-1的坐标系转换矩阵，ci，si，∝i，di为机械臂第i关节的D-H参数，q为机器人关节位置向量，为关节速度向量，为关节加速度向量，M(q)为机器人的质量矩阵，为关节离心力和科式力，τ为关节控制力矩，τfric为关节摩擦力矩，τmotor为关节电机的惯性力矩；步骤2：设定初始参数，包括双机械臂起始位姿q_start，目标位姿q_goal，初始路径随机树tree，初始采样概率p，采样上限k，初始采样步长d_q，单次候选采样点数n，重连接代价r，碰撞距离dlim，最终位置势能误差h；步骤3：根据公式(7)进行初始采样：式中：qrandj为关节j的采样角度，qjmin为关节j的最小转角，qjmax为关节j的最大转角，pj为随机概率，p为采样概率，为目标位置关节j的转角；步骤4：计算树节点距离采样点的距离，找到最近的树节点q_near作为父节点；步骤5：根据公式(8)扩展节点得到子节点，判断子节点是否碰撞，如果不发生碰撞则加入候选采样点；式中：qj为得到的采样点的关节j角度，qnearj为父节点的关节j角度；步骤6：根据公式(9)-(12)计算采样点的势能：U(i)＝Fatt(i)+Frepout(i)+Frepin(i)(9)Fatt(i)＝ε(gi-ggoal)(10)2CN110181515A权利要求书2/2页式中：U(i)为节点i的势能，Fatt(i)为节点i的引力势能，Frepout(i)为节点i与外界的斥力势能，Frepin(i)为节点i内部位姿产生的斥力势能，ε为引力比例系数，gi为节点i的机械臂末端位置，ggoal为目标位置的机械臂末端位置，τ为斥力比例系数，gij为节点i的关节j的位置，map为外部障碍物的包围盒中心，dmap为外部障碍物距离关节j的距离，dlim为最小碰撞距离，dout为外部斥力边界，gijm为机械臂m在节点i处关节j的坐标，drobot为两个机械臂之间的最短距离，din为内部斥力边界；步骤7：根据公式(13)修改采样步长，根据公式(14)修改采样概率；式中：∝与β为步长与概率调整系数，用来调整自适应的采样步长与采样概率。步骤8：重复采样过程，直到采样候选点数达到n；步骤9：根据势能的倒数进行轮盘赌选择，得到采样点q_new；步骤10：根据公式(15)-(19)计算树上各节点到q_new的代价，将代价小于r的点加入重连接点中，计算q_new与各重连接点连接而组成的路径的代价，选择最短代价的连接方式，确定q_new的连接父节点；C(gi,gj)＝WqCq(qi,qj)+WTCT(Ti,Tj)+WECE(Ei,Ej)(14)式中：C代表总代价，Cq代表关节角变化代价，CT代表末端速度代价，CE代表能耗代价,W表示三者权重，costqi,j代表从第i个节点向第j个节点拓展的关节角代价，用与原节点相应代价的比值来统一代价标准，qi,j为从第i个节点向第j个节点