Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods

这篇教程以扩展特殊欧几里得群 SE₂(3) 的李群理论为基础，系统介绍了面向控制的辅助惯性导航系统，通过明确不变性与对称性的作用，构建了融合惯性测量与辅助信息的几何框架，并探讨了高阶状态表示、同步观测器设计及等变滤波等现代扩展方法。

要点

这篇技术报告就像是一份**“自动驾驶汽车的几何导航指南”**。它由魁北克大学渥太华分校（UQO）的机器人实验室撰写，旨在用一种更现代、更聪明的方法来解决“如何在没有 GPS 的情况下，让机器人或汽车知道自己在哪里、朝哪走、走多快”的问题。

为了让你轻松理解，我们可以把这篇报告的核心思想想象成**“在迷雾中驾驶一辆没有里程表的赛车”**。

1. 核心问题：为什么普通的导航会“迷路”？

想象你蒙着眼睛开车，手里只拿着两个工具：

• 陀螺仪：告诉你车在怎么转（比如向左转了多少度）。
• 加速度计：告诉你车在怎么加速或减速。

如果你只靠这两个工具，你会尝试通过“积分”（也就是把速度加起来）来推算位置。

• 问题在于：传感器不完美，会有微小的误差（就像你走路时偶尔会绊一下）。
• 后果：这些微小的误差会随着时间无限累积。就像你蒙眼走直线，每走一步都偏了一毫米，走了一公里后，你可能已经偏到了几公里外。这就是所谓的“漂移”。

为了解决这个问题，我们需要“外援”（比如 GPS、摄像头、磁力计）来时不时地把你拉回正确的轨道。

2. 传统方法的困境：在弯曲的地球上画直线

过去，工程师们用一种叫“扩展卡尔曼滤波（EKF）”的数学工具来融合这些传感器数据。

• 比喻：想象地球是弯曲的（像一个橘子），但传统的数学方法试图在平直的纸（欧几里得空间）上处理这个橘子的问题。
• 后果：当你试图在平纸上描述橘子表面的运动时，会出现很多奇怪的数学“故障”（比如奇点、计算崩溃），特别是在误差很大或者传感器信号很弱的时候，导航系统可能会彻底“发疯”，导致计算出的位置完全错误。

3. 这篇报告的解决方案：顺应几何的“顺势而为”

这篇报告提出了一种基于**“李群（Lie Groups）”**理论的新方法。

• 核心概念：不要试图把弯曲的橘子强行压平，而是直接在橘子的表面上进行计算。
• 比喻：
  • 想象你在一个巨大的旋转木马（代表姿态/方向）上，同时这个木马还在移动（代表位置和速度）。
  • 传统的数学试图把这个旋转木马拆解成 X、Y、Z 三个独立的数字来算，这很麻烦且容易出错。
  • 这篇报告的方法是把整个旋转木马看作一个整体（一个几何形状）。它利用这个形状本身的对称性（比如无论怎么转，圆还是圆）来设计算法。

4. 关键创新： invariant（不变性）的魔法

报告中最精彩的部分是引入了**“不变误差”（Invariant Error）**的概念。

• 传统做法：就像你问“我离目的地还有多远？”，答案取决于你现在在哪里。如果你算错了位置，你的“距离”计算也会跟着错，导致恶性循环。
• 新方法（不变性）：就像你问“我的相对运动状态是什么？”。无论你在世界的哪个角落，无论你的初始位置算得有多离谱，误差的变化规律是固定的、独立的。
  • 比喻：想象你在玩一个拼图游戏。
    • 旧方法：每次拼错一块，整个拼图的形状都变了，你得重新算所有规则。
    • 新方法：无论拼图拼到哪一步，每一块拼图之间的连接规则（几何结构）是不变的。你只需要关注“这块拼图相对于那块拼图”的关系，而不是它们在整个世界地图上的绝对坐标。

这样做的好处是：

1. 更稳定：即使初始位置猜错了，或者 GPS 信号暂时断了，系统也不会崩溃，因为它遵循的是几何规律，而不是脆弱的线性近似。
2. 更一致：系统能更准确地评估自己“有多不确定”，不会盲目自信。

5. 具体的“魔法工具”：SE2(3) 群

报告引入了一个叫 SE2(3) 的数学结构。

• 通俗解释：这是一个超级容器，它把方向（转）、**速度（跑多快）和位置（在哪）**打包在一起。
• 在这个容器里，所有的计算（比如积分、更新）都变成了简单的矩阵乘法。这就像是在玩积木，只要按照特定的几何规则拼接，积木永远不会散架，也不会变形。

6. 实际效果：为什么这很重要？

报告通过实验对比了“旧方法（MEKF）”和“新方法（InvEKF）”：

• 场景：一辆车在画圆圈，没有磁力计（指南针）辅助，只有 GPS 和惯性传感器。
• 旧方法：因为误差积累和线性化失效，车子算出的轨迹开始乱抖，甚至完全偏离，就像喝醉了一样。
• 新方法：即使初始方向猜错了，它也能稳稳地收敛，画出完美的圆圈。因为它利用了“重力”和“运动”之间的几何对称性，自动修正了偏差。

总结

这篇报告不仅仅是在讲数学公式，它是在告诉我们：在处理复杂的物理运动（如机器人导航）时，不要试图用简单的直线思维去强行解释弯曲的世界。

通过**“顺应几何结构”（利用李群理论），我们可以设计出更聪明、更鲁棒、更不容易迷路的导航系统。这就好比，与其在迷宫里死记硬背每一条路，不如直接理解迷宫的整体结构**，这样无论入口在哪里，你都能找到出口。

一句话总结：
这篇报告教我们如何用几何的对称性来给机器人装上一个**“永不迷路的指南针”**，让它在没有 GPS 或信号很差的时候，依然能精准地知道自己在哪里。

技术摘要

这是一份关于《辅助惯性导航系统教程：基于李群理论方法的现代处理》（Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods）的技术报告详细总结。该报告由魁北克大学渥太华分校（UQO）机器人和自主系统实验室（LaRSA）的 Soulaimane Berkane 撰写。

1. 研究背景与问题 (Problem)

• 惯性导航系统 (INS) 的局限性：INS 通过积分加速度计和陀螺仪数据来估计位置、速度和姿态，是自主导航的基石。然而，由于传感器噪声和偏差（Bias）的存在，导航误差会随时间无界增长（漂移）。
• 辅助传感器的必要性：为了抑制漂移，通常引入 GNSS、磁力计、相机、LiDAR 等外部传感器进行融合。
• 传统方法的缺陷：
  • 姿态表示难题：姿态（Orientation）演化在非线性流形（旋转群 $SO(3)$）上，而非欧几里得空间。
  • 扩展卡尔曼滤波 (EKF) 的局限：传统的 EKF 基于欧几里得误差（如欧拉角或四元数的加法修正），会导致奇异性、归一化错误以及线性化误差动力学的不一致性。
  • 乘性扩展卡尔曼滤波 (MEKF) 的不足：虽然 MEKF 通过乘性误差解决了归一化问题，但其线性化误差动力学仍然依赖于估计轨迹。当初始误差较大或辅助信号微弱/间歇时，滤波器可能不一致甚至发散。
• 核心挑战：如何在保持几何结构一致性的同时，设计具有鲁棒性、一致性和全局收敛性保证的辅助惯性导航滤波器。

2. 方法论 (Methodology)

本文提出了一种基于李群理论（Lie-Group Theoretical Methods）的统一框架，核心是扩展特殊欧几里得群 $SE_2(3)$。

• 状态表示：
  • 将姿态 ($R \in SO(3)$)、速度 ($v \in \mathbb{R}^3$) 和位置 ($p \in \mathbb{R}^3$) 统一嵌入到 $SE_2(3)$ 矩阵群中。
  • 系统动力学被表述为李群上的流，利用群-仿射（Group-Affine）结构。
• 不变误差定义 (Invariant Error)：
  • 摒弃传统的欧几里得误差，定义右不变误差（Right-Invariant Error）：$\tilde{X} = \hat{X}X^{-1}$。
  • 关键优势：对于 $SE_2(3)$ 上的群-仿射系统，右不变误差的动力学是自治的（Autonomous），即线性化后的误差动力学矩阵不依赖于估计轨迹。这解决了传统 EKF 中增益矩阵随状态变化的问题。
• 不变扩展卡尔曼滤波 (InvEKF)：
  • 利用不变误差构建滤波器。预测步骤在李群上直接积分，更新步骤通过李代数上的乘性修正（Retraction）将状态拉回流形。
  • 推导了精确的离散时间传播公式（基于 $SE_2(3)$ 的指数映射和雅可比矩阵），避免了传统离散化带来的几何畸变。
• 辅助测量融合：
  • 将各类传感器（GNSS、磁力计、地标等）的观测模型统一转化为右不变形式。
  • 证明了在右不变误差定义下，许多观测模型的雅可比矩阵是常数或仅依赖于测量值，从而保持了滤波器的结构优势。
• 可观测性分析：
  • 利用不变误差框架下的线性时不变（LTI）或线性时变（LTV）系统理论，分析了不同传感器配置下的可观测性（如单地标、GPS、磁力计等），揭示了姿态、速度和位置之间的耦合关系。

3. 主要贡献 (Key Contributions)

1. 统一的几何框架：提供了一个基于 $SE_2(3)$ 的、面向控制的统一教程，将惯性导航的建模、传播、不确定性分析和测量融合整合在一个几何结构中。
2. 精确的离散化传播：推导了基于 $SE_2(3)$ 的精确离散时间传播公式（Lemma 1），利用左雅可比矩阵（Left Jacobian）处理高动态旋转，比传统近似（$J \approx I$）更准确，且能保持圆形轨迹等几何特性。
3. 协方差传播与不确定性建模：
   • 推导了右不变误差下的协方差传播方程。
   • 揭示了在李群指数坐标下，位置不确定性呈现“香蕉形”（Banana-shaped）分布，这是由姿态 - 位置耦合引起的，传统欧几里得协方差无法捕捉这一特性。
4. 可观测性与稳定性分析：
   • 系统性地分析了不同辅助传感器（GPS、磁力计、DVL 等）对系统可观测性的贡献。
   • 证明了 InvEKF 在弱可观测性（如缺乏磁力计）和大初始误差下，比传统 MEKF 具有更好的鲁棒性和一致性。
5. 前沿扩展：
   • $SE_5(3)$ 扩展：介绍了通过状态增广（引入辅助正交标架）到 $SE_5(3)$ 的方法，实现了**几乎全局渐近稳定（AGAS）**的观测器设计。
   • 同步观测器 (Synchronous Observer)：介绍了基于同步观测器设计的 AGAS 方法。
   • 等变滤波 (Equivariant Filtering)：简要提及了处理传感器偏差的等变滤波器（TG-EqF）框架。

4. 实验结果与验证 (Results)

• 数值仿真对比：
  • 在平面圆形轨迹仿真中，对比了经典 MEKF 和 右不变 EKF (RIEKF/InvEKF)。
  • 无磁力计辅助时：MEKF 由于轨迹依赖的线性化，在弱可观测性下出现误差放大甚至发散；而 InvEKF 保持了稳定收敛。
  • 有磁力计辅助时：两者性能均提升，但 InvEKF 表现出更均匀的时间行为和更低的对初始对准误差的敏感性。
• 几何保真度：
  • 通过圆形运动仿真展示了 $SE_2(3)$ 精确离散化与经典离散化的区别。经典方法随采样间隔增大产生明显的几何畸变，而基于李群的方法能完美保持轨迹形状。
• 不确定性可视化：
  • 蒙特卡洛仿真显示，基于 $SE_2(3)$ 的协方差传播准确捕捉了误差分布的“香蕉形”弯曲特征，而欧几里得协方差则呈现为椭圆，无法反映真实的几何耦合。

5. 意义与影响 (Significance)

• 理论深度：该报告清晰地阐述了李群对称性在状态估计中的核心作用，解释了为什么不变性设计能带来自治的误差动力学和更好的收敛性。
• 实践指导：提供了面向实现的公式（包括精确离散传播、协方差更新、各类传感器雅可比矩阵），可直接用于机器人、无人机和自动驾驶系统的导航算法开发。
• 统一视角：打破了传统惯性导航、非线性观测器和现代滤波理论之间的壁垒，将多种方法（InvEKF, 同步观测器, 等变滤波）统一在对称性设计的框架下。
• 未来方向：指出了当前在保持全局稳定性同时处理传感器偏差估计方面的挑战，为未来的研究指明了方向（如 $SE_5(3)$ 和等变滤波器的进一步应用）。

总结：
这篇技术报告不仅是一个教程，更是一份关于现代辅助惯性导航系统的权威指南。它通过引入 $SE_2(3)$ 李群框架和不变误差理论，解决了传统滤波方法在处理非线性姿态和漂移问题时的根本缺陷，为设计高鲁棒性、高一致性的导航系统提供了坚实的数学基础和实用的工程方案。