SLAM中的非线性优化-2D图优化之三轴IMU预积分（九）

最新推荐文章于 2025-05-11 23:27:05 发布

zl_vslam

最新推荐文章于 2025-05-11 23:27:05 发布

阅读量683

点赞数 10

分类专栏： SLAM中非线性优化的从古至今文章标签：人工智能计算机视觉机器学习 SLAM SE2 非线性优化算法

本文链接：https://blog.csdn.net/zl_vslam/article/details/147718132

版权

SLAM中非线性优化的从古至今专栏收录该内容

9 篇文章

订阅专栏

本节开始讲解，预积分的边用于优化，看看优化过程咋使用预积分,再求解前，继续给出SO2一些基础知识，在SLAM（Simultaneous Localization and Mapping）中，SO(2) 的李代数（Lie algebra）表示二维旋转群的切空间，通常用于描述二维旋转的局部扰动或增量。以下是关于 SO(2) 李代数的对数映射（logarithm）的详细说明：

1. SO(2) 的定义

SO(2) 是二维旋转矩阵的群，定义为：

SO(2)={R∈R2×2 ∣ R⊤R=I, det⁡(R)=1}

每个旋转矩阵可以表示为：

R=[cos⁡θ −sin⁡θ

sin⁡θ cos⁡θ]

其中 θ是旋转角度。

2. SO(2) 的李代数

SO(2) 的李代数 so(2) 是其切空间，由所有反对称矩阵组成：

so(2)={w∈R∣

w = [0 −w

w 0]

}

实际上，so(2)是一维的，可以用标量 w 表示（因为反对称矩阵的自由度为 1）

3. 指数映射（exp⁡:so(2)→SO(2)）

李代数到李群的映射通过指数映射实现：

exp⁡(w) = R = [cos⁡w −sin⁡w

sin⁡w cos⁡w]

这里 w直接对应旋转角度 θθ。

4. 对数映射（log⁡:SO(2)→so(2）

对数映射是指数映射的逆运算，从旋转矩阵 R 中提取李代数元素 w：

log⁡(R) = w = atan2(R21, R11)

其中：

R11 = cos⁡θ 是旋转矩阵的第一行第一列元素，
R21=sin⁡θ 是第二行第一列元素，
atan2 是四象限反正切函数，返回角度 θ∈(−π,π]。

标量 w 即为旋转角度 θ，对应的李代数为：

w = [0 −θ

θ 0]

5. 在 SLAM 中的应用

在 SLAM 中，SO(2) 的对数映射通常用于：

优化问题：将旋转矩阵转换为李代数空间，便于在切线空间中计算雅可比矩阵或梯度。
扰动模型：对旋转施加局部扰动 Δθ，形式为 R←R⋅exp⁡(Δθ)
插值：在李代数空间中线性插值旋转（如 w=t⋅θ）。

6. 反对称性质

$\delta \phi ^{\times }=1^{\times }\cdot \delta \phi=\begin{bmatrix} 0 &-1 \\ 1 & 0 \end{bmatrix}\cdot \delta \phi$

一. 预积分残差

在 IMU 相关的应用中，通常把每个时刻的状态建模为包含旋转、平移、线速度、IMU 零偏的
变量，构成状态变量集合 X ：

xk = [R, p, v, ba , bg ] k ∈ X

其中，残差项表示为deltaR, deltaP, deltaV, 参考《视觉融合里程计SLAM算法SE2Lam解析-论文篇》以及预积分（2-1），（2-2），（2-3）三式

$r_{\phi _{i}^{j}}=\phi _{j}-\phi _{i}-\bigtriangleup \phi _{i}^{j}$ （30）

$r_{v_{i}^{j}}=\Phi^{T} \left ( \phi _{i} \right )\left ( v_{j}-v_{i} \right )-\bigtriangleup v_{i}^{j}$ （31）

$r_{p_{i}^{j}}=\Phi^{T} \left ( \phi _{i} \right )\left ( p_{j}-p_{i}-v_{i}\cdot dt \right )-\bigtriangleup p_{i}^{j}$ （32）

其中

$\Phi^{T} \left ( \phi _{i} \right )=\Phi \left ( -\phi _{i} \right )$

通常我们会把 r 统一写成一个 5 维的残差变量。它表面上关联两个时刻的旋转、平移、线速度，但
由于预积分观测内部含有 IMU 零偏，所以实际也和 i 时刻两个零偏有关。在优化过程中，如果对 i
时刻的零偏进行更新，那么预积分观测量也应该线性地发生改变，从而影响残差项的取值。所以，
尽管在这个残差项里似乎不含有 bai , bwi ，它们显然是和残差相关的。

二. 预积分的雅可比矩阵

最后我们来讨论预积分相对状态变量的雅可比矩阵。由于预积分测量已经归纳了 IMU 在短时
间内的读数，残差相对于状态变量的雅可比推导显得十分简单，下面我们来推导它们。如下图所示，图模型如下：

2.1 首先考虑旋转矩阵，式（30）

旋转与 Ri , Rj 和 bwi 有关。由（30）式我们来推导它：

对 ϕi和ϕj的直接观测，因此的导数为：

$\frac{\partial r_{\phi _{i}^{j}}}{\partial \phi _{i}}=-1$ (33)

$\frac{\partial r_{\phi _{i}^{j}}}{\partial \phi _{j}}=1$ (34)

这些推导和位姿图的非常相似。而对于零偏量要稍微麻烦一些。注意在优化过程中，零偏量应该不断地更新，而每次更新时我们会利用(13)式来修正预积分的观测量。由于这个过程是不断进行的，我们总会有一个初始的观测量和当前修正后的观测量，在推导时必须考虑到这一点。旋转与p, v， ba无关，因此对其导数都是0，但是陀螺仪偏置不能忽略，delta_phi与陀螺仪偏置相关。

假设优化初始的零偏为 b wi ，在某一步迭代时，我们当前估计出来的零偏修正为 δ bw i ，而当

前修正得到的残差也需要加上相应的修正量，。为了求导，我们又在上面两项基础上加上了 δ ˜ bw i，那么参考 上一讲（25）式，只需添加负号即可：

$\frac{\partial r_{\phi _{i}^{j}\left (b_{wi}+\delta b_{wi} \right )}}{\partial \phi _{i}}=-(j-i)\cdot dt$ （35）

2.2 其次考虑速度项，式（31）

速度残差与vi , vj 呈线性关系，因此求导后就剩个旋转矩阵相关量；

$^{\frac{\partial r_{v_{i}^{j}}}{\partial v_{i}}}=-\Phi^{T} \left ( \phi _{i} \right )$ (36)

$^{\frac{\partial r_{v_{i}^{j}}}{\partial v_{j}}}=\Phi^{T} \left ( \phi _{i} \right )$ (37)

对旋转求导，j时刻不相关，雅可比为0，只需求i时刻，这里要用到扰动模型，为了简化，用右扰动，重新定义残差

$r_{v_{i}^{j}\left ( \Phi \left ( \phi _{i}exp(\delta \phi ) \right ) \right )^{T}}=(\Phi \left ( \phi _{i} \right )exp(\delta \phi ))^{T}\left ( v_{j}-v_{i} \right )-\bigtriangleup v_{i}^{j}$

其中 delta_phi为小量，利用泰勒展开分解其相关的项可得

$r_{v_{i}^{j}\left ( \Phi \left ( \phi _{i}exp(\delta \phi ) \right ) \right )^{T}}=(\Phi \left ( \phi _{i} \right )(I+\delta \phi^{\wedge } ))^{T}\left ( v_{j}-v_{i} \right )-\bigtriangleup v_{i}^{j}$

将转置展开

$r_{v_{i}^{j}\left ( \Phi \left ( \phi _{i}exp(\delta \phi ) \right ) \right )^{T}}=(I-\delta \phi^{\wedge } )(\Phi \left ( \phi _{i} \right ))^{T}\left ( v_{j}-v_{i} \right )-\bigtriangleup v_{i}^{j}$

继续展开，并利用反对称性质

$r_{v_{i}^{j}\left ( \Phi \left ( \phi _{i}exp(\delta \phi ) \right ) \right )}=r_{v_{i}^{j}\left ( \Phi \left ( \phi _{i} \right ) \right )}-1^{\times }\cdot (\Phi \left ( \phi _{i} \right ))^{T}\left ( v_{j}-v_{i} \right )\delta \phi -\bigtriangleup v_{i}^{j}$

因此导数为

$\frac{\partial r_{v_{i}^{j}}}{\partial \phi _{i}}=-1^{\times }\cdot (\Phi \left ( \phi _{i} \right ))^{T}\left ( v_{j}-v_{i} \right )$ (38)

两个偏置项只与delta_vij有关，因此只需对（21）（22）式添加负号即可；

2.3 最后考虑速度项，式（32）

平移部分和 p i , p j , v i , R i 以及两个零偏有关。然而，它们的关系大多为线性关系，雅可比很容易推出。

$^{\frac{\partial r_{p_{i}^{j}}}{\partial p_{i}}}=-\Phi^{T} \left ( \phi _{i} \right )$ (39)

$^{\frac{\partial r_{p_{i}^{j}}}{\partial p_{j}}}=\Phi^{T} \left ( \phi _{i} \right )$ (40)

$^{\frac{\partial r_{p_{i}^{j}}}{\partial v_{i}}}=-\Phi^{T} \left ( \phi _{i} \right )dt$ (41)

相对旋转的导数

$\frac{\partial r_{p_{i}^{j}}}{\partial \phi _{i}}=-1^{\times }\cdot (\Phi \left ( \phi _{i} \right ))^{T}\left ( p_{j}-p_{i}-v_{i}dt \right )$ (42)