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

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

A Path Planning Method for Double Robot Cooperative Assembly Operation

【技术实现步骤摘要】
一种双机械臂协同装配作业路径规划方法
本专利技术属于机器人控制与自动化装配领域，适用于工业机器人自动化装配领域。具体地说，涉及一种基于快速势能探索随机树算法(Rapidlypotentialenergyexploringrandomtrees，以下简称RPT)的双机械臂协同装配作业路径规划方法。
技术介绍
工业机器人是柔性自动化的核心设备。在生产中的应用，工业机器人对提高劳动生产率，提高产品质量，改善劳动条件，提高企业的竞争能力和应变能力，促进新产业的建立和发展，改变劳动结构，以及促进相关学科的技术进步，均发挥了重大的社会效益和经济效益。在自动化装配领域，单个机械臂往往难以满足日益复杂的装配内容与装配效率要求。因此，为了提高自动化装配的效率，有必要对双机械臂协同作业方法进行研究。对于机械臂路径规划，现有的方法有人工势场法、A*算法和RRT算法等。在自动化装配的过程中，为了保证零件不发生损坏，使零件之间能平稳地进行配合，需要对零件移动的速度进行约束，在保证零件能够平稳装配的情况下规划出一条效率高的合适路径。但是由于人工势场法容易陷入局部最优，A*算法难以在动态路网中发挥作用，RRT算法路径质量差等问题，导致在双机械臂协同装配作业方面难以规划出质量高的路径。
技术实现思路
为了克服现有的规划算法对双机械臂协同装配路径进行规划时遇到的路径质量低的问题，并满足装配作业要求的机械臂末端速度约束，本专利技术提供一种基于RPT算法的双机械臂协同装配作业路径规划方法，可以规划出末端运动距离短、装配作业时间短、机械臂能耗低的高质量路径，也为双机械臂协同控制提供了基础。本专利技术解决其技术问题所采用的技术方案是：一种双机械臂协同装配作业规划方法，所述方法包括以下步骤：步骤1：建立双机械臂作业的运动学模型与动力学模型：式中：Q为关节位置，为机械臂m的第i关节在基础坐标系下的坐标，为机械臂m的第i关节在机械臂m的基坐标系下的坐标，Pm为机械臂m的基坐标系到基础坐标系的转换矩阵，Ri为关节i的位姿矩阵，THi为关节i到基坐标的转换矩阵，Ai为关节i与关节i-1的坐标系转换矩阵，ci，si，∝i，di为机械臂的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)式中：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：根据势能的倒数进行轮盘赌选择，得到采样点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：根据随机树的节点父子关系，确定规划路径。本专利技术的有益效果主要表现在：1.PRT算法以RRT算法为基础，保证采样随机性的情况下，通过增加势能函数引导采样与根据代价重新连接随机树的方法，提高了采样质量，优化了节点间连接方式，提高了路径质量。2.基于RPT算法的双机械臂协同装配路径规划在工业机器人装配方面具有较好的作业效率与较低的能耗。附图说明图1是双机械臂协同装配作业规划方法的流程图。图2是机械臂末端路径对比图。具体实施方式下面结合附图对本专利技术作进一步描述。参照图1和图2，一种双机械臂协同装配作业规划方法，所述方法包括以下步骤：步骤1：建立双机械臂作业的运动学模型与动力学模型：式中：Q为关节位置，为机械臂m的第i关节在基础坐标系下的坐标，为机械臂m的第i关节在机械臂m的基坐标系下的坐标，Pm为机械臂m的基坐标系到基础坐标系的转换矩阵，Ri为关节i的位姿矩阵，THi为关节i到基坐标的转换矩阵，Ai为关节i与关节i-1的坐标系转换矩阵，ci，si，∝i，di为机械臂的D-H参数，q为机器人关节位置向量，为关节速度向量，为关节加速度向量，M(q)为机器人的质量矩阵，为关节离心力和科式力，τ为关节控制力矩，τfric为关节摩擦力矩，τmotor为关节电机的惯性力矩；步骤2：设定初始参数，包括起始位姿q_start，目标位姿q_goal，初始随机树tree，采样概率p＝0.4，采样上限k＝10000，采样步长d_q＝1°，单次候选采样点数n＝10，重连接代价r＝0.1，碰撞距离dlim＝0.1m，最终位置势能误差h＝0.05m；步骤3：根据公式(7)进行初始采样：式中：qranaj为关本文档来自技高网...

【技术保护点】
1.一种双机械臂协同装配作业路径规划方法，其特征在于，所述方法包括以下步骤：步骤1：建立双机械臂作业的运动学模型与动力学模型：

【技术特征摘要】
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)式中：U(i)为节点i的势能，Fatt(i)为节点i的引力势能，Frepout(i)为节点i与外界的斥力势能，Frepin(i)为节点i内部位姿产生的斥力势能，ε为引力比例系数，gi为节点i的机械臂末端位置，g...

【专利技术属性】
技术研发人员：张立彬，林邦扬，陈教料，胥芳，谭大鹏，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江,33