ABB机器人欧拉角与四元数的相互转化以及旋转矩阵的求法

做项目时用到ABB机器人，直接通过ABB内置的函数可以轻松实现四元数读数与欧拉角的相互转化。但实际项目需要从示教器读出相关位置并自行计算，尤其需要计算旋转矩阵。

本文以ABB IRB120机器人（不确定其他机器人是否与ABB机器人一致）为例如下姿态为例来描述上述几个量的计算。

图1 机器人在Robot studio中的姿态

图2 示教器中四元数读数

图3 示教器中欧拉角读数
值得注意的是，ABB机器人的欧拉角是ZYX欧拉角。

1. 求旋转矩阵

(1) 已知四元数求旋转矩阵

此处给出matlab代码：

q=[0.27367, 0.75058, 0.46598, 0.38025];
fprintf('Quaternion rotation matrix: \n');
disp(Rotation(q));

function R = Rotation(Q)
    Q = Q./sqrt(sum(Q.*Q));
    q0 = Q(1); q1 = Q(2); q2 = Q(3); q3 = Q(4);
    R = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2);
             2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1);   
             2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2;
            ];
end

结果为：

Quaternion rotation matrix: 
    0.2765    0.4914    0.8259
    0.9076   -0.4159   -0.0564
    0.3158    0.7652   -0.5610

(2) 已知欧拉角求旋转矩阵

此处用到了Matlab Robotics Toolbox工具箱，这是一个机器人仿真和建模非常好用的工具箱，以下给出安装链接：

Matlab Robotics Toolbox工具箱安装教程
1.https://www.bilibili.com/read/cv14423551
2.https://petercorke.com/toolboxes/robotics-toolbox/

以下为matlab代码：

fprintf('Euler rotation matrix: \n');
disp(rotz(73.06,'deg')*roty(-18.41, 'deg')*rotx(126.25, 'deg'));

其中rotx，roty, rotz为Matlab Robotics Toolbox工具箱中的函数。
结果为：

Euler rotation matrix: 
    0.2765    0.4914    0.8259
    0.9077   -0.4159   -0.0563
    0.3158    0.7652   -0.5610

可以看到采用四元数与欧拉角算得的旋转矩阵一致（有细微的差别，推测来源于小数截断）。

2. 欧拉角、旋转矩阵与四元数转换

(1) 欧拉角转四元数

此处推荐一个python库transformations，安装方法：

pip install transformations

库的github链接：

https://github.com/cgohlke/transformations

直接调用库中函数即可实现转换，以下给出python代码：

import transformations
import numpy
import math


def euler(roll, pitch, yaw, axes='rzyx'):
    yaw = float(yaw) / 180 * numpy.pi
    pitch = float(pitch) / 180 * numpy.pi
    roll = float(roll) / 180 * numpy.pi
    return list(transformations.quaternion_from_euler(roll, pitch, yaw, axes=axes))

roll = 73.06#Ez
pitch = -18.41#Ey
yaw = 126.25#Ex
quaternion = euler(roll, pitch, yaw)
print(quaternion)

结果为：

[0.27362606284972685, 0.7505716495097842, 0.46601038304769293, 0.380270035071476]

此处也手动实现了欧拉角到四元数的转换，代码如下：

def self_euler_quaternion(roll, pitch, yaw):
    yaw = float(yaw) / 180 * numpy.pi
    pitch = float(pitch) / 180 * numpy.pi
    roll = float(roll) / 180 * numpy.pi
    r, p, y = roll/2, pitch/2, yaw/2

    sinr, sinp, siny = math.sin(r), math.sin(p), math.sin(y)
    cosr, cosp, cosy = math.cos(r), math.cos(p), math.cos(y)

    q0 = cosr * cosp * cosy + sinr * sinp * siny
    q3 = sinr * cosp * cosy - cosr * sinp * siny
    q2 = cosr * sinp * cosy + sinr * cosp * siny
    q1 = cosr * cosp * siny - sinr * sinp * cosy

    return numpy.array([q0, q1, q2, q3])
    
roll = 73.06#Ez
pitch = -18.41#Ey
yaw = 126.25#Ex
quaternion = euler(roll, pitch, yaw)
print(self_euler_quaternion(roll, pitch, yaw))

结果为：

[0.27362606 0.75057165 0.46601038 0.38027004]

可以看到，与示教器上的结果基本一致。

(2) 旋转矩阵转四元数

给出如下matlab代码：

function q = vgg_quat_from_rotation_matrix( R )
    % vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix 
    % q = vgg_quat_from_rotation_matrix(R)
    q = [ (1 + R(1,1) + R(2,2) + R(3,3)) (1 + R(1,1) - R(2,2) - R(3,3)) (1 - R(1,1) + R(2,2) - R(3,3)) (1 - R(1,1) - R(2,2) + R(3,3)) ];
    %if ~issym(q)
    A = true;
    if ~A 
      % Pivot to avoid division by small numbers
      [b I] = max(abs(q));
    else
      % For symbolic quats, just make sure we're nonzero
      for k=1:4
        if q(k) ~= 0
          I = k;
          break
        end
      end
    end
    q(I) = sqrt(q(I)) / 2 ;
    if I == 1 
        q(2) = (R(3,2) - R(2,3)) / (4*q(I));
        q(3) = (R(1,3) - R(3,1)) / (4*q(I));
        q(4) = (R(2,1) - R(1,2)) / (4*q(I));
    elseif I==2
        q(1) = (R(3,2) - R(2,3)) / (4*q(I));
        q(3) = (R(2,1) + R(1,2)) / (4*q(I));
        q(4) = (R(1,3) + R(3,1)) / (4*q(I));
    elseif I==3
        q(1) = (R(1,3) - R(3,1)) / (4*q(I));
        q(2) = (R(2,1) + R(1,2)) / (4*q(I));
        q(4) = (R(3,2) + R(2,3)) / (4*q(I));
    elseif I==4
        q(1) = (R(2,1) - R(1,2)) / (4*q(I));
        q(2) = (R(1,3) + R(3,1)) / (4*q(I));
        q(3) = (R(3,2) + R(2,3)) / (4*q(I));
    end
end

测试代码为：

vgg_quat_from_rotation_matrix(rotz(95.03,'deg')*roty(-18.37, 'deg')*rotx(174.57, 'deg'))

结果为：

  0.0860   -0.6716   -0.7221   -0.1422

与示教器结果一致。

(3) 四元数转欧拉角

由于项目暂时没有这个需求，因此这部分没有实现，感兴趣的小伙伴可以提供相关代码。

(4) 旋转矩阵转欧拉角

旋转矩阵到欧拉角的转换公式

假设旋转矩阵 (\mathbf{R}) 是：
$$R=\left[\begin{array}{ccc}\cos ({\theta }_y)\cos ({\theta }_z)& \sin ({\theta }_x)\sin ({\theta }_y)\cos ({\theta }_z)-\sin ({\theta }_z)\cos ({\theta }_x)& \sin ({\theta }_x)\sin ({\theta }_z)+\sin ({\theta }_y)\cos ({\theta }_x)\cos ({\theta }_z)\\ \sin ({\theta }_z)\cos ({\theta }_y)& \sin ({\theta }_x)\sin ({\theta }_y)\sin ({\theta }_z)+\cos ({\theta }_x)\cos ({\theta }_z)& -\sin ({\theta }_x)\cos ({\theta }_z)+\sin ({\theta }_y)\sin ({\theta }_z)\cos ({\theta }_x)\\ -\sin ({\theta }_y)& \sin ({\theta }_x)\cos ({\theta }_y)& \cos ({\theta }_x)\cos ({\theta }_y)\end{array}\right]$$

$$\mathbf{R}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

XYZ顺序的欧拉角

1. 计算 (\theta_y)（绕y轴的旋转角度）：
   $${\theta }_y=\arctan 2(-r_{31},\sqrt{r_{11}^2+r_{21}^2})$$

2. 检查奇异性：
   奇异性条件是 (\sqrt{r_{11}^2 + r_{21}^2} < 1e-6)。

3. 如果不处于奇异状态，计算 (\theta_x) 和 (\theta_z)：

   • 计算 (\theta_x)（绕x轴的旋转角度）：
     $${\theta }_x=\arctan 2(r_{32},r_{33})$$

   • 计算 (\theta_z)（绕z轴的旋转角度）：
     $${\theta }_z=\arctan 2(r_{21},r_{11})$$

4. 如果处于奇异状态，采用备用公式计算 (\theta_x) 和 (\theta_z)：

   • 计算 (\theta_x)（绕x轴的旋转角度）：
     $${\theta }_x=\arctan 2(-r_{12},r_{22})$$

   • 计算 (\theta_z)（绕z轴的旋转角度）：
     $${\theta }_z=0$$

ZYX顺序的欧拉角

1. 计算 (\theta_y)（绕y轴的旋转角度）：
   $${\theta }_y=\arctan 2(r_{31},\sqrt{r_{11}^2+r_{21}^2})$$

2. 检查奇异性：
   奇异性条件是 (\sqrt{r_{11}^2 + r_{21}^2} < 1e-6)。

3. 如果不处于奇异状态，计算 (\theta_x) 和 (\theta_z)：

   • 计算 (\theta_x)（绕x轴的旋转角度）：
     $${\theta }_x=\arctan 2(-r_{32},r_{33})$$

   • 计算 (\theta_z)（绕z轴的旋转角度）：
     $${\theta }_z=\arctan 2(-r_{21},r_{11})$$

4. 如果处于奇异状态，采用备用公式计算 (\theta_x) 和 (\theta_z)：

   • 计算 (\theta_x)（绕x轴的旋转角度）：
     $${\theta }_x=\arctan 2(r_{12},r_{22})$$

   • 计算 (\theta_z)（绕z轴的旋转角度）：
     $${\theta }_z=0$$

代码实现

上述公式可以使用Python代码实现如下：

import numpy as np

# 给定旋转矩阵
R = np.array([
    [0.9994, 0.0359, -0.0025],
    [-0.036, 0.9961, -0.081],
    [-0.004, 0.0811, 0.9967]
])

# 计算欧拉角（XYZ顺序）
def rotation_matrix_to_euler_angles_xyz(R):
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6
    
    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0
    
    return np.array([x, y, z])

# 计算欧拉角（ZYX顺序）
def rotation_matrix_to_euler_angles_zyx(R):
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6
    
    if not singular:
        y = np.arctan2(R[2, 0], sy)
        x = np.arctan2(-R[2, 1], R[2, 2])
        z = np.arctan2(-R[1, 0], R[0, 0])
    else:
        x = np.arctan2(R[1, 2], R[1, 1])
        y = np.arctan2(R[2, 0], sy)
        z = 0
    
    return np.array([x, y, z])

# 获取以弧度表示的欧拉角
euler_angles_xyz_rad = rotation_matrix_to_euler_angles_xyz(R)
euler_angles_zyx_rad = rotation_matrix_to_euler_angles_zyx(R)

# 转换为角度
euler_angles_xyz_deg = np.degrees(euler_angles_xyz_rad)
euler_angles_zyx_deg = np.degrees(euler_angles_zyx_rad)

euler_angles_xyz_deg, euler_angles_zyx_deg

3. 总结

本文以ABB IRB120机器人为例，给出了通过四元数、欧拉角计算旋转矩阵以及四元数到欧拉角的转换代码，实测与示教器的结果一致，相关代码可直接用于工程上坐标转换。