欧拉角法与扩展卡尔曼滤波

在导航坐标系与姿态描述方法一文中，我们已经初步了解到，欧拉角可以直观的描述运载体的位姿。但是由于欧拉角存在奇异性的问题，所以不适用全姿态解算。但是这一缺陷并不意味着它不能够进行插值和迭代，只要运载体的运动能够保证不存在大幅度的水平姿态调整就可以使用。因为其最为直观，所以在船舶、汽车上仍有广泛的使用，在不需要大机动的运输机、民航客机上也是可以使用的。

卡尔曼滤波是一种常用的信息融合与状态估计方法，它是一种适用于线性系统的最优估计方法。为了适应非线性系统，人们对其进行了扩展，将状态方程和观测方程进行一阶泰勒近似，得到一组近似的线性方程，进而应用朴素的卡尔曼滤波的思想完成估计，这种方法就是所谓的扩展卡尔曼滤波(EKF)。

本文中，我们先推导欧拉角的微分方程。然后介绍如何使用扩展卡尔曼滤波器(EKF)进行航姿解算，并利用加表的数据对航姿进行修正。

1. 欧拉角微分方程

欧拉角是一种非常直观的描述姿态的方法，它把飞行器的姿态分解为先后绕不同轴转动的三个角度。根据转轴的顺序不同，同一个姿态可以有多种不同的欧拉角表示方式，我们经常使用的是一种称为rpy的欧拉角。它是指飞行器先后绕机体坐标系的$z_b$轴、$y_b$轴、$x_b$轴转动。这三个角分别被称为偏航角(yaw)、 俯仰角(pitch)、横滚角(row)。由于每次转动都是相对机体坐标系完成的，所以依次右乘基本旋转矩阵就可以得到$C_b^n$：

$$ \begin{equation}\label{f1.1} \begin{array}{rcl} C_b^n & = & \begin{bmatrix} C_Y & -S_Y & 0 \\ S_Y & C_Y & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} C_P & 0 & S_P \\ 0 & 1 & 0 \\ -S_P & 0 & C_P \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & C_R & -S_R \\ 0 & S_R & C_R \end{bmatrix} = \begin{bmatrix} C_Y & -S_Y & 0 \\ S_Y & C_Y & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} C_P & S_PS_R & S_PC_R \\ 0 & C_R & -S_R \\ -S_P & C_PS_R & C_PC_R \end{bmatrix} \\ & = & \begin{bmatrix} C_YC_P & -S_YC_R + C_YS_PS_R & S_YS_R + C_YS_PC_R \\ C_PS_Y & C_YC_R + S_YS_PS_R & -C_YS_R + S_YS_PC_R \\ -S_P & C_PS_R & C_PC_R \end{bmatrix} \end{array} \end{equation} $$

其中，下标$R, P, Y$分别表示横滚角、俯仰角和偏航角。$C_{\bullet}$和$S_{\bullet}$分别表示余弦函数$\cos (\bullet)$和正弦函数$\sin (\bullet)$。在多个坐标系下的速度和角速度分析一文中，我们发现旋转矩阵$C_b^n$及其关于时间的导数$\dot{C_b^n}$与b系下刚体的角速度$\vec{\omega^b}$之间存在如下的关系：

$$ \begin{equation}\label{f1.2} \dot{C_b^n} = C_b^n\vec{\omega_{\times}^b} \end{equation} $$

其中$\vec{\omega_{\times}^b}$是由$\vec{\omega^b}$构成的斜对称矩阵。对$C_b^n$关于时间求导$\dot{C_b^n}$可以写成如下的形式：

$$ \begin{bmatrix} -\dot{Y}S_YC_P - \dot{P}C_YS_P & -\dot{Y}(C_YC_R + S_YS_PS_R) + \dot{R}(S_YS_R + C_YS_PC_R) + \dot{P}C_YC_PS_R & \dot{Y}(C_YS_R - S_YS_PC_R) + \dot{R}(S_YC_R - C_YS_PS_R) + \dot{P}C_YC_PC_R \\ \dot{Y}C_YC_P - \dot{P}S_YS_P & \dot{Y}(C_YS_PS_R - S_YC_R) + \dot{R}(S_YS_PC_R - C_YS_R) + \dot{P}S_YC_PS_R & \dot{Y}(S_YS_R + C_YS_PC_R) - \dot{R}(C_YC_R + S_YS_PS_R) + \dot{P}S_YC_PC_R \\ -\dot{P}C_P & -\dot{P}S_PS_R + \dot{R}C_PC_R & -\dot{P}S_PC_R - \dot{R}C_PS_R \end{bmatrix} $$

由于旋转矩阵$C_b^n$是正交矩阵，所以有：

$$ (C_b^n)^{-1} = (C_b^n)^T = C_n^b = \begin{bmatrix} C_YC_P & C_PS_Y & -S_P \\ -S_YC_R + C_YS_PS_R & C_YC_R + S_YS_PS_R & C_PS_R \\ S_YS_R + C_YS_PC_R & -C_YS_R + S_YS_PC_R & C_PC_R \end{bmatrix} $$

将式($\ref{f1.2}$)的左右两端右乘矩阵$C_n^b$即可求得$\vec{\omega_{\times}^b}$：

$$ \begin{equation}\label{f1.3} \vec{\omega_{\times}^b} = C_n^b\dot{C_b^n} = \begin{bmatrix} 0 & \dot{P}S_R - \dot{Y}C_RC_P & \dot{P}C_R + \dot{Y}S_RC_P \\ -\dot{P}S_R + \dot{Y}C_RC_P & 0 & \dot{Y}S_P - \dot{R} \\ -\dot{P}C_R - \dot{Y}S_RC_P & \dot{R} - \dot{Y}S_P & 0 \end{bmatrix} \end{equation} $$

所以

$$ \begin{equation}\label{f1.4} \begin{cases} \omega_x & = & \dot{R} - \dot{Y}S_P \\ \omega_y & = & \dot{P}C_R + \dot{Y}S_RC_P \\ \omega_z & = & \dot{Y}C_RC_P -\dot{P}S_R \end{cases} \Rightarrow \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = \begin{bmatrix} 1 & 0 & -S_P \\ 0 & C_R & S_RC_P \\ 0 & -S_R & C_RC_P \end{bmatrix}\begin{bmatrix} \dot{R} \\ \dot{P} \\ \dot{Y} \end{bmatrix} \end{equation} $$

对上式求逆，就可以得到欧拉角微分方程：

$$ \begin{equation}\label{f1.5} \begin{bmatrix} \dot{R} \\ \dot{P} \\ \dot{Y} \end{bmatrix} = \begin{bmatrix} 1 & \tan P\sin R & \cos R \tan P \\ 0 & \cos R & -\sin R \\ 0 & \cfrac{\sin R }{\cos P } & \cfrac{\cos R }{\cos P } \end{bmatrix}\begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \end{equation} $$

2. 卡尔曼滤波

卡尔曼滤波早在上个世纪60年代就已经出现了，它是一种时域的方法，通过状态空间方程来进行最优估计，可以递推的形式进行，能够方便的在计算机上实现。最早是为了阿波罗计划中的导航系统提出的，但是卡尔曼最初提出的理论是针对线性系统的，而实际很多系统都是非线性的，比如这里的欧拉角位姿解算就是一种非线性的系统。

为了解决这一问题，人们后来研究了扩展卡尔曼滤波(Extended Kalman Filter, EKF)、无迹卡尔曼滤波(Unscented Kalman Filter, UKF)等方法。著名的PR一书中用了一章来介绍卡尔曼滤波和各种扩展形式。我们先来看一下朴素的卡尔曼滤波以及它的基本方程。

设有一个离散系统，在$t$时刻的系统状态为$\vec{x_t}$，并受到系统噪声$\vec{\varepsilon_t}$作用。$\vec{\varepsilon_t}$是均值为$\vec{0}$，协方差矩阵为$\mathcal{R_t}$的随机白噪声。此外，令$A_{t,t-1}$为$t-1$时刻到$t$时刻的状态转移矩阵，$B_{t-1}$是噪声作用矩阵。那么该离散系统的状态方程可以写作：

$$ \begin{equation}\label{f2.1} \vec{x_t} = A_{t, t-1}\vec{x_{t-1}} + B_{t-1}\vec{\varepsilon_{t-1}} \end{equation} $$

假如在$t$时刻的系统观测量为$\vec{z_t}$，$C_t$为测量矩阵，$\vec{\delta_t}$是均值为$\vec{0}$协方差为$\mathcal{Q_t}$的测量噪声，那么该系统的观测方程可以写作：

$$ \begin{equation}\label{f2.2} \vec{z_t} = C_t\vec{x_t} + \vec{\delta_t} \end{equation} $$

针对这种线性的离散系统，朴素卡尔曼滤波器的五个基本方程就有用武之地了。整个状态估计的迭代过程可以分为预测状态和方差、计算卡尔曼增益、修正状态和方差三个部分：

预测状态和方差: $$ \begin{equation}\label{f2.3} \hat{\vec{x_t}} = A_{t, t-1}\vec{x_{t-1}} \end{equation} $$ $$ \begin{equation}\label{f2.4} \hat{\mathcal{P_t}} = A_{t, t-1} \mathcal{P_t} A_{t, t-1}^T + B_{t-1} \mathcal{R_t} B_{t-1}^T \end{equation} $$
计算卡尔曼增益: $$ \begin{equation}\label{f2.5} K_t = \hat{\mathcal{P_t}}C_t^T\left(C_t \hat{\mathcal{P_t}} C_t^T + \mathcal{Q_t} \right)^{-1} \end{equation} $$
修正状态和方差: $$ \begin{equation}\label{f2.6} x_t = \hat{\vec{x_t}} + K_t\left(z_t - C_t \vec{x_t}\right) \end{equation} $$ $$ \begin{equation}\label{f2.7} \mathcal{P_t} = \left(I - K_t C_t\right)\hat{\mathcal{P_t}} \end{equation} $$

只要给定了初始的系统状态$\vec{x_0}$和协方差矩阵$\mathcal{P_0}$，那么就可以根据上述的五个基本方程迭代的推算出$t$时刻的状态估计。

3. 状态方程观测方程

实际上刚刚提到的五个基本方程并不是应用卡尔曼滤波器的关键，关键是如何正确的写出系统的状态方程和观测方程。在很多介绍卡尔曼滤波器的资料中只是简单的给出了这五个方程，甚至花费了很大的力气去介绍其推导过程，但对工程实现一点用处也没有。只要状态方程和观测方程没有写出来，拿着这五个方程你也不知道怎么用。

想象一下我们的四旋翼上只有一个六轴的IMU，用来测量角速度和线加速度。现在我们就想用欧拉角法来实时的估计飞行器的位姿，暂且不考虑什么奇异性、万向锁之类的奇奇怪怪的名字。回过头来看式($\ref{f1.5}$)中的欧拉角微分方程，可以看出它是一个连续的非线性系统。在控制理论中，有一系列办法把一个线性系统的状态方程转换成离散系统的形式。这里似乎不太适用，但根据式($\ref{f1.5}$)的物理内涵，我们可以近似的写出一个迭代的离散状态方程：

$$ \begin{equation}\label{f3.1} \begin{bmatrix} R_t \\ P_t \\ Y_t \end{bmatrix} = \begin{bmatrix} R_{t-1} \\ P_{t-1} \\ Y_{t-1} \end{bmatrix} + \begin{bmatrix} 1 & \tan P_{t-1}\sin R_{t-1} & \cos R_{t-1} \tan P_{t-1} \\ 0 & \cos R_{t-1} & -\sin R_{t-1} \\ 0 & \cfrac{\sin R_{t-1} }{\cos P_{t-1} } & \cfrac{\cos R_{t-1} }{\cos P_{t-1} } \end{bmatrix}\begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \Delta t \end{equation} $$

上式中$\vec{\omega} = \begin{bmatrix} \omega_x & \omega_y & \omega_z \end{bmatrix}^T$描述的是飞行器当前的角速度，它是由三轴陀螺仪测量得到的。既然是传感器直接测量的，就由测量误差，那么角速度就可以写成$\vec{\omega} = \vec{\omega_{gyro}} + \vec{\varepsilon}$，其中$\vec{\omega_{gyro}}$是陀螺仪的测量值， $\vec{\varepsilon}$则是测量误差向量，其均值为$\vec{0}$，均方差为$\mathcal{R}$。$\Delta t$为迭代的时间间隔。系统的状态方程可以改写成如下的形式：

$$ \begin{equation}\label{f3.2} \vec{x_t} = g(\vec{x_{t-1}}, \vec{\omega_{gyro}}) + B_{t-1}\vec{\varepsilon} \end{equation} $$

其中，$B_{t-1} = \begin{bmatrix} 1 & \tan P_{t-1}\sin R_{t-1} & \cos R_{t-1} \tan P_{t-1} \\ 0 & \cos R_{t-1} & -\sin R_{t-1} \\ 0 & \cfrac{\sin R_{t-1} }{\cos P_{t-1} } & \cfrac{\cos R_{t-1} }{\cos P_{t-1} } \end{bmatrix}$，$g(\vec{x_{t-1}}, \vec{\omega_{gyro}}) = \vec{x_{t-1}} + B_{t-1}\vec{\omega_{gyro}}\Delta t$。

当运载体处于匀速直线运动或者静止状态时，加表测量的就是重力加速度。只是其测量值是相对于b系的，需要将其转换到n系下， $\vec{a^n} = C_b^n \vec{a^b}$，展开如下：

$$ \begin{equation} \label{f3.3} \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} C_YC_P & -S_YC_R + C_YS_PS_R & S_YS_R + C_YS_PC_R \\ C_PS_Y & C_YC_R + S_YS_PS_R & -C_YS_R + S_YS_PC_R \\ -S_P & C_PS_R & C_PC_R \end{bmatrix}\vec{a^b} \end{equation} $$

所以有观测方程：

$$ \begin{equation} \label{f3.4} \begin{cases} a_x^b & = & - S_P \\ a_y^b & = & C_PS_R \\ a_z^b & = & C_PC_R \\ \end{cases} \end{equation} $$

设加表的输出为$\vec{a_{acc}^b} = \vec{a^b} + \vec{\delta}$，其中$\vec{\delta}$是均值为$\vec{0}$协方差矩阵为$\mathcal{Q}$的随机白噪声，令$h(\vec{x_t}) = \begin{bmatrix} -S_P & C_PS_R & C_PC_R \end{bmatrix}^T$，那么系统的观测方程可以写作：

$$ \begin{equation} \label{f3.5} \vec{a_{acc}^b} = h(\vec{x_t}) + \vec{\delta} \end{equation} $$

4. 扩展卡尔曼滤波器

根据式($\ref{f3.2}, \ref{f3.5}$)，我们的系统是一个非线性的系统，那么朴素的卡尔曼滤波的基本方程就不再适用了。但是我们可以通过一阶泰勒近似来线性化，我们在$t$时刻的状态均值向量$\vec{μ_t} = E(\vec{x_t})$上对式($\ref{f3.2}$)进行一阶泰勒近似有：

$$ \begin{equation}\label{f4.1} \begin{array}{rcl} \vec{x_t} & = & \vec{μ_t} + G_t \Delta \vec{x_{t-1}} + B_{t-1}\vec{\varepsilon} \\ G_t & = &\cfrac{\partial g(\vec{x_{t-1}}, \vec{\omega_{gyro}})}{\partial \vec{x_{t-1}}} \\ \Delta \vec{x_{t-1}} & = & \vec{x_{t-1}} - \vec{μ_{t-1}} \end{array} \Rightarrow \Delta \vec{x_t} = G_t \Delta \vec{x_{t-1}} + B_{t-1}\vec{\varepsilon} \\ \end{equation} $$

类似的，我们在$t$时刻的观测均值向量$\vec{\nu_t} = E(\vec{z_t})$上对式($\ref{f3.5}$)进行一阶泰勒近似有：

$$ \begin{equation}\label{f4.2} \begin{array}{rcl} \vec{z_t} & = & \vec{\nu_t} + H_t \Delta \vec{x_t} + \vec{\delta} \\ H_t & = &\cfrac{\partial h(\vec{x_t})}{\partial \vec{x_t}} \\ \Delta \vec{z_t} & = & \vec{z_t} - \vec{\nu_t} \end{array} \Rightarrow \Delta \vec{z_t} = H_t \Delta \vec{x_t} + \vec{\delta} \\ \end{equation} $$

式中$G_t, H_t$分别是状态函数$g(\vec{x_{t-1}}, \vec{\omega_{gyro}})$和$h(\vec{x_t})$的雅可比矩阵：

$$ \begin{equation} \label{f4.3} G_t = \begin{bmatrix} 1 + \tan(P)\left[\cos(R)\omega_y - \sin(R)\omega_z\right] & \cfrac{\sin(R)}{\cos^2(P)}\omega_y + \cfrac{\cos(R)}{\cos^2(P)}\omega_z & 0 \\ -\sin(R)\omega_y - \cos(R)\omega_z & 1 & 0 \\ \cfrac{\cos(R)}{\cos(P)}\omega_y - \cfrac{\sin(R)}{\cos(P)}\omega_z & \cfrac{\sin(R)\tan(P)}{\cos(P)}\omega_y + \cfrac{\cos(R)\tan(P)}{\cos(P)}\omega_z & 1 \end{bmatrix} \end{equation} $$ $$ \begin{equation} \label{f4.4} H_t = \begin{bmatrix} 0 & -\cos(P) & 0 \\ \cos(P)\cos(R) & -\sin(R)\sin(P) & 0 \\ -\cos(P)\sin(R) & -\cos(R)\sin(P) & 0 \end{bmatrix} \end{equation} $$

根据式($\ref{f4.1},\ref{f4.2}$)不难看出，偏差量$\Delta \vec{x}$是近似线性的，可以应用朴素卡尔曼滤波器的基本方程进行估计：

$$ \begin{equation}\label{f4.5} \begin{cases} \hat{\Delta \vec{x_t}} & = & G_t \Delta \vec{x_{t-1}} \\ \hat{\mathcal{P_t}} & = & G_t \mathcal{P_t} G_t^T + B_{t-1} \mathcal{R_t} B_{t-1}^T \\ K_t & = & \hat{\mathcal{P_t}}H_t^T\left(H_t \hat{\mathcal{P_t}} H_t^T + \mathcal{Q_t} \right)^{-1} \\ \Delta \vec{x_t} & = & \hat{\Delta \vec{x_t}} + K_t\left(z_t - H_t \Delta \vec{x_t}\right) \\ \mathcal{P_t} & = & \left(I - K_t H_t\right)\hat{\mathcal{P_t}} \end{cases} \end{equation} $$

最后系统状态的最优估计就是：

$$ \begin{equation}\label{f4.6} \vec{x_t} = g(\vec{x_{t-1}}, \vec{\omega_{gyro}}) + \Delta \vec{x_t} \end{equation} $$

5. 完

欧拉角法虽然存在奇异性的问题，但是只要保证载体在水平方向上没有大的机动动作，这种方法还是可以使用的，而且欧拉角很直观。本文在欧拉角微分方程的基础上介绍了扩展卡尔曼滤波器的应用方法。

需要强调的是，系统的观测方程只有在载体静止或者匀速直线运动的时候才成立，此时加表的测量值才只有重力加速。也就是说卡尔曼滤波的的修正部分只有在这种情况下才可以启用，否则将产生误修。在实际场景中匀速直线运动几乎是不可能的，我们可以通过检测载体静止来触发位姿的修正。

此外，本文中根据重力加速度来估计载体的位姿的方法只能估计横滚角和俯仰角，偏航角是无法直接估计的。所以这种方法实际上是不能修正载体的偏航角的，其适用范围就又狭窄了一些。