VSLAM（2）：李代数求导与扰动模型

聪明的笨小子

已于 2023-07-18 04:33:15 修改

阅读量1.1k

点赞数

CC 4.0 BY-SA版权

分类专栏：视觉SLAM14讲文章标签：自动驾驶 c++ 线性代数

于 2023-06-14 00:20:12 首次发布

本文链接：https://blog.youkuaiyun.com/zhangzhixin1998/article/details/131135710

视觉SLAM14讲专栏收录该内容

10 篇文章

订阅专栏

文章介绍了在SLAM（SimultaneousLocalizationAndMapping）中，如何利用李代数解决旋转矩阵不封闭于加法运算的问题。通过BCH公式和近似表达，当旋转矩阵发生微小变化时，可以用李代数的加法来表示。文章进一步讨论了李代数在求导中的作用，特别是在对旋转矩阵求导时，如何通过扰动模型简化计算过程，以求解最优化问题。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在上一篇提到，因为R和T是定义在一个乘法运算的群上，他们对加法是不封闭的，导致在对其进行求导时会不成立。在引入了李代数这个映射后，那么接下来就是我们感兴趣的对一个旋转矩阵R左乘另外一个旋转矩阵对应到李代数上是什么样的。

一. BCH公式与近似形式

当我们计算两个旋转矩阵相乘时，可以转换为其对应的两个李代数指数映射的形式，即

$R_1 * R_2 = exp(\phi_1^\wedge)*exp(\phi_2^\wedge)$

两个矩阵指数相乘结果可以由BCH(Backer-Campbell-Hausdorff) 公式给出

$ln(exp(A)exp(B)) = A + B +\frac 1{2}[A\cdot B] + \frac 1{12}[A \cdot[A\cdot B]] + ...$

其中 $[ \cdot ]$ 表示A和B所在群的运算。根据该公式可以发现把乘法转换到一个无穷个李括号组成的加法运算上。而当 $\phi^\wedge_1$ 和 $\phi^\wedge_2$ 其中有一项是很小量（其实就是在SLAM中机器人每次运动很小，对应的旋转矩阵和转换矩阵就很小）的时候，那么这个公式的近似表达是：

这说明当其中一项(比如 $\phi_1$ )是小量的时候，结果可以近似成另一项( $\phi_2$ )加上一个 $J_l(\phi_2)^{-1}\phi1$ 。SLAM14讲书中给出了 $J_l$ 表达式：

$J_l = J = \frac{sin\theta}{\theta}I + (1 - \frac{sin\theta}{\theta})\alpha\alpha^T + \frac{1-cos\theta}{\theta}\alpha^\wedge$

而 $J_l$ 的逆表达式为

$J_l^{-1} = \frac{\theta}{2}cot\frac{\theta}{2}I + (1 - \frac{\theta}{2}cot\frac{\theta}{2})\alpha\alpha^T - \frac{\theta}{2}a^\wedge$

其中 $\theta$ 和 $\alpha$ 分别表示旋转向量 $\phi$ 对应的旋转角度和旋转轴。而当 $\phi_2$ 较小时，有

$J_r(\phi) = J_l(-\phi)$

至此，当旋转矩阵R发生一次微小旋转 $\Delta$ R时，在正交群上进行计算应该是 $\Delta R * R$ ,而这个计算映射到李代数上则变成上面表示出来的加法了：

$\Delta R * R = exp(\Delta\phi^\wedge)exp(\phi^\wedge) = exp((\phi + J_l^{-1}(\phi)\Delta\phi)^\wedge)$

而当在李代数上进行加法时，根据上面的公式可以反推出以下公式：

$exp((\phi +\Delta\phi)^\wedge) = exp(J_l\Delta\phi^\wedge)exp(\phi^\wedge) = exp(\phi^\wedge)exp(J_r\Delta\phi^\wedge)$

所以我们就找到了一个用加法表示在机器人位姿不断变化的情况下的位姿表示，这就为后面求导做好了铺垫。

二. 李代数求导

现在返回最初使用李代数的初衷，我们需要使用它来对一个特殊正交群进行求导，以此达到我们想要求解最优化问题的目的。

假设某个时刻机器人的位姿为 $T$ ，此时机器人观测到一个landmark记作P，产生了观测量 $p$ ，那么就有

$Z = Tp + W$

其中W为噪声，所以观测值和观测模型推导出的观测值之间存在误差。那么此时的机器人的最优估计就可以表示为

$min\sum_{i = 1}^{n} ||Z_i - Tp_i||^2_2$ ，其中n表示landmark的总数量。

很明显这是个最小化误差函数的问题，一般的求解思路就是求导，令导数等于零，得到最优解下对应的T。但这里的问题就是对一个关于转换矩阵T函数不能直接求导，因为它对加法并不收敛。所以这时就需要转换到李代数进行求导。

这里举了一个简单的例子来引入李代数求导。考虑空间中的一个点p，经过一次旋转R后，此时的坐标为Rp，那么对Rp对R的求导形式不严谨的表示为 $\frac{\partial Rp}{\partial R}$ 。但是由于特殊正交群对加法不封闭，所以这个式子在李代数上的表示就变为了： $\frac{\partial{(exp(\phi^\wedge)p)}}{\partial \phi}$ 。