Finite-time Stable Pose Estimation on TSE(3) using Point Cloud and Velocity Sensors

📄 arXiv: 2602.09414v1 📥 PDF

作者: Nazanin S. Hashkavaei, Abhijit Dongare, Neon Srinivasu, Amit K. Sanyal

分类: eess.SY, cs.RO

发布日期: 2026-02-10

备注: 17 pages, 8 figures, submitted to Automatica

💡 一句话要点

提出基于点云和速度传感器的有限时间稳定SE(3)位姿估计器，适用于自主车辆。

🎯 匹配领域: 支柱三：空间感知与语义 (Perception & Semantics)

关键词: 位姿估计 有限时间稳定 李群SE(3) 点云 速度传感器 自主导航 机器人

📋 核心要点

  1. 现有位姿估计算法常依赖局部坐标或四元数，易出现奇异性和缠绕问题，限制了其在复杂运动场景下的应用。
  2. 论文提出一种基于李群SE(3)的有限时间稳定位姿估计器，直接在状态空间设计，避免了局部坐标和四元数表示。
  3. 实验结果表明，该方法在稳定性和鲁棒性方面优于对偶四元数扩展卡尔曼滤波器和先前的变分位姿估计器。

📝 摘要（中文）

本文提出了一种用于三维旋转和平移运动刚体的有限时间稳定位姿估计器(FTS-PE)。该估计器利用来自板载传感器的测量数据，这些传感器提供指向惯性固定点的位移向量和刚体速度。FTS-PE是一个用于位姿（位置和方向）和速度的全状态观测器，通过李雅普诺夫分析获得，该分析表明其在有限时间内稳定且对有界测量噪声具有鲁棒性。此外，该观测器直接在状态空间上设计，即刚体运动李群SE(3)的切丛，无需局部坐标或（对偶）四元数表示。因此，它可以估计任意刚体运动，而不会遇到奇异性或缠绕现象，并且可以轻松应用于自主车辆。还获得了一个不需要平移速度测量，仅使用点云和来自速率陀螺仪的角速度测量的观测器版本。它使用几何力学框架进行离散化，以进行数值和实验实现。数值模拟将FTS-PE与对偶四元数扩展卡尔曼滤波器和我们之前开发的变化位姿估计器(VPE)进行了比较。实验结果是使用从Zed 2i立体深度相机传感器获得的点云图像和速率陀螺仪测量获得的。这些结果验证了FTS-PE的稳定性和鲁棒性。

🔬 方法详解

问题定义：论文旨在解决三维空间中刚体的精确、鲁棒和快速位姿估计问题。现有方法，如基于扩展卡尔曼滤波器（EKF）的方法，通常依赖于局部坐标或四元数表示，这可能导致奇异性问题（如万向节锁）和缠绕现象，从而限制了它们在处理大角度旋转和复杂运动时的性能。此外，传统方法的收敛速度可能较慢，难以满足实时性要求高的应用场景。

核心思路：论文的核心思路是直接在刚体运动的李群SE(3)的切丛上设计位姿估计器，避免使用局部坐标或四元数。通过李雅普诺夫分析，证明了所提出的估计器在有限时间内稳定，并且对有界测量噪声具有鲁棒性。这种设计允许估计任意刚体运动，而不会遇到奇异性或缠绕现象。

技术框架：该方法构建了一个全状态观测器，用于估计刚体的位置、方向和速度。该观测器利用来自板载传感器的测量数据，包括指向惯性固定点的位移向量和刚体速度。该框架包含以下主要模块：1) 传感器数据采集模块，用于获取点云和速度信息；2) 位姿估计器模块，基于李群SE(3)的切丛进行设计，实现有限时间稳定位姿估计；3) 离散化模块，采用几何力学框架进行离散化，以便于数值和实验实现。

关键创新：论文最重要的技术创新点在于直接在李群SE(3)的切丛上设计位姿估计器，避免了局部坐标和四元数表示，从而解决了奇异性和缠绕问题。此外，通过李雅普诺夫分析证明了估计器的有限时间稳定性和鲁棒性，使其能够快速、准确地估计刚体的位姿。

关键设计：该方法的关键设计包括：1) 基于李群SE(3)的切丛的状态空间表示；2) 基于李雅普诺夫理论的有限时间稳定控制律设计；3) 针对不同传感器配置（包括仅使用点云和角速度测量）的观测器设计；4) 采用几何力学框架进行离散化，以保证数值实现的稳定性。

📊 实验亮点

数值模拟结果表明，FTS-PE在位姿估计精度和收敛速度方面优于对偶四元数扩展卡尔曼滤波器和先前的变分位姿估计器(VPE)。实验结果使用Zed 2i立体深度相机传感器获取的点云图像和速率陀螺仪测量数据，验证了FTS-PE的稳定性和鲁棒性。具体性能数据（如均方根误差、收敛时间等）在论文中进行了详细展示。

🎯 应用场景

该研究成果可广泛应用于自主导航、机器人控制、增强现实等领域。例如，在无人机、自动驾驶汽车等自主车辆中，该位姿估计器可以提供精确、鲁棒的位姿信息，从而提高导航和控制的性能。在机器人操作中，该方法可以用于精确估计机器人的位姿，从而实现更精确的操作和控制。在增强现实应用中，该方法可以用于精确跟踪用户的头部运动，从而提供更逼真的增强现实体验。

📄 摘要（原文）

This work presents a finite-time stable pose estimator (FTS-PE) for rigid bodies undergoing rotational and translational motion in three dimensions, using measurements from onboard sensors that provide position vectors to inertially-fixed points and body velocities. The FTS-PE is a full-state observer for the pose (position and orientation) and velocities and is obtained through a Lyapunov analysis that shows its stability in finite time and its robustness to bounded measurement noise. Further, this observer is designed directly on the state space, the tangent bundle of the Lie group of rigid body motions, SE(3), without using local coordinates or (dual) quaternion representations. Therefore, it can estimate arbitrary rigid body motions without encountering singularities or the unwinding phenomenon and be readily applied to autonomous vehicles. A version of this observer that does not need translational velocity measurements and uses only point clouds and angular velocity measurements from rate gyros, is also obtained. It is discretized using the framework of geometric mechanics for numerical and experimental implementations. The numerical simulations compare the FTS-PE with a dual-quaternion extended Kalman filter and our previously developed variational pose estimator (VPE). The experimental results are obtained using point cloud images and rate gyro measurements obtained from a Zed 2i stereo depth camera sensor. These results validate the stability and robustness of the FTS-PE.