一种双臂机器人协调操作的路径规划方法
【技术领域】
[0001] 本发明涉及双臂机器人协调操作的路径规划方法，属于双臂路径规划技术领域。
【背景技术】
[0002] 随着科技的不断发展，机械臂广泛应用于工厂车间的加工、装配以及空间站的组 装、维护等工作，机械臂技术已经成为重要研宄方向。近年来，双臂机器人已成为机器人领 域中的研宄热点之一。协调操作路径规划算法是双臂机器人控制中的关键技术，因此，对双 臂机器人协调操作路径规划算法进行研宄具有重要的意义。
[0003] 目前，双臂机器人的主要研宄内容为运动规划，协调控制算法和操作力（力矩）等 方面。但这些方法的计算效率都比较低。
【发明内容】
[0004] 有鉴于此，本发明提供了一种双臂机器人协调操作的路径规划方法，能够实现双 臂的协调路径规划。
[0005] 为了达到上述目的，本发明的技术方案为：双臂末端分别连接一个剪刀的两柄，双 臂机器人的臂为二连杆结构，二连杆的连接处为一个关节，该方法包括以下步骤：
[0006] 步骤1、将剪刀两柄分别记为C柄和D柄，以两柄连接处为原点建立两柄相连接处 三维坐标系为{〇T}，以C柄质心为原点建立三维质心系{0J，以D柄的质心为原点建立三维 质心系{〇D}，以左臂末端为原点建立三维坐标系{0J，以右臂末端为原点建立三维坐标系 {〇K};惯性系为{〇。}。
[0007] 获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角。
[0008] 同时设置操作剪刀的整个运动规划过程中的总时间tz、加速时间ta和时间间隔 tQ，由Kn=ta/t。求得路径规划的总步数Kn。
[0009] 步骤2、求解剪刀初始夹角与终止夹角的角度差0n。
[0010] 步骤3、采用带抛物线过渡的速度梯形法对角度差0n的变化速度进行规划，获得 剪刀左、右柄质心角速度的梯形速度曲线《。、《 d;
[0011] 步骤4、利用公￥
【主权项】
1. 一种双臂机器人协调操作的路径规划方法，双臂末端分别连接一个剪刀的两柄，所 述双臂机器人的臂为二连杆结构，二连杆的连接处为一个关节，其特征在于，包括以下步 骤： 步骤1、将所述剪刀两柄分别记为C柄和D柄，以两柄连接处为原点建立两柄相连接处 三维坐标系为{〇τ}，以C柄质心为原点建立三维质心系{0J，以D柄的质心为原点建立三维 质心系{〇 D}，以左臂末端为原点建立三维坐标系{0J，以右臂末端为原点建立三维坐标系 {〇κ};惯性系为{〇。}; 获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角； 同时设置操作剪刀的整个运动规划过程中的总时间tz、加速时间ta和时间间隔t ^，由 Kn= t a/t。求得路径规划的总步数K n; 步骤2、求解剪刀初始夹角与终止夹角的角度差θη; 步骤3、采用带抛物线过渡的速度梯形法对角度差θ η的变化速度进行规划，获得剪刀 左、右柄质心角速度的梯形速度曲线ω。、《d; 步骤4、利用公式根据剪刀柄质心的角速度ω。、Od，求得剪刀柄质心的 线速度曲线ν。、vd;
其中#和C分别为Pt。和Ptd在惯性系下的表示，Pt。为KU相对于i〇 T}的相对位置 矢量，Ptd为{〇D}相对于{〇τ}的相对位置矢量：
所述Rra为Ι〇τ} 相对于IOcJ的姿态变换矩阵； 步骤5、将上述求解得到的剪刀柄质心角速度曲线ω。、和线速度曲线Ve、vd，代入双 臂协调运动的约束方程
，分别求得左臂末端的线速度曲线V1 和角速度曲线Q1，右臂末端的线速度曲线\和角速度曲线ω ^ 式中，被、以分别为瓔和C的反对称矩阵，^和^分别为。和Pkd在惯性系下 的表示，禮>=凡Zc，C =?? ; IV为{0J相对于{0J的姿态变换矩阵，Pm为{〇 J相 对于IOJ的相对位置矢量，Rrai为{〇κ}相对于IOJ的姿态变换矩阵，Pkd为IOJ相对于{〇 κ} 的相对位置矢量； 步骤6、针对路径规划中的当前步数k，求解当前步数对应的时间tk，利用tk在左臂末端 的线速度曲线V1和角速度曲线ω i，右臂末端的线速度曲线^和角速度曲线中找到当 前步数的左臂末端线速度v1(k)和角速度ω 1(k)、右臂末端线速度'(k)和角速度ω Hk);其中k 的初始值为1 ; 利用公式
挥算获得路径规划中当前步数k下左臂关节角速度A以及 右臂关节角速度t Λμ , 分别为左、右臂的雅可比矩阵，尤_ ，名…=[^n，~J ; 步骤7、利用屯⑷、，求解路径规划中下一步数k+Ι的关节角度，下一步数k+Ι下左 臂关节角度^+d) +么⑷％以及右臂关节角度4_ =巧a) +IaA ;其中θ ιω为当前 步数k下左臂关节角度，Θ Hk)为当前步数k下右臂关节角度；其中，其中双臂初始关节角已 知； 步骤8、判断当前步数k是否达到总步数Kn，若已达到总步数，则路径规划结束，若没有 达到总步数则k自增1，返回步骤6。
【专利摘要】本发明公开了一种双臂机器人协调操作的路径规划方法，双臂末端连接一个剪刀的两柄，首先获取双臂初始关节角、剪刀初始夹角以及剪刀终止夹角，并获取路径规划的总步数Kn；采用带抛物线过渡的速度梯形法对角度差θn规划获得剪刀左、右柄质心角速度的梯形速度曲线ωc、ωd；利用速度约束公式，根据剪刀柄质心的角速度ωc、ωd，求得剪刀柄质心的线速度曲线vc、vd；将ωc、ωd和vc、vd，代入双臂协调运动的约束方程，分别求得左臂末端的线速度曲线vl和角速度曲线ωl，右臂末端的线速度曲线vr和角速度曲线ωr；根据路径规划中的当前步数对应的时间，获取左右臂末端的角速度和线速度，并根据约束公式求解左右臂关节角速度；然后求解下一步数k+1的关节角度，由此顺次求解步数1～Kn的关节角度。
【IPC分类】G05D1-10
【公开号】CN104750116
【申请号】CN201510181789
【发明人】张海涛, 柴玉强, 曲汉伟, 王宏, 于鹏
【申请人】中国航天科技集团公司第五研究院第五一三研究所
【公开日】2015年7月1日
【申请日】2015年4月16日