本发明专利技术公开了一种双臂机器人协调操作的路径规划方法,双臂末端连接一个剪刀的两柄,首先获取双臂初始关节角、剪刀初始夹角以及剪刀终止夹角,并获取路径规划的总步数Kn;采用带抛物线过渡的速度梯形法对角度差θn规划获得剪刀左、右柄质心角速度的梯形速度曲线ωc、ωd;利用速度约束公式,根据剪刀柄质心的角速度ωc、ωd,求得剪刀柄质心的线速度曲线vc、vd;将ωc、ωd和vc、vd,代入双臂协调运动的约束方程,分别求得左臂末端的线速度曲线vl和角速度曲线ωl,右臂末端的线速度曲线vr和角速度曲线ωr;根据路径规划中的当前步数对应的时间,获取左右臂末端的角速度和线速度,并根据约束公式求解左右臂关节角速度;然后求解下一步数k+1的关节角度,由此顺次求解步数1~Kn的关节角度。
【技术实现步骤摘要】
一种双臂机器人协调操作的路径规划方法
本专利技术涉及双臂机器人协调操作的路径规划方法,属于双臂路径规划
技术介绍
随着科技的不断发展,机械臂广泛应用于工厂车间的加工、装配以及空间站的组装、维护等工作,机械臂技术已经成为重要研究方向。近年来,双臂机器人已成为机器人领域中的研究热点之一。协调操作路径规划算法是双臂机器人控制中的关键技术,因此,对双臂机器人协调操作路径规划算法进行研究具有重要的意义。目前,双臂机器人的主要研究内容为运动规划,协调控制算法和操作力(力矩)等方面。但这些方法的计算效率都比较低。
技术实现思路
有鉴于此,本专利技术提供了一种双臂机器人协调操作的路径规划方法,能够实现双臂的协调路径规划。为了达到上述目的,本专利技术的技术方案为:双臂末端分别连接一个剪刀的两柄,双臂机器人的臂为二连杆结构,二连杆的连接处为一个关节,该方法包括以下步骤:步骤1、将剪刀两柄分别记为C柄和D柄,以两柄连接处为原点建立两柄相连接处三维坐标系为{OT},以C柄质心为原点建立三维质心系{OC},以D柄的质心为原点建立三维质心系{OD},以左臂末端为原点建立三维坐标系{OL},以右臂末端为原点建立三维坐标系{OR};惯性系为{O0}。获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角。同时设置操作剪刀的整个运动规划过程中的总时间tz、加速时间ta和时间间隔t0,由Kn=ta/to求得路径规划的总步数Kn。步骤2、求解剪刀初始夹角与终止夹角的角度差θn。步骤3、采用带抛物线过渡的速度梯形法对角度差θn的变化速度进行规划,获得剪刀左、右柄质心角速度的梯形速度曲线ωc、ωd;步骤4、利用公式根据剪刀柄质心的角速度ωc、ωd,求得剪刀柄质心的线速度曲线vc、vd。其中和分别为PTC和PTD在惯性系下的表示,PTC为{OC}相对于{OT}的相对位置矢量,PTD为{OD}相对于{OT}的相对位置矢量,ROT为{OT}相对于{O0}的姿态变换矩阵。步骤5、将上述求解得到的剪刀柄质心角速度曲线ωc、ωd和线速度曲线vc、vd,代入双臂协调运动的约束方程分别求得左臂末端的线速度曲线vl和角速度曲线ωl,右臂末端的线速度曲线vr和角速度曲线ωr。式中,分别为和的反对称矩阵,和分别为PLC和PRD在惯性系下的表示,ROL为{OL}相对于{O0}的姿态变换矩阵,PLC为{OC}相对于{OL}的相对位置矢量,ROR为{OR}相对于{O0}的姿态变换矩阵,PRD为{OC}相对于{OR}的相对位置矢量。步骤6、针对路径规划中的当前步数k,求解当前步数对应的时间tk,利用tk在左臂末端的线速度曲线vl和角速度曲线ωl,右臂末端的线速度曲线vr和角速度曲线ωr中找到当前步数的左臂末端线速度vl(k)和角速度ωl(k)、右臂末端线速度vr(k)和角速度ωr(k);其中k的初始值为1。利用公式解算获得路径规划中当前步数k下左臂关节角速度以及右臂关节角速度分别为左、右臂的雅可比矩阵,步骤7、利用求解路径规划中下一步数k+1的关节角度,下一步数k+1下左臂关节角度以及右臂关节角度其中θl(k)为当前步数k下左臂关节角度,θr(k)为当前步数k下右臂关节角度;其中,其中双臂初始关节角已知。步骤8、判断当前步数k是否达到总步数Kn,若已达到总步数,则路径规划结束,若没有达到总步数则k自增1,返回步骤6。有益效果:本专利技术主要涉及一种双臂机器人协调操作的路径规划算法,其优势在于可实现双臂的协调路径规划,通过姿态约束和速度约束的方式对双臂进行协调操作。附图说明图1是双臂操作剪刀示意图;图2是剪刀夹断路径规划算法流程图。具体实施方式下面结合附图并举实施例,对本专利技术进行详细描述。本专利技术提供了一种双臂机器人协调操作的路径规划算法,下面结合附图对本专利技术作进一步说明。操作物的局部结构能够活动,即至少具有一个自由度,此时双臂末端与操作物之间有相对运动,如剪刀、钳子等,如图1所示。实施例1、运动学约束方程双臂末端位姿约束:剪刀工件的两柄记为C和D,两柄相连接处建立坐标系{OT},分别在C和D的质心c和d处建立坐标系{OC}和{OD},惯性坐标系为{O0},左、右臂的末端分别建立坐标系{OL}和{OR}。由此可建立各坐标系之间的齐次变换矩阵为:左臂末端系相对于惯性系T0L,右臂末端系相对于惯性系T0R,柄C质心系相对于左臂末端系TLC,柄D质心系相对于右臂末端系TRD,柄C和D质心系相对于连接处坐标系分别为TTC和TTD。各齐次矩阵分别对应各自的相对位置矢量和姿态变换矩阵,记为T0L(P0L,R0L),T0R(P0R,R0R),TLC(PLC,RLC),TRD(PRD,RRD),TTC(PTC,RTC)和TTD(PTD,RTD)。根据图1形成的运动链,两臂之间存在的齐次变换矩阵约束方程为:T0LTLCTCT=T0RTRDTDT(1)由此可建立协调运动过程中的位置和姿态应满足约束方程为:一般情况下,两柄的运动是预先规划的,并可通过两柄连接处的运动来体现,即P0T和R0T是已知的,故可由式(2)分别得出左、右臂的末端位姿关系式为:其中,PLC和PRD分别是柄C和D质心相对左、右臂末端的位置,RLC和RRD分别是柄C和D质心系相对左、右臂末端系的姿态变换矩阵,一旦抓持位置确定,PLC、PRD、RLC和RRD均为已知量,同时根据剪刀工件自身的结构,柄C和D质心系与两者连接系之间的相对位置PCT和PDT亦为已知量,同时,姿态变换矩阵RCT和RDT可根据柄C和D绕连接点的相对转动关系确定。于是通过式(3)和(4)即可得出左、右臂位姿,进而可进行双臂末端位姿规划。1.2、双臂末端速度约束令两柄C和D的质心速度分别为两柄连接点速度为vc,ωc、vd,ωd及vt,ωt分别为各自的线速度和角速度分量。一般来说,vt,ωt由运动规划预先确定。则两柄质心与连接点速度之间的关系为:上式中,ωtc和ωtd是C和D质心基于连接处的相对角速度,由两柄操作时绕连接轴的旋转关系确定,C和D质心基于连接处无相对平动速度,PTC和PTD由剪刀结构尺寸确定,和分别为PTC和PTD在惯性系下的表示,于是由式(5)即可确定两柄质心处的速度和左臂末端与柄C之间,右臂末端与柄D之间无相对运动,由此可建立速度关系,并可得左、右臂末端速度为:式中,分别为和的反对称矩阵。其中和分别为PLC和PRD在惯性系下的表示,反对称矩阵可按以下方法求得:设P=[pxpypz],P的反对称矩阵为则有于是式(6)可改写成矩阵形式:式中,I3×3为3×3阶单位阵,JLC为左臂末端速度与柄C质心速度之间的雅可比矩阵,JRD为右臂末端速度与柄D质心速度之间的雅可比矩阵。左、右臂末端速度与关节速度之间所确定的速度关系如下:JL0L,JR0R分别为左、右臂的雅可比矩阵。由式(7)、(8)可以得到左、右臂的关节速度和刚体质心速度的关系:由于PLC和PRD分别是柄C和D质心相对左、右臂末端的位置,由抓持位置确定,R0L和R0R可由姿态约束式(3)和(4)求得,由此得到JLC和JRD;两柄质心处速度和由式(5)得到,于是可根据方程(7)求出左、右臂末端速度,由式(9)可以求出左、右臂的关节速度。实施例2、操作物为剪刀,假设两柄连接处固定不动,且在惯性坐标系{本文档来自技高网...
【技术保护点】
一种双臂机器人协调操作的路径规划方法,双臂末端分别连接一个剪刀的两柄,所述双臂机器人的臂为二连杆结构,二连杆的连接处为一个关节,其特征在于,包括以下步骤:步骤1、将所述剪刀两柄分别记为C柄和D柄,以两柄连接处为原点建立两柄相连接处三维坐标系为{OT},以C柄质心为原点建立三维质心系{OC},以D柄的质心为原点建立三维质心系{OD},以左臂末端为原点建立三维坐标系{OL},以右臂末端为原点建立三维坐标系{OR};惯性系为{O0};获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角;同时设置操作剪刀的整个运动规划过程中的总时间tz、加速时间ta和时间间隔t0,由Kn=ta/to求得路径规划的总步数Kn;步骤2、求解剪刀初始夹角与终止夹角的角度差θn;步骤3、采用带抛物线过渡的速度梯形法对角度差θn的变化速度进行规划,获得剪刀左、右柄质心角速度的梯形速度曲线ωc、ωd;步骤4、利用公式vc=ωc×PTC(0)vd=ωc×PTD(0),]]>根据剪刀柄质心的角速度ωc、ωd,求得剪刀柄质心的线速度曲线vc、vd;其中和分别为PTC和PTD在惯性系下的表示,PTC为{OC}相对于{OT}的相对位置矢量,PTD为{OD}相对于{OT}的相对位置矢量,所述ROT为{OT}相对于{O0}的姿态变换矩阵;步骤5、将上述求解得到的剪刀柄质心角速度曲线ωc、ωd和线速度曲线vc、vd,代入双臂协调运动的约束方程ωl=ωc,vl=vc+P^LC(0)·ωcωr=ωd,vr=vd+P^RD(0)·ωd,]]>分别求得左臂末端的线速度曲线vl和角速度曲线ωl,右臂末端的线速度曲线vr和角速度曲线ωr;式中,分别为和的反对称矩阵,和分别为PLC和PRD在惯性系下的表示,ROL为{OL}相对于{O0}的姿态变换矩阵,PLC为{OC}相对于{OL}的相对位置矢量,ROR为{OR}相对于{O0}的姿态变换矩阵,PRD为{OC}相对于{OR}的相对位置矢量;步骤6、针对路径规划中的当前步数k,求解当前步数对应的时间tk,利用tk在左臂末端的线速度曲线vl和角速度曲线ωl,右臂末端的线速度曲线vr和角速度曲线ωr中找到当前步数的左臂末端线速度vl(k)和角速度ωl(k)、右臂末端线速度vr(k)和角速度ωr(k);其中k的初始值为1;利用公式q.L(k)=JL0L-1X.L(k)q.R(k)=JR0R-1X.R(k),]]>解算获得路径规划中当前步数k下左臂关节角速度以及右臂关节角速度分别为左、右臂的雅可比矩阵,步骤7、利用求解路径规划中下一步数k+1的关节角度,下一步数k+1下左臂关节角度以及右臂关节角度其中θl(k)为当前步数k下左臂关节角度,θr(k)为当前步数k下右臂关节角度;其中,其中双臂初始关节角已知;步骤8、判断当前步数k是否达到总步数Kn,若已达到总步数,则路径规划结束,若没有达到总步数则k自增1,返回步骤6。...
【技术特征摘要】
2014.12.31 CN 20141084930261.一种双臂机器人协调操作的路径规划方法,双臂末端分别连接一个剪刀的两柄,所述双臂机器人的臂为二连杆结构,二连杆的连接处为一个关节,其特征在于,包括以下步骤:步骤1、将所述剪刀的两柄分别记为C柄和D柄,以两柄连接处为原点建立两柄相连接处三维坐标系为{OT},以C柄质心为原点建立三维质心系{OC},以D柄的质心为原点建立三维质心系{OD},以左臂末端为原点建立三维坐标系{OL},以右臂末端为原点建立三维坐标系{OR};惯性系为{O0};获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角;同时设置操作剪刀的整个运动规划过程中的总时间tz、加速时间ta和时间间隔t0,由Kn=ta/to求得路径规划的总步数Kn;步骤2、求解剪刀初始夹角与终止夹角的角度差θn;步骤3、采用带抛物线过渡的速度梯形法对角度差θn的变化速度进行规划,获得剪刀左、右柄质心角速度的梯形速度曲线ωc、ωd;步骤4、利用公式根据剪刀柄质心的角速度ωc、ωd,求得剪刀柄质心的线速度曲线vc、vd;其中和分别为PTC和PTD在惯性系下的表示,PTC为{OC}相对于{OT}的相对位置矢量,PTD为{OD}相对于{OT}的相对位置矢量,所述ROT为{OT}相对于{O0}的姿态变换矩阵...
【专利技术属性】
技术研发人员:张海涛,柴玉强,曲汉伟,王宏,于鹏,
申请(专利权)人:中国航天科技集团公司第五研究院第五一三研究所,
类型:发明
国别省市:山东;37
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。