欧拉角、方向余弦矩阵与四元数变换（MATLAB官方函数实现）

欧拉角、方向余弦矩阵和四元数是导航领域中常用的表示方式，课程中会讲授相关的理论并要求自己编写函数进行实现，以起到学习巩固的目的。但之间的转换具有不同的计算方式，对于不同项目的坐标系的定义不同、代码风格也不同，几乎每一个导航代码包都会配套自己的实现方法，对于多个项目融合可能会带来一定的影响。因此在个人组织项目时选择使用MATLAB官方的函数来进行实现，尽量避免了多个项目进行组合时出现多个命名相似但功能有差异的函数编写。

项目环境：MATLAB R2022b，配置有Robotics System Toolbox工具箱（一般成套的MATLAB中都会自带）

推荐调用方法

先上干货，使用中知己按照该方式调用即可。注：两套体系也不可混用

不使用工具箱中的类

% 已知按顺序绕n系下x,y,z轴旋转对应角度后得到b系
% 即描述物体在n系下的姿态角（角标描述先下后上）
x=deg2rad(-22.2727);
y=deg2rad(-0.9673);
z=deg2rad(-37.7648);

% 欧拉角→方向余弦
Cbn=eul2rotm([z,y,x]);
% 欧拉角→四元数
q=eul2quat([z,y,x])
% 方向余弦→欧拉角
rotm2eul(Cbn);
% 方向余弦→四元数
rotm2quat(Cbn);
% 四元数→方向余弦
quat2rotm(q);
% 四元数→欧拉角
quat2eul(q);

使用工具箱中的类

% 使用类方法进行求解
% 欧拉角→方向余弦
Cbn_=eul2rotm(-eul_,"XYZ")'
% 欧拉角→四元数
q_=quaternion([x,y,z],'euler','XYZ','point')
% 方向余弦→欧拉角
eul_=-rotm2eul(Cbn_',"XYZ")
% 方向余弦→四元数
q2_=quaternion(Cbn,'rotmat','point')
% 四元数→方向余弦
Cbn_=rotmat(q_,"point")
% 四元数→欧拉角
eul_=euler(q_,"XYZ","point")

由于类中不包括方向余弦和欧拉角之间的相互转化，因此依然需要引入上述方法来求解，所以建议在转化的过程中用四元数进行过渡（在使用四元数姿态更新方法的过程中会更方便）。

详细说明

欧拉角与方向余弦阵的相互转换

这里默认读者已经有了基础的理论知识，直接给出转换的示例代码以及相关的经验说明。

x=deg2rad(-22.2727);
y=deg2rad(-0.9673);
% z=deg2rad(322.2352);
z=deg2rad(-37.7648);
% ==/欧拉角转换为方向余弦矩阵/==
Cbn=eul2rotm([z,y,x])
%{
    0.7904    0.5718    0.2198
   -0.6123    0.7276    0.3092
    0.0169   -0.3790    0.9253
%}
% ==/方向余弦矩阵转换为欧拉角/==
eul=rotm2eul(Cbn);
rad2deg(eul)
%  -37.7648   -0.9673  -22.2727

程序非常简单，这里主要做一些说明：

1. 欧拉角其实是一组有序的旋转角，很多项目中喜欢将欧拉角进行分开成单独的角来进行给出，并且给出对应的意义，例如滚转角（roll，phi），俯仰角（pitch，theta），偏航角（yaw，heading，varphi）等，这些具有特定意义的角既和所定义的坐标系有关（东北天-右前上、北东地-前右下等），也与对应的旋转顺序有关（常用的是“ZYX”，但也有“XYZ”，“ZYZ”等方式），因此在使用之前一定要进行辨明坐标系以及对应角度的意义。
2. 通过代码的运行结果可以看出，对应的欧拉角输入需要是标准单位rad，其次欧拉角矩阵里面的顺序需要保持和转动顺序保持一致，计算为Rz*Ry*Rx，通过帮助文档可以知道该函数还有一个可选择的参数，例如“ZYZ”，他会调用计算Rz*Ry*Rz，显然可以发现，前面eul矩阵中的顺序决定了几个R矩阵的顺序，后面的参数决定了是调用哪个R，因此 Cbn=eul2rotm([z,y,x]) 与 Cbn=eul2rotm([x,y,z],"XYZ") 结果是不同的。
3. 输出的值为行向量，范围是（-pi,pi），注意存储和进行并行计算（一般会用列向量）的习惯转换。

四元数和欧拉角、方向余弦的转换

MATLAB中可以使用基础的四元数（单位哈密顿四元数）转换函数进行转换，其调用如下：

x=deg2rad(-22.2727);
y=deg2rad(-0.9673);
% z=deg2rad(322.2352);
z=deg2rad(-37.7648);
% ==/欧拉角转换为四元数/==
q=eul2quat([z,y,x])
%     0.9278   -0.1854    0.0547   -0.3191
% ==/四元数转换为欧拉角/==
eul=quat2eul(q);
rad2deg(eul)
%     -37.7648   -0.9673  -22.2727

欧拉角转换为四元数求解注意事项与转换为方向余弦的注意事项相同。

四元数与方向余弦的转换调用如下：

%{
    0.7904    0.5718    0.2198
   -0.6123    0.7276    0.3092
    0.0169   -0.3790    0.9253
%}
q=rotm2quat(Cbn)
%    0.9278    -0.1854   0.0547    -0.3191
Cbn=quat2rotm(q)
%{
    0.7904    0.5718    0.2198
   -0.6123    0.7276    0.3092
    0.0169   -0.3790    0.9253
%}

四元数类以及相关类函数的说明

具体的使用说明，可以在MATLAB命令行中键入doc quaternion进行查看，这里给出所具有的重要功能的列表

• 加减，数乘
• 两个四元数的乘法 -- 直接使用q1*q2即可（注意顺序不可交换）
• 共轭 -- 可以使用conj函数，也可以使用 ' 来进行标注（这里做了重载）
• 通过等效旋转矢量来进行更新 -- quat = quaternion(rotationVector,'rotvec')

结论

在配置有Robotics System Toolbox工具箱的MATLAB版本中，当进行简单运算时建议使用四元数类进行求解，当运算量较大（涉及到放入循环中进行惯导解算时），建议使用简单函数。