[论文解读] Moving object tracking employing rigid body motion on matrix Lie groups

Josip Ćesić, Ivan Marković+1 more|arXiv (Cornell University)|Aug 18, 2017

Inertial Sensor and Navigation参考文献 19被引用 10

一句话总结

本文提出一种基于矩阵李群上扩展卡尔曼滤波的新型运动目标跟踪方法，具体将状态空间建模为SE(2)×ℝ³或SE(2)²，以捕捉具有二阶动力学的刚体运动。该方法在多种运动类型下均优于传统的基于EKF的恒速与转向率模型，尤其在高旋转动力学条件下，通过精确建模SE(2)中固有的香蕉形不确定性轮廓，实现更优性能。

ABSTRACT

In this paper we propose a novel method for estimating rigid body motion by modeling the object state directly in the space of the rigid body motion group SE(2). It has been recently observed that a noisy manoeuvring object in SE(2) exhibits banana-shaped probability density contours in its pose. For this reason, we propose and investigate two state space models for moving object tracking: (i) a direct product SE(2)xR3 and (ii) a direct product of the two rigid body motion groups SE(2)xSE(2). The first term within these two state space constructions describes the current pose of the rigid body, while the second one employs its second order dynamics, i.e., the velocities. By this, we gain the flexibility of tracking omnidirectional motion in the vein of a constant velocity model, but also accounting for the dynamics in the rotation component. Since the SE(2) group is a matrix Lie group, we solve this problem by using the extended Kalman filter on matrix Lie groups and provide a detailed derivation of the proposed filters. We analyze the performance of the filters on a large number of synthetic trajectories and compare them with (i) the extended Kalman filter based constant velocity and turn rate model and (ii) the linear Kalman filter based constant velocity model. The results show that the proposed filters outperform the other two filters on a wide spectrum of types of motion.

研究动机与目标

• 为解决经典卡尔曼滤波在建模SE(2)上刚体运动中常见的非椭圆、香蕉形不确定性轮廓时的局限性。
• 开发一种直接在矩阵李群上建模二阶动力学（位姿与速度）的状态估计框架，以提升运动建模的灵活性。
• 通过在SE(2)群上直接运行滤波器而非在笛卡尔空间中，实现对全向运动（包括复杂旋转机动）的精确跟踪。
• 提供一种在矩阵李群上的闭式贝叶斯滤波框架，支持高阶运动模型与非线性动力学。
• 在具有不同旋转动力学的合成轨迹上，对所提滤波器与成熟的EKF和KF基模型进行验证。

提出的方法

• 状态空间建模为SE(2)×ℝ³或SE(2)²，其中第一部分用于跟踪SE(2)中的当前位姿，第二部分用于编码二阶动力学（如平动与转动速度）。
• 在矩阵李群上应用扩展卡尔曼滤波（LG-EKF），直接在SE(2)群上进行状态与协方差的传播与更新，以保持运动流形的几何结构。
• 采用指数坐标与矩阵李群运算（如对数映射与指数映射）处理SE(2)上的状态转移与不确定性传播。
• 过程噪声被仔细参数化，以确保与经典滤波器的公平比较，且噪声模型与SE(2)的内在几何结构保持一致。
• 滤波器推导包含在李群上对状态转移与测量模型的详细雅可比矩阵计算，支持递归贝叶斯估计。
• 该框架支持平动与转动运动的联合建模，能够捕捉如转向与全向运动等复杂机动。

实验结果

研究问题

• RQ1 在SE(2)群上直接建模二阶动力学是否能相比基于欧氏空间的经典卡尔曼滤波器提升跟踪性能？
• RQ2 SE(2)×ℝ³与SE(2)²状态空间模型在处理多样化运动类型（尤其在高旋转动力学下）时表现如何比较？
• RQ3 在SE(2)上直接滤波是否比在ℝ²中滤波更能准确捕捉香蕉形不确定性轮廓？
• RQ4 在合成跟踪场景中，所提出的基于李群的滤波器相比EKF基的恒速与恒转向率模型，性能提升程度如何？
• RQ5 该框架能否推广至涉及混合移动性（如汽车、行人、机器人）且具有复杂运动模式的真实世界跟踪应用？

主要发现

• SE(2)×ℝ³与SE(2)²滤波器在所有测试的旋转噪声强度（σω）下，均显著优于基于EKF的恒速与恒转向率模型。
• 在高旋转动力学条件下（σω > 0.5），基于EKF的恒速滤波器性能迅速下降，而所提滤波器保持了较低的位置估计均方根误差（RMSE）。
• 基于EKF的恒转向率模型在旋转非主导时表现较差，虽随σω增加而改善，但仍逊于所提滤波器。
• SE(2)×ℝ³与SE(2)²滤波器性能几乎完全一致，表明将速度建模为ℝ³向量或作为第二个SE(2)群，可实现相当的跟踪精度。
• 滤波器展现出鲁棒性与灵活性，能在一个统一框架中有效结合恒速与恒转向率模型的优势。
• 结果表明，在SE(2)上直接建模不确定性能更准确捕捉姿态不确定性的真实形状，尤其在非高斯、香蕉形分布下，从而实现更精确的状态估计。