李代数求导与扰动模型在位姿优化中的作用

1万 · 2023-3-8 14:36

我们经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。表示姿态的李群

与表示位姿的李群

上并没有定义加法，而求导数是需要加法的。因此此时如何求导就成了一个需要解决的问题，它们的李代数由向量组成，具有良好的加法运算，因此我们可以利用李代数来解决对位姿的求导问题。

上面看到了，因为

上没有定义加法，所以我们不能求R的导数。但是我们上一节中可以用

通过指数映射来得到

，所以我们可以将求李代数的导数。

1万 · 2023-3-8 14:39

BCH公式

但是此处必须说明的一点是，指数映射时，下式是不满足的。

如果上述公式中是标量的话，那么肯定是成立的，但是很遗憾，它是一个矩阵。所以，它到底是什么呢？这需要用到BCH(Baker-Campbell-Hausdorff)公式。完整形式比较复杂，我们此处就不贴出了，直接上有用的。

其中，第一个称为左乘模型，第二个称为右乘模型。

即到转换时求得的称为左乘近似雅可比:

它的逆为:

而右乘雅可比仅需要对自变量取负号即可:

此时，如果将上面的结果反过来，在李代数上做加法，则有（最有用的）:

在

上的运算与

类似，只是雅可比矩阵的求法稍有不同。其实我们发现上面的左乘雅克比与右乘雅克比都有些复杂，其实下面我们会发现，如果使用扰动模型进行求导根本用不到它们！因此这里直接跳过它们的内容求解。

1万 · 2023-3-8 14:41

基于李代数对姿态求导

本来我们想求得是

但是，不再是旋转矩阵。因此我们需要另想办法。有了上面的工作，下面我们就可以完成基于李代数的求导操作了，此处介绍两种模型，因为扰动模型的形式会简单一些，因此大家一般都是使用扰动模型，微分模型也了解一下　

1万 · 2023-3-8 14:41

利用BCH公式的微分模型求导李代数上的微分模型

用李代数表示姿态，然后根据李代数加法来对李代数求导。即传统求导的思路，把增量直接定义在李代数上。

最后一步是将exp(x)近似为1+x，得到的反对称矩阵，利用交换定理变为，RP取负反对称矩阵，上下的相消

1万 · 2023-3-8 14:41

扰动模型求导

对李群左乘或者右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。即把增量扰动直接添加在李群上，然后使用李代数表示此扰动

SO(3)李代数上的扰动模型(左扰动)

这一种方法直接将小量乘在李群上，而不是李代数上。并且需要注意的是这种模型需要区别是左乘还是右乘。左扰动模型比微分模型要少一个雅可比矩阵。

1万 · 2023-3-8 14:42

SE(3)上的扰动模型

含义是：考虑一个空间点P（需要是齐次坐标，否则维数不对），受到刚体变换T，得到。下面要求解是如何随着T变化的。

其中，将一个4×4的矩阵变换成一个4×6的矩阵

假设如下求导中左乘的扰动项的李代数为，那么：

上面最后一行矩阵除法，与矩阵乘法规则类似，只是乘号变成了除号。其使用一个4×1矩阵除以一个1×6矩阵得到一个4×6矩阵

1万 · 2023-3-8 14:42

总结

我们讲解了如何对位姿进行求导，这是后面对位姿进行非线性优化的基础。

引入李群李代数的意义：第一个是因为在欧式变换矩阵上不好定义导数，引入李群李代数使得导数定义变得自然合理；第二个是本来旋转矩阵与欧式变换矩阵具有本身的约束，使得将它们作为优化变量会引入额外约束，通过李群李代数可以使得问题变成一个无约束的优化问题。