我们经常构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。表示姿态的李群与表示位姿的李群
上并没有定义加法,而求导数是需要加法的。因此此时如何求导就成了一个需要解决的问题,它们的李代数由向量组成,具有良好的加法运算,因此我们可以利用李代数来解决对位姿的求导问题。
上面看到了,因为
上没有定义加法,所以我们不能求R的导数。但是我们上一节中可以用
通过指数映射来得到
,所以我们可以将求李代数的导数。
BCH公式
但是此处必须说明的一点是,指数映射时,下式是不满足的。
如果上述公式中是标量的话,那么肯定是成立的,但是很遗憾,它是一个矩阵。所以,它到底是什么呢?这需要用到BCH(Baker-Campbell-Hausdorff)公式。完整形式比较复杂,我们此处就不贴出了,直接上有用的。
其中,第一个称为左乘模型,第二个称为右乘模型。
即
到
转换时求得的
称为左乘近似雅可比:
它的逆为:
而右乘雅可比仅需要对自变量取负号即可:
此时,如果将上面的结果反过来,在李代数上做加法,则有(最有用的):
在
上的运算与
类似,只是雅可比矩阵的求法稍有不同。其实我们发现上面的左乘雅克比与右乘雅克比都有些复杂,其实下面我们会发现,如果使用扰动模型进行求导根本用不到它们!因此这里直接跳过它们的内容求解。
基于李代数对姿态求导
本来我们想求得是
但是,不再是旋转矩阵。因此我们需要另想办法。有了上面的工作,下面我们就可以完成基于李代数的求导操作了,此处介绍两种模型,因为扰动模型的形式会简单一些,因此大家一般都是使用扰动模型,微分模型也了解一下
利用BCH公式的微分模型求导
李代数上的微分模型
用李代数表示姿态,然后根据李代数加法来对李代数求导。即传统求导的思路,把增量直接定义在李代数上。
最后一步是将exp(x)近似为1+x,得到
的反对称矩阵,利用交换定理变为
,RP取负反对称矩阵,上下的
相消
扰动模型求导
对李群左乘或者右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。即把增量扰动直接添加在李群上,然后使用李代数表示此扰动
SO(3)李代数上的扰动模型(左扰动)
这一种方法直接将小量乘在李群上,而不是李代数上。并且需要注意的是这种模型需要区别是左乘还是右乘。左扰动模型比微分模型要少一个雅可比矩阵。
SE(3)上的扰动模型
含义是:考虑一个空间点P(需要是齐次坐标,否则维数不对),受到刚体变换T,得到。下面要求解
是如何随着T变化的。
其中,将一个
4×4
的矩阵变换成一个4×6
的矩阵
假设如下求导中左乘的扰动项的李代数为,那么:
上面最后一行矩阵除法,与矩阵乘法规则类似,只是乘号变成了除号。其使用一个4×1矩阵除以一个1×6矩阵得到一个4×6矩阵
总结
我们讲解了如何对位姿进行求导,这是后面对位姿进行非线性优化的基础。
引入李群李代数的意义:第一个是因为在欧式变换矩阵上不好定义导数,引入李群李代数使得导数定义变得自然合理;第二个是本来旋转矩阵与欧式变换矩阵具有本身的约束,使得将它们作为优化变量会引入额外约束,通过李群李代数可以使得问题变成一个无约束的优化问题。