学习姿态解算的笔记（包括四元数和融合算法）_四元数解算姿态完全解析及资料汇总

参考资料：
姿态解算简介 Craze维基百科
Mahony互补滤波算法 Craze维基百科
卡尔曼滤波 B站
IMU Data Fusing: Complementary, Kalman, and Mahony Filter
四元数解算姿态完全解析及资料汇总 百度文库

---

如何表示姿态

---

我们可以利用旋转来描述物体的姿态，因此我们保存旋转的信息。
旋转指的是两个坐标系之间的变换关系。

如何表示旋转的信息

---

欧拉角

定义

---

欧拉角：由三个角度组成，在特定坐标系下用于描述刚体的姿态（orientation）.

简单来说，就是绕一个三维坐标系统下的三个基轴旋转三个角度，可以用来表示物体通过各种绕七绕八的转，最终转到某种形态。
 

分类

---

欧拉角按旋转的坐标系分为内旋（intrinsic rotation）和外旋（extrinsic rotation）。
按旋转轴分为经典欧拉角（Proper Euler Angle）和泰特布莱恩角（Tait–Bryan angles）。

经典欧拉角（Proper Euler Angle）：按(z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)轴序列旋转，即第一个旋转轴和最后一个旋转轴相同。

泰特布莱恩角（Tait–Bryan angles）：按(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)轴序列旋转，即三个不同的轴 。
 

表示

---

具体可以用 (φ, θ, ψ)（轴序，旋性）来描述一个欧拉角。

规定旋转正负，旋转顺序和旋转参照的坐标系十分重要，三个条件有一个不同，就可能导致用来表示相同姿态的一组欧拉角不同。而这种不同会导致通过四元数转化得来的欧拉角错误。因为不同顺序的旋转产生的欧拉角是不同的。

yaw-pitch-roll（heading-pitch-bank）
我们把（z,y,x 内旋（intrinsic））的这种格式称为 yaw-pitch-roll(heading-pitch-bank)，
常应用于飞行器。注意，yaw-pitch-row 是针对飞行器的概念。

与之（内旋特性）对应的是用欧拉角表示的方向余弦矩阵 DCM 的求法：三个单轴 DCM 左连乘。

如何理解3D动画中的欧拉角以及死锁？知乎-马同学

 
 

矩阵

---

用相对运动和极坐标来证明会简单许多。也可以用平面几何（具体参见第二条链接）

方向余弦矩阵描述的是 坐标系的旋转。
 
 

四元数如何表示旋转呢？

---

$Q=q_0+q_{1}i+q_{2}j+q_{3}k$

（进一步可探究如何证明）

单位四元数描述的是 矢量的旋转。
 

为何要使用四元数来保存姿态信息

---

运算量小。

 

三者之间的关系

---

使用四元数来保存飞行器的姿态（也就是在地球坐标系中的俯仰/横滚/航向情况）。在需要控制的时候，会将四元数转化为欧拉角，然后输入到姿态控制算法中。

而矩阵是工具（四元数，欧拉角）的工具，它表示坐标系变换提供了便利。而变换矩阵的元素可以是四元数的 $q_1,q_2,q_3,q_4$，也可以是欧拉角正余弦。

求出的四元数已经唯一对应了一种姿态，而用来表示这种姿态的三个欧拉角（绕x，绕y，绕z）会因为顺规的不同而不同，反映在矩阵上就是余弦矩阵不同。

$C_b^R$ 是从四元数推出来的DCM，元素由四元数的系数表示；

$R$ 是从欧拉角定义出发推出来的DCM，元素由欧拉角三角函数表示。

这两个矩阵表示同一个变换，而且都是正交阵，所以两个矩阵相等，对应的元素也相等。由此得到欧拉角与四元数系数之间的关系。

 

转化中需要注意的区别

---

变换矩阵有一个很神奇的特性。

它既可以表示坐标系的旋转，也就是通常所说的坐标系变换，然后用来求某个固定向量在新坐标系下的新坐标。（这是转换矩阵在姿态解算中矫正步骤用到的含义。）

能同时它表示，坐标系不变下向量的旋转，然后用来求旋转之后向量的新坐标。（这是四元数所表达的含义。）

旋转是相对的，矩阵表示的含义取决于我们的观察角度。
 

怎么从原始数据得到四元数

---

过程误差 $w_k$ ~ $N(0,R)$

测量误差 $v_k$ ~ $N(0,Q)$

$但这时候我们得到四元数只用到了陀螺仪的数据。$
 

数据融合

---

Mahony算法

卡尔曼滤波

方差和协方差

---

我们如何衡量数据的可靠程度呢？使用方差或协方差。

协方差的大小既受两个随机变量各自的方差的影响，也受两个两个随机变量相关性的影响。而在卡尔曼滤波当中，我们主要用到协方差的第一个性质。

状态观测器

---

记住，$x$ 是我们想要的，而$y$是我们能测到的

线性系统，可以用一阶线性微分方程描述。
不连续的系统，可以用差分方程描述，其实就是递推式。

这里有一种粗略的转换关系可以帮助理解。
$$x^{\prime }=Ax+B$$$$\underset{\Delta t->0}{\lim }\frac{x(t+\Delta t)-x(t)}{\Delta t}=Ax(t)+B$$
假设 $\Delta t$很小，那么有$$x(t+\Delta t)=(A\Delta t+1)x(t)+B\Delta t$$$$x_k=A^{\prime }{x}_{k-1}+B^{\prime }$$

解一下一阶线性微分方程。

卡尔曼滤波的作用

---

状态观测器不考虑噪声，要求系统可检测，并且增益通常是时不变的。

而卡尔曼滤波则详细考虑了噪声和初始估计误差，智能的计算每一步的增益，以达到对状态的最优的估计。

过程误差 $w_k$ ~ $N(0,R)$

测量误差 $v_k$ ~ $N(0,Q)$

在预测部分，系统模型用于计算状态预估值和误差协方差P，对于单状态系统，P是状态预估值的方差，可以把它当作预测状态中的不确定性的度量。这种不确定性来自过程噪声和预估值 ${\hat{x}}_{k-1}$ 的不确定性的影响。

在算法的最开始，预估值 ${\hat{x}}_{k-1}$​ 和 ​$P_{k-1}$ 值来自初始估计值。

在更新步骤，算法使用在预测步骤中计算得到的预估值，计算并更新后验估值和误差协方差。

调整卡尔曼增益，使更新后的状态值误差协方差最小。

卡尔曼滤波应用

如果使用卡尔曼滤波来融合陀螺仪和加速度计，那么实际上就是将陀螺仪的输出作为系统的增量预测，姿态增量方程就是卡尔曼的状态方程（其实就是四元数微分方程），然后从加速度计获得重力加速度作为观测值，然后运用卡尔曼滤波进行融合。