[发明专利]一种双机器人协同旋转的路径规划方法和装置

权利要求书

1.一种双机器人协同旋转的路径规划方法，应用于一双机器人协同系统，所述系统包括二机器人，所述二机器人均包含用以共同夹持待加工工件的末端执行器，所述方法用于对所述机器人末端第六轴进行空间中多角度绕中心点协同旋转运动的路径规划，其特征在于，所述方法包括：

在单个机器人独立建模的基础上，建立机器人平台的相互位置关系，构造具有相对坐标变换的运动学系统；

依据机器人平台布置策略，设置世界坐标系，标定两机器人的基坐标系与末端坐标系；

以两机器人各基坐标系为中心坐标，依据标准D-H参数为基本运动学参数，求取机器人末端坐标的齐次变换矩阵基本形式；

在所述机器人系统协同旋转工况上建立路径圆弧模型，设对称中心坐标系为路径基作为原点；

依据已知路径偏转角和路径圆弧半径，计算路径初始控制点的齐次变换矩阵；

在所述路径初始控制点P₀的齐次变换矩阵中，提取三维旋转矩阵进行基于四元数的坐标旋转插补；

在所述路径初始控制点P₀的齐次变换矩阵中，提取初始坐标向量进行基于空间几何的控制点坐标向量插补；

基于逆运动学的齐次变换矩阵转换，获取最终控制点的关节控制角度；

由初始控制点的齐次变换矩阵提取三维旋转矩阵记cos＝c,sin＝s，与控制点对称中心坐标系P坐标向量的一般形式为r为路径圆弧半径；路径圆弧插补可表述为旋转与坐标向量两个插补环节：

(1)将T₃转化为四元数形式的Q₀＝q₀+q₁i+q₂j+q₃k，其中i、j、k为虚数单位，q₀、q₁、q₂、q₃是具体变量值，设置控制点的四元数表达式为Q_i＝[w_i,(x_i,y_i,z_i)]^T，w_i是实部变量值，x_i,y_i,z_i是虚部变量值，依据Qi＝Q′*Q₀*Q′^-1及四元数乘法的计算公式，代入控制点的圆弧最大角度，得到各旋转后控制点的四元数，转化为任意控制点处的旋转矩阵；

(2)控制点对称中心坐标系P的坐标偏移量同样可由各坐标轴方向上的偏移向量p_x，p_y，p_z表示,将控制点的坐标向量表示为的一般形式；

初始控制齐次变换矩阵的求解方法为：旋转路径相对x_pz_p平面偏转角度为α，路径圆弧半径为r，路径起始点坐标P₀先由y_p顺时针旋转90°，然后相对z轴顺时针旋转(α+90°)，再沿z轴轴向平移-r m，0≤m360；P₀相对P点坐标系的齐次变换矩阵矩阵表示为：

圆弧控制点的最大角度值满足：所取两控制点间隔角度应小于满足表示相邻控制点间隔为n，控制点旋转角度应设置为n,2n,…kn；

初始控制点Q₀的四元数表示一般形式为：

四元数绕轴旋转角度为n，旋转轴的相关四元数表示为：

四元数Q_i＝[w_i,(x_i,y_i,z_i)]^T姿态旋转的一般形式表示为：

四元数转换为旋转矩阵的公式为：

控制点相对点P坐标系坐标向量的各元素满足关系：v为路径控制点个数；

两机器人控制点关于机器人基座标的齐次变换矩阵表示为：

l为两机器人x轴方向相距，b为y轴方向相距，h₁为左侧机器人的底座高度，h₂为右侧机器人的底座高度。

2.根据权利要求1所述的双机器人协同旋转的路径规划方法，其特征在于，在三维旋转坐标的插补过程中考虑圆弧轴向偏移量的影响，依据圆弧轴向偏移量的限制求解控制点的最大间隔角度值。