四元数运算与姿态变换

描述旋转有几种方式：旋转矩阵、欧拉角、旋转向量、四元数。旋转矩阵用９个量描述３个自由度，具有冗余性；欧拉角和旋转向量虽然是紧凑的，但是具有奇异性；因此某个牛人找到了四元数，既紧凑又没有奇异性。

1. 四元数表示

一个四元数有一个实部和三个虚部： $q = q_w + q_xi+q_yj+q_zk$

2. 四元数运算

$q_a = w_a + x_ai+y_aj+z_ak$
$q_b = w_b + x_bi+y_bj+z_bk$
加减：

乘法（不可交换）：

共轭：
$q^{*} = q_{w}-q_{x}i-q_{y}j-q_{z}k$
$qq^{*} = q^{*}q = [q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}, 0.0, 0.0,0.0]$
模长：


逆：



数乘与点乘法：

3. 四元数表示旋转、与欧拉角和旋转矩阵间的变换关系

3.1 四元数表示旋转

已知旋转轴n=[nx;ny;nz]进行了角度为θ的旋转，四元数形式为：

反之同样可以根据四元数得到旋转轴和夹角：

单位四元数，表示没有任何旋转：

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示，及q和-q表示的同一个旋转。

3.2 四元数与欧拉角的变换
此处欧拉角为绕zyx轴旋转的角度为(yaw, pitch, roll)
已知四元数计算欧拉角：



已知欧拉角计算四元数：




3.3 四元数与旋转矩阵的变换
已知四元数计算旋转矩阵：

已知旋转矩阵计算四元数：根据上面矩阵公式很容易得到四元数计算方式，此处略。

4. 四元数进行姿态变换

假设坐标系O1上的点P1(x1, y1, z1),存在变换矩阵Ｒ, 可计算P1点在坐标系O2上的坐标值为P2(x2, y2, z2)：
$P2 = R*P1$
矩阵Ｒ对应的四元数为q, 则使用四元数计算为:
首先三维空间点用一个虚四元数来描述：P1=[0, x1, y1, z1], P2=[0, x2, y2, z2]
则P2和P1将计算关系为：