企业官网建设流程全解析

简介：面向自动驾驶中的多传感器融合与目标跟踪，这份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 四个雷达和一个激光雷达的参数表

雷达在仰角方向的数据生成器没有给出通道数，输出是二维点迹；激光雷达的 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第一个元素是方位角，第二个是仰角。AzimuthResolution=6表示相邻扫描线之间的角度间隔，它决定了同一个目标在雷达点迹里最多产生几个反射点。UpdateRate=20表示雷达帧率 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)); end

ClusterThreshold是点云聚类的核心参数。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(end+1) = struct(... 'TrackID', radarTracks(i).TrackID * 10 + lidarTracks(j).TrackID, ... 'State', fusedState, ... 'Covariance', fusedCov); %#ok<AGROW> break; end end end % 未匹配的轨迹在这里保留，实际文件中会继续处理 end

这段代码只融合了位置分量，速度、朝向没有参与，因为雷达和激光雷达跟踪器对速度的估计方式差别较大，直接加权会产生速度震荡。helperRadarLidarFusionFcn.m里做了完整的公共状态映射，这里为了可读性做了降维。inv函数让公式直观，数值上建议用mldivide或pageinv。权重w=0.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 米外时，激光雷达每个扫描周期能收到的点数远低于MinPoints=5，因此没有激光雷达轨迹，融合输出完全来自雷达；目标接近到 70 米后，激光雷达轨迹出现，融合轨迹的位置噪声明显下降。这说明MinPoints不仅影响单传感器轨迹，还直接影响融合开始作用的距离。想更早融合，可以把MinPoints降到 3，同时把ClusterThreshold提高到 1.5 米，但代价是杂点轨迹变多，需要配合AssignmentThreshold压低虚警。

5.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如果和实物标定结果不一致，融合在传感器视场边界会出现目标速度突变，这也是实车应用中需要先做激光雷达标定的原因。示例中没有给出标定流程，可以直接把这部分当成下一步扩展点。

本文还有配套的精品资源，点击获取