Mahony filter滤波：IMU数据融合最好的算法-论文&代码详细解读（Nonlinear Complementary Filters on the Special Orthogonal）

首先，我的系列原创教程Unleashing Robotics: Mastering Quaternion Kinematics with Python - (原创系列教程)还在继续，今天先插个我经常使用的算法的论文的技术分析和讲解，本来想将这个技术放到我的Mastering Quaternion Kinematics with Python教程中，但是为了保证教程专注性，还是分开讲解下。

这篇论文题为2006年Mahony发表了《基于特殊正交群上非线性互补滤波器》的论文（Nonlinear Complementary Filters on the Special Orthogonal Group)，作者是R. Mahony, Tarek Hamel, Jean-Michel Pflimlin。
论文主要介绍了在特殊正交群SO(3)上进行姿态估计的非线性互补滤波器设计与分析，并提供了一种新的姿态融合算法。在无人机、各类需要IMU的角速度和加速度计融合得出角度的场景中得到了广泛的应用，个人认为最适合工程场景使用的算法，接下来我会从数理和代码实现角度深入剥析。

1. 引言

随着MEMS(微机电系统)技术的发展,低成本轻量化的惯性测量单元(IMU)得到广泛应用。但这类IMU输出信号往往含有高噪声和时变偏置,需要进行滤波处理以获得平滑的姿态估计和偏置校正后的角速率测量。

本方法直接在SO(3)流形上提出了一类新的非线性姿态观测器。与之前的工作相比,主要有以下几点创新:

1. 直接在SO(3)上进行公式推导和稳定性分析,而非使用其他表示方法如欧拉角、四元数等。

2. 提出了三种互补滤波器:直接互补滤波器、被动互补滤波器和显式互补滤波器。

3. 证明了估计误差的局部指数稳定性和几乎全局渐近稳定性。

4. 可以在线估计陀螺仪偏置。

5. 适合在嵌入式硬件上实现。

2. 问题描述和数学准备

2.1 惯性测量模型

考虑基体坐标系{B}和惯性坐标系{A},它们之间的旋转矩阵用 $R\in SO(3)$ 表示。假设IMU输出如下:

• 陀螺仪测量 ${\Omega }_y=\Omega +b+\mu$, 其中 $\Omega$ 为真实角速率, $b$ 为偏置,$\mu$ 为噪声。
• 加速度计测量 $a=R^T(\dot{v}-g_0)+b_a+{\mu }_a$,其中 $\dot{v}$ 是加速度, $g_0$ 是重力加速度,$b_a,{\mu }_a$ 分别为偏置和噪声。在准静态时,可以近似 $v_a=-R^T{e}_3$ 。
• 磁力计测量 $m=R^T{m}_A+b_m+{\mu }_b$, 其中 $m_A$ 为地磁场方向, $b_m,{\mu }_b$ 为偏置和噪声。磁力计向量 $v_m=\frac{m}{|m|}$ 。

利用加速度计和磁力计的矢量测量,可以重构出一个姿态矩阵的代数估计值$R_y$。

2.2 SO(3)上的姿态误差准则

定义估计矩阵 $\hat{R}$ 和姿态误差 $\tilde{R}={\hat{R}}^{T}R$。论文使用如下Lyapunov函数来构建估计误差:

$$E_{tr}=\frac{1}{4}\Vert I-\tilde{R}{\Vert }^2=\frac{1}{2}\mathrm{tr}(I-\tilde{R})=2si{n}^2(\theta /2)$$

其中 $\theta$ 是旋转 $\tilde{R}$ 的旋转角度。驱动 $E_{tr}$ 趋于零可以使估计值趋于真实值。

这个函数有以下几个优点:

它是旋转矩阵误差 $\tilde{R}$ 的二次型函数,类似于线性系统中的误差二次型。
它有明确的几何意义,即误差旋转角的正弦平方,可以度量 $\hat{R}$ 偏离 $R$ 的程度。
在 $\tilde{R}=I$ 处取得最小值0,且在 $\tilde{R}$ 附近是正定的,满足Lyapunov函数的条件1。
其导数 ${\dot{E}}_{tr}=-k_P|\omega {|}^2\le 0$,其中 $\omega$ 是旋转误差的度量,满足Lyapunov函数的条件2。

因此,这个函数是姿态误差 $\tilde{R}$ 的一个合适的Lyapunov函数。通过证明 $E_{tr}$ 沿滤波器轨线收敛到0,就可以证明滤波器是渐近稳定的,即估计值 $\hat{R}$ 会收敛到真实值 $R$。作者通过巧妙构造SO(3)上的Lyapunov函数,并利用其导数为负半定这一性质,证明了所提出的互补滤波器能够实现姿态估计误差的渐近收敛。
当然如果你不知道什么是Lyapunov也没关系，这一步就是构建误差项。

我一步步推导下这个公式的来历。

首先,我们知道旋转矩阵 $R$ 是正交矩阵,即 $R{R}^T=I$。因此,姿态误差 $\tilde{R}={\hat{R}}^{T}R$ 也是正交矩阵。

对于任意正交矩阵 $Q$,我们可以将其写成如下形式:

$$Q=\exp (\theta a^{\times })$$

其中 $\theta$ 为旋转角度, $a$ 为单位旋转轴, $a^{\times }$ 为 $a$ 的反对称矩阵。

利用指数函数的泰勒展开,并注意到 $(a^{\times }{)}^3=-a^{\times }$,我们可以得到:

$$Q=I+\sin \theta a^{\times }+(1-\cos \theta )(a^{\times }{)}^2$$

现在,让我们看看 $\Vert I-Q{\Vert }^2$ 的计算:

$$\begin{array}{rl}\Vert I-Q{\Vert }^2& =\Vert (I-Q{)}^T(I-Q)\Vert \\ & =\Vert I-Q^T-Q+I\Vert \\ & =\Vert 2I-2\cos \theta I\Vert \\ & =4(1-\cos \theta )\Vert I\Vert \\ & =8(1-\cos \theta )\end{array}$$

其中我们用到了 $Q^T=Q^{-1}$,以及 $Q$ 的展开式。

接下来,我们看看 $\mathrm{tr}(I-Q)$ 的计算:

$$\begin{array}{rl}\mathrm{tr}(I-Q)& =\mathrm{tr}(I)-\mathrm{tr}(Q)\\ & =3-(3\cos \theta )\\ & =3(1-\cos \theta )\end{array}$$

其中我们用到了旋转矩阵的迹等于余弦值之和。

最后,我们利用半角公式:

$$1-\cos \theta =2{\sin }^2\frac{\theta }{2}$$

就得到了:

$$\begin{array}{rl}\Vert I-Q{\Vert }^2& =8(1-\cos \theta )=16{\sin }^2\frac{\theta }{2}\\ \mathrm{tr}(I-Q)& =3(1-\cos \theta )=6{\sin }^2\frac{\theta }{2}\end{array}$$

所以,我们有:

$$\boxed{\frac{1}{4}\Vert I-Q{\Vert }^2=\frac{1}{2}\mathrm{tr}(I-Q)=2{\sin }^2\frac{\theta }{2}}$$

这个公式的巧妙之处在于,它将旋转矩阵的"距离"与旋转角度联系起来,而且非常容易计算。在姿态估计问题中,它提供了一个简洁而有效的误差度量。

下面给出被动互补滤波器估计姿态和偏置的伪代码:

def passive_complementary_filter(Omega_y, R_y, dt, k_P, k_I):
    # 初始化估计值
    R_hat = I  # 初始估计为单位矩阵
    b_hat = 0  # 初始偏置估计为零

    while True:
        # 计算误差项
        R_tilde = R_hat.T @ R_y
        omega = vex(Pa(R_tilde))
        
        # 更新姿态估计 
        Omega_hat = Omega_y - b_hat + k_P * omega
        R_hat = R_hat @ exp(Omega_hat * dt)

        # 更新偏置估计
        b_hat = b_hat - k_I * omega * dt

        yield R_hat, b_hat

其中 Pa 表示反对称矩阵投影, vex 是其逆运算, exp 是SO(3)上的指数映射,可以用罗德里格斯公式计算。k_P 和 k_I 分别是比例和积分增益。

总的来说,互补滤波的核心思想是利用Lyapunov函数构造估计误差,并设计修正项使其递减。通过巧妙选取旋转矩阵的误差函数,可以得到简洁高效的滤波器结构。在此基础上,论文进一步提出了被动、显式等改进形式,并证明了它们的稳定性。这些互补滤波器为SO(3)上的姿态估计提供了新的思路。

3. SO(3)上的互补滤波器设计

3.1 直接互补滤波器

假设角速率 $\Omega$ 和重构姿态 $R_y$ 已知,直接互补滤波器设计如下:

$$\begin{array}{rl}\dot{\hat{R}}& =(R_y{\Omega }_y+k_P\hat{R}\omega {)}_{\times }\hat{R}\\ \omega & =\mathrm{vex}(P_a(\tilde{R}))=\mathrm{vex}(P_a({\hat{R}}^T{R}_y))\end{array}$$

其中 $k_P>0$ 为增益, $\omega$ 为修正项。对误差系统进行Lyapunov分析可得:

$${\dot{E}}_{tr}=-\frac{k_P}{2}\mathrm{tr}({\omega }_{\times }\tilde{R})=-k_P|\omega {|}^2$$

这说明误差是渐近稳定的。

我再来详细解释下这部分内容如何在IMU融合姿态中实现：在姿态估计问题中,除了IMU给出的三个轴的角速度直接积分得到角度外，我们通常可以从加速度计和磁力计的测量值中得到关于姿态的部分信息,我们将其称为"重构姿态"。具体来说:

1. 加速度计测量重力在体坐标系下的分量,如果物体处于静止或匀速运动状态,那么加速度计读数 $a$ 就等于重力向量在体坐标系下的投影,即:

   $$a=R^T[0,0,g{]}^T$$

   其中 $g$ 为重力加速度大小。从这个关系可以得到姿态矩阵 $R$ 的部分信息。

2. 磁力计测量地磁场在体坐标系下的分量,设地磁场在惯性坐标系下的单位方向向量为 $m$,则磁力计读数 $b$ 满足:

   $$b=R^{T}m$$

   同样,这个关系也提供了姿态矩阵 $R$ 的部分信息，这就是重构姿态。

通过适当地组合加速度计和磁力计的信息,我们可以得到一个姿态的估计值,即"重构姿态"$R_y$。这个过程通常用到最小二乘法,奇异值分解等优化算法，或是最简单直接加权平均（建议最好不要这样处理，会损失精度）。

下面给出一个Python代码示例,演示如何从9轴IMU数据重构姿态:

import numpy as np

def reconstruct_attitude(accel, mag):
    """
    从加速度计和磁力计数据重构姿态
    :param accel: 加速度计读数,3x1
    :param mag: 磁力计读数,3x1
    :return: 重构的姿态矩阵
    """
    # 归一化加速度计读数
    accel = accel / np.linalg.norm(accel)
    
    # 假设磁力计读数已经校准,即去除了硬铁和软铁效应
    mag = mag / np.linalg.norm(mag)
    
    # 计算东北天坐标系下的重力和磁场方向
    g = np.array([0, 0, 1])
    m = np.array([0, 1, 0])
    
    # 构造最小二乘问题
    A = np.vstack((accel, mag))
    b = np.vstack((g, m))
    
    # 用SVD求解最小二乘问题
    U, _, Vt = np.linalg.svd(A.T @ b)
    R = U @ Vt
    
    # 保证旋转矩阵满足右手定则
    if np.linalg.det(R) < 0:
        R = -R
        
    return R

这个函数的输入是加速度计和磁力计的读数,输出是重构的姿态矩阵。其中主要步骤是:

1. 归一化加速度计和磁力计读数
2. 根据加速度计和磁力计的参考方向(这里假设为重力向下,磁场向北),构造最小二乘问题
3. 用奇异值分解(SVD)求解最小二乘问题,得到姿态矩阵
4. 检查姿态矩阵行列式的符号,保证满足右手定则

得到重构姿态后,就可以用它来修正陀螺仪积分得到的姿态估计值,这就是互补滤波的核心思想。直接互补滤波器就是将重构姿态 $R_y$ 直接代入滤波器方程:

$$\dot{\hat{R}}=(R_y{\Omega }_y+k_P\hat{R}\omega {)}_{\times }\hat{R}$$

其中修正项 $\omega$ 由姿态估计值 $\hat{R}$ 和重构姿态 $R_y$ 计算得到:

$$\omega =\mathrm{vex}(P_a({\hat{R}}^T{R}_y))$$

这样,互补滤波器就可以融合陀螺仪、加速度计和磁力计的信息,得到更准确和稳定的姿态估计。Lyapunov分析保证了滤波器的收敛性和稳定性。

下面我来推导这个方程。

直接互补滤波器的推导与实现

符号说明

• $R$: 真实姿态矩阵
• $\hat{R}$: 估计姿态矩阵
• $\tilde{R}$: 姿态误差矩阵,$\tilde{R}={\hat{R}}^{T}R$
• $\Omega$: 真实角速度
• ${\Omega }_y$: 陀螺仪测量角速度
• $R_y$: 重构姿态矩阵
• $\omega$: 修正项
• $k_P$: 比例增益
• $P_a$: 反对称矩阵投影算子
• $\mathrm{vex}$: $P_a$ 的逆运算

首先,真实姿态 $R$ 满足运动学方程:

$$\dot{R}=R{\Omega }_{\times }$$

而估计姿态 $\hat{R}$ 的运动学方程可以写为:

$$\dot{\hat{R}}=\hat{R}({\Omega }_y+\delta \omega {)}_{\times }$$

其中 $\delta \omega$ 是一个修正角速度,用于补偿陀螺仪误差。我们的目标是设计 $\delta \omega$,使估计姿态 $\hat{R}$ 收敛到真实姿态 $R$。

为此,我们引入姿态误差 $\tilde{R}={\hat{R}}^{T}R$,并设计如下的Lyapunov函数:

$$E_{tr}=\frac{1}{2}\mathrm{tr}(I-\tilde{R})$$

对 $E_{tr}$ 求导,并利用 $\tilde{R}$ 的运动学方程 $\dot{\tilde{R}}=\tilde{R}(\Omega -{\hat{R}}^T({\Omega }_y+\delta \omega ){)}_{\times }$,可得:

$$\begin{array}{rl}{\dot{E}}_{tr}& =-\frac{1}{2}\mathrm{tr}(\dot{\tilde{R}})\\ & =-\frac{1}{2}\mathrm{tr}(\tilde{R}(\Omega -{\hat{R}}^T({\Omega }_y+\delta \omega ){)}_{\times })\\ & =-\frac{1}{2}\mathrm{tr}(P_a(\tilde{R}){\hat{R}}^T({\Omega }_y-\Omega +\delta \omega {)}_{\times })\\ & =⟨\mathrm{vex}(P_a(\tilde{R})),{\hat{R}}^T({\Omega }_y-\Omega +\delta \omega )⟩\end{array}$$

为了使 ${\dot{E}}_{tr}\le 0$,我们可以选择:

$$\delta \omega =k_P\mathrm{vex}(P_a(\tilde{R}))+{\hat{R}}^T(\Omega -{\Omega }_y)$$

将其代入 $\dot{\hat{R}}$ 的表达式,并利用 $\tilde{R}={\hat{R}}^{T}R$,可得:

$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}({\Omega }_y+k_P\mathrm{vex}(P_a(\tilde{R}))+{\hat{R}}^T(\Omega -{\Omega }_y){)}_{\times }\\ & =(\hat{R}{\Omega }_y+k_P\hat{R}\mathrm{vex}(P_a({\hat{R}}^{T}R))+R\Omega -R{\Omega }_y{)}_{\times }\hat{R}\\ & =(R\Omega +k_P\hat{R}\mathrm{vex}(P_a({\hat{R}}^T{R}_y)){)}_{\times }\hat{R}\end{array}$$

其中最后一步利用了重构姿态 $R_y$ 作为真实姿态 $R$ 的估计值。这就得到了直接互补滤波器的核心方程。

Python实现

下面给出直接互补滤波器的Python实现:

import numpy as np
from scipy.spatial.transform import Rotation

def direct_complementary_filter(gyro, accel, mag, dt, kp):
    """
    直接互补滤波器
    :param gyro: 陀螺仪读数,3x1
    :param accel: 加速度计读数,3x1
    :param mag: 磁力计读数,3x1
    :param dt: 采样时间
    :param kp: 比例增益
    :return: 估计姿态矩阵
    """
    # 归一化加速度计和磁力计读数
    accel = accel / np.linalg.norm(accel)
    mag = mag / np.linalg.norm(mag)
    
    # 重构姿态
    R_y = reconstruct_attitude(accel, mag)
    
    # 初始化估计值
    R_hat = Rotation.from_matrix(np.eye(3))
    
    # 滤波器更新
    omega = vex(Pa(R_hat.as_matrix().T @ R_y))
    delta_omega = kp * omega
    R_omega = Rotation.from_rotvec((gyro + delta_omega) * dt)
    R_hat = R_omega * R_hat
    
    return R_hat.as_matrix()

def reconstruct_attitude(accel, mag):
    """
    从加速度计和磁力计数据重构姿态
    :param accel: 加速度计读数,3x1
    :param mag: 磁力计读数,3x1
    :return: 重构的姿态矩阵
    """
    ... 上一部分给出了
    
def Pa(R):
    """
    反对称矩阵投影算子
    :param R: 旋转矩阵
    :return: R的反对称部分
    """
    return 0.5 * (R - R.T)

def vex(R):
    """
    反对称矩阵到向量的转换
    :param R: 3x3反对称矩阵
    :return: 3x1向量
    """
    return np.array([R[2, 1], R[0, 2], R[1, 0]])

这个实现中,我们首先从加速度计和磁力计数据重构姿态 $R_y$,然后根据陀螺仪数据和修正项更新姿态估计 $\hat{R}$。其中 Pa 和 vex 函数分别实现了反对称矩阵投影算子及其逆运算。

直接互补滤波器的优点是结构简单,易于实现和调试。但其缺点是重构姿态 $R_y$ 的误差会直接引入滤波器,影响估计精度。针对这一问题,论文还提出了间接互补滤波器等改进方案。

3.2 被动互补滤波器

考虑到直接滤波器中重构姿态的高频噪声会耦合进反馈,提出了被动互补滤波器:

$$\dot{\hat{R}}=(\hat{R}{\Omega }_y+k_P\hat{R}\omega {)}_{\times }\hat{R}$$

这里预测项使用估计姿态 $\hat{R}$ 来映射角速率。Lyapunov分析与直接滤波器类似。滤波器结构可以进一步化简,避免将 ${\Omega }_y$ 和 $\omega$ 变换到惯性系:

$$\dot{\hat{R}}=\hat{R}({\Omega }_y+k_P\omega {)}_{\times }$$

我依然进行下推导便于你理解，实际上在工程中你直接拿最终公式用就可以，考虑到部分读者是研究人员有理解算法的需求，我还是推导一下。

被动互补滤波器的推导与实现

符号说明

• $R$: 真实姿态矩阵
• $\hat{R}$: 估计姿态矩阵
• $\tilde{R}$: 姿态误差矩阵,$\tilde{R}={\hat{R}}^{T}R$
• $\Omega$: 真实角速度
• ${\Omega }_y$: 陀螺仪测量角速度
• $R_y$: 重构姿态矩阵
• $\omega$: 修正项
• $k_P$: 比例增益
• $P_a$: 反对称矩阵投影算子
• $\mathrm{vex}$: $P_a$ 的逆运算

滤波器方程推导

被动互补滤波器的核心方程为:

$$\dot{\hat{R}}=(\hat{R}{\Omega }_y+k_P\hat{R}\omega {)}_{\times }\hat{R}$$

其中修正项 $\omega$ 仍由下式计算:

$$\omega =\mathrm{vex}(P_a({\hat{R}}^T{R}_y))$$

与直接互补滤波器不同,被动互补滤波器在预测项中使用估计姿态 $\hat{R}$ 来映射角速度测量值 ${\Omega }_y$。这样做的好处是避免了将重构姿态 $R_y$ 的噪声引入预测项。

下面我们来推导被动互补滤波器的稳定性。姿态误差 $\tilde{R}$ 的运动学方程为:

$$\begin{array}{rl}\dot{\tilde{R}}& ={\dot{\hat{R}}}^{T}R+{\hat{R}}^T\dot{R}\\ & =((\hat{R}{\Omega }_y+k_P\hat{R}\omega {)}_{\times }\hat{R}{)}^{T}R+{\hat{R}}^{T}R{\Omega }_{\times }\\ & =-({\Omega }_y+k_P\omega {)}_{\times }\tilde{R}+\tilde{R}{\Omega }_{\times }\\ & =(\tilde{R}\Omega -{\Omega }_y-k_P\omega {)}_{\times }\tilde{R}\end{array}$$

对Lyapunov函数 $E_{tr}=\frac{1}{2}\mathrm{tr}(I-\tilde{R})$ 求导,可得:

$$\begin{array}{rl}{\dot{E}}_{tr}& =-\frac{1}{2}\mathrm{tr}(\dot{\tilde{R}})\\ & =-\frac{1}{2}\mathrm{tr}((\tilde{R}\Omega -{\Omega }_y-k_P\omega {)}_{\times }\tilde{R})\\ & =\frac{1}{2}\mathrm{tr}(\tilde{R}(\tilde{R}\Omega -{\Omega }_y-k_P\omega {)}_{\times })\\ & =\frac{1}{2}\mathrm{tr}(P_a(\tilde{R})(\tilde{R}\Omega -{\Omega }_y-k_P\omega {)}_{\times })\\ & =-⟨\mathrm{vex}(P_a(\tilde{R})),\tilde{R}\Omega -{\Omega }_y-k_P\omega ⟩\\ & =-⟨\mathrm{vex}(P_a(\tilde{R})),\tilde{R}\Omega -{\Omega }_y⟩-k_P\Vert \mathrm{vex}(P_a(\tilde{R})){\Vert }^2\end{array}$$

如果真实角速度 $\Omega$ 与姿态误差 $\tilde{R}$ 不相关,那么第一项的均值为零,而第二项始终非正。因此,我们可以期望 $E_{tr}$ 在被动互补滤波器作用下收敛到零,即 $\tilde{R}$ 收敛到单位矩阵 $I$。

进一步,我们可以将被动互补滤波器改写为如下形式:

$$\dot{\hat{R}}=\hat{R}({\Omega }_y+k_P\omega {)}_{\times }$$

这种形式避免了将 ${\Omega }_y$ 和 $\omega$ 变换到惯性系,减少了计算量。

Python实现

下面给出被动互补滤波器的Python实现:

import numpy as np
from scipy.spatial.transform import Rotation

def passive_complementary_filter(gyro, accel, mag, dt, kp):
    """
    被动互补滤波器
    :param gyro: 陀螺仪读数,3x1
    :param accel: 加速度计读数,3x1
    :param mag: 磁力计读数,3x1
    :param dt: 采样时间
    :param kp: 比例增益
    :return: 估计姿态矩阵
    """
    # 归一化加速度计和磁力计读数
    accel = accel / np.linalg.norm(accel)
    mag = mag / np.linalg.norm(mag)
    
    # 重构姿态
    R_y = reconstruct_attitude(accel, mag)
    
    # 初始化估计值
    R_hat = Rotation.from_matrix(np.eye(3))
    
    # 滤波器更新
    omega = vex(Pa(R_hat.as_matrix().T @ R_y))
    R_omega = Rotation.from_rotvec((gyro + kp * omega) * dt)
    R_hat = R_hat * R_omega
    
    return R_hat.as_matrix()

def reconstruct_attitude(accel, mag):
    """
    从加速度计和磁力计数据重构姿态
    :param accel: 加速度计读数,3x1
    :param mag: 磁力计读数,3x1
    :return: 重构的姿态矩阵
    """
    ... # 此处省略,同个给你的代码
    
def Pa(R):
    """
    反对称矩阵投影算子
    :param R: 旋转矩阵
    :return: R的反对称部分
    """
    return 0.5 * (R - R.T)

def vex(R):
    """
    反对称矩阵到向量的转换
    :param R: 3x3反对称矩阵
    :return: 3x1向量
    """
    return np.array([R[2, 1], R[0, 2], R[1, 0]])

与直接互补滤波器相比,被动互补滤波器的实现主要有两点不同:

1. 在滤波器更新中,先计算旋转矢量 gyro + kp * omega,再将其转换为旋转矩阵 R_omega,最后用 R_hat * R_omega 更新姿态估计。这避免了将 ${\Omega }_y$ 和 $\omega$ 变换到惯性系。

2. 在滤波器更新的最后,直接用 R_hat * R_omega 更新姿态估计,而不是用 R_omega * R_hat。这是因为在被动互补滤波器中,我们希望用 $\hat{R}$ 左乘角速度,而不是右乘。

总的来说,被动互补滤波器通过巧妙的方程设计,避免了直接互补滤波器中的一些问题,提高了滤波器的鲁棒性和计算效率。同时,它也为后续的显式互补滤波器奠定了基础。

4. 滤波器的稳定性分析

4.1 带偏置估计的直接互补滤波器

考虑系统方程:

$$\dot{R}=R{\Omega }_{\times },{\Omega }_y=\Omega +b+\mu$$

直接互补滤波器带偏置估计的形式为:

$$\begin{array}{rl}\dot{\hat{R}}& =(R_y({\Omega }_y-\hat{b})+k_P\hat{R}\omega {)}_{\times }\hat{R}\\ \dot{\hat{b}}& =-k_I\omega \end{array}$$

其中 $k_I>0$ 为积分增益。定义 Lyapunov 函数:

$$V(t)=\frac{1}{2}\mathrm{tr}(I-\tilde{R})+\frac{1}{2k_I}|\tilde{b}{|}^2,\tilde{b}=b-\hat{b}$$

对其求导并利用 $\mathrm{tr}([\tilde{R},{\Omega }_{\times }])=0$,可得:

$$\dot{V}=-k_P|\omega {|}^2$$

根据LaSalle不变集原理,系统状态将收敛到最大不变集:

$$M=\{(\tilde{R},\tilde{b}):P_a(\tilde{R})=0,\tilde{b}\times \tilde{R}=0\}$$

论文进一步分析了不变集 $M$ 的稳定性,得到如下结论:

1. 平衡点 $(I,0)$ 是局部指数稳定的。

2. 除了零测集之外的所有初值,估计值都会渐近收敛到真实值。

4.2 带偏置估计的被动互补滤波器

被动互补滤波器的偏置估计形式为:

$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}({\Omega }_y-\hat{b}+k_P\omega {)}_{\times }\\ \dot{\hat{b}}& =-k_I\omega \end{array}$$

其误差动力学满足:

$$\dot{\tilde{R}}=[\tilde{R},{\Omega }_{\times }]-k_P{\omega }_{\times }\tilde{R}-{\tilde{b}}_{\times }\tilde{R},\dot{\tilde{b}}=k_I\omega$$

取相同的 Lyapunov 函数,其导数为:

$$\dot{V}=-k_P|\omega {|}^2$$

由于存在非自治项 $\Omega (t)$,不能直接应用LaSalle不变集原理,而是使用Barbalat引理证明 $\omega \to 0$ 。再进一步分析得到如下结论:

1. 除了平衡点 $(I,0)$,还存在不稳定平衡点集合 U 0 = { R ~ : t r ( R ~ ) = − 1 , b ~ = 0 } U_0=\{\tilde{R}:\mathrm{tr}(\tilde{R})=-1, \tilde{b}=0\} U0​={R~:tr(R~)=−1,b~=0}。

2. 平衡点 $(I,0)$ 依然局部指数稳定。

3. 除了 $U_0$ 内的零测集,其他所有初值的估计值都收敛到真实值。

这里的关键是要求 $\Omega (t)$ 与 $\tilde{R}$ “渐近独立”,这样 $\Omega (t)$ 的扰动可以保证 $\tilde{b}\to 0$。

5. 显式互补滤波器

为了避免在滤波器中重构姿态 $R_y$,提出了显式互补滤波器。考虑系统直接输出若干惯性方向的观测值:

$$v_i=R^T{v}_{0i}+{\mu }_i,i=1,\cdots ,n$$

其中 $v_{0i}$ 为惯性坐标系中的已知参考方向。定义如下代价函数:

$$\begin{array}{rl}{E}_i& =1-⟨{\hat{v}}_i,v_i⟩=1-\mathrm{tr}({\hat{R}}^T{v}_{0i}{v}_{0i}^{T}R)\\ E_{mes}& =\sum _{i=1}^n{k}_i{E}_i=\sum _{i=1}^n{k}_i-\mathrm{tr}(\tilde{R}M),M=R^T{M}_{0}R,M_0=\sum _{i=1}^n{k}_i{v}_{0i}{v}_{0i}^T\end{array}$$

其中 ${\hat{v}}_i={\hat{R}}^T{v}_i$, $k_i>0$ 为权重。当 $n\ge 2$时,矩阵 $M_0$ 正定。利用它构造如下滤波器:

$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}({\Omega }_y-\hat{b}+k_P{\omega }_{\times })\\ \dot{\hat{b}}& =-k_I\omega \\ \omega & =\sum _{i=1}^n{k}_i{v}_i\times {\hat{v}}_i\end{array}$$

可以证明它有如下性质:

1. 存在与特征值对应的三个不稳定平衡点:$({\hat{R}}_i^{\ast },{\hat{b}}_i^{\ast })=(U_0{D}_i{U}_0^{T}R,b)$, 其中 $D_i=diag(1,-1,-1),(1,1,-1)$ 或 $(1,-1,1)$。

2. 误差在 $(I,0)$ 处局部指数稳定。

3. 除了前述三个平衡点外,其他所有初值都收敛到真实值。

当 $n=2$时,有类似的结论。特别地,当只有一个方向矢量(如重力加速度)时,有如下结果:

1. 不稳定平衡集合为 U 1 = { R ~ : v 0 T R ~ v 0 = − 1 , b ~ = 0 } U_1=\{\tilde{R}: v_0^T\tilde{R}v_0=-1, \tilde{b}=0 \} U1​={R~:v0T​R~v0​=−1,b~=0}。

2. 除 $U_1$外,估计值收敛到真实值。

这说明利用单个参考方向也可以实现除旋转轴外两个方向的渐近估计。

这里我再次详细介绍和讲解下并给出示例代码

显式互补滤波器的推导与实现

符号说明

• $R$: 真实姿态矩阵
• $\hat{R}$: 估计姿态矩阵
• $\tilde{R}$: 姿态误差矩阵,$\tilde{R}={\hat{R}}^{T}R$
• ${\Omega }_y$: 陀螺仪测量角速度
• $b$: 陀螺仪偏置
• $\hat{b}$: 偏置估计
• $v_i$: 惯性方向在体坐标系下的观测值
• $v_{0i}$: 惯性方向在惯性坐标系下的已知值
• ${\hat{v}}_i$: 惯性方向在估计姿态下的计算值,${\hat{v}}_i={\hat{R}}^T{v}_{0i}$
• ${\mu }_i$: 观测噪声
• $k_i$: 第i个观测值的权重
• $k_P$: 比例增益
• $k_I$: 积分增益
• $\omega$: 修正项

滤波器方程推导

显式互补滤波器的核心思想是直接利用惯性方向的观测值,而不是重构姿态。首先,我们定义了如下的姿态误差代价函数:

$$E_i=1-⟨{\hat{v}}_i,v_i⟩=1-\mathrm{tr}({\hat{R}}^T{v}_{0i}{v}_{0i}^{T}R)$$

其中 ${\hat{v}}_i={\hat{R}}^T{v}_{0i}$ 是第i个惯性方向在估计姿态下的计算值。这个代价函数表示了估计值 ${\hat{v}}_i$ 与观测值 $v_i$ 之间的误差。

进一步,我们定义了总的代价函数:

$$E_{mes}=\sum _{i=1}^n{k}_i{E}_i=\sum _{i=1}^n{k}_i-\mathrm{tr}(\tilde{R}M),M=R^T{M}_{0}R,M_0=\sum _{i=1}^n{k}_i{v}_{0i}{v}_{0i}^T$$

其中 $k_i>0$ 是第i个观测值的权重。可以证明,当观测值个数 $n\ge 2$ 时,矩阵 $M_0$ 是正定的。

基于这个代价函数,我们提出了如下的显式互补滤波器:

$$\begin{array}{rl}\dot{\hat{R}}& =\hat{R}({\Omega }_y-\hat{b}+k_P{\omega }_{\times })\\ \dot{\hat{b}}& =-k_I\omega \\ \omega & =\sum _{i=1}^n{k}_i{v}_i\times {\hat{v}}_i\end{array}$$

其中 $\hat{b}$ 是陀螺仪偏置的估计值, $k_P$ 和 $k_I$ 分别是比例和积分增益。修正项 $\omega$ 是所有观测值与估计值之间误差的加权和。

对总代价函数 $E_{mes}$ 求导,可得:

$$\begin{array}{rl}{\dot{E}}_{mes}& =-\mathrm{tr}(\dot{\tilde{R}}M+\tilde{R}\dot{M})\\ & =-\mathrm{tr}((\tilde{R}\Omega -{\Omega }_y+\hat{b}-k_P\omega {)}_{\times }\tilde{R}M)\\ & =⟨\omega -\hat{b},\mathrm{vex}(P_a(\tilde{R}M))⟩-k_P\Vert \omega {\Vert }^2\end{array}$$

其中我们用到了 $\dot{M}=0$ 和 $\mathrm{tr}(A_{\times }B)=2⟨\mathrm{vex}(P_a(A)),\mathrm{vex}(P_a(B))⟩$。

进一步可以证明,修正项 $\omega$ 满足:

$${\omega }_{\times }=P_a(\tilde{R}M)$$

将其代入 ${\dot{E}}_{mes}$ 的表达式,可得:

$${\dot{E}}_{mes}=-⟨\hat{b},\omega ⟩-k_P\Vert \omega {\Vert }^2$$

这说明,在显式互补滤波器作用下,总代价函数 $E_{mes}$ 是单调递减的,而且递减的速度与修正项 $\omega$ 的大小成正比。因此,我们可以期望滤波器能够使姿态估计值 $\hat{R}$ 收敛到真实值 $R$。

进一步的稳定性分析表明,显式互补滤波器有如下性质:

1. 存在与矩阵 $M_0$ 的特征值对应的三个不稳定平衡点。
2. 真实值 $(R,b)$ 处是局部指数稳定的。
3. 除了那三个不稳定平衡点外,其他所有初值都能收敛到真实值。

特别地,当只有一个参考方向(如重力加速度)时,滤波器仍然能保证除参考方向外的两个方向上的收敛性。

Python实现

下面给出显式互补滤波器的Python实现:

import numpy as np
from scipy.spatial.transform import Rotation

def explicit_complementary_filter(gyro, dt, kp, ki, *vs):
    """
    显式互补滤波器
    :param gyro: 陀螺仪读数,3x1
    :param dt: 采样时间
    :param kp: 比例增益
    :param ki: 积分增益
    :param vs: 惯性方向观测值,每个为3x1
    :return: 估计姿态矩阵和偏置
    """
    # 初始化估计值
    R_hat = Rotation.from_matrix(np.eye(3))
    b_hat = np.zeros(3)
    
    # 计算参考方向和权重
    v0s = [v / np.linalg.norm(v) for v in vs]
    n = len(v0s)
    ks = np.ones(n) / n
    
    # 滤波器更新
    vhs = [R_hat.as_matrix().T @ v0 for v0 in v0s]
    omega = np.sum([ki * np.cross(vi, vhi) for ki, vi, vhi in zip(ks, vs, vhs)], axis=0)
    b_hat = b_hat - ki * omega * dt
    R_omega = Rotation.from_rotvec((gyro - b_hat + kp * omega) * dt)
    R_hat = R_hat * R_omega
    
    return R_hat.as_matrix(), b_hat

def Pa(R):
    """
    反对称矩阵投影算子
    :param R: 旋转矩阵
    :return: R的反对称部分
    """
    return 0.5 * (R - R.T)

def vex(R):
    """
    反对称矩阵到向量的转换
    :param R: 3x3反对称矩阵
    :return: 3x1向量
    """
    return np.array([R[2, 1], R[0, 2], R[1, 0]])

这个实现中,我们首先归一化所有的参考方向,并设置相应的权重。然后根据当前的姿态估计计算每个参考方向的估计值,并计算它们与观测值之间的误差。修正项 omega 就是所有这些误差的加权和。最后,我们用修正后的角速度更新姿态估计,并用 omega 更新偏置估计。

显式互补滤波器的优点是不需要重构姿态,而是直接利用惯性方向的观测值。这不仅简化了计算,而且提高了滤波器的鲁棒性。同时,显式互补滤波器也继承了被动互补滤波器的优点,即避免了将重构姿态的噪声引入预测项。

总的来说,显式互补滤波器是一种简洁、高效、鲁棒的姿态估计算法,非常适合在嵌入式系统中实现。它的理论分析和实验验证都表明,它能够在各种услов件下实现良好的姿态估计性能。

6. 仿真验证

论文最后通过两个实验验证了算法的有效性。第一个实验在机械臂上安装IMU,模拟飞行器的机动。分别使用直接和被动互补滤波器。从实验结果看,两者的渐近性能接近,但被动滤波器的瞬态响应和估计噪声略优于直接滤波器。

论文的第二个实验在一个四旋翼无人机Hoverye上进行,使用显式互补滤波器。通过与纯积分得到的偏航角对比,滤波器可以较好地估计出偏置,最终偏航误差小于5度。

总结

这篇论文提出了一类直接在SO(3)流形上进行姿态估计的非线性互补滤波器。通过巧妙的误差函数设计和Lyapunov分析,证明了估计误差的局部指数稳定性和全局渐近稳定性。同时,通过集成偏置估计,可以在线校正陀螺仪偏置。相比欧拉角和四元数等表示,基于SO(3)的方法在公式推导和稳定性分析上都更加简洁自然。

论文还提出了几种不同的滤波器实现形式。直接和被动互补滤波器在渐近意义下等价,但被动实现的噪声抑制性能更优。进一步利用方向传感器的直接输出,得到了显式互补滤波器,它避免了在线重构姿态,计算负荷小,非常适合在嵌入式系统上实现。同时,即使只有一个参考方向(如重力加速度),该滤波器也能实现局部渐近估计。丰富的数值仿真和实物实验验证了算法的有效性和优越性。

互补滤波的基本思想

互补滤波的基本思想是将两个互补的传感器信号融合,常用于姿态估计问题。例如,陀螺仪的积分误差为低频,而加速度计和磁力计的噪声主要在高频。互补滤波就是设计一个低通和一个高通滤波器,将它们融合。

设计互补滤波器的关键是如何在SO(3)流形上定义姿态误差。本文利用旋转矩阵的迹(trace)构造二次型误差函数,既简单又能反映旋转误差的大小。

Q&A

Lyapunov函数与稳定性

Lyapunov稳定性理论是控制中分析非线性系统稳定性的有力工具。其基本思想是,构造一个标量函数 $V(x)$ 作为系统"能量"的度量。如果 $V(x)$ 在平衡点处取得最小值,且随时间单调递减,则平衡点是渐近稳定的。常见的Lyapunov函数有二次型、线性矩阵不等式等。

本文中的Lyapunov函数由两项组成: $V=E_{tr}+\frac{1}{2k_I}{\tilde{b}}^2$。第一项表征姿态误差,第二项表征偏置估计误差。如果 $\dot{V}\le 0$,则姿态误差和偏置误差都有界。进一步利用LaSalle不变集原理和Barbalat引理,可以分析收敛到哪些平衡点。

编程实现

下面我给出一个完整的Mahony滤波器的Python实现示例,包括数据生成、滤波器实现和结果可视化。你可以直接运行这个脚本来观察滤波器的效果。

import numpy as np
from scipy.spatial.transform import Rotation
import matplotlib.pyplot as plt

# 生成模拟数据
def generate_data(t, dt):
    """
    生成模拟数据
    :param t: 时间序列
    :param dt: 采样时间间隔
    :return: 角速度、加速度计和磁力计读数
    """
    # 真实姿态
    w = 2 * np.pi * 0.1  # 角速度幅值
    R_true = Rotation.from_euler('ZYX', [w * t, 0.1 * np.sin(0.7 * t), 0.2 * np.sin(0.3 * t)])
    
    # 角速度
    gyro = R_true.as_rotvec() / dt + 0.1 * np.random.randn(len(t), 3)
    gyro_bias = 0.01 * np.random.randn(3)
    gyro += gyro_bias
    
    # 加速度计
    a0 = np.array([0, 0, 1.0])  # 参考重力方向
    a_true = R_true.apply(a0)
    accel = a_true + 0.05 * np.random.randn(len(t), 3)
    
    # 磁力计
    m0 = np.array([1.0, 0, 0])  # 参考磁场方向
    m_true = R_true.apply(m0) 
    mag = m_true + 0.05 * np.random.randn(len(t), 3)
    
    return gyro, accel, mag

# Mahony滤波器
def mahony_filter(gyro, accel, mag, dt, kp, ki):
    """
    Mahony滤波器
    :param gyro: 陀螺仪读数
    :param accel: 加速度计读数
    :param mag: 磁力计读数
    :param dt: 采样时间间隔
    :param kp: 比例增益
    :param ki: 积分增益
    :return: 估计姿态和偏置
    """
    # 初始化估计值
    R_hat = Rotation.from_matrix(np.eye(3))
    b_hat = np.zeros(3)
    
    # 参考方向
    a0 = np.array([0, 0, 1.0])
    m0 = np.array([1.0, 0, 0])
    
    # 存储估计值
    R_hist = [R_hat]
    b_hist = [b_hat]
    
    for i in range(len(gyro)):
        # 归一化测量值
        a = accel[i] / np.linalg.norm(accel[i])
        m = mag[i] / np.linalg.norm(mag[i])
        
        # 计算误差项
        R = R_hat.as_matrix()
        va = R.T @ a
        vm = R.T @ m
        e = np.cross(a0, va) + np.cross(m0, vm)
        
        # 更新估计值
        b_hat = b_hat - ki * e * dt
        R_omega = Rotation.from_rotvec((gyro[i] - b_hat + kp * e) * dt)
        R_hat = R_hat * R_omega
        
        # 存储估计值
        R_hist.append(R_hat)
        b_hist.append(b_hat)
        
    return R_hist, b_hist

# 主函数
if __name__ == "__main__":
    # 参数设置
    t = np.arange(0, 10, 0.01)
    dt = t[1] - t[0]
    kp = 5.0
    ki = 0.1
    
    # 生成数据
    gyro, accel, mag = generate_data(t, dt)
    
    # 运行滤波器
    R_hist, b_hist = mahony_filter(gyro, accel, mag, dt, kp, ki)
    
    # 绘制结果
    fig, axs = plt.subplots(3, 1, figsize=(8, 8))
    
    axs[0].plot(t, [R.as_euler('ZYX', degrees=True)[0] for R in R_hist], label='Estimated')
    axs[0].set_ylabel('Z (deg)')
    axs[0].legend()
    
    axs[1].plot(t, [R.as_euler('ZYX', degrees=True)[1] for R in R_hist], label='Estimated')
    axs[1].set_ylabel('Y (deg)')
    axs[1].legend()
    
    axs[2].plot(t, [R.as_euler('ZYX', degrees=True)[2] for R in R_hist], label='Estimated')
    axs[2].set_xlabel('Time (s)')
    axs[2].set_ylabel('X (deg)')
    axs[2].legend()
    
    fig.suptitle('Attitude Estimation by Mahony Filter')
    fig.tight_layout()
    plt.show()

这个示例首先生成了一组模拟数据,包括真实姿态、角速度(带偏置)、加速度计和磁力计读数。然后,它实现了Mahony滤波器,并用生成的数据对其进行测试。最后,它绘制了滤波器估计的姿态(欧拉角)随时间的变化曲线。

在generate_data函数中,我们模拟了一个周期性的姿态运动,并加入了一定的测量噪声和偏置。在mahony_filter函数中,我们按照Mahony滤波器的算法,依次更新姿态和偏置的估计值。注意,我们使用了scipy.spatial.transform.Rotation类来表示旋转,这大大简化了姿态的计算和转换。

你可以运行这个脚本,观察Mahony滤波器对姿态的估计效果。你也可以尝试改变模拟数据的特性(如噪声大小、偏置等)或滤波器的参数(如增益),看看会有什么影响。

在实际应用中,你可以将这个滤波器应用于从IMU获取的真实数据,以获得实时、鲁棒的姿态估计。

矩阵的特征值

在上面的推导中用到了矩阵的特征值说明微分方程的稳定性，这里就一并介绍下在机器人学中经常用的几个方面。

1. 矩阵对角化
   如果一个 $n\times n$ 矩阵 $A$ 有 $n$ 个线性无关的特征向量,那么 $A$ 可以对角化,即存在可逆矩阵 $P$ 使得 $P^{-1}AP=\Lambda$,其中 $\Lambda$ 是对角矩阵,其对角元素为 $A$ 的特征值。这种对角化可以简化矩阵的运算,如矩阵的幂等。

2. 微分方程的稳定性

   在线性微分方程组 $\dot{x}=Ax$ 中,系数矩阵 $A$ 的特征值决定了系统的稳定性:如果所有特征值的实部都是负的,那么系统是渐近稳定的;如果有特征值的实部为正,那么系统是不稳定的。这个原理被广泛应用于控制理论和动力系统分析。

3. 最优化问题

   在一些最优化问题中,目标函数的Hessian矩阵的特征值提供了关于函数曲率的信息。特别地,如果Hessian矩阵在最优点处是正定的(即所有特征值都是正的),那么这个最优点是局部最小点。这个原理被用于牛顿法、拟牛顿法等二阶优化算法中。

个人陈述

SO(3)上的互补滤波为姿态估计提供了一种简洁高效的新思路。通过巧妙的误差函数设计和Lyapunov稳定性分析,可以直接对旋转矩阵进行滤波,并证明其局部指数稳定性和全局渐近稳定性。进一步耦合偏置估计,可以在线校正陀螺仪漂移。采用被动互补结构和显式误差反馈,有助于提高滤波器性能并简化其工程实现。这些思想对其他非线性估计问题也有一定启发意义。

如果对SO3上的运动学不了解的可以看我的hands-on系列教程：Unleashing Robotics: Mastering Quaternion Kinematics with Python - (原创系列教程)，看完之后你就可以轻松理解了。