💻 computer science

Invariant Stochastic Filtering on SE(3) for Inertial-Encoder State Estimation of Serial Rigid Manipulators

本文提出了一种在李群 SE(3) 上构建的模块化、不变扩展卡尔曼滤波器，用于具有任意连杆数量的串联刚性机械臂的状态估计，其特点是具有物理分离的噪声模型和确保线性计算成本及可证明指数稳定性的链式结构。

原作者： S. Yaqubi, J. Mattila

发布于 2026-07-02

📖 1 分钟阅读☕ 轻松阅读

原作者： S. Yaqubi, J. Mattila

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

想象一下，一个机械臂就像是由刚性杆件通过关节连接而成的链条，类似于人的手臂或起重机。为了完美地控制这个机器人，计算机需要精确地知道每一根杆件在每一时刻的确切位置和运动速度。这被称为“状态估计”。

然而，机器人上的传感器（陀螺仪和加速度计）充满了噪声，就像是在飓风中试图听清耳语。传统的清理噪声的方法通常假设机器人的运动发生在平坦的直线上。但实际上，机械臂在三维空间中运动，进行着复杂的扭转和旋转，这并不符合平面地图的逻辑。

本文介绍了一种更聪明的方法，用于清理传感器噪声并追踪机器人的位置。以下是他们解决方案的分解，使用了简单的类比：

1. “曲面地图”对比“平面地图”

问题： 想象尝试将地球绘制在平整的纸上。如果你试图在纸上画一个围绕北极的圆，它就会发生畸变。传统的滤波器试图将机器人的三维运动简化为二维数字列表。当机器人进行大幅度转向时，“平面地图”就会失效，数学计算也会陷入混乱。

解决方案： 作者使用了一种“曲面地图”（在数学上称为 Lie 群 SE(3)）。他们不再强迫机器人的运动进入一条直线，而是让数学运算存在于三维空间的自然曲面上。这确保了无论机器人如何扭转，数学逻辑始终保持一致，并且不会出现“万向节死锁”（即数学逻辑失去方向感的状态）。

2. 滤波器的“模块化链条”

问题： 机械臂有很多个环节。如果你把整个机械臂看作一个巨大的、复杂的拼图，那么修复一个环节的位置就需要同时重新计算其他所有环节的位置。随着机器人变得越来越长，这种计算会变得极其缓慢。

解决方案： 作者构建了一个模块化滤波器链。可以把它想象成一场接力赛：

环节 1 跑自己的赛道，计算自己的位置，然后将一个“接力棒”（其估计的位置和不确定性）传递给环节 2。
环节 2 接过接力棒，加入自身的传感器数据，然后将一个新的接力棒传递给环节 3。
益处： 这意味着计算机不需要解决一个巨大的拼图，而只需为每个环节解决一个小拼图。如果给机器人增加第 100 个环节，计算机并不会变慢，它只是多做了一个小步骤。计算成本呈线性增长，而非指数级增长。

3. “双通道”噪声模型

问题： 机器人有两种主要的传感器：陀螺仪（感知旋转）和加速度计（感知运动）。

陀螺仪就像一个旋转的陀螺；它的噪声是恒定的。
加速度计则像汽车的速度计，你必须对其进行积分（随时间累加）才能得到速度。如果你等待检查的时间过长，噪声带来的误差就会随之增大。

解决方案： 作者意识到这两种传感器的行为方式不同。

他们将陀螺仪噪声视为一种持续的嗡嗡声。
他们将加速度计噪声视为一个随着等待检查时间增长而不断扩大的“雪球”。
他们还加入了一个特殊的“科里奥利”（Coriolis）项。想象机器人正在挥动一个重物；来自陀螺仪的噪声会随着挥动的速度而被放大。他们的滤波器考虑到了这一点：如果机器人处于静止状态，噪声较低；如果机器人正在剧烈摆动，滤波器会预见到更多的噪声并做出相应调整。

4. “稳定性证书”

问题： 即使一个滤波器今天表现良好，你如何知道它在明天机器人进行奇怪运动时不会失控？

解决方案： 作者不仅进行了测试，还通过数学证明了误差始终会保持在一个特定的、可预测的“气泡”内。

他们使用了一个“李雅普诺夫函数”（可以理解为一个数学能量计）来证明误差会随时间呈指数级缩小。
他们还证明了即使环节 1 的误差稍微变大，也不会导致整个链条崩溃。由于环节长度的存在，误差在沿链条传递时可能会略微增大，但绝不会爆炸到无穷大。他们称之为“指数最终有界性”。

5. 结果

他们在模拟的三维两环节机械臂复杂运动模式下进行了测试。

对比： 他们将这种新滤波器与标准的“平面地图”滤波器以及仅使用原始传感器数据的方法进行了对比。
结果： 他们的滤波器显著提高了精度（比标准滤波器高出约 24%，比原始数据高出约 33%）。
关键发现： 即使在机器人进行大幅度、快速运动时，新滤波器仍能保持准确，而标准滤波器则开始出现漂移和错误。

总结

简而言之，本文提出了一种用于机械臂的新型“GPS”，它具有以下特点：

尊重三维空间的曲面特性（不再有平面地图的畸变）。
在长链式机器人环节上高效运行（类似于接力赛）。
理解不同传感器会产生不同类型的误差。
从数学上保证了无论机器人如何运动，都不会“失去理智”。

技术摘要：用于串联刚性机械臂惯性-编码器状态估计的 $SE(3)$ 不变随机滤波

问题陈述
精确的串联刚性机械臂状态估计需要同时具备几何一致性和随机严谨性。每个连杆的构型都存在于李群 $SE(3)$ 中，然而传统的卡尔曼滤波器在平坦向量空间中运行，这会导致在大角度下的线性化误差，并产生欧拉角或四元数形式固有的失锁（gimbal lock）或归一化约束问题。此外，现有的几何滤波器通常将单个物体或整个链视为单体状态，导致其计算成本随连杆数量呈三次幂增长。在建模惯性测量单元（IMU）噪声方面也存在关键空白：标准方法未能考虑到陀螺仪和加速度计噪声不同的物理起源，即加速度计噪声在积分过程中的采样间隔缩放问题，以及陀ля仪噪声通过非线性科里奥利（Coriolis）动力学产生的状态相关传播问题。

方法论
作者开发了一种完全在 $SE(3)$ 李群内构建的针对任意数量连杆的不变扩展卡尔曼滤波（IEKF）。该方法依赖于四个核心组件：

几何误差动力学： 该滤波器严格区分了估计值 $(\hat{g}_i, \hat{V}_i)$ 与李代数误差向量 $\tilde{\xi}_i$ 。通过利用运动学方程的群仿射性质（ $\dot{g}_i = g_i V_i$ ），证明了线性化后的误差动力学是自治的。这确保了 Riccati 方程控制的是真实的误差协方差，而非局部近似。
物理分离的噪声模型： 本文引入了一种全新的噪声模型，将陀螺仪通道和加速度计通道独立对待。
- 加速度计： 比力（specific force）的测量通过积分产生平移扭转（translational twist）。由此产生的测量协方差随采样间隔 $\Delta t$ 进行缩放，类似于过程噪声的离散化。
- 陀螺仪与科里奥利项： 陀螺仪噪声直接进入旋转通道，但也通过非线性牛顿-欧拉动力学中的科里奥利项进行传播。这产生了一个状态相关的过程噪声贡献，该贡献在静止时消失，并随扭转量的大小而增长。
模块化链式合成： 滤波器被构建为由逐个连杆 IEKF 组成的链。连杆 $i$ 的预测协方差仅通过伴随变换（Adjoint-transformed）依赖于连杆 $i-1$ 的后验分布。这种传播机制确保了总计算成本随连杆数量呈线性（ $O(n)$ ）增长，避免了单体状态估计带来的三次幂成本。
稳定性分析： 作者在均方意义下建立了指数最终有界性（EUB）。利用李代数 Lyapunov 函数，他们推导出了逐连杆的界限，并通过伴随算子范数将这些界限传递至整个机械臂。至关重要的是，该分析通过考虑伴随映射在标准欧几里得内积下并非等距映射这一事实，修正了过于简单的稳定性论证，这意味着不确定性的放大取决于连杆几何结构（长度）。

核心贡献
本文声称在几何随机估计方面有四项具体贡献：

(C1) 噪声建模： 包含状态相关科里奥利噪声项和加速度计采样间隔缩放的物理分离 IMU 噪声模型。
(C2) 模块化架构： 通过伴随协方差传播实现线性计算成本的逐连杆 IEKF 链。
(C3) 完整推导： 完整的 IEKF 推导，包括混合位姿-扭转观测雅可比矩阵。
(C4) 稳定性证书： 通过李代数 Lyapunov 函数推导出的链式指数最终有界性证书，提供了可扩展至任意链长的模块化稳定性保证。

结果
通过对执行大振幅 3D 运动的 3 自由度两连杆刚性串联机械臂进行数值模拟，验证了该设计。将所提 IEFK 与基于坐标的 EKF（CEKF）以及原始编码器基准进行了对比。

精度： 相对于原始编码器基准，IEKF 在所有关节上的 RMSE 降低了 33%；相对于 CEKF，降低了 24%。
鲁棒性： 由于坐标线性化误差，CEKF 的误差随运动振幅增加而增大，而 IEKF 的误差保持稳定，展示了 $SE(3)$ 几何误差模型的振幅不变性。
一致性： 归一化估计误差平方（NEES）分析证实了滤波器的统计一致性，其均值落在 95% 卡方接受带内。
不确定性传播： 结果证实，随着下游连杆（如连杆 2）通过伴随映射整合上游估计不确定性，其协方差带会正确地变宽。

意义
本文将这项工作定位为一个连接几何直觉与随机严谨性的严密框架。通过确保滤波器原生运行在 $SE(3)$ 上，它避免了平坦空间方法的奇异性和线性化不一致问题。其模块化结构使得该方法能够以极低的计算成本扩展到复杂的长链机械臂。此外，通过显式处理状态相关噪声并推导考虑了几何放大因子的稳定性证书，相比现有的几何滤波器，该方法为控制柔性及刚性机械臂提供了更符合物理实际且在理论上更完备的基础。作者指出，该框架可以作为基于学习的估计器的几何一致先验，解决在学习修正过程中保持李群一致性的挑战。