[发明专利]一种双臂机器人协调操作的路径规划方法

摘要

本发明公开了一种双臂机器人协调操作的路径规划方法，双臂末端连接一个剪刀的两柄，首先获取双臂初始关节角、剪刀初始夹角以及剪刀终止夹角，并获取路径规划的总步数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下左臂关节角速度以及右臂关节角速度JL0L，JR0R分别为左、右臂的雅可比矩阵，步骤7、利用求解路径规划中下一步数k+1的关节角度，下一步数k+1下左臂关节角度以及右臂关节角度其中θl(k)为当前步数k下左臂关节角度，θr(k)为当前步数k下右臂关节角度；其中，其中双臂初始关节角已知；步骤8、判断当前步数k是否达到总步数Kn，若已达到总步数，则路径规划结束，若没有达到总步数则k自增1，返回步骤6。