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

原创已于 2025-06-01 19:38:47 修改 · 789 阅读

15 ·

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

GEO检测

收录于

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

于 2025-04-20 10:14:01 首次发布

四足机器人 SLAM 导航实战

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

一键订阅

本讲开始正式三轴imu预积分推导，由于imu预积分较为复杂，因此同样分为多个章节讲解，预积分模型按大方向分，主要为假设噪声传播阶段零偏不更新跟优化过程中零偏更新的处理过程；本节主要讲解噪声传播过程零偏不更新阶段，部分公式需要参考《视觉融合里程计SLAM算法SE2Lam解析-论文篇》和《SLAM中的非线性优化-2D图优化之三轴IMU预积分前传（四）》

一、噪声分离推导

结合前传跟SE2Lam中的公式可记状态传播为：

$S_{k+1}=\begin{bmatrix} p_{k+1}\\ v_{k+1}\\ \phi _{k+1} \end{bmatrix}=\begin{bmatrix} p_{k}+v_{k}\cdot dt+\frac{1}{2}\cdot \Phi \left ( \phi _{k}\right )\left ( a_{k}-b_{a}-\eta _{ak} \right )dt^{2}\\\ v_{k}+\Phi \left ( \phi _{k} \right )\left ( a_{k}-b_{a}-\eta _{ak} \right )dt\\ \phi _{k}+\left ( w_{k}-b_{w}-\eta _{wk} \right )dt \end{bmatrix}$ (1)

上式是两帧间的递推公式，也叫直接积分，仅单帧间的变换，实际上两个关键帧间起码会大于等于两帧，因此推导下通用的公式，时间用i和j时刻表示，为了把直接积分变为预积分，只需再上式两边同时乘以i时刻旋转的逆即可，角度部分直接做减法，记为 $\Phi \left ( -\phi _{i} \right )$

$\Phi \left ( -\phi _{i} \right )p_{k+1}= \Phi \left ( -\phi _{i} \right )p_{k}+\Phi \left ( -\phi _{i} \right )v_{k}dt+\frac{1}{2}\Phi \left ( \phi _{k}-\phi _{i} \right )\left ( a_{k}-b_{a}-\eta _{ak} \right )dt^{2}$

如果有多帧的话

$\Phi \left ( -\phi _{i} \right )p_{j}=\Phi \left ( -\phi _{i} \right )p_{i}+\sum_{k=i}^{j-1}(\Phi \left ( -\phi _{i} \right )v_{k}dt+\frac{1}{2}\Phi \left ( \phi_{i}^{k} \right )(a_{n}-b_{a}-\eta _{an})dt^{2})$ (2)

将pi移项，并合起来，写为如下

$\bigtriangleup p_{i}^{j} =\Phi \left ( -\phi _{i} \right )p_{j}-\Phi \left ( -\phi _{i} \right )p_{i}$

$\bigtriangleup p_{i}^{j} =\sum_{k=i}^{j-1}(\Phi \left ( -\phi _{i} \right )v_{k}dt+\frac{1}{2}\Phi \left ( \phi_{i}^{k} \right )(a_{k}-b_{a}-\eta _{ak})dt^{2})$ (2-0)

实际上式整理下如下

$\bigtriangleup p_{i}^{j} =\sum_{k=i}^{j-1}v_{ik}dt+\frac{1}{2}\Phi \left ( \phi_{i}^{k} \right )(a_{k}-b_{a}-\eta _{ak})dt^{2})$ (2-1)

速度项也同样的道理

$\bigtriangleup v_{i}^{j}=\sum_{k=i}^{j-1}\Phi \left ( \phi_{i}^{k} \right )\left ( a_{k}-b_{a}-\eta _{ak} \right )dt$ (2-2)

唯独旋转部分不需要这么复杂，只需直接累加即可

$\bigtriangleup \phi _{i}^{j}=\sum_{k=i}^{j-1}\left (w_{k}-b_{w}-\eta _{wk}\right )\cdot dt$ (2-3)

因此，关键帧i和j之间的预积分测量和噪声项可表示为：

1. 先看旋转部分

$\phi _{i}^{j}=\sum_{k=i}^{j-1}\left ( \hat{\phi _{k}}-\eta _{wk}dt \right ) =\hat{\phi _{i}^{j}}-\delta \phi _{i}^{j}$ (3)

上式中上尖表示测量值，该式的含义是预积分状态等于测量值减噪声

2. 再看速度部分，可类似于（2）式，将i和j时刻的速度移动到等式左边，并用（3）式右侧替换左侧则：

$v_{i}^{j}=\sum_{k=i}^{j-1}\Phi \left ( \hat{\phi _{i}^{k}} \right )\cdot \Phi \left ( -\delta \phi_{i}^{k} \right )\left ( a_{k}-b_{a}-\eta _{ak} \right )dt \approx \sum\Phi \left ( \phi _{i}^{k} \right )\left ( I-\delta \phi _{i}^{k\times } \right ) \left ( a_{k}-b_{a}-\eta _{ak} \right )dt$

上式对delta小量用了一阶近似，x代表反对称矩阵，由《视觉融合里程计SLAM算法SE2Lam解析-论文篇》中的（18）式决定

$v_{i}^{j}=\sum \Phi \left ( \phi \right )\left ( a-b_{a}-\eta _{ak} \right )dt-\sum \left ( \Phi \left ( \phi \right )\delta \phi ^{\times }( a-b_{a}-\eta _{ak} \right )dt)$

上式省略下标并展开，目的是将含噪声的项放在后边，不含噪声的项放再前面，因此，展开重新组合下

$v_{i}^{j}=\sum \left ( \Phi\left ( \phi \right ) \left ( a-b_{a} \right )dt- \Phi \left ( \phi \right )\eta dt-\Phi\left ( \phi \right )\delta \phi ^{\times }\left ( a-b_{a} \right )+\Phi \left ( \phi \right )\delta \phi ^{\times }\eta dt\right )$

如上述所示，总共有四项组成，最后一项两个噪声小量相乘，为二阶销量，可忽略掉，因此剩下的项重新组合可得

$v_{i}^{j}=\sum \left ( \Phi \left ( \phi \right )\left ( a-b_{a} \right )dt -\Phi \left ( \phi \right )\left ( \eta dt-\delta \phi ^{\times }\left ( a-b_{a} \right )dt \right )\right)$

利用反对称性质a x b^ = -b^ x a，类似于《视觉融合里程计SLAM算法SE2Lam解析-论文篇》中的（21）式，但这里是直接将旋转向量的反对称提取公共部分即可，可把其分离出来,这里给出结论

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

可以推出

$v_{i}^{j}=\sum \left ( \Phi \left ( \phi \right )\left ( a-b_{a} \right )dt -\Phi \left ( \phi \right )\left ( \eta dt-1 ^{\times }\left ( a-b_{a} \right )\cdot \delta \phi \cdot dt \right )\right)$

第一项为速度测量，第二项为噪声项；

$v_{i}^{j}=\hat{v_{i}^{j}}-\delta v_{i}^{j}$ (4)

3. 再看位置部分，直接由（2）式推导

$p_{i}^{j}=\sum_{n=i}^{j-1}(\Phi \left ( -\phi _{i} \right )v_{n}dt+\frac{1}{2}\Phi \left ( \phi _{ni} \right )(a_{n}-b_{a}-\eta _{an})dt^{2})$

第一项速度项为预积分速度，带入可得

$p_{i}^{j}=\sum_{n=i}^{j-1}(v_{i}^{n}dt+\frac{1}{2}\Phi \left ( \phi _{ni} \right )(a_{n}-b_{a}-\eta _{an})dt^{2})$

并将（3）（4）分别带入

$p_{i}^{j}=\sum_{n=i}^{j-1}((\hat{v}_{i}^{n}-\delta v_{i}^{n})dt+\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{n}} \right ) \Phi\left ( \delta \phi _{i}^{n} \right ) (a_{n}-b_{a}-\eta _{an})dt^{2})$

将旋转噪声项用一阶泰勒展开，并忽略二阶及以上的项，可得

$p_{i}^{j}=\sum_{n=i}^{j-1}((\hat{v}_{i}^{n}-\delta v_{i}^{n})dt+\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{n}} \right ) \left (I-\delta \phi _{i}^{n\times }\right ) (a_{n}-b_{a}-\eta _{an})dt^{2})$

同速度项，展开并忽略二阶小量,事实上，这一项位置推导类似于六轴imu预积分，敲公式实在太麻烦了，后边推导也不难，直接给出结论

$p_{i}^{j}=\hat{p_{i}^{j}}-\delta p_{i}^{j}$ (5)

其中

$\hat{p_{i}^{j}} = \sum_{k=i}^{j-1}\left [ \left ( \hat{v_{i}^{k}} dt\right ) +\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{k}} \right )\left ( a_{k}-b_{a} \right )dt^{2}\right ]$

$-\delta p_{i}^{j}=\sum_{k=i}^{j-1}\left [ -\delta v_{i}^{k}dt+\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{k}} \right )\cdot 1^{\times }\cdot \left ( a_{k}-b_{a} \right )\delta \phi _{i}^{k}dt^{2}-\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{k}} \right )\eta _{ak}dt^{2} \right ]$

二、噪声递推公式

上面已经将预积分写为测量值减噪声项，但是不同时刻的噪声项，并不相关，需要重新计算，为了简化噪声项，将其写为递推形式，即已知上一时刻的噪声项，通过递推关系，即可知道当前帧的噪声大小，知道了噪声大小，即可获得协防差矩阵的传播方法；

1. 旋转噪声递推模型，由（3）式可知

$\delta \phi _{i}^{j}=\sum_{k=i}^{j-1}\eta _{wk}\cdot dt=\sum_{k=i}^{j-2}\eta _{wk}\cdot dt+\eta _{w\left ( j-1 \right )}\cdot dt=\delta \phi _{i}^{j-1}+\eta _{w\left ( j-1 \right )}\cdot dt$ （6）

2. 速度噪声递推模型，由（4）式可知

$\delta v_{i}^{j}=\sum_{k=i}^{j-1} \left (\Phi \left ( \phi_{i}^{k} \right )\left ( \eta _{ak}dt-1 ^{\times }\left ( a_{k}-b_{a} \right )\cdot \delta \phi_{i}^{k} \cdot dt \right )\right)$

同旋转部分，表示成j-2时刻与j-1时刻相加的形式

$\delta v_{i}^{j}=\sum_{k=i}^{j-2} \left (\Phi \left ( \phi_{i}^{k} \right )\left ( \eta _{ak}dt-1 ^{\times }\left ( a_{k}-b_{a} \right )\cdot \delta \phi_{i}^{k} \cdot dt \right )\right)+\Phi \left ( \phi_{i}^{j-1} \right )\left ( \eta _{aj-1}dt-1 ^{\times }\left ( a_{j-1}-b_{a} \right )\cdot \delta \phi_{i}^{j-1} \cdot dt \right )$

i到j-2时刻的累加可以表示j-2时刻的速度噪声，因此

$\delta v_{i}^{j}=\delta v_{i}^{j-1}+\Phi \left ( \phi_{i}^{j-1} \right )\left ( \eta _{aj-1}dt-1 ^{\times }\left ( a_{j-1}-b_{a} \right )\cdot \delta \phi_{i}^{j-1} \cdot dt \right )$ (7)

3. 位置噪声递推模型，由（5）式可知

$\delta p_{i}^{j}=-\sum_{k=i}^{j-1}\left [ -\delta v_{i}^{k}dt+\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{k}} \right )\cdot 1^{\times }\cdot \left ( a_{k}-b_{a} \right )\delta \phi _{i}^{k}dt^{2}-\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{k}} \right )\eta _{ak}dt^{2} \right ]$

同样处理

$\delta p_{i}^{j}=\delta p_{i}^{j-1} +\delta v_{i}^{j-1}dt-\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{j-1}} \right )\cdot 1^{\times }\cdot \left ( a_{j-1}-b_{a} \right )\delta \phi _{i}^{j-1}dt^{2}+\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{j-1}} \right )\eta _{aj-1}dt^{2}$ (8)

将噪声写成一个矩阵形式：

$\eta _{ik}=\begin{bmatrix} \delta \phi _{ik}\\ \delta v_{ik}\\ \delta p_{ik} \end{bmatrix}_{5\times 1}$ (9)

将imu零偏噪声定义为：

$\eta _{dj}=\begin{bmatrix} \eta _{wj}\\ \eta _{aj} \end{bmatrix}_{3\times 1}$ (10)

写成递推形式：

$\eta _{ij}=A_{j-1}\eta _{ij-1}+B_{j-1}\eta _{dj-1}$ （11）

由（6）（7）（8）可写出系数矩阵分别为如下：

$A_{j-1} = Eigen::Matrix5d::Zero()$

// 公式(6)

$A.block<1,5>(0,0) = Eigen::Vector5d(1, 0,0,0,0)$

// 公式（7）

$A.block<2,1>(1,0)=-\Phi \left ( \phi_{i}^{j-1} \right )\left ( 1 ^{\times }\left ( a_{j-1}-b_{a} \right )\cdot \delta \phi_{i}^{j-1} \cdot dt \right )$

$A.block<2,2>(1,1)=I_{2\times 2}$

// 公式（8）

$A.block<2,1>(3,0)=-\frac{1}{2}\Phi \left ( \hat{\phi _{i}^{j-1}} \right )\cdot 1^{\times }\cdot \left ( a_{j-1}-b_{a} \right )dt^{2}$

$A.block<2,2>(3,1)=dt\cdot I_{2\times 2}$

$A.block<2,2>(3,3)=I_{2\times 2}$

噪声项系数矩阵B

$B_{j-1} = Eigen::Matrix<double, 5, 3>::Zero()$

// 公式(6)

$B.block<1,5>(0,0) = dt\cdot Eigen::Vector5d(1, 0,0,0,0)$

// 公式(7)

$B.block<2,2>(1,1)=\Phi \left ( \phi_{i}^{j-1} \right )\cdot dt$

// 公式(8)

$B.block<2,2>(3,1)=\frac{1}{2}\cdot \Phi \left ( \phi_{i}^{j-1} \right )\cdot dt^{2}$

因此协方差递推形式如下：

$\sum_{i,k+1}=A_{k+1}\sum_{i,k}A_{k+1}^{T}+B_{k+1}Cov(\eta _{dk}))B_{k+1}^{T}$ (12)

其中,将(10)带入

$\sum_{0,0}=Eigen::Matrix<double, 5,5>::Zero();$

$Cov(\eta _{dk})=Eigen::Matrix<double>(3,3)::Zero();$

$Cov(\eta _{dk}).block<1,1>(0,0)=\eta _{dk}[0]=\eta _{wk}*\eta _{wk}$

$Cov(\eta _{dk})(1,1)=\eta _{dk}[1]=\eta _{ak}[0]*\eta _{ak}[0]$

$Cov(\eta _{dk})(2,2)=\eta _{dk}[2]=\eta _{ak}[1]*\eta _{ak}[1]$

上述公式即为IMU传播阶段，预积分推导，注意与六轴IMU的区别

三、总结

本讲推导了三轴IMU预积分公式以及噪声模型递推公式，大部分内容与六轴imu基本相似，但也有着一定区别，目前很少见，三轴IMU预积分的推导，本次已三轴推导为例，力求将SLAM门槛进一步降低，希望对初学者有所帮助，下一节开始讲解，优化过程中，偏置更新后预积分处理过程，请大家务必要先学习直接积分。

四足机器人 SLAM 导航实战

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

一键订阅

标签

#算法 #人工智能 #SLAM SE2 非线性优化