简介面向自动驾驶中的多传感器融合与目标跟踪这份MATLAB程序包演示了雷达与激光雷达数据的跟踪级融合实现方法。示例使用扩展目标跟踪器处理雷达测量值使用JPDA跟踪器处理激光雷达测量值再通过轨道级融合框架生成统一目标轨迹同时提供驾驶场景、雷达/激光雷达传感器模型配置参数如雷达视场角、方位角分辨率激光雷达360°视场与32高程通道等方便还原实验环境。压缩包共14个文件包括8个.m源码、4个gif动态演示和2个png流程示意图整体大小4.61MB覆盖仿真场景构建、跟踪算法实现、融合逻辑展示与可视化面板便于按模块阅读和二次开发。目前已有369人学习适合正在研究目标级/轨迹级融合算法的读者可直接运行示例观察各环节输出并结合工作流图示快速理解雷达、激光雷达融合的完整链路。1. 跟踪级融合为什么雷达和激光雷达不直接在点云层合并在自动驾驶多传感器融合里雷达和激光雷达的“跟踪级融合”不是把点云扔进同一个网络而是让两条轨迹在状态空间里对齐。很多项目做到最后才发现早期点云融合对时间同步和标定误差太敏感稍微换一批传感器数据就出现目标重复。这个资源是一套可运行的 MATLAB 示例来自 RadarLidarTrackFusionExample四个 2D 雷达负责远距离稳定跟踪一台 3D 激光雷达负责近距离稠密点云。整体分两步先各自用独立跟踪器生成轨迹列表再通过轨迹级融合输出统一的目标列表。包里的主脚本RadarLidarTrackFusionExample.m和几个 helper 函数已经把流程串好跑通后按自己的传感器参数替换即可。适合正在做多传感器感知、自动驾驶仿真和传感器融合验证的工程师。2. 场景与传感器建模用驾驶场景工具箱搭建雷达和激光雷达的“共同世界”2.1 为什么要用场景仿真而不是直接回放真实数据这个示例没有直接回放路测数据而是用自动驾驶工具箱的drivingScenario构建场景。原因很简单真实传感器数据很难做可重复实验你想看“目标从左侧切入并横穿雷达视场”的片段需要在原始数据里翻很久仿真模型里改一个坐标就能生成。helperCreateRadarLidarScenario.m负责创建道路、自车和周围目标车ego 上安装四个 2D 雷达和一个 3D 激光雷达目标车从前方和侧向切入把远距离目标稳定跟踪、侧向雷达交接、近距离激光雷达密集点云这几个关键场景压缩到一个可复现的脚本里。仿真场景的另一个好处是每个目标都有 ground truth后面量化融合误差时能直接计算 RMSE 或 OSPA真实路测基本做不到这么方便。所以先用这个示例把融合链路跑通再切换自己的数据是代价最低的路线。2.2 四个雷达和一个激光雷达的参数表传感器数量方位视场仰角视场方位角分辨率主要覆盖区域前后向雷达245°-6°车辆前方和后方远距离左右侧雷达2150°-6°车辆两侧3D 激光雷达1360°40°0.2°车辆周围 360°雷达在仰角方向的数据生成器没有给出通道数输出是二维点迹激光雷达的 32 个高程通道由 40° 仰角视场和 1.25° 高程分辨率共同决定每个通道承担不同仰角这就是点云在垂直方向分层的原因。另一个值得注意的点是四个雷达视场加在一起并不是无缝的 360° 覆盖。前后雷达视场 45°左右雷达 150°车辆四角方向存在小范围盲区激光雷达的 360° 方位视场正好补上这部分。但激光雷达的仰角只有 40°且 32 个高程通道分布不均匀低处目标点云密集高处目标可能只有一两个点。所以融合后轨迹在跨越传感器视场边界时如果不处理轨迹交接会出现位置跳变。这套参数对比也很能说明问题。激光雷达方位角分辨率 0.2°在 50 米处相邻扫描线的间距大约 0.17 米能清晰区分两辆车。雷达方位角分辨率 6°在 50 米处角向间隔约 5.2 米100 米处约 10.5 米一个 4 米宽的目标在雷达看来就是一个点或者几个反射点。因此后端融合不能把两者当成同精度的测量源必须用协方差矩阵表达各自的不确定性。2.3 创建场景和传感器的关键调用% 创建驾驶场景采样周期 0.05s对应 20Hz scenario drivingScenario(SampleTime, 0.05); road(scenario, [0 -20 0; 200 -20 0], Lanes, 4); % 自车以 20m/s 沿 x 轴正方向行驶 ego vehicle(scenario, Position, [10 0 0], Velocity, [20 0 0]); % 前方远距离目标用于测试雷达远距离跟踪 targetVehicle vehicle(scenario, Position, [120 -3 0], ... Velocity, [18 0 0]); % 前向雷达安装在车头中央 radarFront drivingRadarDataGenerator(... SensorIndex, 1, ... MountingLocation, [3.4 1.2 0.5], ... MountingAngles, [0 0 0], ... FieldOfView, [45 10], ... AzimuthResolution, 6, ... UpdateRate, 20);drivingRadarDataGenerator的FieldOfView第一个元素是方位角第二个是仰角。AzimuthResolution6表示相邻扫描线之间的角度间隔它决定了同一个目标在雷达点迹里最多产生几个反射点。UpdateRate20表示雷达帧率 20Hz激光雷达通常是 10Hz后面融合循环要以较慢的传感器为基准。实际打开helperCreateRadarLidarScenario.m能看到完整版本左右雷达的SensorIndex不同用于区分测量来源。激光雷达点云生成器的参数在这个脚本里也会一并配好主循环不需要关心点云是怎么生成的。2.4 激光雷达点云生成与数据格式激光雷达点云生成器按照激光雷达原理工作每个通道固定仰角水平旋转时按方位角分辨率发射脉冲返回pointCloud对象。比如 32 通道、方位角分辨率 0.2°水平旋转一圈产生约 1800 列点每列 32 个高程点一帧最多接近五万多个点但大量点落在地面。这个数据量直接送入跟踪器不现实所以示例中的HelperBoundingBoxDetector内部做了地面分割和降采样剔除地面点后再聚类否则聚类阈值会被地面点撑大目标框会被拉长。时间同步上雷达 20Hz、激光雷达 10Hz 的差异必须在融合前处理。常见做法是在每个激光雷达帧到来时用最近邻时间查找对应的雷达帧如果时间差大于半个雷达周期就丢弃或插值。示例主循环按激光雷达帧步进因为激光雷达帧率更低融合输出频率不会超过 10Hz。2.5 坐标系统一与融合的输入输出雷达和激光雷达数据都在自车坐标系下但雷达返回的是极坐标距离、方位角、多普勒激光雷达返回的是笛卡尔三维点云。跟踪器内部必须先统一坐标系雷达检测通常这样转换% 雷达测量 [range; angle; doppler] [x, y] pol2cart(measurements(:, 2), measurements(:, 1)); cartMeas [x, y];pol2cart第一个参数是角度第二个参数是距离输出的cartMeas就是自车系下的 x/y 位置。转换后雷达和激光雷达轨迹才能表示成统一的 [x; y; vx; vy] 状态。需要注意雷达的多普勒速度是径向速度不能直接当 vx 用必须结合目标方位角分解到 x/y 方向否则融合后的速度向量会偏向传感器径向。3. 单传感器跟踪JPDA 跟踪器和扩展对象跟踪器的分工3.1 为什么激光雷达用 JPDA雷达用扩展对象跟踪激光雷达聚类后候选框很多目标靠近时同一个目标可能被两个相邻框覆盖。JPDA 不像最近邻那样必须把每个检测分配给唯一轨迹而是对每个检测和每条轨迹的关联概率做加权天然容忍这种模糊。雷达侧则不同同一辆车在近距离会产生多个反射点直接把这些反射点当成独立目标会导致一辆车出现多条轨迹。扩展对象跟踪器的思路是把一个目标产生的全部反射点作为一组量测再送入滤波器。这里要说明示例中的helperRadarTrackingAlgorithm.m并不是复杂的目标形心估计算法而是先把多反射点聚类成单个检测再交给 JPDA 跟踪器效果上等价于扩展对象跟踪。这里的 JPDA 跟踪器内部维护一个混合状态位置、速度、目标尺寸和朝向。激光雷达检测框提供了边界框观测所以initBoxFilter会把状态初始化成 [x; y; vx; vy; length; width; yaw]。雷达侧的状态通常只有 [x; y; vx; vy]个别实现会加 yaw 率。融合时如果两边的状态向量的物理意义不一致需要先做投影示例中统一取前四维位置速度就是避免把激光雷达框的长度宽度直接和雷达的速度状态混在一起。3.2 激光雷达侧聚类、边界框和 JPDA 跟踪器% 用聚类和边界框拟合把一帧点云变成检测列表 detector HelperBoundingBoxDetector(... ClusterThreshold, 1.2, ... % 同一目标点云之间的最大距离单位米 MinPoints, 5, ... % 少于5个点的簇不输出抑制地面杂点 BoundingBox, [4 1.8 1.5]); % 目标先验长宽高单位米 % JPDA 跟踪器滤波器初始化函数返回扩展状态 lidarTracker trackerJPDA(... FilterInitializationFcn, (det) initBoxFilter(det), ... AssignmentThreshold, 30, ... TrackLogic, History); % 主循环每帧点云经检测器转成检测再更新轨迹 for frame 1:numel(pointClouds) detections detector(pointClouds{frame}); tracks lidarTracker(detections, timeStamps(frame)); endClusterThreshold是点云聚类的核心参数。0.2° 方位角分辨率下30 米处两个相邻点的距离约 0.1 米1.2 米阈值可以把同一辆车的点聚在一起又不会把 3 米外的护栏连进来。MinPoints用来过滤稀疏噪点但也会滤掉远距离目标100 米外的一辆轿车可能只剩十来个点如果丢点后低于 5就不会输出检测。因此近距离和远距离需求是矛盾的开箱配置一般取 3 到 5之后根据实际点云密度调整。trackerJPDA的AssignmentThreshold决定检测和轨迹关联的最大马氏距离30 对应较高的门限激光雷达点云检测协方差很小门限太大会把杂点吸进来我一般从 15 开始试看轨迹 ID 跳变的程度。3.3 雷达侧从原始点迹到扩展对象检测% 雷达一帧可能包含多个反射点先按距离和角度聚类 measurements radarData.Measurement; % Nx3 [range angle doppler] clusterIdx clusterDBSCAN(measurements(:,1:2), 2.5, 3); % 把每个簇转成 objectDetection送入跟踪器 detections cell(max(clusterIdx), 1); for k 1:numel(detections) group measurements(clusterIdx k, :); position group(:,1) .* [cos(group(:,2)), sin(group(:,2))]; detections{k} objectDetection(radarData.Time, mean(position, 1), ... MeasurementNoise, blkdiag(1, 1)); end % 雷达跟踪器 radarTracker trackerJPDA(... FilterInitializationFcn, initRadarFilter, ... AssignmentThreshold, 15, ... ClutterDensity, 1e-6);clusterDBSCAN的距离阈值 2.5 米参考了雷达距离分辨率。如果距离分辨率是 2.5 米阈值就不能小于它否则同一个目标的反射点会被拆开但阈值太大两个并行目标会被合并。MeasurementNoise设为blkdiag(1, 1)表示位置标准误约 1 米这个值会直接影响后续融合时协方差矩阵的尺度。helperRadarTrackingAlgorithm.m里还会把多普勒、RCS 等信息带进检测但位置部分和高斯噪声的配置和上面这段相似。雷达跟踪器输出的轨迹结构包含TrackID、State、StateCovariance和激光雷达轨迹格式一致这是融合函数能直接比较的前提。3.4 两条轨迹的时间对齐% 雷达 20Hz激光雷达 10Hz按激光雷达时间戳取最近的雷达轨迹 sampleTime 0.05; for tLidar 0:0.1:20 idxRadar round(tLidar / sampleTime) 1; fused helperRadarLidarFusionFcn(... radarTracksAll{idxRadar}, lidarTracksAll{tLidar / 0.1 1}, tLidar); end这段代码示意了时间对齐的基本思路实际示例中两条轨迹都在循环内由 helper 函数返回不需要手动索引数组。round会造成最多半个雷达周期的时间误差高速场景下会引入几厘米到几十厘米的位置偏差更稳的做法是线性插值。如果要做线性插值需要对雷达轨迹的 State 做加权平均同时保留协方差矩阵简单做法是state (1-a)*state1 a*state2; cov (1-a)*cov1 a*cov2;其中 a 是时间比例。注意这不是严格最优因为交叉协方差被忽略了但在跟踪级融合里通常足够用。3.5 单传感器轨迹的数据结构跟踪器输出的轨迹列表是结构体数组每个元素包含TrackID、Age、State、StateCovariance。State的长度由FilterInitializationFcn决定激光雷达和雷达通常不一样。融合函数第一步要做状态对齐把雷达的 [x;y;vx;vy] 和激光雷达的 [x;y;vx;vy;length;width] 映射到公共状态。示例中helperRadarLidarFusionFcn只取前 4 维后几维留给可视化。如果两条轨迹的State长度差异过大直接索引State(1:4)可能越界调用前要判断numel(State)。这是从单传感器跟踪切到多传感器融合最常见的报错我一般在融合函数入口加一行assert(numel(radarTracks(1).State) 4)把错误暴露在初始化阶段而不是在循环中突然崩溃。4. 跟踪级融合把两条轨迹列表合成一个可用的目标列表4.1 先关联再融合顺序不能反跟踪级融合最容易犯的错误是不做关联直接把雷达轨迹和激光雷达轨迹相加求平均。两者坐标系统一但同一个目标的两条轨迹位置可能相差 2 米直接平均会把目标拉到错误位置。正确流程是先用马氏距离判断两条轨迹是否属于同一目标通过关联门限的轨迹才进入融合未匹配的轨迹保留单独输出避免漏检。马氏距离和欧氏距离的区别在于它把协方差也放进距离计算。雷达轨迹在距离维误差大激光雷达轨迹在横向误差大马氏距离会按两个方向的不确定性自动加权如果用欧氏距离定位误差大的传感器会主导关联结果导致错误配对。融合函数返回的轨迹列表还需要做航迹管理。雷达单独跟踪时一条轨迹可能连续 10 帧没有观察激光雷达因为点云衰减临时丢了几帧如果立即删除融合轨迹下游决策会看到目标消失又出现。一般做法是给融合轨迹增加Age计数连续消失超过 3 帧才删除。这个逻辑在helperRadarLidarFusionFcn.m末尾的未匹配轨迹处理里能体现如果直接注释掉会看到目标闪烁。4.2 协方差交叉融合的实现function fusedTracks helperRadarLidarFusionFcn(radarTracks, lidarTracks, time) fusedTracks struct(TrackID, {}, State, {}, Covariance, {}); if isempty(radarTracks) || isempty(lidarTracks) return; end for i 1:numel(radarTracks) rPos radarTracks(i).State(1:2); rCov radarTracks(i).StateCovariance(1:2, 1:2); for j 1:numel(lidarTracks) lPos lidarTracks(j).State(1:2); lCov lidarTracks(j).StateCovariance(1:2, 1:2); % 马氏距离门限95%置信度2自由度 mahal (rPos - lPos) / (rCov lCov) * (rPos - lPos); if mahal 5.99 % 协方差交叉固定权重 0.5 w 0.5; invRCov inv(rCov); invLCov inv(lCov); fusedCov inv(w * invRCov (1-w) * invLCov); fusedState fusedCov * (w * invRCov * rPos ... (1-w) * invLCov * lPos); fusedTracks(end1) struct(... TrackID, radarTracks(i).TrackID * 10 lidarTracks(j).TrackID, ... State, fusedState, ... Covariance, fusedCov); %#okAGROW break; end end end % 未匹配的轨迹在这里保留实际文件中会继续处理 end这段代码只融合了位置分量速度、朝向没有参与因为雷达和激光雷达跟踪器对速度的估计方式差别较大直接加权会产生速度震荡。helperRadarLidarFusionFcn.m里做了完整的公共状态映射这里为了可读性做了降维。inv函数让公式直观数值上建议用mldivide或pageinv。权重w0.5意味着完全对称但雷达在远距离更可靠激光雷达在近距离更可靠更合理的做法是根据目标距离动态调整权重比如距离大于 80 米时给雷达更高权重。协方差交叉之所以在这里比最小方差融合更稳是因为最小方差融合需要知道两条轨迹的互协方差而互协方差通常无法精确获得。两条轨迹虽然来自不同传感器但都用同一个场景的状态转移模型预测下一帧预测噪声相同所以误差高度相关。CI 用权重 w 和两个协方差矩阵的凸组合近似保守估计不知道互协方差也能工作。代价是估计结果稍微保守在传感器精度差异特别大的场景下融合精度会比最小方差略低但不会出现发散。4.3 主程序执行流程cd /path/to/RadarLidarTrackFusionExample run RadarLidarTrackFusionExample.m在 MATLAB 中打开示例目录运行主脚本后会出现两个窗口一个三维场景显示展示自车、雷达点迹、激光雷达点云和融合轨迹另一个是俯视面板显示helperPanelDisplay.m绘制的目标航迹。运行结束后工作区有fusedTracks、radarTracks、lidarTracks等变量可以直接用来做后续误差分析。如果发现某条轨迹没融合优先检查两个跟踪器输出的StateCovariance尺度是否差一个数量级。常见原因是雷达协方差初始值设置太小导致马氏距离认为两条轨迹不是同一目标诊断方法是打印mahal看它离门限 5.99 有多远。4.4 轨迹 ID 重映射是最后一步雷达轨迹 ID 和激光雷达轨迹 ID 各自从trackerJPDA内部重新编号它们不属于同一命名空间。假设雷达第 3 条轨迹和激光雷达第 7 条轨迹融合成新轨迹直接取雷达 ID 3 或激光雷达 ID 7 都可能和另一个目标冲突。示例中把TrackID设置成radarID * 10 lidarID既保留来源信息又避免和单传感器轨迹 ID 重复。下游路径规划器通常要求轨迹 ID 从 1 开始连续编号所以融合函数输出前还要做一次压缩映射。这个细节容易被忽略但多目标场景下 ID 错乱会造成目标数量忽多忽少的表现问题调试时如果遇到目标闪烁先检查 ID 是否唯一。4.5 运行结果解读运行示例后把fusedTracks中轨迹位置按时间连成线叠加在场景图上。如果轨迹在雷达视场和激光雷达视场交界处出现台阶状跳跃说明外参或时间对齐有问题。如果轨迹数量在目标靠近时从 1 变 2再变回 1则关联门限过紧。如果融合后的轨迹始终比单传感器轨迹更抖通常是协方差交叉权重固定为 0.5 导致低精度传感器把高精度结果拉偏把权重改成距离函数后再看。5. 远距离目标融合的坑从 LongRangeTargetFusion.gif 里能学到什么5.1 远距离行为与激光雷达 MinPoints 的矛盾LongRangeTargetFusion.gif演示了远距离目标从雷达单独跟踪到融合跟踪的过程。目标在 120 米外时激光雷达每个扫描周期能收到的点数远低于MinPoints5因此没有激光雷达轨迹融合输出完全来自雷达目标接近到 70 米后激光雷达轨迹出现融合轨迹的位置噪声明显下降。这说明MinPoints不仅影响单传感器轨迹还直接影响融合开始作用的距离。想更早融合可以把MinPoints降到 3同时把ClusterThreshold提高到 1.5 米但代价是杂点轨迹变多需要配合AssignmentThreshold压低虚警。5.2 调参检查顺序参数所在文件失败现象调整建议MinPointsHelperBoundingBoxDetector.m远距离无激光雷达轨迹降到 3杂点增多再提高ClusterThresholdClusterThresholdHelperBoundingBoxDetector.m目标框分裂或合并0.2° 分辨率下取 1.01.5 米AssignmentThreshold两个跟踪算法轨迹跳变或丢失激光雷达 1530雷达 1540融合马氏门限helperRadarLidarFusionFcn.m同目标出现两条轨迹用chi2inv(0.95, 2)动态计算5.3 用真值评估融合是否有效metrics trackErrorMetrics(ErrorPosition, rmse); assignments metrics.assign(fusedTracks, groundTruth); metrics.rmse(assignments);trackErrorMetrics返回融合轨迹相对真值的 RMSE。如果融合后的 RMSE 大于雷达单独跟踪的 RMSE说明融合权重或关联有问题优先检查协方差是否被低估。协方差被低估的典型现象是mahal普遍远小于门限但轨迹位置抖动反而更严重。把融合权重改成距离函数通常能缓解。在 MATLAB 工作区里可以直接用fusedTracks(end).State(1:2)查看最后一个融合轨迹的位置不过要先确认fusedTracks非空。更直观的方式是看示例里的helperLidarRadarTrackFusionDisplay.m它会把雷达轨迹、激光雷达轨迹和融合轨迹画成不同颜色如果融合轨迹始终偏向雷达一侧说明权重或协方差设定不合理。另外MountingLocation和MountingAngles如果和实物标定结果不一致融合在传感器视场边界会出现目标速度突变这也是实车应用中需要先做激光雷达标定的原因。示例中没有给出标定流程可以直接把这部分当成下一步扩展点。本文还有配套的精品资源点击获取