刚体在三维空间的旋转(关于旋转矩阵、DCM、旋转向量、四元数、欧拉角)

最近学习了一些关于三维空间旋转相关的知识，借此梳理一下备忘。

三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以用绕三维空间中某一个轴的一次旋转来表示。表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。本篇文章主要梳理一下这些表示方式及相互转换的方法。

1. 欧拉角(Euler Angle)

最直观的表示方式是绕刚体自身的X、Y、Z三个轴分别进行旋转某个角度，这就是所谓的欧拉角(Euler Angle)表示方式。

需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！

另外需要注意的是，在欧拉角的表示方式里，三个旋转轴一般是随着刚体在运动，即wikipedia中所谓的intrinsic rotation，见下图动画所示(图来自wikipedia)。相对应的另一种表示方式是，三个旋转轴是固定的，不随刚体旋转而旋转，即extrinsic rotation，这种表示方式在计算机视觉中不是很常用。

欧拉角的表示方式比较直观，但是有几个缺点：

(1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的，关于万向锁的解释，有条件的同学看看Youtube的视频或许会比较直观。

(2) 欧拉角的插值比较难。

(3) 计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大。

2. 旋转矩阵(Rotation Matrix)和方向余弦矩阵(Direction Cosine Matrix)

在计算坐标变换时，旋转更方便的表示形式是旋转矩阵(Rotation Matrix)。三维空间的旋转矩阵可以表示成3x3的矩阵，将欧拉角转换为旋转矩阵的计算方式如下，假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma，则旋转矩阵可以计算如下：

其中，

这里也可以看出，如果yaw、pitch、roll的顺序有改变，矩阵相乘的顺序需要作出相应改变，所得的旋转矩阵结果也会有所改变。

需要注意的是，旋转矩阵的虽然有9个元素，但是只有3个自由度，所以不是任何矩阵都可以作为旋转矩阵，旋转矩阵需要是正交矩阵 (即逆矩阵等于转置矩阵)。

此外，旋转矩阵的另一个名字叫方向余弦矩阵(Direction Cosine Matrix)，简称DCM，在陀螺力学领域较为常用。DCM的名字来历其实是用欧拉角之外的另一种用3个角度值表示三维旋转的方式，假设刚体在起始朝向时三个坐标轴的向量为I,J,K，而刚体在目标朝向时的三个坐标轴的向量为i,j,k，则该旋转可以通过三个坐标轴分别与原始坐标轴的夹角表示，如下图所示：

DCM可以通过三个夹角的余弦计算如下：

这就是DCM名称的由来。其实可以验证，DCM其实就是旋转矩阵，所以，下文不再区分DCM和旋转矩阵的称呼。

在Matlab中(R2006a以后的版本中，需安装Aerospace Toolbox)，可以方便地用angle2dcm和dcm2angle来转换欧拉角和旋转矩阵。下面的Matlab代码可以验证，两个不同的欧拉角方式可以转换到相同的旋转矩阵：

% 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表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，theta可以用一个角度值来表示，直观来讲，一个四维向量(theta,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值theta。这样以来，可以用一个三维向量(theta*x, theta*y, theta*z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

Axis-Angle的表示方法还可以推导出另一种很常用的三维旋转表示方法，叫四元数(Quaternion)，这里有一篇非常通俗易懂介绍四元数的文章。同上，假设(x,y,z)是axis方向的单位向量，theta是绕axis转过的角度，那么四元数可以表示为[cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)]。注意，这里可以推导出，用于表示旋转的四元数向量也必须是单位向量。四元数的神奇之处在于，对于三维坐标的旋转，可以通过四元数乘法直接操作，与上述旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。首先，四元数的乘法是如下规定的：

由此定义，四元数的逆也可以求出。作为旋转四元数，由于其单位向量的特性，四元数的逆其实等于四元数的共轭，也就是如果四元数q=[a,b,c,d]，由于a^2+b^2+c^2+d^2=1，那么q的逆和共轭都是q'=[a,-b,-c,-d]。需要注意的是，四元数的乘法是不可交换的。通过四元数计算旋转的方式为(将三维空间一个点v_I旋转到v_B，四元数是q)：

在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之间的转换来任意互转。另外，从四元数计算axis和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];

关于旋转四元数的比较好的文档，这里列几个参考文献：

[1] Indirect Kalman Filter for 3D Attitude Estimation (by Nikolas Trawny and Stergios Roumeliotis)

[2] Quaternion Kinematics for Error-State KF (by Joan Sola)

[3] A Mathematical Introduction to Robotic Manipulation (by Richard Murray, Zexiang Li, and S. Sastry)

4. 陀螺仪(Gyroscope)

随着MEMS陀螺仪的微型化与普及，越来越多的计算机视觉算法会增加IMU作为辅助信息输入，增加系统的稳定性。关于陀螺仪的数据融合和姿态角解算，这里列几篇比较好的参考文献：

[1]. IMU Data Fusing: Complementary, Kalman, and Mahony Filter

[2]. Crazepony Open Source Project