四元数、欧拉角和旋转矩阵
四元数、欧拉角和旋转矩阵
四元数、欧拉角和旋转矩阵都可以描述三维空间中的旋转，三者可以相互转化。这里四元数指的都是单位四元数，不改变向量的模，和旋转矩阵是正交的一个道理。欧拉角的转动顺序为Z\to Y\to X，转动的角度分别为(\psi，\theta，\phi)。
欧拉角介绍
欧拉角包括偏航角(yaw)\psi，俯仰角(pitch)\theta，滚转角(roll)\phi，导航坐标系S-Ox_gy_gz_g经过三次旋转可以得到机体坐标系B-x_by_bz_b。需要注意的是，这里旋转的是坐标轴，不是向量。旋转过程遵循右手定则，过程如下：
- 以z_g正方向为轴，S系右旋得到过渡坐标系S'-Ox'y'z'
- 以y'正方向为轴，S'系右旋得到过渡坐标系S''-Ox''y''z''
- 以x''正方向为轴，S''系右旋得到机体坐标系B-Ox_by_bz_b
每次旋转都用一个旋转矩阵表示如下：
第一次旋转：
第二次旋转：
第三次旋转：
将三个旋转矩阵相乘得到：
R_n^b就是用欧拉角表示的旋转矩阵。
四元数介绍
四元数一般形式为：\vec q = w+x\vec i+y\vec j+z\vec k，如果四元数的模为1，称之为单位四元数，可以表示为：\vec q = cos(\frac{\theta}{2})+sin(\frac{\theta}{2})(cos(\phi_x)\vec i+cos(\phi_y)\vec j+cos(\phi_z)\vec k，含义：以当前坐标系中(cos(\phi_x),cos(\phi_y),cos(\phi_z))为轴，旋转角度。下面以导航坐标系(东北天)到机身坐标系的转化为例，介绍四元数的用法。
四元数的旋转是通过乘法实现的：p'= qpq^{-1}。q^{-1}表示四元数的逆，对于单位四元数，q^{-1}=q^*。(注：四元数的乘法和共轭去网上查)
比如：取p=[1,1,1]，q=[cos(\frac {90^\circ}{2}),0,0,sin(\frac{90^\circ}{2})]=[0.707,0,0,0.707]，在MATLAB中计算：
>> q=[0.707,0,0,0.707];
>> q1=quatmultiply(q,[0,1,1,1]);
>> quatmultiply(q1,quatinv(q))
ans =
0 -1.0000 1.0000 1.0000
matlab
旋转之后的向量为(-1,1,1)，向量和坐标轴的转动是相对的，因此有两种理解方式：
- 坐标轴固定不动，向量以Z正方向为轴，按照右手定则旋转了45^\circ，计算得到的是新向量在此坐标系下的坐标。
- 向量固定不动，坐标轴以Z正方向为轴，按照左手定则旋转了，计算得到的是原向量在新的坐标系下的坐标。
理解两种情况下转动的对象和转动的正方向是很重要的，否则在进行三者之间的相互转换时会出错。
若记q=(q_0,q_1,q_2,q_3)，p=(x,y,z)，p'=(x',y'z')，则也可以写成矩阵形式：
此矩阵便是四元数对应的旋转矩阵。
下面我们用四元数表示欧拉角的三次旋转，并推导出二者的转化关系。(注：根据四元数旋转的第二种理解，如果我们旋转的是坐标轴，那么应该按照左手定则指定旋转的正方向，而欧拉角旋转时采用的是右手定则，因此四元数转动角度为-)
也许你会发现这与 WIKI上的结果是互逆的或者互为共轭。原因很简单：按照我们对欧拉角的定义，WIKI上的Body坐标系就相当于我们的导航坐标系，Lab坐标系相当于我们的机体轴系。如果 \vec q可以实现 B\to S，那么 {\vec q}^*必然可以实现 S\to B。
相互转换
欧拉角转换为四元数
根据(3)式，可以实现欧拉角转换四元数。C++实现代码如下：
void cEulerAngle::toQuat(cQuaternion &quat) const
{
double cos_hroll = cos(_roll / 2.0);
double sin_hroll = sin(_roll / 2.0);
double cos_hpitch = cos(_pitch / 2.0);
double sin_hpitch = sin(_pitch / 2.0);
double cos_hyaw = cos(_yaw / 2.0);
double sin_hyaw = sin(_yaw / 2.0);
quat._w = cos_hroll*cos_hpitch*cos_hyaw + sin_hroll*sin_hpitch*sin_hyaw;
quat._x = cos_hroll*sin_hpitch*sin_hyaw - sin_hroll*cos_hpitch*cos_hyaw;
quat._y = -sin_hroll*cos_hpitch*sin_hyaw - cos_hroll*sin_hpitch*cos_hyaw;
quat._z = -cos_hroll*cos_hpitch*sin_hyaw + sin_hroll*sin_hpitch*cos_hyaw;
}
cpp
四元数转化为欧拉角
(1)式和(2)式两个旋转矩阵相等，可以得到：
然后呢，atan()函数的返回值位于(-\frac{\pi}{2},\frac{\pi}{2})之间，而和的范围为(-\pi,\pi)，因此应该换用atan2()函数。同时，asin()的返回值位于，这也就是俯仰角的范围。不要怀疑俯仰角的范围是不是小了，因为这三个角的范围已经能够完全表述三维空间中的旋转了。最终得到：
C++示例代码如下：
void cQuaternion::toEuler(cEulerAngle &angle) const
{
angle._roll = atan2(2 * (_z*_y - _w*_x), _w*_w - _x*_x - _y*_y + _z*_z);
angle._pitch = asin(-2 * (_w*_y + _z*_x));
angle._yaw = atan2(2 * (_y*_x - _w*_z), _w*_w + _x*_x - _y*_y - _z*_z);
}
cpp
旋转矩阵转四元数
根据(2)式，便可以推得：
void cDcm::toQuat(cQuaternion &quat) const
{
double trace = m11 + m22 + m33;
double temp = 0.0;
if (trace > 0)
{
temp = sqrt(trace + 1.0)*0.5;
quat._w = temp;
temp = temp*0.5;
quat._x = (m23 - m32)*temp;
quat._y = (m31 - m13)*temp;
quat._z = (m12 - m21)*temp;
}
else
{
if (m11 > m22 && m11 > m33)
{
temp = 2.0 * sqrt(1.0 + m11 - m22 - m33);
quat._w = (m32 - m23) / temp;
quat._x = 0.25* temp;
quat._y = (m12 + m21) / temp;
quat._z = (m13 + m31) / temp;
}
else if (m22 > m33)
{
temp = 2.0 * sqrt(1.0 + m22 - m11 - m33);
quat._w = (m13 - m31) / temp;
quat._x = (m12 + m21) / temp;
quat._y = 0.25 * temp;
quat._z = (m23 + m32) / temp;
}
else
{
temp = 2.0 * sqrt(1.0 + m33 - m11 - m22);
quat._w = (m21 - m12) / temp;
quat._x = (m13 + m31) / temp;
quat._y = (m23 + m32) / temp;
quat._z = 0.25 * temp;
}
}
}
cpp
旋转矩阵与欧拉角相互转换
欧拉角到旋转矩阵根据(1)式便可以得到，旋转矩阵到欧拉角也是求反正弦、反余弦，代码比较简单，不贴了。