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) 四元数转欧拉角

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

3. 总结

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