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

原创已于 2025-05-24 19:39:34 修改 · 1.6k 阅读

42 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

收录于

SLAM中非线性优化的从古至今

于 2025-05-03 21:51:06 首次发布

四足机器人 SLAM 导航实战

从零实现 Unitree Go2 的 SLAM 建图与 ROS2 导航，手把手集成 slam_toolbox

一键订阅

本节开始讲解，优化过程中偏置更新后预积分处理过程，前边推导都是假设i时刻IMU的零偏不变。由直接积分可以看到，优化时候i时刻偏置因为在状态中，也会随之改变，如果零偏改变，预积分就要重新计算，但这样就与直接积分没有多大区别了，因此这里做写技巧性的改变，假定与积分观测是随着零偏线性变化的。当i时刻零偏变化时，只需在原先的观测量上进行修正。接下来看如何操作。

一、预积分观测修正

给出SO(2)部分相关性质：SO(2) 表示二维旋转群，其对应的李代数是 ‌实数轴‌ R。对于二维旋转：

旋转矩阵形式：R(θ)=[cos⁡θ −sin⁡θ
sin⁡θ cos⁡θ] ∈ SO(2)
李代数元素 θ ∈ Rθ ∈ R，指数映射为：exp⁡(θ) = R(θ) .

‌关键性质‌：

‌SO(2) 是交换群‌（阿贝尔群）：
exp⁡(a) ⋅ exp⁡(b) = exp⁡(b) ⋅ exp⁡(a) = exp⁡(a+b)
‌李代数加法直接对应旋转叠加‌：
exp⁡(a+b) = exp⁡(a) ⋅ exp⁡(b)

3. ‌对比 SO(3) 的情况‌

与三维旋转群 ‌SO(3)‌ 不同，SO(2) 的交换性使得分解变得简单：

性质	SO(2)	SO(3)
‌交换性‌	可交换 exp⁡(a) . exp⁡(b) = exp⁡(b) . exp⁡(a)	不可交换（依赖BCH公式）
‌李括号‌	[a,b]=0	[ω1,ω2]=ω1×ω2
‌指数映射分解‌	exp⁡(a+b) = exp⁡(a) . exp⁡(b))	exp⁡(a+b) ≈ exp⁡(a) . exp⁡(b) . exp⁡(−[a,b]/2)

性质

SO(2)

SO(3)

‌交换性‌

可交换

exp⁡(a) . exp⁡(b) = exp⁡(b) . exp⁡(a)

不可交换（依赖BCH公式）

‌李括号‌

[a,b]=0

[ω1,ω2]=ω1×ω2

‌指数映射分解‌

exp⁡(a+b) = exp⁡(a) . exp⁡(b))

exp⁡(a+b) ≈ exp⁡(a) . exp⁡(b) . exp⁡(−[a,b]/2)

注意上述性质，我已编程验证过，此处直接使用即可

先上结论，由上节中的公式（3）(4) (5)可得

$\phi _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )= \phi _{i}^{j}\left ( b_{wi} \right )-\sum _{k=i}^{j-1}(\delta b_{wk}\cdot dt)= \phi _{i}^{j}\left ( b_{wi} \right )-(j-i)\cdot \delta b_{wi}\cdot dt$

写成旋转矩阵形式

$R _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )= R _{i}^{j}\left ( b_{wi} \right )Exp(\frac{\partial R_{i}^{j}}{\partial b_{wi}}\cdot \delta b_{wi})$

$v _{i}^{j}\left ( b_{wi}+\delta b_{wi}, b_{ai}+\delta b_{ai}\right )=v _{i}^{j}\left ( b_{wi}, b_{ai}\right )+\frac{\partial v_{i}^{j}}{\partial b_{wi}}\delta b_{wi}+\frac{\partial v_{i}^{j}}{\partial b_{ai}}\delta b_{ai}$

$p_{i}^{j}\left ( b_{wi}+\delta b_{wi}, b_{ai}+\delta b_{ai}\right )=p _{i}^{j}\left ( b_{wi}, b_{ai}\right )+\frac{\partial p_{i}^{j}}{\partial b_{wi}}\delta b_{wi}+\frac{\partial p_{i}^{j}}{\partial b_{ai}}\delta b_{ai}$ （13）

上式除了旋转部分与六轴IMU预积分有区别外，速度，位置均保持一致，主要看这两项对应雅可比怎么求解。

1. 先看旋转矩阵部分

有了上述SO（2）性质，此处就将变得很简单，将（3）式变为exp形式

$R _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )= \sum_{k=i}^{j-1}Exp((w_{k}-(b_{wi}+\delta b_{wi}))dt)$

将噪声delta分离出来

$R _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )= \sum_{k=i}^{j-1}Exp((w_{k}-b_{wi})dt)Exp\left ( -\delta b_{wi} dt\right )$

将求和符号（实际式乘法）拆开成一项一项的可得

$R _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )=Exp((w_{i}-b_{wi})dt)Exp\left ( -\delta b_{wi} dt\right )\cdot Exp((w_{i+1}-b_{wi})dt)Exp\left ( -\delta b_{wi} dt\right )\cdot Exp((w_{i+2}-b_{wi})dt)Exp\left ( -\delta b_{wi} dt\right ).......$

再利用交换性，把含delta的往后排，不含delta的往前提，再利用指数映射性质可以看到

$R _{i}^{j}\left ( b_{wi}+\delta b_{wi} \right )=R_{i}^{j}\cdot Exp((j-i)\cdot \delta b_{wi}\cdot dt))=Exp(\phi _{i}^{j}-(j-1)\cdot \delta b_{wi}dt)$

由上式可见，与（13）中直接相加的结论完全一致。符号写着写着写串了，再改回来可得

$\Phi \left ( \phi_{i}^{j}\left ( b_{wi}+\delta b_{wi} \right ) \right )=\Phi \left ( \phi _{i}^{j} \right )\cdot \Phi\left ( \left ( j-i \right )\delta b_{wi}\cdot dt \right )$ （14）

因此

$\frac{\partial \Phi \left ( \phi _{i}^{j} \right )}{\partial b_{wi}}=\left ( j-i \right )\cdot dt$ （15）

2. 再看速度项

仿照（2）式，给（1）式中第二个公式速度项，左右同乘以i时刻旋转矩阵的逆，并将（13）中更新后的角度带入可得,目的式要将上式变为（3）（4）（5）式后加减一系列跟delta相关的量，因此这里旋转部分需要分离delta_bwi

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=\sum_{k=i}^{j-1}\Phi \left ( \phi_{i}^{k}\left ( b_{wi}+\delta b_{wi} \right ) \right )\left ( a_{k}-b_{ai}-\delta b_{ai} \right )dt$

直接将其展开为含delta跟不含delta的项，利用（14）式，展开

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=\sum_{k=i}^{j-1}\Phi \left ( \phi_{i}^{k} \right )\cdot\Phi \left (\left ( k-i \right ) \delta _{wbi}\cdot dt \right )\left ( a_{k}-b_{ai}-\delta b_{ai} \right )dt$

实际上，这里先用微分形式表示，将（15）带入

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=\sum_{k=i}^{j-1}\Phi \left ( \phi_{i}^{k} \right )\cdot\Phi\left ( \frac{\partial \Phi \left ( \phi _{i}^{k} \right )}{\partial \delta _{bwi}}\delta _{bwi} \right )\left ( a_{k}-b_{ai}-\delta b_{ai} \right )dt$

将第二个带微分的用一阶泰勒展开

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=\sum_{k=i}^{j-1}\Phi \left ( \phi_{i}^{k} \right )\cdot\left (I+ (\frac{\partial \Phi \left ( \phi _{i}^{k} \right )}{\partial \delta _{bwi}}\delta _{bwi})^{\wedge } \right )\left ( a_{k}-b_{ai}-\delta b_{ai} \right )dt$

括号展开即可获得速度的递推形式

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=v_{i}^{j}\left (b_{ai} \right )-\sum _{k=i}^{j-1}\Phi \left ( \phi _{i}^{k} \right )\delta b_{ai}dt-\sum_{k=i}^{j-1}\Phi \left ( \phi _{i}^{k} \right )\cdot 1^{\times }\cdot \left ( \frac{\partial \Phi \left ( \phi _{i}^{k} \right )}{\partial \delta b_{wi}} \cdot \delta b_{wi}\right )\left ( a_{k}-b_{ai} \right )dt$ （16）

最后写成微分形式

$v _{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=v _{i}^{j}\left ( b_{ai}\right )+\frac{\partial v_{i}^{j}}{\partial b_{wi}}\delta b_{wi}+\frac{\partial v_{i}^{j}}{\partial b_{ai}}\delta b_{ai}$ （17）

注意陀螺仪的偏置及雅可比相关项，均为1x1的数字，可以随意移项，因此对比含相应元素的项，很容易可以得出各项的雅可比；主要就对比（16）和（17）式即可

3. 最后看平移部分

由公式（2），并将（17）式带入，替换速度项，（14）（15）替换旋转部分，可参考上节（3. 再看位置部分，直接由（2）式推导）

$p_{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=\sum_{k=i}^{j-1}\left ( \left ( v_{i}^{j}\left ( b_{ai} \right )+\frac{\partial v_{i}^{j}}{\partial b_{wi}}\cdot \delta b_{wi}+\frac{\partial v_{i}^{j}}{\partial b_{ai}}\cdot \delta b_{ai} \right )\cdot dt +\frac{1}{2}\cdot \Phi \left ( \phi _{i}^{k} \right )\cdot \left (I+(\frac{\partial \Phi \left ( \phi _{i}^{k} \right )}{\partial \delta b_{wi}}\delta b_{wi} \right )^{\wedge })\cdot \left (a_{k}-b_{ai}-\delta b_{ai} \right )\cdot dt^{2}\right )$

展开，并合并同类项，忽略二阶小量可得

$p_{i}^{j}\left ( b_{ai}+\delta b_{ai}\right )=p_{i}^{j}\left ( b_{ai}\right )+\sum_{k=i}^{j-1}\left ( \frac{\partial v_{i}^{j}}{\partial b_{ai}}\cdot dt-\frac{1}{2}\Phi \left ( \phi _{i}^{j} \right )dt^{2} \right )\delta b_{ai}+\sum_{k=i}^{j-1}\left ( \frac{\partial v_{i}^{j}}{\partial b_{wi}}dt-\frac{1}{2}\cdot \Phi \left ( \phi _{i}^{j} \right )\cdot 1^{\times }\cdot \frac{\partial \Phi \left ( \phi _{i}^{j} \right )}{\partial \delta b_{wi}}\cdot \left ( a_{k}-b_{ai} \right )\cdot dt^{2}\right )\cdot \delta b_{wi}$ （18）

因此

$p_{i}^{j}\left (b_{ai}+\delta b_{ai}\right )=p _{i}^{j}\left (b_{ai}\right )+\frac{\partial p_{i}^{j}}{\partial b_{wi}}\delta b_{wi}+\frac{\partial p_{i}^{j}}{\partial b_{ai}}\delta b_{ai}$ （19）

最后，把以上所求的雅可比矩阵整理一下，得到如下的结果：

（一）旋转雅可比由（15）所得

$\frac{\partial \Phi \left ( \phi _{i}^{j} \right )}{\partial b_{wi}}=\left ( j-i \right )\cdot dt$ （20）

(二) 速度雅可比由（16）（17）对比而来

$\frac{\partial v_{i}^{j}}{\partial b_{ai}}=-\sum _{k=i}^{j-1}\Phi \left ( \phi _{i}^{k} \right )dt$ (21) $\frac{\partial v _{i}^{j}}{\partial b_{wi}}=-\sum_{k=i}^{j-1}\Phi \left ( \phi _{i}^{k} \right )\cdot 1^{\times }\cdot \left ( \frac{\partial \Phi \left ( \phi _{i}^{k} \right )}{\partial \delta b_{wi}} \right )\left ( a_{k}-b_{ai} \right )dt$ (22)

(三) 位置雅可比由（18）（19）对比而来

$\frac{\partial p_{i}^{j}}{\partial \delta b_{ai}}=\sum_{k=i}^{j-1}\left ( \frac{\partial v_{i}^{j}}{\partial b_{ai}}\cdot dt-\frac{1}{2}\Phi \left ( \phi _{i}^{j} \right )dt^{2} \right )$ （23）

$\frac{\partial p_{i}^{j}}{\partial \delta b_{wi}}=\sum_{k=i}^{j-1}\left ( \frac{\partial v_{i}^{j}}{\partial b_{wi}}dt-\frac{1}{2}\cdot \Phi \left ( \phi _{i}^{j} \right )\cdot 1^{\times }\cdot \frac{\partial \Phi \left ( \phi _{i}^{j} \right )}{\partial \delta b_{wi}}\cdot \left ( a_{k}-b_{ai} \right )\cdot dt^{2}\right )$ （24）

由于这几种雅可比都是累加形式，因此很容易写成递推形式，即j时刻和j-1时刻的关系

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

$\frac{\partial v_{i}^{j}}{\partial b_{ai}}=\frac{\partial v_{i}^{j-1}}{\partial b_{ai}}-\Phi \left ( \phi _{i}^{j-1} \right )dt$ (26)

$\frac{\partial v _{i}^{j}}{\partial b_{wi}}=\frac{\partial v _{i}^{j-1}}{\partial b_{wi}}-\Phi \left ( \phi _{i}^{j-1} \right )\cdot 1^{\times }\cdot \left ( \frac{\partial \Phi \left ( \phi _{i}^{j-1} \right )}{\partial \delta b_{wi}} \right )\left ( a_{j-1}-b_{ai} \right )dt$ (27)

$\frac{\partial p_{i}^{j}}{\partial \delta b_{ai}}=\frac{\partial p_{i}^{j-1}}{\partial \delta b_{ai}}+\left ( \frac{\partial v_{i}^{j-1}}{\partial b_{ai}}\cdot dt-\frac{1}{2}\Phi \left ( \phi _{i}^{j-1} \right )dt^{2} \right )$ (28)

$\frac{\partial p_{i}^{j}}{\partial \delta b_{wi}}=\frac{\partial p_{i}^{j-1}}{\partial \delta b_{wi}}+\left ( \frac{\partial v_{i}^{j-1}}{\partial b_{wi}}dt-\frac{1}{2}\cdot \Phi \left ( \phi _{i}^{j-1} \right )\cdot 1^{\times }\cdot \frac{\partial \Phi \left ( \phi _{i}^{j-1} \right )}{\partial \delta b_{wi}}\cdot \left ( a_{j-1}-b_{ai} \right )\cdot dt^{2}\right )$ (29)