mpu6050姿态解算与卡尔曼滤波（1）数学

定义地理坐标系n系：x轴指向东，y轴指向北，z轴指向天。在mpu6050芯片上定义载体坐标系b系。那么b系的姿态就是指n系与b系相对的旋转关系，即如何由n系旋转到b系。

描述这种旋转关系通常使用欧拉角$(\psi, \theta,\gamma)^T$，姿态矩阵T(3x3)，四元数$Q=(q_0，q_1，q_2，q_3)^T$。

欧拉角指的是将n系按照z轴->x’轴->y”轴的顺序依次转动 $\psi, \theta,\gamma$ 后就是b系，事实上欧拉角并没有规定转动顺序，以上转动顺序z轴->x’轴->y”轴为惯性导航中常使用的规定，$(\psi, \theta,\gamma)^T$依次命名为偏航、俯仰、滚转。

姿态矩阵T描述的是三维空间中的两个标准正交基之间的向量转换关系，T是一个3x3的正交阵。若一个向量在n系下的坐标为三维列向量$x_n$，则该向量在b系下的坐标为$x_b=T_n^b\cdot x_n$，反之$x_n=T_b^n\cdot x_b$。T阵是一个正交矩阵，故$T_n^b$与$T_b^n$互为转置矩阵，互为逆矩阵。

四元数Q的本质涉及到更高等的数学，若仅考虑用四元数描述姿态，只需记住：描述姿态的四元数是归一化的，即:

$$|Q|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}=1$$
每个归一化的四元数都描述了一个姿态，或者说描述了一个可由n系旋转得到的b坐标系。

以上三种描述方式是相互等价的，只要确定了两个坐标系的旋转关系，那么就唯一地确定了一组欧拉角，也唯一地确定了一个姿态矩阵和归一化四元数。三种描述方式可以互相推导，由欧拉角可以推导出姿态矩阵，再由姿态矩阵可以推导出四元数。同样，由四元数可以得到姿态矩阵，再得到欧拉角。
改正：以上三种描述方式是等价的，但是要注意对于两个确定的旋转关系的坐标系，表示两者旋转关系的三种方式中，姿态矩阵一定是唯一的，但是四元数却可以有两个，而欧拉角可以有很多。在做彼此的转换，尤其是涉及到四元数表示的时候，一定要注意。

转换关系如下：

由欧拉角$\psi, \theta,\gamma$得到姿态矩阵$T_n^b$：
绕z轴旋转矩阵$T_z$：

$$T_z=\begin{bmatrix} \cos \psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$$
绕x轴旋转矩阵 $T_x$:
$$T_x=\begin{bmatrix} 1 & 0& 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta&\cos\theta \\ \end{bmatrix}$$
绕y轴旋转矩阵 $T_y$:
$$T_y=\begin{bmatrix} \cos \gamma &0& -\sin\gamma \\ 0&1 & 0 \\ \sin\gamma & 0 & \cos\gamma \\ \end{bmatrix}$$
则姿态矩阵 $T_n^b=T_y\cdot T_x\cdot T_z$
所以：
$$T_n^b= \begin{bmatrix} \cos\gamma \cos\psi+\sin\gamma \sin\theta \sin\psi&\cos\gamma \sin\psi+\sin\gamma \sin\theta \cos\psi&-\sin\gamma \cos\theta\\ -\cos\theta \sin\psi&\cos\theta \cos\psi&\sin\theta\\ \sin\gamma \cos\psi+\cos\gamma \sin\theta\sin\psi&\sin\gamma\sin\psi-\cos\gamma\sin\theta\cos\psi&\cos\gamma\cos\theta\\ \end{bmatrix}$$

由姿态矩阵得到欧拉角$\psi,\theta,\gamma$

由四元数 $Q$ 转换到姿态矩阵T：

$$T_n^b=\begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2)\\ 2(q_1q_2-q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3+q_0q_1)\\ 2(q_1q_3+q_0q_2)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$$

由姿态矩阵转换到四元数：
由矩阵 T 的对角线元素可知:

$$\begin{matrix}q_0^2+q_1^2-q_2^2-q_3^2=T_{11}\\q_0^2-q_1^2+q_2^2-q_3^2=T_{22}\\q_0^2-q_1^2-q_2^2+q_3^2=T_{33}\\q_0^2+q_1^2+q_2^2+q_3^2=1\end{matrix}$$
故而有
$$\begin{matrix}|q_1|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\|q_2|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\|q_3|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\|q_0|=\sqrt{1-q_1^2-q_2^2-q_3^2}\end{matrix}$$
其中，正负号如下：
$$\begin{matrix}sign(q_0)=+\\sign(q_1)=sign(T_{32}-T_{23})\\sign(q_2)=sign(T_{13}-T_{31})\\sign(q_3)=sign(T_{21}-T_{12})\end{matrix}$$

——————————————————————————————————————————————————————————
考虑这样一个问题：若b系相对n系的欧拉角为$(\psi,\theta,\gamma)$，以角速度 $\omega_{nb}$ 转动了$\Delta t时间$，注意角速度$\omega_{nb}$ 是一个三维向量，那么欧拉角的变化量$(\Delta\psi,\Delta\theta,\Delta\gamma)$应该是多少呢？这个问题旨在找出欧拉角的变化速率与转动角速度的关系，以下给出这个关系。
欧拉角微分方程

$$\begin{bmatrix}\dot\psi\\\dot\theta\\\dot\gamma\end{bmatrix}=\frac{1}{\cos\theta}\begin{bmatrix}-\sin\gamma&0&\cos\gamma\\ \cos\gamma\cos\theta&0&\sin\gamma\cos\theta\\ \sin\theta\sin\gamma&\cos\theta&-\sin\theta\cos\gamma\end{bmatrix}\begin{bmatrix}\omega_{nbx}^b\\\omega_{nby}^b\\\omega_{nbz}^b\end{bmatrix}$$
上式中 $\omega_{nbx}^b$ 表示转动角速度 $\omega_{nb}$ 在b坐标系x轴上的投影，也就是固连在转动物体上的三轴陀螺仪测量的角速度。若已知$t_0$时刻的欧拉角$(\psi,\theta,\gamma)$，以及从$t_0$到$t$时刻的角速度曲线，那么$t$时刻的欧拉角自然是可以计算出的，这是一个求解一阶微分方程组的问题。
然而欧拉角微分方程是一个非线性微分方程组，它的解析解并不能以简单函数的形式表达出来。若把目光投向姿态矩阵和四元数，亦有微分方程如下：

姿态矩阵微分方程

$$\begin{bmatrix}\dot T_{11}&\dot T_{12}&\dot T_{13}\\ \dot T_{21}&\dot T_{22}&\dot T_{23}\\ \dot T_{31}&\dot T_{32}&\dot T_{33}\end{bmatrix}= \begin{bmatrix}T_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\\ \end{bmatrix} \begin{bmatrix} 0&-\omega_{nbz}^b&\omega_{nby}^b\\ \omega_{nbz}^b&0&-\omega_{nbx}^b\\ -\omega_{nby}^b&\omega_{nbx}^b&0\\ \end{bmatrix}$$

四元微分方程：

$$\begin{bmatrix}\dot q_0\\\dot q_1\\\dot q_2\\\dot q_3\end{bmatrix}=\frac{1}{2} \begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}$$

到这里，对于载体的姿态问题就有了一个解决办法：在载体上安装相互正交的三个陀螺仪，测量载体相对导航坐标系的转动，选取欧拉角/姿态矩阵/四元数描述姿态，按照微分方程积分，就可以得到姿态了。
若以四元数描述姿态，则一个典型公式为：

$$\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}^{k+1}=\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}^{k}+\begin{bmatrix}\dot q_0\\\dot q_1\\\dot q_2\\\dot q_3\end{bmatrix}\Delta t$$
若记 $$[ \omega \times]=\begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}$$
则 $$Q^{k+1}=\{I_{4\times 4}+\frac{1}{2}[ \omega \times]\}Q^k$$
由于这是一个积分的做法，在实际应用中不可避免的会出现误差发散，长时间之后这个办法给出的姿态就不具有太高的可信度了。此外实际应用中还需注意一个问题，由于角速度测量的误差，四元数/姿态矩阵在一段时间后可能会不满足归一性/正交性，需要对四元数/姿态矩阵做归一化/正交化处理。
——————————————————————————————————————————————————————————
关于mpu6050加速度计，很多人可能无法理解为什么在相对n系静止的情况下加速度计会测量到一个与重力加速度相反的加速度，建议阅读这篇知乎专栏文章中关于加速度计的部分：https://zhuanlan.zhihu.com/p/20082486这里对加速度计的测量解释的较为形象。
理想的载体相对n系无加速度状态下，加速度计的测量为：
$$\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}=T_n^b\begin{bmatrix}0\\0\\g\end{bmatrix}=\begin{bmatrix} \cos\gamma \cos\psi+\sin\gamma \sin\theta \sin\psi&\cos\gamma \sin\psi+\sin\gamma \sin\theta \cos\psi&-\sin\gamma \cos\theta\\ -\cos\theta \sin\psi&\cos\theta \cos\psi&\sin\theta\\ \sin\gamma \cos\psi+\cos\gamma \sin\theta\sin\psi&\sin\gamma\sin\psi-\cos\gamma\sin\theta\cos\psi&\cos\gamma\cos\theta\\ \end{bmatrix}\begin{bmatrix}0\\0\\g\end{bmatrix} =\begin{bmatrix}-g\sin\gamma \cos\theta\\g\sin\theta\\g\cos\gamma\cos\theta\end{bmatrix}$$
可以看到，在载体本身无加速度的情况下，加速度计的测量输出是与 $\psi$ 无关的，也就是说加速度计只能反映姿态的一部分信息，仅凭加速度计的测量并不能唯一确定一个姿态，这是显而易见的。
要测量姿态，我们至少需要在b系下测量两个n系下已知的向量，很容易想到，引入磁场传感器测量地磁向量，这样就能唯一地计算出姿态矩阵，从而确定载体的姿态。
若考虑载体本身的加速度，如果不依靠其他手段对载体本身的加速度做出估计，那么加速度计的测量就既与载体姿态变化有关，亦与载体加速度有关。