python中 欧拉角＜--＞四元数＜--＞旋转矩阵 相互转化

本来想使用ROS中 tf 进行姿态角表示方式的转换，但是 ubuntu 版本是 16.04，仅支持ROS自带的 python2.7的tf库，然而我需要使用python3.7的程序，折腾了一会儿，没有成功安装双版本的tf，准备直接使用 scipy 来实现 欧拉角<–>四元数<–>旋转矩阵 的相互转化。

conda 切换到目标环境后，安装 scipy

pip install scipy

演示一个小程序

# 定义欧拉角
yaw = 0.1  # 绕z轴的旋转角度
pitch = 0.2  # 绕y轴的旋转角度
roll = 0.3  # 绕x轴的旋转角度

# 创建Rotation对象
r_a = Rotation.from_euler('ZYX', [yaw, pitch, roll], degrees=False)

# 获取旋转矩阵
rotation_matrix_a = r_a.as_matrix()

print(rotation_matrix_a)
print(r_a.as_euler('ZYX'))

输出

[[ 0.97517033 -0.03695701  0.21835066]
 [ 0.0978434   0.95642509 -0.27509585]
 [-0.19866933  0.28962948  0.93629336]]
[0.1 0.2 0.3]

根据上述程序可以得出以下结论：

• 程序参数默认是 弧度制，如果想使用角度制，需要设置 degrees = True
• 欧拉角的输入顺序与 参数 ‘ZYX’ 相匹配，第一个角度是绕 Z 轴旋转的，第二个角度是绕 Y 轴旋转的，第三个角度是绕 X 轴旋转的
• 注意要使用大写的 Z Y X （大小写表示旋转方向不同）

上述程序运算结果可以通过MATLAB进行验证，需要确认 myfunc_euler2Rotation 中定义的旋转顺序是否符合设计预期

clc

% 定义欧拉角
yaw = 0.1;  % 绕z轴的旋转角度
pitch = 0.2;  % 绕y轴的旋转角度
roll = 0.3;  % 绕x轴的旋转角度

eulerAngles = [roll, pitch, yaw];
Rnb = myfunc_euler2Rotation(eulerAngles)


function [RIb] = myfunc_euler2Rotation(eulerAngles)
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here
    roll = eulerAngles(1);
    pitch = eulerAngles(2);
    yaw = eulerAngles(3);

    cr = cos(roll); sr = sin(roll);
    cp = cos(pitch); sp = sin(pitch);
    cy = cos(yaw); sy = sin(yaw);

    RIb = [cy*cp ,   cy*sp*sr - sy*cr,   sy*sr + cy* cr*sp,
            sy*cp,    cy *cr + sy*sr*sp,  sp*sy*cr - cy*sr,
            -sp,         cp*sr,           cp*cr];
end

matlab 程序输出如下，与python的计算结果一致

Rnb =

    0.9752   -0.0370    0.2184
    0.0978    0.9564   -0.2751
   -0.1987    0.2896    0.9363

旋转矩阵->四元数

使用 python 计算如下

print(r_a.as_quat())
# 输出
[0.14357218 0.10602051 0.0342708  0.98334744]

使用 matlab 计算如下

quat = rotm2quat(Rnb)
% 输出如下
0.9833    0.1436    0.1060    0.0343

注意到 matlab 中四元数使用四个元素的向量表示，顺序为 [w, x, y, z]，其中 w 是实部，(x, y, z) 是虚部
scipy 中的四元数顺序为 (x,y,z,w)

旋转矩阵->欧拉角

print(r_a.as_euler('ZYX'))
# 输出
[0.1 0.2 0.3]