多传感器融合定位理论基础（十）：基于优化的融合方法

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

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

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

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

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

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

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

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

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

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

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

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

常用的转换包括

$R=\exp \left(\boldsymbol{\phi}^{\wedge}\right)\\$

$R^{-1}=\operatorname{cxp}\left((-\phi)^{\wedge}\right)\\$

$\phi=\ln \left(\exp \left(\boldsymbol{\phi}^{\wedge}\right)\right)^{\vee}=\ln (R)^{\vee}\\$

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

常用的转换包括

$T=\exp \left(\boldsymbol{\xi}^{\wedge}\right)\\$

$T^{-1}=\operatorname{cxp}\left((-\boldsymbol{\xi})^{\wedge}\right)\\$

$\boldsymbol{\xi}=\ln \left(\exp \left(\boldsymbol{\xi}^{\wedge}\right)\right)^{\vee}=\ln (T)^{\vee}\\$

3.BCH公式

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

$\ln (\exp (A) \exp (B))=A+B+\frac{1}{2}[A, B]+\frac{1}{12}[A,[A, B]]-\frac{1}{12}[B,[A, B]]+\cdots\\$

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

$\left[\phi_{1}, \phi_{2}\right]=\left(\Phi_{1} \Phi_{2}-\Phi_{2} \Phi_{1}\right)^{\vee}\\$

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

$\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee}\\$

其中

$\Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi(3) & \phi(2) \\ \phi(3) & 0 & -\phi(1) \\ -\phi(2) & \phi(1) & 0 \end{array}\right]\\$

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

$\ln \left(\operatorname{cxp}\left(\phi_{1}^{\wedge}\right) \operatorname{cxp}\left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2}, \text { 当 } \phi_{1} \text { 为小量 } \\ J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1}, \text { 当 } \phi_{2} \text { 为小量 } \end{array}\right.\\$

其中左乘雅可比为

$J_{l}=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) a a^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} a^{\wedge}\\$

即

$J_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} I+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) a a^{\mathrm{T}}-\frac{\theta}{2} a^{\wedge}\\$

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

$J_{r}(\boldsymbol{\phi})=J_{l}(-\boldsymbol{\phi})\\$

SE(3)对应的BCH公式为

$\ln \left(\exp \left(\boldsymbol{\xi}_{1}^{\wedge}\right) \exp \left(\boldsymbol{\xi}_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} \mathcal{J}_{\ell}\left(\boldsymbol{\xi}_{2}\right)^{-1} \boldsymbol{\xi}_{1}+\boldsymbol{\xi}_{2}, \text { 当 } \boldsymbol{\xi}_{1} \text { 为小量 } \\ \mathcal{J}_{r}\left(\boldsymbol{\xi}_{1}\right)^{-1} \boldsymbol{\xi}_{2}+\boldsymbol{\xi}_{1}, \text { 当 } \boldsymbol{\xi}_{2} \text { 为小量 } \end{array}\right.\\$

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

$\mathcal{J}_{r}(\boldsymbol{\xi})^{-1} \approx I+\frac{1}{2}\left[\begin{array}{rr} \phi^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ 0 & \boldsymbol{\phi}^{\wedge} \end{array}\right]\\$

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

$\ln \left(\exp \left(\boldsymbol{\xi}_{1}^{\wedge}\right) \exp \left(\boldsymbol{\xi}_{2}^{\wedge}\right)\right)^{\vee} \approx \ln \left(\exp \left(\boldsymbol{\xi}_{1}^{\wedge}+\boldsymbol{\xi}_{1}^{\wedge}\right)\right)^{\vee}\\$

4.伴随性质

SO(3)上的伴随性质为

$\operatorname{cxp}\left(\phi^{\wedge}\right) R=R \operatorname{cxp}\left(\left(R^{-1} \phi\right)^{\wedge}\right)\\$

SE(3)上的伴随性质

$\operatorname{cxp}\left(\xi^{\wedge}\right) T=T \operatorname{cxp}\left(\left(\operatorname{Ad}\left(T^{-1}\right) \xi\right)^{\wedge}\right)\\$

其中伴随矩阵的定义如下

$\operatorname{Ad}(T)=\left[\begin{array}{cc} R & t^{\wedge} R \\ 0 & R \end{array}\right]\\$

四、融合模型推导

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

$\mathbf{F}(\mathbf{x})=\sum_{\langle i, j\rangle \in \mathcal{C}} \underbrace{\mathbf{e}_{i j}^{T} \mathbf{\Omega}_{i j} \mathbf{e}_{i j}}_{\mathbf{F}_{i j}}\\$

此时优化问题可以表示为

$\mathbf{x}^{*}=\underset{\mathbf{x}}{\operatorname{argmin}} \mathbf{F}(\mathbf{x})\\$

$单个残差项\\$

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

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

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

第 i 帧和第 j 帧之间的相对位姿，在李群SE(3)上可以表示为

$T_{i j}=\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\\$

也可以在李代数上表示为

$\begin{aligned} \boldsymbol{\xi}_{i j} &=\ln \left(\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee} \\ &=\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_{j}^{\wedge}\right)\right)^{\vee} \end{aligned}\\$

若位姿没有误差，则上面两个式子是精确相等的，但当位姿有误差存在时，便可以使用等式的左右两端计算残差项

$\begin{aligned} \boldsymbol{e}_{i j} &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee} \\ &=\ln \left(\operatorname{cxp}\left(\left(-\boldsymbol{\xi}_{i j}\right)^{\wedge}\right) \operatorname{cxp}\left(\left(-\boldsymbol{\xi}_{i}\right)^{\wedge}\right) \operatorname{cxp}\left(\boldsymbol{\xi}_{j}^{\wedge}\right)\right)^{\vee} \end{aligned}\\$

位姿图优化的思想是通过调整状态量(即位姿)，使残差项的值最小化，这就需要用残差项对位姿求雅可比，才能使用高斯牛顿方法进行优化。

求雅可比的方式是对位姿添加扰动，此时残差表示为

$\hat{\boldsymbol{e}}_{i j}=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\delta \boldsymbol{\xi}_{j}^{\wedge}\right) \boldsymbol{T}_{j}\right)^{\vee}\\$

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

$\begin{aligned} \hat{\boldsymbol{e}}_{i j} &=\ln \left(T_{i j}^{-1} T_{i}^{-1} \operatorname{cxp}\left(\left(-\delta \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \operatorname{cxp}\left(\delta \boldsymbol{\xi}_{j}^{\wedge}\right) T_{j}\right)^{\vee} \\ &=\ln \left(T_{i j}^{-1} T_{i}^{-1} \exp \left(\left(-\delta \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \boldsymbol{T}_{j} \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \delta \boldsymbol{\xi}_{j}\right)^{\wedge}\right)^{\vee}\right.\\ &=\ln \left(T_{i j}^{-1} T_{i}^{-1} T_{j} \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \delta \xi_{i}\right)^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(T_{j}^{-1}\right) \delta \xi_{j}\right)^{\wedge}\right)^{v}\right.\\ & \approx \ln \left(T_{i j}^{-1} T_{i}^{-1} T_{j} \operatorname{cxp}\left(\left(-\operatorname{Ad}\left(T_{j}^{-1}\right) \delta \xi_{i}\right)^{\wedge}+\left(\operatorname{Ad}\left(T_{j}^{-1}\right) \delta \xi_{j}\right)^{\wedge}\right)^{v}\right.\\ &=\ln \left(\operatorname{cxp}\left(e_{i j}^{\wedge}\right) \operatorname{cxp}\left(\left(-\operatorname{dd}\left(T_{j}^{-1}\right) \delta \xi_{i}\right)^{\wedge}+\left(\operatorname{ld}\left(T_{j}^{-1}\right) \delta \xi_{j}\right)^{\wedge}\right)^{V}\right.\\ & \approx e_{i j}-\mathcal{J}_{r}^{1}\left(\alpha_{i j}\right) \operatorname{Ad}\left(T_{j}^{-1}\right) \delta \xi_{i}+\mathcal{J}_{r}^{1}\left(e_{i j}\right) \operatorname{Ad}\left(T_{j}^{-1}\right) \delta \xi_{j} \end{aligned}\\$

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

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

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

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

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

5）残差的定义

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

上面的式子表明，残差关于

$T_{i}$ 的雅可比为

$A_{i j}=\frac{\partial e_{i j}}{\partial \delta \boldsymbol{\xi}_{i}}=-\mathcal{J}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right)\\$

残差关于

$T_{j}$ 的雅可比为

$B_{i j}=\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{j}}=\mathcal{J}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right)\\$

其中

$\mathcal{J}_{r}^{-1}\left(e_{i j}\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \phi_{e}^{\wedge} & \rho_{e}^{\wedge} \\ 0 & \phi_{c}^{\wedge} \end{array}\right]\\$

按照高斯牛顿法的流程，需要对残差进行一阶泰勒展开，即求雅可比

$\begin{array}{l} \mathbf{e}_{i j}\left(\mathbf{x}_{i}+\Delta \mathbf{x}_{i}, \mathbf{x}_{j}+\Delta \mathbf{x}_{j}\right) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx \mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x} \end{array}\\$

其中

$\mathbf{J}_{i j}$ 即为前面推导的残差关于位姿的雅可比组成的矩阵

$\mathbf{J}_{i j}=(0 \cdots 0 \underbrace{\mathbf{A}_{i j}}_{\text {node } i} \mathbf{0} \cdots 0 \underbrace{\mathbf{B}_{i j}}_{\text {node } j} \mathbf{0} \cdots 0)\\$

对于每一个残差块，便有

$\begin{array}{l} \mathbf{F}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x})^{T} \Omega_{i j} \mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right)^{T} \Omega_{i j}\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right) \\ =\underbrace{\mathbf{e}_{i j}^{T} \Omega_{i j} \mathbf{e}_{i j}}_{c_{i j}}+2 \underbrace{\mathbf{e}_{i j}^{T} \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{b}_{i j}^{T}} \Delta \mathbf{x}+\Delta \mathbf{x}^{T} \underbrace{\mathbf{J}_{i j}^{T} \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{H}_{i j}} \Delta \mathbf{x} \\ =c_{i j}+2 \mathbf{b}_{i j}^{T} \Delta \mathbf{x}+\Delta \mathbf{x}^{T} \mathbf{H}_{i j} \Delta \mathbf{x} \end{array}\\$

其中

$\mathbf{H}_{i j}=\left(\begin{array}{cccc} \ddots & & & \\ & \mathbf{A}_{i j}^{T} \boldsymbol{\Omega}_{i j} \mathbf{A}_{i j} & \cdots & \mathbf{A}_{i j}^{T} \Omega_{i j} \mathbf{B}_{i j} \\ \vdots & \ddots & \vdots & \\ & \mathbf{B}_{i j}^{T} \Omega_{i j} \mathbf{A}_{i j} & \cdots & \mathbf{B}_{i j}^{T} \Omega_{i j} \mathbf{B}_{i j} \\ \ddots & & & \end{array}\right)\\$

$\mathbf{b}_{i j}=\left(\begin{array}{c} \vdots \\ \mathbf{A}_{i j}^{T} \boldsymbol{\Omega}_{i j} \mathbf{e}_{i j} \\ \vdots \\ \mathbf{B}_{i j}^{T} \boldsymbol{\Omega}_{i j} \mathbf{e}_{i j} \\ \vdots \end{array}\right)\\$

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

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

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

$\begin{aligned} \boldsymbol{e}_{i} &=\ln \left(\boldsymbol{Z}_{i}^{-1} \boldsymbol{T}_{i}\right)^{\vee} \\ &=\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{z i}\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_{i}^{\wedge}\right)\right)^{\vee} \end{aligned}\\$

对残差添加扰动，可得

$\hat{\boldsymbol{e}}_{i}=\ln \left(\boldsymbol{Z}_{i}^{-1} \exp \left(\delta \boldsymbol{\xi}_{i}^{\wedge}\right) \boldsymbol{T}_{i}\right)^{\vee}\\$

利用伴随性质和BCH公式进行化简，可得

$\begin{aligned} \hat{e}_{i} &=\ln \left(Z_{i}^{-1} T_{i} \operatorname{cxp}\left(\left(\Lambda \mathrm{d}\left(T_{i}^{-1}\right) \delta \xi_{i}\right)^{\wedge}\right)\right)^{V} \\ &=\ln \left(\exp \left(e_{i}^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(T_{i}^{-1}\right) \delta \xi_{i}\right)^{\wedge}\right)\right)^{v} \\ & \approx e_{i}+\mathcal{J}_{r}^{-1}\left(e_{i}\right) \operatorname{Ad}\left(T_{i}^{-1}\right) \delta \xi_{i} \end{aligned}\\$

因此残差关于

$T_{i}$ 的雅可比为

$\frac{\partial \boldsymbol{e}_{i}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{i}}=\mathcal{J}_{r}^{-1}\left(\boldsymbol{e}_{i}\right) \mathrm{Ad}\left(\boldsymbol{T}_{i}^{-1}\right)\\$

其中

${\mathcal{J}}_{r}^{-1}\left(e_{i}\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \phi_{e}^{\wedge} & \rho_{e}^{\wedge} \\ 0 & \phi_{e}^{\wedge} \end{array}\right]\\$

后面的推导过程，便与相对位姿做观测的推导过程完全一致.

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

五、总结

。。。。

多传感器融合定位理论基础（十）：基于优化的融合方法

任乾

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

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

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

3.BCH公式

4.伴随性质

四、融合模型推导

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

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

五、总结

为你推荐

[5](含源码)基于非线性模型预测控制(NMPC)的四旋翼轨迹跟踪

自抗扰控制理论（三十一）永磁同步电机控制系统

pybullet接触力设置-UR5坐标系构建(一)

[1](含源码)通过关节力矩指令控制LBR/iiwa机械臂运动

工业机器人牵引示教

机器人接触动力学建模方法描述

评论（0）

关于作者

任乾

65

0

6

3

VINS-Mono源码解读(二)：IMU预积分

多传感器融合定位理论基础（十五）：第二阶段总结

多传感器融合定位理论基础（十二）：滑动窗口原理及应用

相关推荐

Matlab 仿真——单自由度倒立摆（3）PID控制器设计

Webots 机器人仿真平台(四) 机器人基础控制器

基于adams与simulink的七自由度机械臂模型与控制仿真

从零开始搭建无人机导航系统（三）——状态估计与概率论(一)

gazebo教程（五）添加传感器噪声& 简单控制插件

VINS-Mono源码解读(三)：视觉惯性联合初始化

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板