四元数法与互补滤波

在上一章中我们介绍了欧拉角法与扩展卡尔曼滤波，我们也知道欧拉角法在理论上就不支持全姿态解算，而扩展卡尔曼滤波的计算量还是稍微有些大的。针对这两方面的不足，我们在本文中介绍支持全姿估计的四元数法，以及算力需求更小的互补滤波。

在定点转动与四元数一文中，我们已经了解到四元数是一种扩展的复数，它使用一个标量和一个三轴矢量来描述旋转。四元数是不存在欧拉角那样的奇异问题的，所以可以进行全姿估计，也是主流的姿态估计方法之一。

互补滤波与复杂的卡尔曼滤波不同的是，它并不是一个迭代的算法，而是参照控制器的设计思想，找到一个合理的传递函数，来构造针对高频数据的高通滤波器和针对低频数据的低通滤波器，达到融合不同特性的传感器数据，提高状态估计的精度和可靠性的目的。互补滤波的计算量要小很多，在计算平台算力有限的情况下，是一种比较理想的选择。

本文中，我们先讨论四元数的微分方程以及求解该微分方程的毕卡算法，然后介绍互补滤波的基本原理，最后使用互补滤波完成陀螺与加表的数据。

1. 四元数的微分方程

在定点转动与四元数一文中，我们介绍过两个坐标系之间的旋转关系可以通过四元数的形式描述。在导航系统中，从b系到n系的坐标变换可以表示为:

$$ \begin{equation}\label{f1.1} \vec{Q} = \cos \cfrac{\theta}{2} + \vec{u}^n \sin \cfrac{\theta}{2} \end{equation} $$

其中$\vec{u}$描述了$n$系下的旋转瞬轴和方向，$\theta$则是绕该转轴转动的角度。对上式求导：

$$ \cfrac{d\vec{Q}}{dt} = -\cfrac{\dot{\theta}}{2} \sin \cfrac{\theta}{2} + \vec{u}^n\cfrac{\dot{\theta}}{2}\cos \cfrac{\theta}{2} + \sin \cfrac{\theta}{2}\cfrac{d\vec{u}^n}{dt} $$

根据科氏定理，相对转动的n系和b系下的转轴矢量存在如下的导数关系，其中$\vec{u}^b$是在b系下的转动瞬轴的矢量坐标， $\vec{\omega_{nb}}^n$是b系相对于n系的转动角速率在n系下的投影：

$$ \cfrac{d\vec{u}^n}{dt} = C_b^n \cfrac{d\vec{u}^b}{dt} + \vec{\omega_{nb}}^n \times \vec{u}^n $$

因为矢量$\vec{u}$是转动瞬轴，所以在b系下观察该矢量是静止的，因此$\cfrac{d\vec{u}^b}{dt} = \vec{0}$。又因为$\vec{\omega_{nb}}^n = \dot{\theta}\vec{u}^n$，所以$\vec{\omega_{nb}}^n \times \vec{u}^n = \vec{0}$。因此：

$$ \begin{equation}\label{f1.2}\begin{array}{rcl} \cfrac{d\vec{Q}}{dt} & = & -\cfrac{\dot{\theta}}{2} \sin \cfrac{\theta}{2} + \vec{u}^n\cfrac{\dot{\theta}}{2}\cos \cfrac{\theta}{2} \\ & = & \cfrac{\dot{\theta}}{2}\vec{u}^n\cos \cfrac{\theta}{2} + \vec{u}^n \otimes \vec{u}^n \cfrac{\dot{\theta}}{2} \sin \cfrac{\theta}{2} \\ & = & \cfrac{\dot{\theta}}{2}\vec{u}^n \otimes \left(\cos \cfrac{\theta}{2} + \vec{u}^n \sin \cfrac{\theta}{2} \right) \\ & = & \cfrac{\dot{\theta}}{2}\vec{u}^n \otimes \vec{Q} = \cfrac{1}{2} \vec{\omega_{nb}}^n \otimes \vec{Q} = \cfrac{1}{2} \vec{Q} \otimes \vec{\omega_{nb}}^b \end{array}\end{equation} $$

令$\vec{\omega_{nb}}^b = \begin{bmatrix} \omega_x & \omega_y & \omega_z \end{bmatrix}^T$，表示飞行器的角速度，那么上式可以写成矩阵的形式：

$$ \begin{equation}\label{f1.3} \begin{bmatrix} \dot{q_0} \\ \dot{q_1} \\ \dot{q_2} \\ \dot{q_3} \end{bmatrix} = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \end{equation} $$

在我们的四旋翼中，通过MEMS陀螺测量飞行器的角速度。它定时的对飞行器角速度进行采样，使得我们可以很容易计算出角增量，然后利用毕卡算法计算四元数。求解上式微分方程有：

$$ \begin{equation}\label{f1.4} \vec{Q}_{k+1} = \exp\left\{\cfrac{1}{2}\int_{t_k}^{t_{k+1}} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} dt \right\} \vec{Q}_k \end{equation} $$

令

$$ \begin{equation}\label{f1.5} \Delta \Theta = \int_{t_k}^{t_{k+1}} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} dt \approx \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \Delta t \end{equation} $$

其中，$\Delta t = t_{k+1} - t_k$为陀螺的采样时间间隔。对式($\ref{f1.4}$)进行泰勒展开：

$$ \begin{equation}\label{f1.6} \vec{Q}_{k+1} = e^{\frac{1}{2}\Delta \Theta} \vec{Q}_{k} = \left[I + \cfrac{\frac{1}{2}\Delta \Theta}{1!} + \cfrac{\left(\frac{1}{2}\Delta \Theta\right)^2}{2!} + \cdots \right] \vec{Q}_{k} \end{equation} $$

令$\Delta \vec{\theta} = \begin{bmatrix} \omega_x \Delta t & \omega_y \Delta t & \omega_z \Delta t \end{bmatrix}^T$，其模长为$\left\| \Delta \vec{\theta}\right\| = \left(\omega_x \Delta t \right)^2 + \left(\omega_y \Delta t \right)^2 + \left(\omega_z \Delta t \right)^2$。忽略式($\ref{f1.6}$)高阶项就得到了各阶近似方程：

$$ \begin{array}{rcl} \text{一阶近似} & : & \vec{Q}_{k+1} = \left[I + \cfrac{\Delta \Theta}{2}\right] \vec{Q}_k \\ \text{二阶近似} & : & \vec{Q}_{k+1} = \left[I\left(1 - \cfrac{\left\|\Delta \vec{\theta}\right\|}{8}\right) + \cfrac{\Delta \Theta}{2}\right] \vec{Q}_k \\ \text{三阶近似} & : & \vec{Q}_{k+1} = \left[I\left(1 - \cfrac{\left\|\Delta \vec{\theta}\right\|}{8}\right) +\left(\cfrac{1}{2} - \cfrac{\left\|\Delta \vec{\theta}\right\|}{48} \right)\cfrac{\Delta \Theta}{2}\right] \vec{Q}_k \\ \text{四阶近似} & : & \vec{Q}_{k+1} = \left[I\left(1 - \cfrac{\left\|\Delta \vec{\theta}\right\|}{8} + \cfrac{\left\|\Delta \vec{\theta}\right\|^2}{384}\right) +\left(\cfrac{1}{2} - \cfrac{\left\|\Delta \vec{\theta}\right\|}{48} \right)\cfrac{\Delta \Theta}{2}\right] \vec{Q}_k \\ \end{array} $$

2. 互补滤波原理

我们知道在常用的IMU中，高频的陀螺数据具有较高的带宽其动态特性良好，但由于零偏和累积误差的存在其低频数据的可信度较低。而通过加表或者磁罗盘解算出来的姿态角虽然动态特性很差但不存在累计误差。如果将这两种数据互相取长补短，就可以得到比较理想的估计。互补滤波通过为陀螺的输出数据构建高通滤波器，为加表或者磁罗盘的数据构建低通滤波器，达到融合这两类数据提高定位精度和稳定性的目的。

现在让我们先暂时抛开传感器和姿态估计，考虑一个简单的一阶系统：

$$ \dot x = u $$

我们有两种手段对系统状态进行测量，其一直接测量系统状态本身，即$y_x = x + \varepsilon_x$，其中$y_x$是测量值，$\varepsilon_x$是测量噪声。其二是间接的测量$u$然后通过积分得到对系统状态的估计，即$y_u = u + \varepsilon_u + b_0$，其中$y_u$是测量值，$\varepsilon_u$是测量噪声， $b_0$是测量值中的一个固定的偏移量，它将在积分过程中得到累积。

互补滤波就是通过设计滤波器$F_x(s) + F_u(s) = 1$来估计系统状态：

$$ \hat x(s) = F_x(s) y_x(s) + F_u(s) \cfrac{y_u(s)}{s} $$

其中$F_x(s)$是一个低通滤波器，$F_u(s)$是一个高通滤波器，两者的传递函数求和为1可以保证它们的穿越频率(cross over frequency)相同，并在频域上形成互补的关系。一个典型的互补滤波器的系统框图如下所示：

图 1互补滤波器的系统框图。

系统状态的估计输出可以写成：

$$ \begin{equation}\label{f2.1} \hat x(s) = \cfrac{C(s)}{s + C(s)} y_x(s) + \cfrac{s}{s + C(s)} \cfrac{y_u(s)}{s} \end{equation} $$

显然$\cfrac{C(s)}{s + C(s)} + \cfrac{s}{s + C(s)} = 1$，图1构成一个互补滤波器。因此我们可以借鉴经典的控制理论，来设计补偿器的传递函数$C(s) = \cfrac{A(s)}{B(s)}$，将其代入式($\ref{f2.1}$)，有：

$$ \begin{equation}\label{f2.2} sB(s)\hat{x}(s) = B(s)y_u(s) + A(s)\left(y_x(s) - \hat{x}(s)\right) \end{equation} $$

最简单的互补滤波器就是使用线性反馈即$C(s) = k$，那么系统状态的动力学方程可以写作：

$$ \dot{\hat{x}} = y_u + k(y_x - \hat{x}) $$

而互补滤波的低通和高通滤波传递函数分别为$F_x(s) = \cfrac{k}{s+k}, F_u(s) = \cfrac{s}{s + k}$，它们的穿越频率为k(rad/s)。互补滤波的关键也正是这个穿越频率的选择。综合考量测量值$y_x$的低频特性与$y_u$的高频特性，合理的选择增益系数$k$，调整滤波器的穿越频率，可以在两种信息之间取得一个较好的平衡。

如果测量$y_u$中涵有固定的偏移量，那么通过积分运算该偏移量势必会产生累积误差。这一问题可以通过在补偿器$C(s)$中添加积分项来解决，即：

$$ C(s) = k + \cfrac{1}{\lambda s} = \cfrac{\lambda k s + 1}{\lambda s} $$

将其代入式($\ref{f2.2}$)，有：

$$ \begin{equation}\label{f2.3} \begin{array}{crcl} & \lambda s^2\hat{x}(s) & = & \lambda s y_u(s) + \left(\lambda k s + 1 \right)\left(y_x(s) - \hat{x}(s)\right) \\ \Rightarrow & s\hat{x}(s) & = & y_u(s) + \cfrac{y_x(s) - \hat{x}(s)}{\lambda s} + k\left(y_x(s) - \hat{x}(s)\right) \\ \end{array}\end{equation} $$

对上式进行反拉氏变换有：

$$ \begin{equation}\label{f2.4} \begin{array}{rcl} \dot{\hat{x}} & = & y_u - \hat{b} + k\left(y_x - \hat{x}\right) \\ \dot{\hat{b}} & = & - \cfrac{y_x - \hat{x}}{\lambda} \end{array}\end{equation} $$

据Mahony等人说，通过李雅普诺夫直接法(Lyapunov’s direct method)可以证明系统式收敛的，并且LaSalles principal可以证明$\hat{b} \to b_0$。

3. 融合数据

现在让我们回到四旋翼的姿态估计的问题上来。假设我们有一个六轴的IMU，集成了三轴的陀螺和三轴的加表。现在我们想用四元数法完成对陀螺仪的积分，并通过互补滤波来融合这些的数据。

使用四元数法完成陀螺仪数据的积分是很容易的，我们只需要以一个固定的频率通过陀螺仪获取飞行器的角增量，然后根据系统算力以及精度需求对式($\ref{f1.6}$)选择一个合理的近似方程，迭代运算就可以了。难点在于如何使用互补滤波来进行数据融合。

假设我们的飞行器的运动比较平稳，运动的加速度相比于重力加速度而言小很多，近似可以忽略。那么在已知飞行器姿态矩阵$C_b^n$的情况下，我们可以计算出重力加速度$\vec{g}^n = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^T$在b系中的投影$\vec{g}^b = (C_b^n)^T \vec{g}^n$。加表的测量数据则是在b系下观测到的重力加速度和测量误差$\vec{a} = \vec{g}^b + \vec{\varepsilon_a}$。我们可以通过矢量的叉乘来估计姿态角的误差，但在通过下式计算的时候需要保证$\vec{a}$是已经归一化后的矢量：

$$ \vec{e} = \vec{y_x} - \vec{\hat{x}} = \vec{a} \times \vec{g}^b $$

陀螺仪测量的则是飞行器的角速度，其测量值$\vec{y_u} = \vec{u} + \vec{\varepsilon_u} + \vec{b}$，其中$\vec{u}$为飞行器角速度真值， $\vec{\varepsilon_u}$是测量噪声，$\vec{b}$则是陀螺仪零偏。所以我们选用PI调节器来进行互补滤波，那么陀螺的零偏估计量为：

$$ \hat{\vec{b}}_{k} = K_i \vec{e} + \hat{\vec{b}}_{k-1} $$

其中$\hat{\vec{b}}_k$是第$k$次迭代时的零偏估计量，$K_i$为积分项系数。那么角速度估计量为：

$$ \hat{\dot{\vec{u}}}_{k} = \vec{y_u} + \hat{\vec{b}}_{k} + K_p\vec{e} $$

其中$K_p$为比例项系数。然后我们根据修正后的角速度估计量计算角增量，并代入式($\ref{f1.6}$)中更新四元数。

4. 完

本文中，我们介绍了四元数的微分方程以及根据角增量进行积分运算的毕卡算法。由于四元数不存在奇异性的问题，可以用于全姿解算。此外还介绍了互补滤波的基本原理，以及融合陀螺和加表数据的方法。

为了充分利用陀螺良好的动态特性并通过加表或者其它传感器的数据修正积分过程中的累积误差，互补滤波通过为陀螺的输出数据构建高通滤波器，为加表或者磁罗盘的数据构建低通滤波器，达到融合这两类数据提高定位精度和稳定性的目的。我认为这一思想应当是数据融合的基本指导思想，融合的数据源应当具有不同的特性，相互之间能够有互补的作用。

第三节中的陀螺和加表数据融合的介绍只是一个互补滤波器的应用示例，我们完全可以使用磁罗盘等其它能够直接测量位姿的传感器替代这里的加表。此外这个示例假设飞行器的飞行过程是平稳的，限制了它的应用场景。