学Simulink——毫米波雷达与激光雷达的数据融合通信

 目录

手把手教你学Simulink——毫米波雷达与激光雷达的数据融合通信

一、系统目标与融合层级

1.1 传感器分工（教学初值）

1.2 三种融合架构

二、场景与坐标基准

2.1 坐标链

2.2 时间同步

三、毫米波雷达建模

3.1 理想/概率目标模型（快、做融合首选）

3.2 FMCW物理层模型（要测距测速曲线时用）

3.3 噪声与误差

四、激光雷达建模

4.1 点云生成

4.2 点云预处理（进融合前必做）

五、融合与跟踪模型

5.1 目标级融合Simulink树（推荐首发）

5.2 滤波与运动模型

5.3 关联与航迹管理

5.4 异步与通信延迟

六、通信回传与协同

6.1 回传内容分级

6.2 通信-融合联合指标

七、关键代码示例

7.1 场景+双传感器+融合脚本骨架

7.2 激光点云→包围盒（简化MATLAB Function）

7.3 航迹级融合函数（自定义）

八、仿真验证与判定

8.1 工况

8.2 指标

九、工程坑位

十、Simulink实现检查清单

手把手教你学Simulink——毫米波雷达与激光雷达的数据融合通信

目标不是“两个传感器都出图”，而是把毫米波雷达的全天候距离/速度/角度信息与激光雷达的高精度三维点云在统一时空基准下融合，再通过车载/回传通信把目标航迹、点云摘要或原始数据发给下游感知、规划或云端。Simulink 推荐用 Automated Driving Toolbox 做场景/传感器、Sensor Fusion and Tracking Toolbox 做跟踪融合、必要时用 Radar Toolbox/Phased Array 做 FMCW 物理层、用通信模块做回传时延与带宽评估。以下为可运行教学框架，参数按样车/路侧回填。

一、系统目标与融合层级

1.1 传感器分工（教学初值）

传感器

输出

典型频率

优势

短板

雷达可用 Driving Radar Data Generator 或 radarDetectionGenerator 生成概率检测/目标，激光可用 Lidar Point Cloud Generator 或 Lidar Sensor 生成点云。

1.2 三种融合架构

二、场景与坐标基准

2.1 坐标链

传感器原始坐标
├─ 雷达：传感器坐标(前/左/上) → 本车ego坐标 → 全局ENU/Local
└─ 激光：点云传感器坐标 → 本车ego坐标 → 全局
↓
时间同步：雷达t_radar、激光t_lidar、IMU/GPS t_ins
↓
融合跟踪坐标系（建议本车前-左-上或全局ENU）
↓
通信输出：目标列表/栅格/点云摘要

• 本车坐标：x前、y左、z上（Automated Driving 常用ego车辆坐标）。
• 激光点云块默认输出 ego 坐标的 m×n×3 位置，可带强度、分割。
• 雷达检测块输出 objectDetection bus，含位置、速度、协方差、传感器编号。

2.2 时间同步

雷达10~30Hz、激光10~20Hz、IMU100~200Hz、GPS1~10Hz，天然异步。做法：

• 仿真定步长取所有传感器最小公倍数相关步长，例如雷达25Hz（40ms）、激光20Hz（50ms）、跟踪器20Hz（50ms）可统一用10ms步长，子模块按各自更新间隔触发。
• 实系统用PTP/GPS PPS打硬件时间戳；Simulink用“传感器更新时刻+延迟模块”模拟。
• 乱序量测用 tracker 的 out-of-sequence measurement 处理，异步雷达+相机示例用 retrodiction 回推修正。

三、毫米波雷达建模

3.1 理想/概率目标模型（快、做融合首选）

Automated Driving 的 Driving Radar Data Generator：

radar = radarDetectionGenerator( …
'SensorIndex',1, …
'UpdateRate',20, … % Hz
'FieldOfView',[20 10], … % 方位×俯仰 deg，可改
'MaxRange',150, … % m
'RangeResolution',1, …
'AzimuthResolution',1, …
'HasRangeRate',true, … % 输出径向速度
'HasNoise',true, …
'HasFalseAlarms',true, …
'FalseAlarmRate',1e-6, …
'DetectionProbability',0.9, …
'SensorLocation',[1.5 0], … % 本车前保险杠左右
'Yaw',0);

输出 detections 总线，含目标距离/角度/径向速度及协方差，可直接进 Detection Concatenation→tracker。

3.2 FMCW物理层模型（要测距测速曲线时用）

Radar Toolbox/Phased Array 思路：

fc=77e9; B=300e6; Tc=40e-6; fs=2e6; % 扫频带宽/周期/采样
c=physconst('Lightspeed');
range_max=150;
lambda=c/fc;
% 发射chirp→目标延时→混频→FFT距离→速度(多chirp)→CFAR→DOA

教学若只做融合，不建议把完整FMCW塞进主模型；用“概率雷达目标”代替，物理层单独建子模型验证距离/速度/角分辨。

3.3 噪声与误差

• 距离噪声：σR≈0.1~0.3m（77GHz概率模型可按Resolution/信噪比配）。
• 速度噪声：σVr≈0.1~0.3m/s。
• 角度噪声：σθ随目标距离、RCS、多径变化；路侧目标偏航时点云会集中到车身侧面，边界框误差变大。
• 虚警/漏检：城市金属反射、立交多径要开FalseAlarm和DetectionProbability调参。

四、激光雷达建模

4.1 点云生成

Lidar Point Cloud Generator / Lidar Sensor：

lidar = lidarPointCloudGenerator( …
'SensorIndex',2, …
'UpdateRate',10, … % 10Hz，可改20
'HorizontalFieldOfView',[-180 180], …
'VerticalFieldOfView',[-15 25], …
'VerticalResolution',0.5, … % 32线≈0.5~1°，64线更细
'MaxRange',120, …
'RangeAccuracy',0.02, …
'AddNoise',true, …
'HasRain',false,'HasFog',false, … % 天气衰减可开
'SensorLocation',[0 0 1.8], …
'Yaw',0,'Pitch',0);

输出点云 m×n×3；可带强度、ActorID/ClassID。Scenario Reader 给actor poses，Lidar Sensor 给路网/车辆/行人点云。

4.2 点云预处理（进融合前必做）

• 去地：segmentGroundFromLidarData / 基于地面平面拟合。
• 去噪：pcdenoise，或统计离群点滤波。
• 聚目标：pcsegdist（欧氏聚类），DBSCAN用于雷达点云/行人多反射也常用。
• 出包围盒：激光点云→Bounding Box Detector（MATLAB System/Helper），输出3D cuboid检测供JPDA/IMM；官方激光车辆跟踪示例即点云→BoundingBox→JPDA+IMM。
• 运动补偿：激光扫描周期内本车转弯/加减速会产生拖影，用INS姿态对每帧点云做补偿；GPS+IMU异步可用 insfilterAsync 估位姿后重投影。

五、融合与跟踪模型

5.1 目标级融合Simulink树（推荐首发）

[Scenario Reader]→actor poses + ego pose
├─[Driving Radar Data Generator]→radar detections(objectDetection bus)
└─[Lidar Point Cloud Generator]→point cloud
↓
[Lidar Detection Preprocessing/MATLAB Function]→INS/ego补偿
↓
[Bounding Box Detector]→lidar cuboid detections
↓
[Detection Concatenation] 雷达+激光检测（统一objectDetection格式）
↓
├─雷达支路：multiObjectTracker/JPDA/GNN + CV/CA/IMM滤波 → radar tracks
└─激光支路：JPDA+IMM cuboid滤波 → lidar tracks
↓
[Track-To-Track Fuser] 关联+状态加权 → fused tracks
↓
[GOSPA Metric] vs Ground Truth → 精度
[Communication Out]→CAN/ethernet/V2X打包

官方雷达+激光目标级示例：激光点云经BoundingBox出cuboid检测，雷达出矩形目标；分别JPDA/IMM跟踪后用Track-To-Track Fuser做GNN关联融合，GOSPA对比单传感器与融合航迹，融合误差更低。

5.2 滤波与运动模型

• 直线/低速：constant velocity（CV）+EKF，initcvekf。
• 城区加减速：constant acceleration（CA）。
• 弯道/行人混行：IMM-CV+CTR（常速+常转率），激光cuboid示例用IMM-CV+CTR cuboid。
• 扩展目标（大车占多分辨率单元）：用cuboid/矩形滤波，不用点目标EKF。
• 雷达无横向速度只有径向速度：量测方程只观测量测径向分量；融合激光横向位置可补横向速度可观测性。

5.3 关联与航迹管理

• 单传感器：GNN（multiObjectTracker）或JPDA（杂波多、目标近）。
• 航迹级：Track-To-Track Fuser用源配置（雷达状态维度、激光cuboid状态维度分别定义），GNN或自定义融合函数做状态拼接/协方差加权。
• 确认/删除：例如N/M检测确认（2/3或4/5）、长时间无检测删除；城市遮挡用“雷达维持、激光重捕获”降删除率。

5.4 异步与通信延迟

用Message Send/Receive+Entity Queue+Entity Transport Delay模拟：

雷达20Hz→打包Message→Queue(LIFO/容量1)→传输延迟τ_radar→融合触发器
激光10Hz→打包Message→Queue→传输延迟τ_lidar→融合触发器
融合触发器20~50Hz统一预测/更新

异步雷达+视觉示例用雷达25Hz、相机约22.7Hz、tracker20Hz，Random Transport Delay模拟网络时延，OOSM用retrodiction回推，避免早到/迟到量测被丢弃。

六、通信回传与协同

6.1 回传内容分级

等级

内容

带宽估算（教学）

适用

通信模块用Simulink总线→打包→UDP/TCP/CCS/MATLAB System；评估丢包可用Bernoulli、时延用Transport Delay、拥塞用Entity Queue。

6.2 通信-融合联合指标

• 端到端航迹时延：传感器采集→融合→回传→接收端可用。
• 新鲜度：回传航迹相对本车预测时刻的年龄，大于跟踪协方差允许值则降权。
• 带宽占用：点云级回传在雨雾可改航迹级，保证安全功能不中断。
• 多车一致性：两车间同一目标ID位置差、速度差、ID切换率。

七、关键代码示例

7.1 场景+双传感器+融合脚本骨架

% 场景
scenario = drivingScenario('SampleTime',0.05);
ego = vehicle(scenario,'ClassID',1,'Position',[0 0 0]);
front = vehicle(scenario,'ClassID',1,'Position',[50 0 0]);
trajectory(ego,[0 0 0; 200 0 0],0:0.1:200);
trajectory(front,[50 0 0; 120 0 0],0:0.1:200); % 前车变速可改

% 雷达
radar = radarDetectionGenerator('SensorIndex',1,'UpdateRate',20,…
'FieldOfView',[20 10],'MaxRange',150,'HasRangeRate',true,…
'SensorLocation',[1.5 0],'Yaw',0);

% 激光
lidar = lidarPointCloudGenerator('SensorIndex',2,'UpdateRate',10,…
'HorizontalFieldOfView',[-180 180],'VerticalFieldOfView',[-15 25],…
'VerticalResolution',0.5,'MaxRange',120,'RangeAccuracy',0.02,…
'SensorLocation',[0 0 1.8]);

% 跟踪器（激光cuboid示例）
tracker = trackerJPDA('FilterInitializationFcn',@helperInitIMMUKFFilter,…
'AssignmentThreshold',[30 100],'ConfirmationThreshold',[4 5],…
'MaxNumTracks',50,'DetectionProbability',0.9);

% 仿真循环（简化）
for tk=0:0.05:20
acts = actorPoses(scenario, tk);
rdDet = radar(acts, egoPose(scenario, tk), tk); % 概率雷达检测
ptCloud = lidar(acts, egoPose(scenario, tk), tk); % 点云
lidDet = boundingBoxFromPointCloud(ptCloud, tk); % 自写/Helper
allDet = [rdDet; lidDet];
tracks = tracker(allDet, tk);
evaluateGOSPA(tracks, groundTruth(scenario, tk));
end

实际Simulink可直接用官方 TrackLevelFusionOfRadarAndLidarDataInSimulink 改传感器参数，不必从零写全部块。

7.2 激光点云→包围盒（简化MATLAB Function）

function dets = boundingBoxFromPointCloud(ptCloud, t)
% ptCloud: MxNx3 或pointCloud对象；返回objectDetection数组
pc = pointCloud(ptCloud);
pc = pcdenoise(pc);
ground = segmentGroundFromLidarData(pc,'ElevationAngleDelta',3);
pcNoGround = removeGround(pc, ground); % 自写或用pcsegdist
idx = pcsegdist(pcNoGround.Location, 0.8); % 欧氏聚类0.8m
dets = [];
for k = unique(idx(:))'
if k==0, continue; end
pts = pcNoGround.Location(idx==k,:);
if size(pts,1)<20, continue; end % 滤小簇
bbox = boundingBoxDimensions(pts); % 长/宽/高/中心/航偏
dets(end+1) = objectDetection(t, bbox.center, …
'MeasurementNoise',0.1*eye(3), …
'ObjectClassID',1, 'SensorIndex',2);
end
end

生产级用Lidar Toolbox的Bounding Box Detector块或深度学习检测器替代规则聚类。

7.3 航迹级融合函数（自定义）

function fused = helperRadarLidarFusion(radarTracks, lidarTracks, predTime)
% 简单状态拼接+协方差逆加权；实际按状态维度做变换
ids = associateByDistance(radarTracks, lidarTracks, 1.5); % 1.5m门限
for m=1:height(ids)
xr = radarTracks(ids(m,1)).State(1:6);
Pr = radarTracks(ids(m,1)).StateCovariance(1:6,1:6);
xl = lidarTracks(ids(m,2)).State(1:6);
Pl = lidarTracks(ids(m,2)).StateCovariance(1:6,1:6);
Pinv = inv(Pr)+inv(Pl);
xf = Pinv \\ (inv(Pr)*xr + inv(Pl)*xl);
fused(m).State=[xf; zeros(size(xf,1)-6,1)];
fused(m).StateCovariance=inv(Pinv);
fused(m).ID=radarTracks(ids(m,1)).TrackID;
end
end

官方Track-To-Track Fuser可配SourceConfig和自定义fusion函数，不必手搓全部逻辑。

八、仿真验证与判定

8.1 工况

场景

条件

看什么

8.2 指标

• 位置RMSE：融合相对单雷达/单激光降低比例；目标级融合示例GOSPA融合低于单传感器。
• 速度误差：雷达径向速度基准，融合横向速度误差应明显低于纯雷达。
• 漏检/虚警率：城市目标漏检<1~3%（按安全等级定），虚警不形成稳定轨。
• 跟踪延迟：新目标从进入FOV到确认航迹时间；20Hz融合可控制在100~200ms。
• 通信指标：航迹回传端到端时延、点云回传带宽占用、丢包10%时ID不频繁重置。
• GOSPA：定位/漏报/误报分项分别看，不只看总分数。

九、工程坑位

• 雷达只给径向速度：直接进CV EkF会把横向速度估错，必须激光/IMU补横向或改用IMM。
• 激光运动补偿漏做：转弯时点云“撕开”，聚类出多个假目标；高速用INS姿态补偿。
• 坐标没统一：雷达SensorLocation/Yaw、激光安装高/Yaw、ego原点不统一，融合门限再小也关联不上。
• 异步不处理OOSM：低频率激光晚到会覆盖雷达新预测；用retrodiction或时间戳缓冲。
• 点云全分辨率回传：16线不降采样都可能压垮车载以太网，协同感知默认航迹级、关键事件点云级。
• 天气模型太理想：激光雨雾只用高斯噪声不够，按能见度模型调衰减；雷达湿路面多径单独加虚假目标。
• 目标级融合丢几何：需要可行驶区域/标线时，航迹级不够，要加点云栅格或占据图。

十、Simulink实现检查清单

• [ ] drivingScenario/Scenario Reader建路网、本车、目标轨迹。
• [ ] Driving Radar Data Generator配FOV/距离/速度/虚警；Lidar Point Cloud Generator配线数/视场/噪声/天气。
• [ ] 激光预处理子模型：去地、去噪、聚类、Bounding Box检测。
• [ ] 雷达检测→objectDetection；激光检测→cuboid objectDetection；坐标/时间统一。
• [ ] 单传感器tracker（GNN/JPDA/IMM）先调通，再接Track-To-Track Fuser。
• [ ] 通信子模型：Message Send/Receive、Queue、Transport Delay、丢包、带宽限制。
• [ ] 指标：RMSE、速度误差、漏/虚警、GOSPA、端到端时延、回传带宽。
• [ ] 跑直道/路口/雨雾/异步/遮挡/多车五类以上工况，出对比曲线。
• [ ] 若做HIL：雷达用TI毫米波板读点云→DBSCAN→JPDA，激光用真实PCAP回放。