李群李代数快速入门

李群李代数入门及应用

原创于 2025-11-24 17:47:29 发布 · 501 阅读

17 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #优化 #sophus

李群李代数专栏收录该内容

3 篇文章

订阅专栏

李群和李代数是机器人学、计算机视觉、控制理论等领域中处理旋转和平移变换的重要数学工具。它们为三维空间中的位姿（位置和姿态）提供了紧凑且高效的表示方法，特别适用于优化问题（如SLAM、Bundle Adjustment等）。以下是李群和李代数的快速入门指南：

一、基本概念

1. 李群（Lie Group）

定义：李群是同时具有群结构和光滑流形结构的数学对象。在机器人学中，常用的李群包括：
- SO(3)：三维旋转群，表示所有3×3旋转矩阵的集合。
- SE(3)：三维刚体变换群（旋转+平移），表示所有4×4齐次变换矩阵的集合。
性质：
- 封闭性：两个李群元素的乘积仍属于该李群。
- 逆元存在：每个李群元素都有逆元。
- 光滑性：李群上的乘法运算和求逆运算是光滑的。

2. 李代数（Lie Algebra）

定义：李代数是李群的切空间，描述了李群在单位元附近的局部性质。每个李群对应一个李代数。
与李群的关系：
- 李代数通过指数映射（Exponential Map）生成李群元素。
- 李群元素通过对数映射（Logarithm Map）映射到李代数。
常用李代数：
- so(3)：对应SO(3)的李代数，是3×3反对称矩阵的集合，可用三维向量（旋转向量）表示。
- se(3)：对应SE(3)的李代数，是6维向量（前3维为旋转向量，后3维为平移向量）。

二、为什么需要李群和李代数？

避免万向节锁（Gimbal Lock）：
- 欧拉角表示旋转时会出现奇点（万向节锁），而李群和李代数无此问题。
优化友好：
- 李代数上的加法操作对应李群上的“接近”操作，适合梯度下降等优化算法。
插值平滑：
- 李群上的插值（如Slerp）比直接线性插值更平滑。
紧凑表示：
- SO(3)用9个参数（旋转矩阵）或4个参数（四元数）表示，而李代数so(3)仅用3个参数（旋转向量）。

三、核心操作

1. SO(3) 与 so(3)

旋转矩阵（SO(3)）：

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0,0,1)).toRotationMatrix();

旋转向量（so(3)）：

Eigen::Vector3d so3 = Sophus::SO3d::log(R);  // 对数映射：SO(3) -> so(3)
Eigen::Matrix3d R_exp = Sophus::SO3d::exp(so3).matrix();  // 指数映射：so(3) -> SO(3)

四元数与SO(3)的转换：

Eigen::Quaterniond q(R);  // 旋转矩阵 -> 四元数
Sophus::SO3d SO3_q(q);   // 四元数 -> SO(3)

2. SE(3) 与 se(3)

齐次变换矩阵（SE(3)）：

Eigen::Vector3d t(1, 0, 0);  // 平移向量
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = t;
Sophus::SE3d SE3_T(T);

变换向量（se(3)）：

Eigen::Matrix<double,6,1> se3 = SE3_T.log();  // 对数映射：SE(3) -> se(3)
Sophus::SE3d SE3_exp = Sophus::SE3d::exp(se3); // 指数映射：se(3) -> SE(3)

四、关键应用场景

1. 位姿优化（Bundle Adjustment）

问题：优化相机位姿（SE(3)）和三维点坐标，最小化重投影误差。
方法：
- 将位姿表示为SE(3)，通过李代数se(3)更新位姿。
- 使用高斯-牛顿或LM算法迭代优化。

2. SLAM中的位姿图优化

问题：优化机器人轨迹中的位姿节点，使相邻位姿的变换与观测一致。
方法：
- 每个位姿节点用SE(3)表示，边用SE(3)或so(3)表示约束。
- 通过李代数计算误差和雅可比矩阵。

3. 机器人运动控制

问题：控制机器人末端执行器到达目标位姿。
方法：
- 计算目标位姿与当前位姿的误差（SE(3））。
- 通过李代数将误差映射到控制输入（如关节速度）。

五、代码示例（C++ with Sophus）

#include <iostream>
#include <Eigen/Core>
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

int main() {
    // SO(3) 示例
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Sophus::SO3d SO3_R(R);
    Eigen::Vector3d so3 = SO3_R.log();  // 旋转向量
    std::cout << "Rotation vector (so3): " << so3.transpose() << std::endl;

    // SE(3) 示例
    Eigen::Vector3d t(1, 0, 0);
    Sophus::SE3d SE3_Rt(R, t);
    Eigen::Matrix<double,6,1> se3 = SE3_Rt.log();  // 变换向量
    std::cout << "Transformation vector (se3): " << se3.transpose() << std::endl;

    return 0;
}

六、学习资源

书籍：
- 《Visual SLAM十四讲》（高翔）：第4-5章详细讲解李群李代数。
- 《State Estimation for Robotics》（Timothy D. Barfoot）：第5章。
论文：
- “A Tutorial on SE(3) Transformation Parameterizations and On-Manifold Optimization”（Blanco et al.）。
在线教程：
- Sophus GitHub Wiki
- Eigen与李群李代数

通过掌握李群和李代数，你可以更高效地处理三维空间中的旋转和平移问题，尤其在优化和插值场景中表现优异。建议从SO(3)和SE(3)的基本操作入手，逐步深入到实际应用中。