惯性导航之四元数与欧垃角互转（五）

虽然还没有搞得特别明白，这一次说一下四元数与欧拉角之间的转换。

单位四元数（Unit quarternion）可以用于表示三维空间里的旋转[1]。它与常用的另外两种表示方式（三维正交矩阵和欧拉角）是等价的，但是避免了欧拉角表示法中的万向锁问题。比起三维正交矩阵表示，四元数表示能够更方便地给出旋转的转轴与旋转角。（维基百科）

---

工具：方向余弦矩阵

---

开始正文

---

设 q 为一个单位四元数，而 p 是一个纯四元数，定义
Rq(p)=qpq−1
则 Rq(p)) 也是一个纯四元数，可以证明 Rq 确实表示一个旋转，这个旋转将空间的点 p 旋转为空间的另一个点Rq(p))。(维基百科四元数与空间旋转)

下面进行推导过程：

一、四元数转换为欧拉角

已知，单位四元数 q=q0+q1i+q2j+q3k和纯四元数q=0+xi+yj+zk，Rq(p)=qpq−1表示一个旋转。

则：Rq(p)=(q0+q1i+q2j+q3k)⋅(0+xi+yj+zk)⋅(q0−q1i−q2j−q3k)
将上式按照乘法分配律展开式子太长,可以将三个式子全部表示为矩阵(向量)，然后相乘，最终化简成下面的形式：
Rq(p)=⎡⎣⎢⎢q20+q21−q22−q232(q1q2+q0q1)2(q1q3+q0q2)2(q1q2−q0q3)q20−q21+q22−q232(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q20−q21−q22+q23⎤⎦⎥⎥⎡⎣⎢xyz⎤⎦⎥
这个就是四元数表示旋转的旋转矩阵。
cosα⋅i+cosβ⋅j+cosγ⋅k
用欧拉角表示旋转的方向余弦矩阵：
Rx(α)=⎡⎣⎢10 00cosα−sinα0sinαcosα⎤⎦⎥
Ry(β)=⎡⎣⎢cosβ0sinβ010−sinβ0cosβ⎤⎦⎥
Rz(γ)=⎡⎣⎢cosγ−sinγ0sinγcosγ0001⎤⎦⎥
之前说过，欧拉旋转是要有顺序的，我们使用的是ZXY顺规：
A=Rz(γ)Rx(α)Ry(β)
=⎡⎣⎢10 00cosα−sinα0sinαcosα⎤⎦⎥⎡⎣⎢cosβ0sinβ010−sinβ0cosβ⎤⎦⎥⎡⎣⎢cosγ−sinγ0sinγcosγ0001⎤⎦⎥
=⎡⎣⎢cosγcosβ+sinγsinαsinβcosγsinαsinβ−sinγcosβcosαsinβsinγcosαcosγcosα−sinαsinγsinαcosβ−sinβcosγsinγsinβ+cosγsinαcosβcosαcosβ⎤⎦⎥
=⎡⎣⎢A11A21A31A12A22A32A13A23A33⎤⎦⎥

用字母表示前面的式子，可以表示tanβ=A13A33,−sinα=A32,tanγ=A12A22
则

• α=−arcsin(A32)=−sin(2(q2q3−q0q1))
• β=arctan(A13A33)=arctan(2(q1q3+q0q2)1−2q21−2q22)
• γ=arctan(A12A22)=arctan(2(q1q2+q0q3)1−2q21−2q23)

这样就用四元数表示了欧拉角，在后面的姿态解算中就可以直接转换成程序代码使用了。

---

二、欧拉角转换为四元数

欧拉角旋转还是按照ZXY顺规，分解为三次旋转;

• 第一次，绕Z轴：α=β=0,qγ=cosγ2+sinγ2⋅k
• 第二次，绕X轴：γ=β=0,qα=cosα2+sinα2⋅i

• 第三次，绕Y轴：α=γ=0,qβ=cosβ2+sinβ2⋅j

  则四元数 q=q0+q1i+q2j+q3k可以表示为：
  q=qγqαqβ=(cosγ2+sinγ2⋅k)(cosα2+sinα2⋅i)(cosβ2+sinβ2⋅j)
  =(cosα2cosγ2cosβ2+sinα2sinγ2sinβ2)+(sinα2cosγ2cosβ2−cosα2sinγ2sinβ2)⋅i
  +(cosα2cosγ2sinβ2−sinα2sinγ2cosβ2)⋅j
  +(cosα2sinγ2cosβ2−sinα2cosγ2sinβ2)⋅k

可得：

• q0=cosα2cosγ2cosβ2+sinα2sinγ2sinβ2
• q1=sinα2cosγ2cosβ2−cosα2sinγ2sinβ2
• q2=cosα2cosγ2sinβ2−sinα2sinγ2cosβ2
• q3=cosα2sinγ2cosβ2−sinα2cosγ2sinβ2

这样就得到了欧拉角到四元数的转化，在这两种转换的过程中，也使用了不少的矩阵，也涉及到了方向余弦矩阵等，这些不详细说了。。翻了好多线代资料。

---

学会欧拉角和四元数之间的像话转换之后对四元数和欧拉角有了相对清晰的认识之后就可以直接写程序了。(本文手打，字母和角度太多，会有错误….)