基于卡尔曼滤波器的同构多传感器融合方案

作者 | 岩火云  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/372772020?

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【多传感器融合】技术交流群

后台回复【多传感器融合综述】获取图像/激光雷达/毫米波雷达融合综述等干货资料！

说到融合，先提一下卡尔曼滤波器是如何被用来做跟踪的，这里先提一下Sort跟踪算法，整个Sort算法维护一个列表，表示当前有多少个对象。在上一帧计算结束后，所有对象执行一次卡尔曼预测，得到当前帧目标的预测位置，等当前的观测器数据到来，相互关联上的目标对象执行一次更新，未关联上的观测目标加入跟踪列表，然后列表中的目标执行一次卡尔曼预测...关联...更新，如此往复。

整个跟踪算法中，当现存目标被关联上后，执行一次更新，实际是现存目标的位置（服从高斯分布）和观测目标位置（也服从高斯分布）的数据融合。

当传感器的数量增加后，即同一时刻对目标的观测数据变多了，这时候怎么更新目标状态？

1.目标状态更新

这边的题设是同构传感器，具体的应用场景可以是车上的多个摄像头，或者多个雷达。异构传感器的融合比较复杂，本文不涉及。

使用同构传感器的话，即各个传感器的观测矩阵是相同的，这时候可以将目标物体的状态转换到观测空间中，即 (Hx,HP) 中，然后将数据融合到一起。

这里以一维情况举例，多维的情况以此类推。

由此可以得到

于是，

看到这里，似乎是把故事讲完了。

但是还是有一个问题没有解答：

1.目标关联的时候，构建的关联矩阵一般是二维的，只能完成预测目标和观测目标两种数据源的匹配，在多传感器的场景下，n组观测数据+一组预测数据，一共n+1个数据源怎么完成关联并融合？

下面来解答这个问题：

由于关联只能完成一种数据源到另一种数据源的关联，假设首先完成预测数据和传感器1数据的关联和融合，这时候融合完的状态是

两个传感器按顺序融合的结果与一起融合的结果完全一致，并且，无论是传感器1先与预测数据融合还是传感器2先与预测数据融合，得到结果都一样。说明传感器按序分别融合是可行的方案，并且可以以任意顺序完成融合。

那么，可以得到同构多传感器融合的具体迭代流程

2.扩展

上面提到的多传感器融合是同构传感器融合，也就是可以转换到同一个空间中完成融合，卡尔曼增益的维度也是相同的，可以用下面的公式表示融合后的结果

但是涉及到异构多传感器时，情况就变得复杂了。各个传感器的观测矩阵不一样，没有统一的空间完成融合，卡尔曼增益的维度也不同。

异构多传感器的情况，同样可以使用上述的按序融合，但此时的顺序就会影响到最终结果。好在不管以什么顺序融合，都是往使协方差变小的情况演进，所以使用按序融合无伤大雅。

如果希望异构传感器融合获得更高的精度，可以考虑根据各个观测值的协方差，计算最优权重，完成一次加权融合，再与预测数据完成一次卡尔曼更新。至于这种方法是否科学，我也不知道，此处不再赘述。

视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称