自动驾驶融合定位：基于优化的融合方法

自动驾驶融合定位：基于优化的融合方法

一、概述

上一篇文章啰嗦了一大堆，其实是为这一篇文章打了一点基础。本篇文章其实和上一个系列《从零开始做自动驾驶定位》关联比较密切，那个系列讲了如何把各种传感器融合在一起，建立一个完整的可用于定位的点云地图，我们花了大量的篇幅用在实践上，但没有仔细讲它的内部算法原理，所以本篇文章既是这个系列的组成部分，又是对上个系列的理论补充。

附赠自动驾驶学习资料和量产经验：链接

二、融合建图任务解析

为了把本篇文章的任务讲清楚，定好核心方向，我们需要把该算法所引用的场景任务梳理一下。

任务用一句话描述就是融合建图嘛，融合的传感器包括：

1）组合导航系统（输出绝对位姿）

2）雷达里程计（输出相邻帧间相对位姿）

3）回环检测系统（输出再次走到同一场景时，当前帧与历史帧之间的相对位姿）

综合以上各种信息，整个系统的结构图大概应该长这个样子

如果对这些东西做融合的具体实现还不清楚，可以再回头去看一下《从零开始做自动驾驶定位》系列文章中的实现过程。

需要注意的是，图中还有一部分（虚线框中的部分）并没有在上面介绍，是因为跟他相关的融合方法比较复杂，我们把它放在下一篇文章进行单独介绍。

让面的框图部分中的内容，如果用图优化的形式表达成下图中的样子

三、李群、李代数基础知识

想把图优化中的数学部分推导清楚，是需要一定的李群、李代数基础的，还是有必要单独用一个小节把这些基础弄清楚，这样不会妨碍后面流程的流畅性。

1.三维旋转(SO(3))上的李群、李代数

常用的转换包括

2.三维变换(SE(3))上的李群、李代数

常用的转换包括

3.BCH公式

BCH公式在后面的推导中会多次用到，它的完整定义为

其中的中括号代表“李括号”，当A、B为SO(3)时，李括号的含义为

当A、B为SE(3)时，李括号的含义为

其中

因此，SO(3)对应的BCH公式为

其中左乘雅可比为

即

右乘雅可比仅需要在左乘雅可比的基础上对自变量取负号，即

SE(3)对应的BCH公式为

由于SE(3)上的雅可比形式过于复杂，此处直接给出本章所用到的近似形式如下。详细内容可参考《机器人中的状态估计》中公式7.83的推导过程。

若 𝜉1 和 𝜉2 非常小，则左右雅可比均可以直接约等于单位阵，此时有

4.伴随性质

四、融合模型推导

根据上一篇文章的介绍，我们了解到，只要给出残差定义，并给出对应的雅可比，就可以进行优化，得到要求的位姿。而在本篇介绍的这个融合的实际任务中，位姿图优化是把所有的观测和状态放在一起优化，残差项是前面所讲的残差项的总和。在实际使用中，各残差会被分配一个权重，也就是信息矩阵，它相当于对残差进行加权。考虑信息矩阵后，总的残差项可以表示为

此时优化问题可以表示为

单个残差项单个残差项

多个残差项组成位姿图多个残差项组成位姿图

在这个实际任务中，一共有三类信息：相邻帧相对位姿、回环相对位姿、组合导航先验位姿，由于前两个约束的都是两帧之间的相对位姿，即信息形式是完全相同的，因此残差形式及其推导也完全相同（这里组合导航先验位姿是只约束单帧的观测），因此按照种类把残差分成两类来推导。

1.相对位姿优化模型推导

进一步对前面的式子进行化简

上式的6个等号（包括约等号），可能需要做一些解释（按照等号序号）：

1）就是从上面复制下来的，不解释

2）利用了三维变换下的伴随性质

3）利用了三维变换下的伴随性质

4）利用了BCH公式，且是两个李代数都比较小情况下的近似

5）残差的定义

6）利用了三维变化下的伴随性质

其中

此后便可以使用高斯牛顿法进行优化。

2.先验位姿优化模型推导

先验观测是一元边，它不像前面所述的帧间观测连接两个位姿状态，而是只连接一个位姿状态量，它直接给出的就是该状态量的观测值，因此它对应的残差就是观测值与状态量之间的差异，即

此外，部分场合提供的观测只有位置，没有姿态，比如只有RTK，而没有组合导航，这里的残差便只剩下位置误差。相应的雅可比公式，可自行推导。