多传感器融合定位十三-基于图优化的建图方法其二

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

3.4 预积分方差计算

3.4.1 核心思路

在融合时，需要给不同信息设置权重，而权重由方差得来，因此对于IMU积分，也要计算其方差。方差的计算形式与第四章相同，即：
$${\mathbf{P}}_{i,k+1}={\mathbf{F}}_k{\mathbf{P}}_{i,k}{\mathbf{F}}_k^{\top }+{\mathbf{B}}_k\mathbf{Q}{\mathbf{B}}_k^{\top }$$但需注意的是，此处 ${\mathbf{F}}_k$ 和 ${\mathbf{G}}_k$ 是离散时间下的状态传递方程中的矩阵，而我们一般是在连续时间下推导微分方程，再用它计算离散时间下的传递方程。

3.4.2 连续时间下的微分方程

连续时间下的微分方程一般写为如下形式：
$$\begin{array}{c}\dot{\mathbf{x}}={\mathbf{F}}_t\mathbf{x}+{\mathbf{B}}_t\mathbf{w}\\ \mathbf{x}=\left[\begin{array}{l}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{b}}_{{\mathbf{a}}_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]\\ \mathbf{w}=\left[\begin{array}{l}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]\end{array}$$以上变量排列顺序是为了与lio/vio等常见系统顺序保持一致。

推导方法已经在第8讲介绍，此处直接给出结果：
$\delta {\dot{\mathbf{\theta }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\theta }}}_t^{b_k}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta {\mathbf{\theta }}_t^{b_k}+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{{\omega }_t}$$$\delta {\dot{\mathbf{\beta }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\beta }}}_t^{b_k}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta {\mathbf{\theta }}_t^{b_k}+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)$$$\delta {\dot{\mathbf{\alpha }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\alpha }}}_t^{b_k}=\delta {\mathbf{\beta }}_t^{b_k}$$

3.4.3 离散时间下的传递方程

得到连续时间微分方程以后，就可以计算离散时间的递推方程了，表示为：
$${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$$其中：
$${\mathbf{x}}_{k+1}=\left[\begin{array}{l}\delta {\mathbf{\alpha }}_{k+1}\\ \delta {\mathbf{\theta }}_{k+1}\\ \delta {\mathbf{\beta }}_{k+1}\\ \delta {\mathbf{b}}_{a_{k+1}}\\ \delta {\mathbf{b}}_{{\omega }_{k+1}}\end{array}\right]{\mathbf{x}}_k=\left[\begin{array}{c}\delta {\mathbf{\alpha }}_k\\ \delta {\mathbf{\theta }}_k\\ \delta {\mathbf{\beta }}_k\\ \delta {\mathbf{b}}_{a_k}\\ \delta {\mathbf{b}}_{{\omega }_k}\end{array}\right]{\mathbf{w}}_k=\left[\begin{array}{c}{\mathbf{n}}_{a_k}\\ {\mathbf{n}}_{w_k}\\ {\mathbf{n}}_{a_{k+1}}\\ {\mathbf{n}}_{w_{k+1}}\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]$$

1. $\delta {\mathbf{\theta }}_{k+1}$ 的求解
   由于连续时间下有：
   $$\delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{{\omega }_t}$$则离散时间下应该有：
   $$\delta {\dot{\mathbf{\theta }}}_k=-{\left[\frac{{\mathbf{\omega }}_k+{\mathbf{\omega }}_{k+1}}{2}-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta {\mathbf{\theta }}_k+\frac{{\mathbf{n}}_{{\omega }_k}+{\mathbf{n}}_{{\omega }_{k+1}}}{2}-\delta {\mathbf{b}}_{{\omega }_k}$$因此有：
   $$\delta {\mathbf{\theta }}_{k+1}=\left[\mathbf{I}-{\left[\frac{{\mathbf{\omega }}_k+{\mathbf{\omega }}_{k+1}}{2}-{\mathbf{b}}_{{\omega }_k}\right]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\delta t\frac{{\mathbf{n}}_{{\omega }_k}+{\mathbf{n}}_{{\omega }_{k+1}}}{2}-\delta t\delta {\mathbf{b}}_{{\omega }_k}$$令 $\overline{\mathbf{\omega }}=\frac{{\omega }_k+{\omega }_{k+1}}{2}-{\mathbf{b}}_{{\omega }_k}$ ，则上式可以重新写为：
   $$\delta {\mathbf{\theta }}_{k+1}=\left[\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_k}+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_{k+1}}-\delta t\delta {\mathbf{b}}_{{\omega }_k}$$

2. $\delta {\mathbf{\beta }}_{k+1}$ 的求解
   由于连续时间下有：
   $$\delta \dot{\mathbf{\beta }}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)$$则离散时间下应该有：
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_k\\ & -\frac{1}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_{k+1}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$把前面求得的 $\delta {\mathbf{\theta }}_{k+1}$ 的表达式代入上式，可得：
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_k\\ & -\frac{1}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left\{\left[\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_k}+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_{k+1}}-\delta t\delta {\mathbf{b}}_{{\omega }_k}\right\}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$经过一系列合并同类项以后，最终的合并结果为：
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由导数形式可以得到递推形式如下：
   $$\begin{array}{rl}\delta {\mathbf{\beta }}_{k+1}=& \delta {\mathbf{\beta }}_k\\ & -\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$

3. $\delta {\mathbf{\alpha }}_{k+1}$ 的求解
   由于连续时间下有 $\delta {\dot{\mathbf{\alpha }}}_t=\delta {\mathbf{\beta }}_t$，则离散时间下应该有：
   $$\begin{array}{rl}\delta {\dot{\mathbf{\alpha }}}_k=& \frac{1}{2}\delta {\mathbf{\beta }}_k+\frac{1}{2}\delta {\mathbf{\beta }}_{k+1}\\ =& \delta {\mathbf{\beta }}_k\\ & -\frac{\delta t}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^2}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^2}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]\times \delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t}{4}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由导数形式可以写出递推形式：
   $$\begin{array}{rl}\delta {\mathbf{\alpha }}_{k+1}=& \delta {\mathbf{\alpha }}_k\\ & +\delta t\delta {\mathbf{\beta }}_k\\ & -\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\bar{\omega }{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t^2}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由以上的推导结果，便可以写出 ${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$ 中的矩阵：
   $$\begin{array}{rl}& {\mathbf{F}}_k=\left[\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& -\frac{1}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t& 0& 0& -\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\\ & {\mathbf{B}}_k=\left[\begin{array}{cccccc}\frac{1}{4}{\mathbf{R}}_k\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{R}}_{k+1}\delta t^2& {\mathbf{g}}_{14}& 0& 0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{R}}_k\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{R}}_{k+1}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}\right]\end{array}$$上面的矩阵中，有：
   $$\begin{array}{rl}& {\mathbf{f}}_{12}=-\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{15}=\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & {\mathbf{f}}_{32}=-\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{35}=\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{12}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{14}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{32}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{34}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\end{array}$$

3.5 预积分更新

回到 bias 变化时，预积分结果怎样重新计算的问题，再次给出它的泰勒展开形式：
$$\begin{array}{c}{\mathbf{\alpha }}_{b_i{b}_j}={\overline{\mathbf{\alpha }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\alpha }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\alpha }\delta {\mathbf{b}}_i^g\\ {\mathbf{\beta }}_{b_i{b}_j}={\overline{\mathbf{\beta }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\beta }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\beta }\delta {\mathbf{b}}_i^g\\ {\mathbf{q}}_{b_i{b}_j}={\overline{\mathbf{q}}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^q}^q\delta {\mathbf{b}}_i^g\end{array}\right]\end{array}$$这里，雅可比没有明确的闭式解，但是在推导方差的更新时，我们得到了：
$${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$$通过该递推形式，可以知道：
$${\mathbf{J}}_{k+1}={\mathbf{F}}_k{\mathbf{J}}_k$$即预积分结果的雅可比，可以通过这种迭代方式计算。
${\mathbf{J}}_j$ 中关于 bias 的项，就是预积分泰勒展开时，各 bias 对应的雅可比。

4. 典型方案介绍

LIO-SAM 是一个比较好的 IMU 和其他传感器通过预积分公式来融合的开源工程。它的思路比较简单清晰。

它没有用大的融合模型来做----把 GPS、IMU 和激光里程计的特征都放在一个大的模型里一起优化搞出一个优化结果。理论上放在一个大模型里面可以得到一个比较好的结果，因为信息的耦合度更高，LIO-MAPPING 使用的就是这种思路，但是效果并不好而且效率非常低(模型大)。

LIO-SAM 并没有采用这种思路：它先使用 LEGO-LOAM 把激光里程计相对位姿计算出来，然后使用这个相对位姿、GPS、IMU 的预积分去做融合。这样整个问题就变简单了，相对位姿只是一个边而已，它没有把雷达的很多特征带到优化内。也就是说模型有两个：1. 雷达特征自己做一个优化模型 2.雷达的位姿和 GPS+IMU 做一个融合。这里面两个最大的计算量分别在两个模型内，这样整个的优化就比较快，如果放在同一个大模型内就比较慢了。

可能会认为耦合度不高会影响融合效果，但是 LIO-SAM 的效果证明精度非常高且稳定性非常好。

4.1 LIO-SAM介绍

论文名称：LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
代码地址：https://github.com/TixiaoShan/LIO-SAM

最大特点：分两步完成，先通过点云特征计算出相对位姿，再利用相对位姿、IMU 预积分和 GPS 做融合。

相比于直接一步做紧耦合，大大提高了效率，而且实测性能也很优异。

LIO-SAM 本身使用预积分是 GTSAM 封装的死死的，不会让我们看到上面推导的公式，就是一个很简单的调用。因此 LIO-SAM 的代码比较好懂，但是并不太能通过代码了解预积分的原理。

5. 融合编码器的优化方案

5.1 整体思路介绍

理论上，只要是在载体系下测量，且频率比关键帧的提取频率高，就都可以做预积分。
编码器与IMU基于预积分的融合有多种方法，此处选相对简单的一种：
把它们当做一个整体的传感器来用，IMU提供角速度，编码器提供位移增量，且不考虑编码器的误差。
此时对预积分模型，输入为：
角速度：${\omega }_k=\left[\begin{array}{l}{\omega }_{xk}\\ {\omega }_{yk}\\ {\omega }_{zk}\end{array}\right]$ 位移增量：${\varphi }_k=\left[\begin{array}{c}{\varphi }_{xk}\\ 0\\ 0\end{array}\right]$
输出中包含位置、姿态，但不包含速度。

5.2 预积分模型设计

连续时间下，从 $i$ 时刻到 $j$ 时刻 IMU的积分结果为：
$$\begin{array}{rl}{\mathbf{p}}_{w{b}_j}& ={\mathbf{p}}_{w{b}_i}+{\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}{\mathbf{\varphi }}^{b_t}\delta t\\ {\mathbf{q}}_{w{b}_j}& ={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$把 ${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_t}$ 代入上式可得：
$$\begin{gathered} {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{q}}_{w{b}_i}{\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{\varphi }}^{b_t}\right)\delta t \\ {\mathbf{q}}_{w{b}_j}={\mathbf{q}}_{w{b}_i}[{\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t \end{gathered}$$为了整理公式，把积分相关的项用下面的式子代替：
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{\varphi }}^{b_t}\right)\delta t\\ & {\mathbf{q}}_{b_i{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$使用中值积分方法，把连续方法改成离散形式如下：
$$\begin{array}{rl}{\mathbf{\omega }}^b& =\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]\\ {\mathbf{\varphi }}^w& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}{\mathbf{\varphi }}^{b_k}+{\mathbf{q}}_{b_i{b}_{k+1}}{\mathbf{\varphi }}^{b_{k+1}}\right)\end{array}$$那么预积分的离散形式可以表示为：
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_{k+1}}={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\varphi }}^w\delta t\\ & {\mathbf{q}}_{b_i{b}_{k+1}}={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{\omega }}^b\delta t\end{array}\right]\end{array}$$此时状态更新的公式可以整理为：
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^{g_j}\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^g\end{array}\right]$$残差形式可以写为：
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}\right)-{\mathbf{\alpha }}_{b_{b^b}{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$其余部分(方差递推、残差对状态量雅可比、bias更新等)的推导，留作作业(仅优秀标准需要做)。