智能驾驶基础理论总结

如果把汽车作为一个刚体，设计汽车的自动驾驶系统将会涉及到刚体运动、李群、李代数等基础知识...本文即是对这些知识进行简单归纳总结。

一、刚体运动

1、一个刚体(汽车)在三维空间是如何运动的，这涉及到以下知识：
-- 旋转矩阵、四元数、欧拉角的意义，以及它们是如何运算和转换的
-- 线性代数库 Eigen

2、刚体运动可以拆解为两个部分来描述：旋转和平移
-- 旋转可以由旋转矩阵R表示，平移由平移向量t表示，旋转和变换可以用变换矩阵T表示

二、旋转矩阵与旋转向量

1、旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转
-- 正交矩阵：R-1 = RT
-- a′ = RR-1a = RT a
-- RT 刻画了一个相反的旋转

2、旋转矩阵和旋转向量之间的关系
假设有一个旋转轴为 n，角度为 θ 的旋转，显然，它对应的旋转向量为 θn
由旋转向量到旋转矩阵的过程由罗德里格斯公式（Rodrigues’s Formula ）表明：

-- 符号 ∧ 是向量到反对称的转换符
-- 旋转矩阵与旋转向量是指数映射关系

三、李群

指具有连续（光滑）性质的群
像整数群 Z 那样离散的群没有连续性质，所以不是李群
-- 而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群
-- 由于 SO(3) 和 SE(3) 对于相机姿态估计尤其重要，我们主要讨论这两个李群

1、旋转除了表示之外，还涉及估计和优化
因为在 SLAM 中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题
-- 一种典型的方式是把它构建成一个优化问题，求解最优的 R, t，使得误差最小化。
-- 旋转矩阵自身是带有约束的（正交且行列式为 1）。它们作为优化变量时，会引入额外的约束，使优化变得困难
   -- 这篇文章举了SVM的例子，说明优化变量引入额外约束，优化会变得困难：https://ming-lian.github.io/2019/05/03/Understand-MachineLearning/
-- 通过李群——李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式

2、智能驾驶基础种的李群
三维旋转矩阵构成了特殊正交群SO(3)
变换矩阵构成了特殊欧式群SE(3)
不同时刻的旋转矩阵R组成的集合即是一种李群
-- 是的，李群是一种集合的规范，符合这个规范的都是李群

四、李代数

李代数由一个集合 V，一个数域 F 和一个二元运算 [,] 组成。如果它们满足以下几条性质，称 (V, F, [, ]) 为一个李代数，记作 g。
-- 1. 封闭性 ∀X,Y ∈ V, [X,Y ] ∈ V.
-- 2. 双线性 ∀X,Y , Z ∈ V, a, b ∈ F, 有：
   -- [aX + bY , Z] = a[X, Z] + b[Y , Z], [Z, aX + bY ] = a[Z, X] + b[Z,Y ].
-- 3. 自反性 ∀X ∈ V, [X, X] = 0
   -- 自反性是指自己与自己的运算为零
-- 4. 雅可比等价 ∀X,Y , Z ∈ V, [X, [Y , Z]] + [Z, [Y , X]] + [Y , [Z, X]] = 0.

1、二元运算 [, ]     

[,] 中二元运算被称为李括号
-- 它不要求结合律，而要求元素和自己做李括号之后为零的性质
-- 三维向量 R3 上定义的叉积 × 是一种李括号
   -- 也即是，此时的[,]即是叉积：×
-- 因此 g = (R3, R, ×) 构成了一个李代数

2、每个李群都对应一个李代数
-- 李代数反应李群中元素的定义域、运算规则
-- 旋转矩阵R组成的集合是李群
-- R对应的 旋转矩阵ϕ组成的集合 和 对应的ϕ的运算规则 即是李括号，表示ϕ与ϕ之间的能进行的运算逻辑，这就是李代数

3、李代数的作用：使用李代数中的元素和运算规则，表示出李群中的元素

五、指数映射与对数映射、及其作用

即是指数映射是从李代数表示出李群
对数映射是李群表示出李代数

1、指数映射

exp(ϕ∧)
-- 这是一个矩阵的指数
-- 在李群和李代数中，称为指数映射（Exponential Map）

指数映射只是一个满射
-- 这意味着每个SO(3) 中的元素，都可以找到一个 so(3) 元素与之对应；
-- 但是可能存在多个 so(3) 中的元素，对应到同一个 SO(3)。
-- 至少对于旋转角 θ，我们知道多转 360 度和没有转是一样的
   ——它具有周期性。
   
但是，如果我们把旋转角度固定在 ±π 之间，那么李群和李代数元素是一一对应的。

2、对数映射

我们可以类比推得对数映射而求出李代数se(3)中的元素
-- 不过根据变换矩阵 T 求 so(3) 上的对应向量(ξ)也有更省事的方式分别求出se(3)中的各个元素
-- 从左上的 R 计算旋转向量(ϕ)，而右上的 t 满足：
-- t = Jρ.
-- 由于 J 可以由 ϕ 得到，所以这里的 ρ 亦可由此线性方程解得
-- 即是J由R(R求出ϕ)已知，t是平移向量也一直，由此可以解出ρ