做自动驾驶感知融合这几年有个体会越来越深单传感器的感知能力再强也扛不住“看不到”“看不清”“看错”三个字。摄像头在逆光下把白色厢式货车当背景毫米波雷达把静止的铁护栏当成目标激光雷达在雨雾天点云稀疏到没法聚类。真正到了工程落地没人指望某一个传感器永远是对的大家心里都清楚要靠融合把信息捏在一起。可融合做到哪一级用什么理论框架不同做法之间的差距相当大。这篇内容我围绕“信息矩阵融合算法 Dempster-Shafer证据理论 目标级融合”这个技术组合展开结合一套Matlab仿真代码谈谈整体设计、核心原理、实现细节和调试过程中踩过的坑适合正在做多传感器融合、算法预研或者毕业设计的开发者参考。先交代一下这套方案解决的核心问题在自动驾驶中摄像头、毫米波雷达、激光雷达各自输出目标列表位置、速度、类型、置信度但这些输出来自异构传感器坐标系不同、精度不同、语义信息也不同。目标级融合要做的就是把这些“已经形成的目标假设”合并成一份完整、一致、可信的环境目标列表给后续的决策规划模块用。这里的状态估计层面我用信息矩阵做加权融合属性识别层面用DS证据理论处理不确定性。1. 目标级融合的整体思路为什么把“决策”放在融合之后1.1 从数据级、特征级到目标级的演进逻辑多传感器融合按照数据处理层次通常分成数据级、特征级和目标级三类。数据级融合直接对原始信号做处理像激光雷达点云直接叠加、多摄像头图像像素级拼接这种方法信息损失最小但对通信带宽、时间同步和计算资源的要求极高而且不同传感器如果分辨率差太多原始数据根本对不上。特征级融合是从原始数据里提取特征后再融合比如分别提取边缘、角点、深度特征再在特征空间里做匹配灵活性比数据级高一些但特征对齐本身就是一个难题。目标级融合是层级最高、也是最贴合自动驾驶工程实践的方案。它的输入是各传感器已经完成目标检测和跟踪后的输出通常是结构化目标列表包括目标ID、三维位置、速度、尺寸、类型概率等属性。融合模块不关心这些目标是怎么检测出来的只关心怎么确认两个传感器报告的是同一个物理目标、怎么把多份有冗余且可能冲突的信息合并成更可信的结果。这种方案的工程优势很明显传感器之间松耦合任意一路传感器坏了或暂时掉线系统整体还能运行通信量小适合分布式部署模块边界清晰检测、跟踪、融合、决策可以分开开发和测试。1.2 双通道融合设计状态通道用信息矩阵属性通道用DS证据拿到目标级数据之后要融合的信息其实是两类东西。一类是连续量比如目标的横纵向位置、速度、加速度这些量有明确的物理单位适合用估计理论处理另一类是离散量比如目标是车辆、行人还是自行车每个传感器会给出类型置信度还有可能是“未知”这些量是语义层面的判断存在重叠、模糊和冲突不适合用传统的最小二乘去“平均”。所以我把融合拆成双通道。状态通道走信息矩阵融合每个传感器上报目标时附带状态估计和协方差矩阵把协方差求逆得到信息矩阵按互不相关的原则相加再还原成融合后的状态估计和协方差。这个通道的理论基础是信息滤波本质上是把“不确定性越低贡献越大”这件事变成了数学上的加权。属性通道走Dempster-Shafer证据融合构造辨识框架把各传感器的类型置信度映射成基本概率分配再用Dempster组合规则合并多路证据输出各候选类型的信任度和似然度。为什么要这样拆分因为这两类信息在数学结构上完全不同。位置和速度可以用高斯分布近似表达误差传播和更新有一套成熟的体系类型判断本质上是分类问题分类器输出的概率并不总是满足可加性和完备性直接做贝叶斯融合在“传感器可能互相矛盾”的场景下会给出偏激的结果。DS理论允许把概率质量赋予集合而不是单个元素天然支持“不确定”和“不知道”的表达这一点和自动驾驶中“目标类型确实不确定”的现实非常契合。2. Dempster-Shafer理论的核心细节与BPA构造2.1 辨识框架、BPA、信任与似然的关系DS证据理论最关键的概念是辨识框架也就是所有可能结论组成的完备集合记作Θ。在自动驾驶目标类型融合里可以设Θ {车辆, 行人, 自行车}这就是一个三元素辨识框架共有2的3次方等于8个子集。基本概率分配函数BPA给每个子集分配一个质量值满足空集的质量为0所有子集质量之和为1。和概率分布的区别在于概率必须把质量分给单元素而BPA允许把质量分给多元素集合比如m({车辆, 行人}) 0.3这条表示“我认为是车或行人但区分不了”。我对这个特性特别看重它在传感器被遮挡、目标轮廓模糊时非常实用。信任函数Bel和似然函数Pl是从BPA推导出来的两个区间端点。Bel(A)是所有A子集的质量之和表示“对A的完全信任下限”Pl(A)是所有与A有交集的质量之和表示“对A的潜在信任上限”。区间[Bel(A), Pl(A)]就是A的不确定区间区间越大说明证据越拿不准。在目标类型判定时不能只用Bel大小排序还要看Pl和Bel的差距如果差距太大说明当前证据不足以支撑一个果断的决策不如输出“未知”。2.2 从分类器输出到BPA的三种映射方案在Matlab代码里把传感器输出的类型概率转换成BPA是整套算法里最容易出错的一步。我实际用过三种方案各有适用场景。第一种是直接映射适用于神经网络分类器输出层带softmax并且训练数据分布比较均衡的情况。如果模型输出车辆0.6、行人0.3、自行车0.1直接把这三个置信度作为单元素焦元的BPAm({车辆}) 0.6m({行人}) 0.3m({自行车}) 0.1不分配多元素质量。这种做法的缺陷是模型如果看到未知目标比如一辆三轮车softmax还是会强行给出一个分布哪一类都不高但硬要选一个直接映射会把这种错误固化进融合结果。第二种是基于先验概率打折的方案也是我在项目里最常用的。每种传感器都有一个混淆矩阵表示它把真实目标判断成各类别的概率。假设通过经验标定知道某摄像头的目标类型识别可靠性只有70%我就从总质量1里取出0.3作为全局不确定质量分配给全集Θ剩下0.7按模型输出比例分配给单元素。这样即使模型输出带偏差融合公式里也会保留“我不知道”的空间。第三种是基于似然比或证据距离的构造用于处理传感器输出不是概率而是边界框置信度加类型的场景通过归一化把检测置信度转成BPA。具体到代码实现我会用一个结构体数组存储BPA每个元素包含焦元列表focal_elements和对应的质量值mass_values。建议不要把BPA写成一个固定长度的数组因为不同传感器在不同时刻的有效焦元集合是变的。比如传感器A输出车辆0.8、全集0.2传感器B输出车辆0.7、行人0.2、全集0.1焦元集合不同组合时要动态求交集。Matlab用cellfun和unique配合可以比较优雅地完成这个操作。2.3 Dempster组合规则的实现与冲突困境Dempster组合规则是DS融合的核心计算公式是组合后的质量正比于所有交叠为空集的两两组合质量乘积。规则本身不复杂但实现时要注意归一化系数K的处理。K是所有空集交叠质量乘积之和它反映了两路证据的冲突程度。如果K趋近1说明两路证据几乎完全矛盾比如传感器A强烈认为是车辆传感器B强烈认为是行人此时组合结果对K极其敏感微小数值误差都会被放大。我建议代码里做一个冲突系数阈值判断K高于某个上限比如0.8时丢弃融合结果改用单路高置信证据或者输出未知类型而不是强行组合出一个没有依据的结论。实现Dempster组合规则时有几个数值细节值得注意。第一浮点数累加时K会存在误差质量极小的焦元可以设置一个截断阈值比如1e-6低于阈值的质量直接清零再重新归一化避免淹没在后续计算里。第二如果有多路证据不要一次性把所有BPA塞进组合规则里而是两两递归组合。组合顺序对结果有影响但不是理论缺陷实际工程项目里一般按传感器可信度从高到低组合这样低可信传感器引入的噪声影响更小。第三组合后检查是否出现了不正常的全零质量这通常意味着某路BPA构造错误尤其是出现了m(空集)不为0的情况一定要在BPA构造函数入口做断言检查。3. 信息矩阵融合与目标关联的实现要点3.1 信息矩阵与协方差的关系为什么要用信息矩阵状态融合这一路我选择用信息矩阵而不是直接加权平均原因是信息矩阵在“融合多路具备不同不确定性的估计”时数学形式非常自然。对一个高斯分布估计信息矩阵定义为协方差矩阵的逆矩阵信息向量定义为信息矩阵乘以状态向量。两路相互独立的估计做融合时融合后的信息矩阵等于各路信息矩阵之和融合后的信息向量等于各路信息向量之和这个加法操作不需要额外计算权重信息量大的估计自动占据主导地位这是卡尔曼滤波和信息滤波的等价性推导出来的结果。我在Matlab里写这段代码时会用状态向量x [px, py, vx, vy]协方差矩阵P。每个传感器上报目标时会附带对应的P里面包含位置方差、速度方差以及px和vx等状态量之间的协方差项。融合前先判断两路输入是否确实是同一个目标然后做信息矩阵融合。举个例子摄像头测位置精度高协方差较小雷达测速度精度高协方差在速度维度上更小。融合后位置维度摄像头贡献大速度维度雷达贡献大得到的协方差矩阵每个维度都比单路估计小。这个效果在加权平均方案里很难自然体现因为固定权重不会随场景变化。有一点我在实际代码里踩过坑直接用协方差求逆计算信息矩阵时如果协方差矩阵病态比如某个维度方差极小接近1e-8量级求逆结果会爆炸导致融合结果震荡。解决方案是加正则项或者做特征值截断。特征值截断的逻辑是小于最大特征值1e-6倍的特征值强制设为该阈值再重构对称正定矩阵。Matlab里用eig分解之后修正再组合代码量不大但对数值稳定性提升非常明显。3.2 时间同步与空间对准目标级融合有一件绕不开的脏活时间同步和空间对准。时间不同步直接融合出来的位置全是指向不同时刻的毫无意义。我用的方案是选择融合主时间基准一般是精度高、更新率稳定的那路传感器的时间戳其他传感器通过线性插值或者恒定速度外推把目标位置和速度对齐到主时间戳上。数据更新率差异大的情况下比如激光雷达10赫兹、毫米波雷达20赫兹我会选择缓存最近两帧目标状态在融合瞬时做线性外推代码实现不复杂但很有效。空间对准是坐标系转换问题。每路传感器都有自己的安装位置和朝向角目标位置要从传感器坐标系转换到统一的车辆坐标系。这个转换包含平移和旋转安装参数可以通过标定得到。我在代码里建议把坐标系转换封装成一个独立函数sensor_to_vehicle(x_sensor, y_sensor, sensor_pose)其中sensor_pose包含安装位置的x,y偏置和航向角。注意航向角的正方向定义要统一别一个传感器用度、另一个用弧度这种低级错误排查起来非常折磨人。统一转换后再检查目标是否在合理范围内比如目标在车辆后方30米却在雷达上报里显示为前方目标这种大概率是坐标转换或传感器安装参数出了问题。3.3 目标关联马氏距离与属性联合门限关联是目标级融合承上启下的关键步骤关联错了后面一切融合都白费。我先用空间位置做初步关联两张目标观测的差值除以联合协方差得到马氏距离公式是d (z1-z2)^T (P1P2)^-1 (z1-z2)。这里用联合协方差而不是某个固定阈值是为了适应不同传感器精度差异。摄像头位置精度高同样5米的欧式距离在马氏距离里会被放大雷达相反这就要求关联门限必须同时考虑位置差和协方差否则容易把不同目标误关联或把同一目标漏关联。光靠空间关联还不够。两个距离很近的目标比如一个行人贴着车站雷达和摄像头分别检测如果空间位置相近马氏距离可能过门但属性一个是行人摄像头一个是车辆雷达这种关联值得怀疑。所以在空间关联的基础上我加了属性一致性检查把两路目标类型转化成BPA形式计算它们的证据距离超过阈值就拒绝这段关联。用证据距离而不是简单地比较概率最高的类别是因为BPA中“未知”质量的存在会让“车辆vs车辆”和“车辆vs行人的全集”在语义上有微妙差异证据距离能更合理地量化这种差异。关联正确性直接影响后续融合效果我在代码里维护一个关联矩阵每行代表融合目标ID每列代表传感器来源矩阵值标识某路传感器是否关联到对应目标这样后续实现时逻辑清楚调试也方便。关联算法建议从最简单的最近邻法开始确认整体流程正确后再考虑复杂算法贪心最近邻在目标稀疏、传感器质量好的场景下表现并不差贸然上多目标指派算法反而增加了调试难度。4. Matlab代码实现的完整流程与关键节点4.1 仿真场景与传感器模型设计Matlab仿真的第一步是定义场景我设计了一个四车加一个行人的路口场景。在车体坐标系下目标1是正前方25米以60km/h行驶的轿车目标2是左侧车道15米处以50km/h行驶的SUV目标3是前方8米处静止站在人行道旁的行人目标4是右侧30米处对向行驶的自行车。每路传感器都有各自特性摄像头位置精度高但速度精度一般类型识别对行人和车辆较好但晚上容易把自行车误判成行人毫米波雷达速度精度高但位置横向精度差对静止目标的检测能力弱激光雷达位置和尺寸精度高但远距离点云稀疏时类型判别不敢下结论。这些差异都体现在传感器协方差矩阵和BPA构造参数里让仿真结果贴近真实。仿真循环以0.1秒为步长共20秒对应200帧。每帧中按一定检测概率决定传感器是否检测到目标比如激光雷达对行人的检测概率设为0.85毫米波雷达对行人的检测概率只有0.6以此模拟漏检。生成传感器观测时在真值上加协方差对应的随机扰动再把目标类型置信度输入到混淆矩阵模型里得到分类输出。这样一套流程跑下来得到了三路带噪、带漏检、偶尔分类错误的目标列表作为融合算法的输入。4.2 核心函数拆分与关键代码逻辑代码拆分遵循“一个函数只做一件事”的原则main_simulation.m负责场景循环和数据流控制特殊处理的部分封装成独立函数。BPA构造函数make_bpa_from_classifier接收分类输出向量和混淆矩阵按2.2中的打折方案生成BPA。比如摄像头分类器输出车辆0.8、行人0.1、自行车0.1混淆矩阵主对角线表示各类型正确识别率其他位置表示误判率函数内部会动态计算质量分配。可以看下这个函数的核心逻辑function bpa make_bpa_from_classifier(type_prob, confusion, unknown_mass) % 输入type_prob是分类器输出的类型概率向量 % confusion是混淆矩阵unknown_mass是分配给全集的不确定质量 % 输出bpa结构体包含焦元集合和质量 n_type length(type_prob); bpa.focal {}; bpa.m []; % 保留部分质量给全集表达不确定性 bpa.focal{1} 1:n_type; bpa.m(1) unknown_mass; % 剩余质量按分类概率分配到单元素焦元 remaining 1 - unknown_mass; for t 1:n_type bpa.focal{end1} t; bpa.m(end1) remaining * type_prob(t); end % 用混淆矩阵进一步打折可靠度 reliability mean(diag(confusion)); bpa.m(1) 1 - reliability * (1 - bpa.m(1)); for t 1:n_type bpa.m(t1) reliability * bpa.m(t1); end % 归一化保证和为1 bpa.m bpa.m / sum(bpa.m); endDS组合函数ds_combine是实现Dempster规则的模块输入是两个bpa结构体输出组合后的bpa。核心是双层循环遍历所有焦元组合计算交集和冲突累加。组合后用冲突系数K做一致性判定K超过阈值时返回一个明确的冲突标记由上层决定处理策略。这个函数我建议固化后做单元测试用几个经典手算案例验证输出比如车辆0.9和质量1.0的证据组合结果应该还是车辆0.9这种守恒性质能帮你很快排除bug。信息矩阵融合函数info_matrix_fusion的输入是两个目标状态x1、x2及协方差P1、P2先对P做特征值截断修正再求逆得到信息矩阵按信息形式加权得到融合状态。代码实现时要特别注意矩阵维度一致性Matlab里四维状态对应的协方差是4x4矩阵索引错一位很容易出现维度不匹配的报错。4.3 融合决策输出与可视化评估融合完成后需要输出决策结果并评估质量。属性通道对每个目标计算融合BPA后依据信任函数和似然函数判定类型若最大信任度超过0.7且对应不确定区间宽度小于0.3才输出具体类型否则输出“未知”。这个保守策略对下游的规划模块非常重要乱猜一个类型导致的后果比承认不知道严重得多。状态通道输出融合后的目标位置、速度和协方差椭圆。可视化方面我用Matlab的figure和plot函数绘制车体坐标系下的目标分布图。用不同颜色标记不同传感器上报的目标用椭圆画置信区间融合后目标用另一个颜色加粗绘制。我还叠加一个Confusion/Acceptance面板显示200帧内每个目标的类型判定结果和关联关系错误关联和类型误判在这种图上一眼可见。这个可视化面板帮我找到了不少代码问题强烈建议在实际项目里也做类似的可视化。另外写一个metrics函数统计平均位置误差相对单传感器误差、类型判定正确率、漏关联率和误关联率。我跑仿真时发现融合后的平均位置误差一定要明显优于任何单传感器否则融合就没有工程意义。一般来说融合误差比最优单传感器提升20%到30%是合理的预期如果提升不明显大概率是协方差估计偏大或者融合权重不合理。5. 常见问题与排查技巧实录5.1 高冲突场景下组合结果反直觉调试中最常见的问题就是K值很大时Dempster规则给出违反直觉的结果。我举一个真实场景摄像头识别目标A为车辆置信度0.9雷达识别同一目标为行人置信度0.8。经典Dempster组合后算法结果可能倾向于支持一个两者都不是的结论或者输出不稳定的低置信度结果。这本质是因为组合规则把冲突证据当作“待归一化的噪声”而冲突大时这个噪声会主导结果。我采用的处理方法分三步。第一步是关联阶段先做属性一致性校验证据距离过大的关联对直接拒绝不进入融合第二步是给每路传感器的BPA引入基于历史可靠性的折扣系数历史性能差的传感器贡献自动削弱第三步是设置冲突阈值K超过0.8时不对两路证据做Dempster组合而是执行“选择信任度更高者”或“输出未知”策略。这样处理之后仿真中即使出现传感器互相台杠的情况融合输出也不会出现离谱的错误。5.2 同一目标被重复融合导致过置信这个问题隐蔽但后果严重。同一个物理目标可能在一帧内被同一传感器输出两个跟踪轨迹比如摄像头在目标被短暂遮挡时生成了两个ID融合模块如果没识别出两个关联假设描述的是同一个目标会把两路“实际上是同一个来源”的证据当独立证据做组合DS组合规则会把置信度推到接近1的虚高水平。更复杂的情况是历史帧已经融合过一次当前帧又用同样的传感器测量做融合累积置信度看上去越来越强实际上是在重复计算同一个来源。解决办法是融合模块里维护一个目标生命周期表记录每个融合目标在最近N帧关联到的传感器ID列表。融合前检查当前证据的来源是否已经存在于目标历史证据源中如果存在不重复组合BPA只更新状态估计或者对重复证据做信息矩阵层面的一致性处理。这个机制在状态通道同样重要信息矩阵融合假设各来源独立如果违反独立性信息会膨胀协方差被低估Matlab里表现为椭圆越画越小但实际上位置还在漂移。5.3 信息矩阵奇异与数值不稳定最开始跑代码时遇到过信息矩阵求逆报错的情况。协方差矩阵在某些场景下会变得接近奇异比如速度方差极小或位置方差极小对应信息矩阵元素巨大融合时状态更新剧烈震荡。这不是算法原理有问题是数值层面没有做保护。我后来在求逆前统一做一次对称正定修正先用eig分解得到特征值向量d和特征向量矩阵V再把小于最大特征值0.000001倍的特征值替换为该值然后重构矩阵最后再求逆。这个操作在高维状态空间下也能稳定工作。还有一个容易忽视的细节是信息矩阵相加时如果直接对原协方差矩阵求逆再加和计算量虽小但浮点误差会被放大。折中方案是先对协方差做Cholesky分解再用分解后的下三角矩阵操作精度更好Matlab里chol函数的数值稳定性比inv好很多。如果仿真中目标数量大增导致组合爆炸提前把循环改成矩阵运算可以在保证仿真实时性的同时减少数值误差。5.4 漏检与遮挡情况下的一次实战处理我印象比较深的一次调试是仿真中设置了行人被前方车辆短暂遮挡的场景。遮挡期间摄像头漏检雷达返回一个“未知”类型且位置扰动较大的观测。按照最初的代码逻辑漏检传感器不参与融合融合结果退化为单传感器类型置信度掉得很快决策模块在几帧内不断变换对目标类型的判断导致了后续规划动作的犹豫。这个现象在真实系统里同样存在解决的关键是“记忆与预测”。我在融合模块中增加了一个泄漏积分机制如果当前帧某传感器漏检不立即清零它的贡献而是把上一帧该传感器对该目标的BPA做时间折扣后保留如果连续多帧漏检折扣因子逐渐衰减最终自动退出。这样在短暂遮挡期间融合类型置信度不会剧烈跳动位置估计也可以依靠预测模型维持。这套机制的实际效果是目标类型判定在遮挡期间保持了稳定明显减少了下游模块的误决策。再分享一个排查方法和协作建议。如果你跑代码时发现融合结果比单传感器还差先别急着调算法参数先可视化传感器各自的输出以及融合输出逐帧点查关联是否错误。大部分问题出现在关联而不是融合本身建议写一个关联结果回放的脚本模块逐帧慢放盯住跳变和错配的马氏距离。如果发现DS融合的置信度始终过于乐观检查BPA构造里是不是“未知”质量分配得太少给全集的质量调大一些往往能显著改善序贯融合的稳定性。按我的经验这套“状态通道信息矩阵 属性通道DS证据”的组合框架适合在中等规模的工程原型里落地代码可控、性能稳定、逻辑清晰也不会引入过重的计算负担。如果后续需要扩展到相机与激光雷达原始数据的深度融合可以在这个框架里继续增加特征层分支但注意保持目标层信号处理与决策模块解耦避免以后替换底部模型时让上层不兼容。仿真参数建议用随机种子固定之后再逐步调不同随机数下结果差异往往比参数差异还大对着充满随机性的输出调参数容易调到“正好适配某个随机种子”的假象白白浪费时间。这个内容其实还能继续延伸比如把分类器输出的软概率用温度系数标定后再进BPA或者用证据距离做自适应的传感器权重调节都能明显提升融合质量。我从这套仿真代码里收获的最大经验其实不是某个公式而是意识到“知道不知道”才是融合算法最难也是最有价值的部分——能够合理表达不确定性比一味追求高置信度的说服力强得多。