刚体在三维空间的旋转(关于旋转矩阵、DCM、旋转向量、四元数、欧拉角)
最近学习了一些关于三维空间旋转相关的知识，借此梳理一下备忘。
在三维空间中描述物体的旋转变换是一种极具魅力且富有挑战性的课题：对于同一固定点的所有可能旋转变换，在不考虑外部参照系的情况下，它们都可以被统一地用一种简洁的形式加以描述。在不同的应用领域中存在多种数学表达形式来描述物体的空间变换，在这些形式之间可以通过特定的方法实现信息的有效转化与共享。本文旨在系统地探讨各种旋转变换的不同数学表述及其相互间的转换关系。
1. 欧拉角(Euler Angle)
经典的表示方法是围绕物体自身的X轴、Y轴和Z轴依次完成特定角度的旋转操作；这种表示方法通常被称为欧拉角(Euler Angle)，其详细信息可参考维基百科（https://en.wikipedia.org/wiki/Euler_angles）。
需要注意的是，在欧拉角表示法中，默认采用yaw-pitch-roll（偏航-俯仰-偏 roll）的顺序来描述三维空间中的任意一次旋转变换操作。给定一组特定角度值（例如： yaw=45°, pitch=30°, roll=60°），当按照"先执行 yaw 轴旋转变换、接着执行 pitch 轴旋转变换、最后执行 roll 轴旋转变换"这一特定顺序进行旋转变换时，在执行完这三个基本旋转变换之后所得到的整体姿态与另一种情况下的结果可能会不同！具体而言，在满足某种特定姿态要求时（即希望同一个物体以两种不同的旋转序列达到相同的姿态），则所需的欧拉角参数值也会不同。
需要注意的是，在欧拉角表示法中三个旋转轴通常与刚体运动同步
欧拉角的表示方式比较直观，但是有几个缺点：
欧拉角的表达方式并非唯一确定。基于给定的起始方向和目标方向，在指定绕x轴、y轴、z轴的旋转顺序下（即通常所说的绕x-y-z轴旋转），仍可通过不同的绕x/y/z轴旋转角度组合来实现同一目标姿态变换。例如，在相同的x-y-z旋转顺序下（即通常所说的绕x-y-z轴旋转），(0°,90°,0°)与(90°,90°,90°)这两种不同的绕x/y/z轴旋转角度组合会导致物体最终到达相同的位置与姿态。这一现象的根本原因在于万向节锁（Gimbal Lock）现象所导致的角度冗余问题。如需进一步理解，请-condition同学参考YouTube上的教学视频。
(2) 欧拉角的插值比较难。
在进行旋转变换时，通常会将其表示为旋转矩阵的形式。这种情况下，必须计算大量的正弦和余弦值来完成相应的运算步骤。由于这些运算的复杂性，在实际应用中可能会导致较大的工作量。
2. 旋转矩阵(Rotation Matrix)和方向余弦矩阵(Direction Cosine Matrix)
在进行坐标变换时，在处理涉及旋转的问题时更为便捷的方式是采用**旋转矩阵(Rotation Matrix)** 这一数学工具。三维空间中的一种常用方法是将欧拉角转换为对应的旋转矩阵。具体计算方法如下：基于给定的角度参数——即物体绕着X轴（Roll）、Y轴(Pitch)和Z轴(Yaw）的旋转角度α、β和γ，在这些参数的基础上构建相应的旋转矩阵进行运算即可完成坐标系之间的旋转变换操作。
其中，
从这一角度来看，在旋转变换中也能够观察到以下关键点：当执行绕不同轴的旋转操作时（即当 yaw、pitch 和 roll 的顺序发生变化时），必须相应调整矩阵相乘的顺序；这将导致最终得到的旋转矩阵的结果也会随之发生变化。
必须注意的是，在讨论旋转变换时, 虽然旋转变换表中包含9个参数, 但仅有3个自由度可用, 因此并非所有方阵都能直接被视为旋转变换表. 只有满足特定条件的方阵才能被归类为旋转变换表, 即所谓的正交变换. 这种变换要求其逆变换与转置变换相等.
此外，在陀螺力学领域较为常见的另一种表示三维空间中物体旋转变换的方式被称为**方向余弦矩阵(Direction Cosine Matrix)** ，简称DCM（Direction Cosine Matrix）。这一名称来源于以下原理：当刚体处于初始姿态时（即起始朝向），其三个坐标轴方向分别为I、J、K；而当刚体处于目标姿态时（即当前朝向），其三个坐标轴方向则分别为i、j、k。那么该空间变换可通过刚体绕三个不同轴线分别转过的角度来描述。（如图所示）
DCM可以通过三个夹角的余弦计算如下：
这就是DCM名称的由来。实际上可以通过验证发现DCM本质上就是旋转矩阵 ，因此在后续讨论中无需对DCM与旋转矩阵进行区分。
在MATLAB环境中（从R2006a版本及以上开始），需要安装Aerospace Toolbox这一工具箱。该环境提供了将欧拉角与旋转矩阵相互转换的功能。以下示例代码用于验证不同欧拉角表示法如何转换为相同的旋转矩阵：
% Matlab code by MulinB, Aerospace Toolbox is needed
% Gimbal Lock experiments
yaw1 = 0;
pitch1 = 90;
roll1 = 0;
yaw2 = 90;
pitch2 = 90;
roll2 = 90;
R1 = angle2dcm(yaw1/180*pi,pitch1/180*pi,roll1/180*pi);
R2 = angle2dcm(yaw2/180*pi,pitch2/180*pi,roll2/180*pi);
disp(R1);disp(R2);3. 四元数(Quaternion)、旋转向量(Rotation Vector)、轴-角表示(Axis-Angle)
旋转的一个独特之处在于它能够以一种简洁而优雅的方式表达三维空间中的任意旋转动作。具体而言，在这一过程中，
我们可以选择一条特定轴线并赋予一定转角，
从而完成对物体的空间变换。
这种表述方法通常被称为Axis-Angle表述法。
在此框架下，
我们可用一个方向矢量(x,y,z)来指定轴线的方向，
同时借助一个角度值theta来进行转角描述。
从直观上讲，
将这些参数组合成一个四元组(theta,x,y,z)
即可完整地描述出任何可能的空间旋转效果。
值得注意的是，
虽然原始表述中使用的是三维坐标系中的点来代表轴线方向，
但在实际应用中为了提高效率和准确性，
我们通常会将这些坐标转换为单位矢量形式（即除以模长）。
这样一来，
只需通过缩放后的坐标分量与转角相乘即可得到完整的旋转向量表达：
即(thetax, thetay, theta*z)
这一过程的前提条件是原始坐标分量必须构成单位矢量。
由此引出了另一种常用的旋转向量表述形式——Rotation Vector
这种方法在计算机视觉领域得到了广泛应用，
尤其是像OpenCV这样的库中就大量采用了这种表述方式
（见OpenCV相机标定部分
中的rvec参数）。
Axis-Angle的表示方法还可以推导出另一种很常用的三维旋转表示方法，叫四元数(Quaternion)，这里有一篇非常通俗易懂介绍四元数的文章。同上，假设(x,y,z)是axis方向的单位向量，theta是绕axis转过的角度，那么四元数可以表示为**[cos(theta/2), xsin(theta/2), ysin(theta/2), z*sin(theta/2)]** 。注意，这里可以推导出，用于表示旋转的四元数向量也必须是单位向量。四元数的神奇之处在于，对于三维坐标的旋转，可以通过四元数乘法直接操作，与上述旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。首先，四元数的乘法是如下规定的：
基于此定义，在旋转变换中能够求得一个相应的逆元素。用作旋转变换的四元数具有特殊的性质——它们具有单位向量属性。因此，在这种情况下我们可以说该元素不仅存在而且等于它的共轭元素。具体来说，在这种情况下如果我们将一个四元素表示为[a, b, c, d]的形式（其中满足条件a² + b² + c² + d² = 1），那么它的逆运算等同于取其共轭运算即得到的结果形式均为[a, -b, -c, -d]。值得注意的是该乘法运算不满足交换律即对于两个任意选取的不同位置上的元素相乘结果会因顺序不同而不同。通过该方法实现旋转变换的过程如下：将三维空间中的点v_I绕某个轴旋转至v_B的位置这一操作可借助相应的旋转矩阵或者更为简洁地利用 quaternion 运算来进行处理
在MATLAB中可以通过QuatMultiply实现四元数相乘运算通过QuatInv实现四元数求逆运算通过QuatConj实现四元数取共轭运算通过以下MATLAB代码可以验证四元数旋转与其对应的旋转矩阵之间的转换关系
% Matlab code by MulinB, Aerospace Toolbox is needed
pt = [10,20,30]; % point coordinate
yaw = 45;
pitch = 30;
roll = 60;
q = angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi);
R = angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi);
pt1 = R*pt';
pt2 = quatmultiply(quatconj(q), quatmultiply([0,pt],q)); % NOTE the order
disp(pt1');disp(pt2(2:4));在上述代码中，我们可以观察到四元数与欧拉角以及DCM之间的转换关系。在MATLAB环境中，通过 quat, dcm, angle 之间的转换关系可以实现任意格式间的切换。此外，在基于四元数计算轴向量及其对应角度时，在基于四元数计算轴向量及其对应角度时
% Matlab code by MulinB, Compute the axis and angle from a quaternion
function [axis, theta] = quat2axisangle(q)
theta = acos(q(1)) * 2;
axis = q(2:4)/sin(theta/2);从OpenCV的rotation vector和quaternion的互转可以用以下代码：
% Matlab code by MulinB, Convert a quaternion to a rotation vector
function rvec = quat2rvec(q)
theta = acos(q(1)) * 2;
axis = q(2:4)/sin(theta/2);
axis = axis / norm(axis);
rvec = axis*theta; % Matlab code by MulinB, Convert a rotation vector to a quaternion
function q = rvec2quat(rvec)
theta = norm(rvec);
axis = rvec/theta;
sht = sin(theta/2);
q = [cos(theta/2), axis*sht];关于旋转四元数的比较好的文档，这里列几个参考文献：
Indirect Extended Kalman Filter Approach for 3D Attitude Estimation Using Quaternions (Contributed by Nikolas Trawny and Stergios Roumeliotis, https://www-users.cs.umn.edu/~trawny/Publications/Quaternions_3D.pdf)
quaternion kinematics underlying the error-state extended Kalman filter framework by Joan Sola
该教材详细阐述了机器人操作的基础理论。
4. 陀螺仪(Gyroscope)
随着微型化与普及使得MEMS陀螺仪的应用日益广泛，在计算机视觉领域中，更多采用IMU作为辅助数据输入以提升系统稳定性。针对陀螺仪数据融合及姿态角计算方面的问题，在此列出一些较为优秀的参考文献
[1]. IMU数据融合：互补型滤波器、卡尔曼滤波器和马哈尼滤波器
[2]. Crazepony 开源项目