UR机械臂位置数据旋转矢量与欧拉角变换

本文主要是介绍UR机械臂位置数据旋转矢量与欧拉角变换，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

UR机械臂位置数据旋转矢量与欧拉角变换

本文介绍在UR机械臂控制过程在，使用函数返回的数据为旋转矢量，但是该数据对于控制非常不友好，我们需要将旋转转化为欧拉角的数据，才可以直观观察到工具端相对于不同轴旋转的角度。本文针对这一问题，描述旋转矢量与欧拉角转换的概念，以及如何通过Python代码进行两者之间相互转换的函数。

参考链接：https://forum.universal-robots.com/t/pose-rotation-order/223/2

旋转矢量->欧拉角

欧拉角是用来描述刚体在固定坐标系下的方向的三个角任何方向都可以通过围绕一个坐标系的轴组成三个基本旋转来实现。具体来说，从参照系出发，分别沿X、Y、Z轴旋转γ、β、α的角度，就可以得到方向。参考框架以蓝色显示，旋转方向以红色显示。角γ， β和α也可以称为RPY(滚转，俯仰和偏航)。

在这里插入图片描述

欧拉角用于理解机器人的方向，但它应该转换为旋转矩阵，以进行运动学和动力学等计算。给定欧拉角γ， β和α，旋转矩阵可由下式计算。

在这里插入图片描述

给定旋转矩阵R，欧拉角由下式确定。

在这里插入图片描述

python解算代码如下所示：

# 将旋转矢量转为欧拉角
def rotvec2rpy(self, pose):rx, ry, rz = posetheta = np.sqrt(rx * rx + ry * ry + rz * rz)kx = rx / thetaky = ry / thetakz = rz / thetacth = np.cos(theta)sth = np.sin(theta)vth = 1 - np.cos(theta)r11 = kx * kx * vth + cthr12 = kx * ky * vth - kz * sthr13 = kx * kz * vth + ky * sthr21 = kx * ky * vth + kz * sthr22 = ky * ky * vth + cthr23 = ky * kz * vth - kx * sthr31 = kx * kz * vth - ky * sthr32 = ky * kz * vth + kx * sthr33 = kz * kz * vth + cthbeta = np.arctan2(-r31, np.sqrt(r11 * r11 + r21 * r21))# 这里需要判断分母为cos(beta)=0的情况if beta > np.deg2rad(89.99):beta = np.deg2rad(89.99)alpha = 0gamma = np.arctan2(r12, r22)elif beta < -np.deg2rad(89.99):beta = -np.deg2rad(89.99)alpha = 0gamma = -np.arctan2(r12, r22)else:cb = np.cos(beta)alpha = np.arctan2(r21 / cb, r11 / cb)gamma = np.arctan2(r32 / cb, r33 / cb)rpy = np.zeros(3)rpy[0] = gammarpy[1] = betarpy[2] = alphareturn rpy

欧拉角->旋转矢量

轴角表示通过一个单位向量𝑢表示旋转轴的方向，一个角度e .描述绕轴旋转的大小来参数化线性空间中的旋转轴角表示可以定义两个方向之间的关系。

在这里插入图片描述

如果我们有一个固定的参考系，比如UR机器人的基本框架，我们可以用四个数字来表示任何方向:三个元素代表单位方向矢量和一个角度。如果我们乘以单位方向矢量的每个元素的角度，我们可以减少到三个数值，我们可以称之为旋转矢量。

在这里插入图片描述

轴角表示在机器人控制中很有用，因为它不受欧拉角的连续性和旋转序列问题的限制。然而，物理方向与旋转矢量的数值之间很难匹配。在UR机器人中，我们使用旋转向量来表示机器人姿势的方向。要转换为旋转矩阵，旋转矢量应分为角度矢量和单位方向矢量。

在这里插入图片描述

然后，需要进一步计算如下。

在这里插入图片描述

给定一个旋转矩阵，旋转向量可以计算如下：

在这里插入图片描述

python解算代码如下所示：

# 将欧拉角转为旋转矢量
def rpy2rotvec(self, pose):gamma, beta, alpha = poseca = np.cos(alpha)cb = np.cos(beta)cg = np.cos(gamma)sa = np.sin(alpha)sb = np.sin(beta)sg = np.sin(gamma)rotation_matrix = np.zeros((3, 3))rotation_matrix[0, 0] = ca * cbrotation_matrix[0, 1] = ca * sb * sg - sa * cgrotation_matrix[0, 2] = ca * sb * cg + sa * sgrotation_matrix[1, 0] = sa * cbrotation_matrix[1, 1] = sa * sb * sg + ca * cgrotation_matrix[1, 2] = sa * sb * cg - ca * sgrotation_matrix[2, 0] = -sbrotation_matrix[2, 1] = cb * sgrotation_matrix[2, 2] = cb * cgtheta = np.arccos((rotation_matrix[0, 0] + rotation_matrix[1, 1] + rotation_matrix[2, 2] - 1) / 2)sth = np.sin(theta)if sth == 0:return np.zeros(3)kx = (rotation_matrix[2, 1] - rotation_matrix[1, 2]) / (2 * sth)ky = (rotation_matrix[0, 2] - rotation_matrix[2, 0]) / (2 * sth)kz = (rotation_matrix[1, 0] - rotation_matrix[0, 1]) / (2 * sth)rovetc = np.zeros(3)rovetc[0] = theta * kxrovetc[1] = theta * kyrovetc[2] = theta * kzreturn rovetc