引言
旋转矩阵内的各个元素取值收到内在约束,即旋转矩阵为正交矩阵,且行列式为1。在采用优化方法进行SLAM位姿估计时,这种内在约束让优化求解变的非常困难。因此这里需要引入李群、李代数的概念,通过李群-李代数的转换可以去除这种内在约束,简化优化求解过程。
群
群(group)是一种集合加上一种运算的代数结构。我们把集合记作A,运算记作 ,那么群可以记作。群要求这个运算满足以下几个条件:
- 封闭性:
- 结合律:
- 幺元:
- 逆:
李群是指具有连续(光滑)性质的群,也就是说李群是群,并且该群是连续光滑的。旋转矩阵R和转移矩阵T满足群的定义,所以旋转矩阵R所构成的群称为特殊正交群,转移矩阵T所构成的群称为特殊欧式群。值得注意的是,和在加法运算上不封闭,即, ,而在乘法运算上封闭,即, 。
由于刚体能在空间中连续运动,也就是说描述其运动的旋转矩阵R和转移矩阵T也具有连续(光滑)的性质,因此和也是李群。那么李群 和 的数学描述如式(1)和式(2)所示。
(1)
(2)
李代数
李代数与李群相对应,李代数由一个集合、一个数域和一个二元运算组成,这里的二元运算[,]也成为李括号。如果它们满足以下几条性质,则称为一个李代数,记作。
- 封闭性
- 双线性 ,有
- 自反性 .
- 雅可比等价
机器人中最常用的李群是和,其对应的李代数分别为和,这里的记法是李群用大学字母,对应的李代数用相应的小写字母。这里直接给出李代数和的数学描述,如式(3)和式(4)所示。
(3)
其中,李括号形式为:
(4)
其中李括号形式为:
李代数描述了李群的局部性质,每个李群都有相对应的李代数。李代数对应李群的切(tangent)空间,它描述了李群局部的导数。
指数与对数映射
李代数通过指数映射转换成李群,李群通过对数映射转换成李代数。下面直接给出李群和及其对应的李代数和的互相转换。和
相互转换如式(5)和式(6)所示
(5)
其中:
(6)
其中:
- 求a,解特征方程, 为矩阵R在特征值为1时的特征向量;
- 求,
和相互转换如式(7)和式(8)所示
(7)
其中:
- ,左雅可比。
(8)
其中:
BCH公式
使用李代数的一大动机是进行优化,而在优化过程中导数是非常必要的信息,我们经常需要知道在李群中两个矩阵相乘,相应的在李代数上会发生什么改变。BCH(Baker-Campbell-Hausdorff)公式给出了答案。BCH公式如式(9)所示
(9)
其中左雅可比矩阵为:
它的逆为:
右乘雅可比矩阵仅需要对自变量取负号即可:
(10)
为了方便理解,我们重新描述BCH近似的意义。假定对某个旋转R,对应的李代数为,我们给它左乘一个微小旋转,记作,对应的李代数为。那么,在李群上,得到的结果就是,而在李代数上,根据BCH近似,为。合并起来,可以简单的写成:
(11)
反之,如果我们在李代数上进行加法,让一个加上,那么可以近似为李群上带左右雅可比的乘法:
(12)
同样的,对于,也有类似的BCH近似:
(13)
李代数求导
首先考虑上的求导,某个空间点p,经过旋转矩阵R(对应的李代数是)的变换后的位姿为。求目标位姿关于旋转矩阵R的导数。这里采用左扰动模型,通过给旋转矩阵R左乘一个微小扰动,对应的李代数是。然后对求导,即
考虑上的求导。假设某空间点p经过一次变换T(对应的李代数为),得到Tp,现在给T左乘一个微小扰动,设扰动项的李代数为,那么: