SLAM--李群、李代数与姿态估计

关于姿态变换、李群和李代数之间的一些思考

姿态估计

在观测中，我们会得到物体的各个时间的姿态，并且此观测数据中也是包含一定的观测噪声。一般有，其中是世界坐标系的一点，是观测噪声，是观测数据

我们的目的是估计出位姿，也就是计算出，某个时刻下位姿是固定的，如果可以从多个角度或者使用不同的方式进行观测，那么就会有多组观测数据，可以使用类似最小二乘法来估计出一个最小的误差的位姿。

如果是正常情况，因为这是一个凸函数，所以可以直接求导计算出最优解。但是，如果一旦求导，一定会出现加法运算，旋转矩阵对于加法运算不存在封闭性，所以无法直接使用求导来求解。

李群

群即是一个集合+一种运算组成的代数结构，满足封闭性、结合律、单位元、逆四大性质，而李群即具有连续性质的群。

旋转矩阵或欧式变化矩阵都可以构成群，他们对于矩阵乘法运算均满足定义。

对于旋转矩阵而言，对两边求导很容易得到是一个反对称矩阵，可以与一个三维向量一一对应，即

两边右乘，有

即，每次对旋转矩阵求一次导数，相当于左乘一个反对称矩阵，也就是Φ反映了旋转矩阵的导数性质。将其看成是一个微分方程，可以求出通解

通过在0处进行泰勒展开，有，这意味着与是一一对应的的关系

通过可以看出，旋转矩阵和对应的向量之间可以相互计算，这也正是李群和李代数之间的关系

李代数

每个李群都有唯一的李代数相对应，因为公式不好打，就只以正交群SO(3)来说明了（也就是旋转矩阵群）。

李代数描述李群的局部性质，由一个集合、一个数域和一个二元运算符李括号组成。其满足封闭性、双线性、自反性、雅可比性。李代数的李括号一般比较复杂。

比如，SO(3)对应的李代数so(3)，就是一个三维向量，其反对称矩阵就是，与旋转矩阵群的导数存在联系(即前述的对旋转矩阵求导，等于乘上一个李代数对应的反对称矩阵)

指数与对数映射

由于已知李代数，可以用指数映射来求出对应的旋转矩阵，可以使用泰勒公式来化简

由于是一个三维向量，可以定义为模长+旋转角的形式，即

考虑到

和

可以把泰勒展开式拆分重写成sin和cos的泰勒展开式，最终可以得到

而罗德里格斯公式为

所以，经过这样的推导后发现，旋转矩阵群对应的李代数就是旋转向量，或者说，李群代表一个球面，那么球上所有点的切线(即单位元处李群的切空间的李代数)也会组成一个球面，且新组成的这个球面跟原来的球面一样

通过李代数求解李群我们可以使用指数映射，或者罗德里格斯公式
通过李群求解对应的李代数我们可以使用之前的旋转向量与旋转矩阵的关系来求解

欧式变换群也可以推导出类似的指数映射形式，不过多一个雅可比矩阵的系数。

位姿估计模型

我们一开始的问题就是，来计算出运动物体的位置，而当时我们不能直接求解的原因是旋转矩阵群对加法不封闭，所以导出了李群李代数的关系。

且之前我们看到李群和李代数之间是指数/对数映射这样的关系，所以自然希望李群能够被转化成李代数，这样求导之后的加法运算对李代数是封闭的，就能够估计出位姿。但是此时需要保证映射和运算是可交换的，即满足

只有这样，我们才能先映射，再做加法运算。但是事实上，对于矩阵而言，根据BCH公式，当处理两个矩阵的指数的积时，会产生一些由李括号组成的余项，并有以下的近似表达

$较小较小$

其中第一种情况描述左乘一个较小的值；而第二种情况描述右乘一个较小的值，其结果均为自身加上微小量乘以雅可比矩阵。

导数模型

对于旋转变化，我们可以计算旋转后的结果相对旋转矩阵本身的导数。如果空间上的点位，而旋转矩阵为，即求解，并把旋转矩阵用李代数来表示，有

第一行是(变化后-变化前)/小变化，是导数的定义。不过分母是旋转矩阵，所以分子上是旋转矩阵对应的李代数加上一个微小值。为了消去极限符号，需要把李群转换成李代数，用李代数的相加来完成计算，此时涉及到BCH近似，所以会多出一个雅可比矩阵
第二行是BCH的近似线性化
第三行是泰勒展开去掉高阶项
第五行将反对称符号看作叉积，而叉积交换乘积项时需要引入负号

左扰动模型

扰动模型是对进行一次扰动并求出结果相对于扰动的变化率，即，若对应的李代数为，则

可以看到，左扰动模型比导数模型少了一个雅可比矩阵，运算量大幅度降低。其中中间仍然是泰勒展开取前两项。

第一行是(变化后-变化前)/小变化，是导数的定义。不过分母是扰动，所以分子上只需要左乘一个扰动就可以后续消掉了，不涉及到李群李代数之间的近似变换，所以没有雅可比矩阵
由于只有乘法封闭，所以增量用乘法表示
小的扰动是指几乎不变的旋转，所以趋近于单位矩阵

右扰动模型

右扰动模型与左扰动模型类似，只是右乘一个扰动矩阵

与左扰动差不多，就是中间两个乘积换了一下位置而已。

扰动模型比较

无论是导数模型，还是左右扰动模型，其本质都是用李代数表示姿态，然后使用李代数的加法来求导。而使用李代数的原因是李群加法不封闭，而李群和李代数又存在较好的指数映射关系。

不过导数模型是求解该变化量对于旋转矩阵的变化，会多出一个雅可比矩阵。而扰动模型是求解变化量对于增量的变化率，省去了一个雅可比矩阵计算。

李群、李代数、四元数、旋转矩阵、旋转向量

旋转矩阵和旋转向量是描述三维物体运动的方式，只不过描述存在奇异性，所以需要引出四元数来得到一种没有奇异性的描述。

在求解位姿的时候，为了使用类似最小二乘法的方法来求解出位姿，需要不断地去求解导数，而导数会涉及到很多的加法运算，由于旋转矩阵(李群)对于加法不封闭，所以引出了可以描述李群导数性质的李代数。

有了李代数后，由于李群李代数一一对应，且具有指数/对数映射关系，所以可以使用李代数的加法运算去计算李群元素的导数。

在计算导数时，还需要考虑BCH近似映射，可以按照求导的对象不同，划分为导数模型和扰动模型。其中导数模型较为复杂；而扰动模型分为左乘扰动模型和右乘扰动模型，形式较为简洁。