利索能及
我要发布
收藏
专利号: 2019104961392
申请人: 浙江工业大学
专利类型:发明专利
专利状态:已下证
更新日期:2026-07-29
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

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为关节摩擦力矩,τmotar为关节电机的惯性力矩;

步骤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)

式中:U(i)为节点i的势能,Fatt(i)为节点i的引力势能,Frepout(i)为节点i与外界的斥力势能,Frepin(i)为节点i内部位姿产生的斥力势能,ε为引力比例系数,gi为节点i的机械臂末端位置,ggoal为目标位置的机械臂末端位置,τ为斥力比例系数, 为节点i的关节j的位置,map为外部障碍物的包围盒中心,dmap为外部障碍物距离关节j的距离,dlim为最小碰撞距离,dout为外部斥力边界, 为机械臂m在节点i处关节j的坐标,drobot为两个机械臂之间的最短距离,din为内部斥力边界;

步骤7:根据公式(13)修改采样步长,根据公式(14)修改采样概率;

式中:∝与β为步长与概率调整系数,用来调整自适应的采样步长与采样概率;

步骤8:重复采样过程,直到采样候选点数达到n;

步骤9:根据势能U(i)的倒数进行轮盘赌选择,得到采样点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个节点拓展的关节角变化,costTi,j代表从第i个节点向第j个节点拓展的速度变化代价,vi,j代表从第i个节点向第j个节点拓展的速度变化值,CE代表从第i个节点向第j个节点拓展的能耗代价,costEi,j代表从第i个节点向第j个节点拓展的能耗值,kτ代表电机的电流-转矩系数,τ代表电机转矩;

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

步骤12:当q_new与q_goal的势能差小于h时,用q_goal替换q_new,完成随机树;

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