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 = pose
    theta = np.sqrt(rx * rx + ry * ry + rz * rz)
    kx = rx / theta
    ky = ry / theta
    kz = rz / theta
    cth = np.cos(theta)
    sth = np.sin(theta)
    vth = 1 - np.cos(theta)

    r11 = kx * kx * vth + cth
    r12 = kx * ky * vth - kz * sth
    r13 = kx * kz * vth + ky * sth
    r21 = kx * ky * vth + kz * sth
    r22 = ky * ky * vth + cth
    r23 = ky * kz * vth - kx * sth
    r31 = kx * kz * vth - ky * sth
    r32 = ky * kz * vth + kx * sth
    r33 = kz * kz * vth + cth

    beta = np.arctan2(-r31, np.sqrt(r11 * r11 + r21 * r21))
	# 这里需要判断分母为cos(beta)=0的情况
    if beta > np.deg2rad(89.99):
        beta = np.deg2rad(89.99)
        alpha = 0
        gamma = np.arctan2(r12, r22)
    elif beta < -np.deg2rad(89.99):
        beta = -np.deg2rad(89.99)
        alpha = 0
        gamma = -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] = gamma
    rpy[1] = beta
    rpy[2] = alpha

    return rpy

欧拉角->旋转矢量

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

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

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

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

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

​ python解算代码如下所示：

# 将欧拉角转为旋转矢量
def rpy2rotvec(self, pose):
    gamma, beta, alpha = pose

    ca = 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 * cb
    rotation_matrix[0, 1] = ca * sb * sg - sa * cg
    rotation_matrix[0, 2] = ca * sb * cg + sa * sg
    rotation_matrix[1, 0] = sa * cb
    rotation_matrix[1, 1] = sa * sb * sg + ca * cg
    rotation_matrix[1, 2] = sa * sb * cg - ca * sg
    rotation_matrix[2, 0] = -sb
    rotation_matrix[2, 1] = cb * sg
    rotation_matrix[2, 2] = cb * cg

    theta = 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 * kx
    rovetc[1] = theta * ky
    rovetc[2] = theta * kz

    return rovetc

如有表述错误或侵权，请指正，谢谢