[论文解读] The Geometry and Kinematics of the Matrix Lie Group $SE_K(3)$

Yarong Luo, Mengyuan Wang+1 more|arXiv (Cornell University)|Dec 2, 2020

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

一句话总结

本文对矩阵李群 $SE_K(3)$ 进行了全面推导，将 $SE(3)$ 扩展以在机器人学中建模扩展位姿，包含 $K$ 个额外的平移状态。推导了闭式逆雅可比矩阵、位姿组合法则，并证明了不确定性传播的单调性，从而在不变扩展卡尔曼滤波中实现一致的状态估计，适用于惯性导航与SLAM。

ABSTRACT

Currently state estimation is very important for the robotics, and the uncertainty representation based Lie group is natural for the state estimation problem. It is necessary to exploit the geometry and kinematic of matrix Lie group sufficiently. Therefore, this note gives a detailed derivation of the recently proposed matrix Lie group $SE_K(3)$ for the first time, our results extend the results in Barfoot \cite{barfoot2017state}. Then we describe the situations where this group is suitable for state representation. We also have developed code based on Matlab framework for quickly implementing and testing.

研究动机与目标

• 为 $SE_K(3)$ 建立理论基础，该矩阵李群用于在机器人学中建模扩展位姿。
• 推导 $SE_K(3)$ 中逆雅可比矩阵的闭式表达式，这对不变滤波至关重要。
• 通过群运算形式化 $SE_K(3)$ 中一般位姿的组合。
• 分析 $SE_K(3)$ 上不确定性传播的单调性，确保状态估计的一致性。
• 提供基于MATLAB的实用实现，用于机器人应用中的测试与部署。

提出的方法

• 将 $SE_K(3)$ 的几何结构推导为半直积 $SO(3) \rtimes \mathbb{R}^{3K}$，推广了 $SE(3)$。
• 通过块矩阵表示定义群，其中旋转部分 $R \in SO(3)$，$K$ 个平移向量 $p_k \in \mathbb{R}^3$。
• 推导群组合规则：$T_1 T_2 = \begin{bmatrix} R_1 R_2 & R_1 p_2 + p_1 \end{bmatrix}$，推广了 $SE(3)$ 的组合规则。
• 推导群元素的逆为 $T^{-1} = \begin{bmatrix} R^{-1} & -R^{-1} p_k \end{bmatrix}$，保持群的封闭性。
• 推导李代数 $\mathfrak{se}_K(3)$ 为单位元处的切空间，其元素由 $\xi \in \mathbb{R}^{3(K+1)}$ 参数化。
• 利用Weyl不等式和Minkowski行列式不等式证明不确定性传播的单调性，表明 $\det(\Sigma_{ik}) > \det(\Sigma_{ij})$。

实验结果

研究问题

• RQ1 如何系统地推导 $SE_K(3)$ 的几何与运动学结构，将其作为 $SE(3)$ 和 $SO(3)$ 的推广？
• RQ2 在 $SE_K(3)$ 中，逆雅可比矩阵的闭式表达式是什么？它如何支持不变滤波？
• RQ3 如何推导 $SE_K(3)$ 中一般位姿的组合？其代数结构是什么？
• RQ4 在A-、D-和E-最优性准则下，$SE_K(3)$ 上的不确定性传播是否保持单调性？
• RQ5 在哪些机器人应用中，$SE_K(3)$ 能够提供一致且有效的状态表示？

主要发现

• 在 $SE_K(3)$ 中，逆雅可比矩阵已推导出闭式表达，可在不变扩展卡尔曼滤波中实现高效计算。
• $SE_K(3)$ 中两个元素的组合被显式推导为 $T_1 T_2 = \begin{bmatrix} R_1 R_2 & R_1 p_2 + p_1 \end{bmatrix}$，推广了 $SE(3)$ 的组合规则。
• $SE_K(3)$ 上的不确定性传播具有单调性：不确定性矩阵的所有特征值随时间增加，确保滤波的一致性。
• 不确定性矩阵的行列式在传播过程中增加，证实了在D-最优性准则下的单调性。
• 不确定性分布的Rényi熵单调递增，证实了与信息论最优性的兼容性。
• $SE_K(3)$ 群适用于建模惯性导航与SLAM中的扩展状态，如姿态、位置和速度。