简介面向计算机、电子信息等专业课程设计与毕业设计人群这套ROS环境下的激光雷达与毫米波雷达数据融合资料提供可运行的C算法源码和项目说明。内容涵盖传感器融合主流程、卡尔曼滤波实现并内置Eigen矩阵运算依赖方便学习者对照理解多传感器融合的完整工程链路。压缩包共收录340个文件主要由260个h头文件、3个cpp源文件与40个txt说明文本构成另有2个msg消息定义等ROS接口文件整体体积仅879KB结构紧凑且便于下载后直接查阅。当前已有294人学习浏览常被用作课程设计、期末大作业和毕业设计的参考资料。除完整项目源码外还附带项目说明和目录结构可支撑从节点配置、话题通信到融合算法调用的一站式学习读者既能把握激光点云与毫米波目标数据的对齐、关联及滤波融合思路也能以此为基座进行二次开发或功能扩展。1. 先把这套融合源码的价值说清楚它到底帮你解决什么做无人物流车、园区自动驾驶的团队几乎都经历过同一个阶段的纠结激光雷达点云一稀目标就“断”毫米波雷达点云发虚轮廓就没法看。把两者做数据融合在 ROS 工作区里同时订阅两路话题、对齐时间戳、把速度信息补进几何目标里正是这套「激光雷达毫米波雷达数据融合算法源码包」能替你省去重复造轮子的地方。它不是黑匣子而是一套能编译、能跑、能改参数的工程框架适合正在做多传感器融合课题的学生、刚接手感知模块的开发者以及想快速验证融合方案的预研团队。读完并照着走一遍你至少能自己把压缩包拆开、跑通一次完整链路并且知道效果不好时先调什么、别调什么。2. 激光雷达与毫米波雷达为什么要融合先看数据脾气再看融合层级2.1 两种雷达的数据特性差异互补的不是重叠是对方的短板激光雷达的强项在角分辨率16 线在 30 米外每线间距约 10 cm64 线能做到厘米级所以做目标分类、轮廓还原都靠它。但它在雨雾、扬尘、逆光下会有明显退化点云密度下降是常见现象。更重要的是激光雷达直接测距测速要靠帧间差分低速目标、近距离切入目标很容易在速度上出现跳变。毫米波雷达走的是电磁波反射直接给径向速度几十米到两百米的探测距离在恶劣天气下退化程度比激光小得多。缺点也明摆着角分辨率低点云可能只有稀疏的十几个反射点聚类出来的目标轮廓不可靠静止目标要连续多帧确认才能滤掉杂波。如果把两类数据放到同一帧里看它们的“强答题区域”几乎不重叠激光负责回答“这东西是什么、长什么样、在哪”毫米波负责回答“它朝我靠近还是远离、速度多快、在低能见度下还存不存在”。这种互补关系决定了融合不是锦上添花而是多传感器方案绕不开的一步。不少团队在纯激光方案里遇到“货车贴近车身时点云被车体遮挡、目标瞬间丢失”的问题多接一路毫米波之后丢失窗口被覆盖住规控模块的心态会好很多。更现实的选型理由是成本。量产和预研项目里毫米波雷达的单价通常只有中低线数激光雷达的一个零头把融合框架搭好就能用较低线数的激光雷达加上毫米波逼近高线数激光雷达的感知下限。还有一层安全兜底激光雷达镜头脏污、毫米波偶尔失锁两路互为备份之后单传感器退化的概率被摊薄。这就是为什么源码包里一般都会保留独立的纯雷达模式开关而不是把融合做成锁死的黑盒。融合效果好不好完全取决于你对这两类数据“脾气”的把握。许多融合翻车案例翻的不是算法本身而是团队习惯了激光点云的稠密均匀一看到毫米波那稀疏十几个点就开始怀疑是数据坏了。实际上那十几个点已经够用前提是后面的关联和跟踪算法能接受“稀疏但带速度”的输入。拿到源码包以后先别急着夸算法拿你自己的数据喂进去跑一帧看看预处理的默认参数是不是按你的传感器型号调过这一步比什么都重要。2.2 前融合还是后融合源码包常见三种融合层级融合不只有一种做法。早期项目常看到把两个传感器“各自聚类再做目标关联”这是后融合也有人先把激光点云和毫米波点云投影到同一坐标系再做联合聚类这是前融合还有人在中间层基于各自检测结果做特征层融合。常见套路有三种源码包里往往只实现了其中一种另外两种以开关或预留接口的形式存在。方案 A是数据级前融合先做坐标系对齐和时间同步把两种点云合并成一张点云再统一做聚类、跟踪。优点是实现直接一张点云里同时有几何和速度后处理不用区分来源缺点是毫米波点云的噪声会污染激光聚类点云密度不均衡时容易把一个目标切碎。方案 B是目标级后融合激光先聚类出目标框毫米波先输出目标列表再按位置和速度做关联用卡尔曼滤波把速度属性补到激光目标上。优点是可解释性强任一路传感器失效都能降级缺点是关联错了会出“人格分裂”——同一个目标同时有两个 id。方案 C是中间层融合把毫米波目标当作“种子”回投到激光点云强制聚类器在毫米波目标附近多保留点云特别适合稀疏激光雷达配合 4D 毫米波雷达的组合。这套源码包里比较常见的设计是“B 为主、A 为辅”默认走目标级关联同时预留前融合开关。这样选的理由很现实——毫米波点云密度低于激光前融合会把激光的几何优势摊薄而目标级融合里对激光目标的“速度补偿”恰好是下游避障最需要的能力。包内常见有一个fusion_mode参数可以切换target或cloud两种模式。在cloud模式下激光点云和毫米波点云会先合并再聚类适合拿来对比效果但不推荐量产默认跑。选择哪一层融合还要看下游要什么。如果下游只有避障和跟车目标级融合足够了如果下游要做可行驶区域分割、路面元素识别那必须保留前融合的几何信息。源码把所有传感器数据先缓存到一帧里是为了让两种模式可以共用同一份时间同步和坐标变换代码你改动时不要只改其中一个模块否则很容易让两种模式的行为分叉得越来越远。2.3 坐标系对齐和时间同步融合的前提也是大多数翻车现场不管选哪种融合层级都绕不开两件前置工作让数据到同一坐标系、让消息在同一时刻。激光雷达一般装在车顶毫米波雷达装在前保或车侧安装位置不同同一时刻的目标在两个传感器坐标系下的坐标也不同。最常用也最稳的办法是用 TF 树表达外参让两个雷达坐标系都变换到整车坐标系一般取后轴中心或 IMU 位置再用 message_filters 做时间同步。ROS 里常见写法是同时订阅两个话题设置一个容差 slop比如 20 ms超过这个时间差的消息对直接丢弃。这里最容易踩的坑是不同雷达的时钟源不一致。激光雷达时间戳可能来自传感器内部时钟或 PTP 授时毫米波雷达时间戳来自车端控制器两者上电先后不同会产生一个几十到几百毫秒的固定偏置。源码包里通常会留一个time_offset参数或者提供标定脚本你拿到包后第一件事就是录一段 bag统计两路header.stamp的差值分布把中位数写进time_offset不要直接把这个深度调参问题丢给 message_filters 去硬磨。另外融合结果坐标系的父系最好固定一个。有的团队把融合结果发到/base_link有的发到/map如果 TF 树里父系来回跳下游看到的目标坐标会忽左忽右。我的习惯是感知融合结果固定发在车身系然后由规控模块自己决定要不要变换到地图系接口约定越简单越不容易出幺蛾子。外参标定误差、时间戳偏置、TF 抖动这三者在融合效果上长得非常像目标位置飘、速度突跳、目标框抖动。所以遇到融合结果不稳先检查时间戳和 TF再查关联参数顺序别反否则很容易在算法里做无用功。3. 把源码包在本地跑通ROS 工作区搭建与最小启动流程3.1 从压缩包到可编译的工作区目录规划与依赖检查拿到 zip 后第一步不是急着写 launch而是把它放进化好的 catkin 工作区。常见做法是建一个~/fusion_ws目录把解压后的功能包放进src下再编译。如果你之前从没搭过 ROS 工程先确认环境里已经装好了一套 ROS 1 基础发行版并且rosdep可用。下面这套命令在 Linux 环境下是通用的mkdir -p ~/fusion_ws/src cd ~/fusion_ws/src unzip ~/Downloads/ROS下的一个激光雷达和毫米波雷达的数据融合算法源码.zip # 确认 src 下存在带 package.xml 和 CMakeLists.txt 的功能包目录 ls -la cd ~/fusion_ws rosdep install --from-paths src --ignore-src -y catkin_make source devel/setup.bashrosdep install的作用是扫描每个功能包的 package.xml把缺失的 ROS 依赖自动补上。参数--from-paths src指定依赖解析范围--ignore-src告诉它忽略本地源码树里的包、不要重复编译避免它把系统里已经被覆盖的依赖再动一遍。如果你这台机器要长期调试建议把source ~/fusion_ws/devel/setup.bash追加进~/.bashrc省得每个终端都敲一次。注意不要用 sudo 跑 catkin_make工作区属主设为普通用户即可用 root 编译出来的产物切回普通用户后常常会遇到权限怪问题这不属于算法问题但会耗掉你一个下午。编译期如果报缺少依赖常见有三类缺少消息生成包、缺少pcl_conversions或message_filters、缺少tf2相关组件。它们都能通过rosdep补齐。比较隐蔽的是包内自带消息定义比如FusionTrackArray.msg这类消息必须由本包先编译生成如果你的编译命令没有先构建消息包会报“找不到头文件”之类的错。出现这种情况时单独catkin_make --pkg那个消息包再全量编译通常就能解决。3.2 节点结构、话题名与最小启动流程源码包的 ROS 工程一般由三类节点组成激光雷达预处理节点去畸变、地面分割、毫米波雷达预处理节点杂波过滤、聚类、融合主节点时间同步、关联、卡尔曼跟踪。也有把预处理写成库、只留一个融合节点的更精简做法但多数能直接拿来改的源码包还是带独立预处理节点方便你单独调试某一路。启动前先打开包里的 launch 文件看一下订阅和发布的话题名再用roslaunch拉起来roslaunch lidar_mmwave_fusion fusion.launch # 另一个终端里检查话题是否齐全 rostopic list | grep -E lidar|mmwave|fusion # 打印融合输出消息类型确认和包内自定义 msg 一致 rostopic type /fusion_tracks如果你的数据来自实车雷达需要把 launch 里remap fromlidar_points to你的激光话题/这类规则改成实际话题名如果数据来自 bag则直接rosbag play --clock回放让/clock驱动整个融合。这里有个容易忽略的点launch 里的param可能写死了优先级默认参数往往面向某个公开数据集换成自己的传感器后第一步不是调算法而是把话题名、坐标系前缀、传感器安装高度全部对齐。调试期我一般会给融合节点加一个debug参数。置为 true 后节点会把每帧两路消息的时间差、关联对数、被剔除的点云数、卡尔曼更新次数打印到终端。源码包如果没有这个开关也可以在订阅回调里临时加几行ROS_INFO打印header.stamp和关联对数。比盯着 rviz 猜问题快得多因为肉眼只能看到“目标在动”看不到“目标是哪两帧数据融合出来的”。拿到包后先用这个方式跑 10 分钟你对整个软件链路就有了基本盘。3.3 用数据回放和 rviz 验证融合是否真的生效跑通 launch 之后验证不能只看有没有报错要看三样东西时间差是否收敛、融合目标数量是否合理、目标位置是否抖动。我建议先录一段包含“直道接近、横穿、静止目标”的 bag然后回放两遍一遍跑激光纯检测一遍跑融合把两路输出都可视化。rviz 里加载包自带的 rviz 配置或者手动加三个显示PointCloud2 显示激光点云、MarkerArray 显示毫米波目标点、BoundingBox 显示融合目标框。判断融合是否生效可以用一个很简单的对照实验把一个静止箱子放在车前车不动看融合输出里该目标的速度是否接近 0再拿着箱子快速靠近看速度是否变为负且量级接近真实速度。静止目标速度为 0、动态目标速度符号正确说明速度补偿和关联方向没接反。如果目标 id 乱跳、框忽大忽小先回到第 2 章说的时间戳和 TF 排查不要急着调关联阈值。实际调试量大的时候我发现很多人只看 rviz 上的点云颜色这并不够。把rostopic echo /fusion_tracks打印出来比较每帧header.stamp与各传感器消息时间戳的差值是判断时间同步最直接的手段。如果包内带数据统计脚本可以直接统计连续 200 帧的融合输出有效目标帧占比、目标丢失帧数、相邻帧同一 id 的平均位置跳变。这些数字比肉眼可靠。没有现成脚本就自己写一个订阅节点攒统计几百行 Python 的事但能让你真正知道参数改动是变好了还是变坏了。4. 参数怎么调才不玄学时间同步、关联阈值和效果评估4.1 时间同步与帧率适配参数融合节点最常见的两个参数是time_sync_slop和max_fps。time_sync_slop控制两路消息时间戳差多大时算同步成功。激光雷达常见 10 Hz毫米波雷达有的 20 Hz、有的 25 Hz。slop 设太小两路消息几乎对不上配对率低融合节点每帧都在丢数据设太大等于拿着 0.1 秒前的毫米波数据去和当前激光数据融合动态目标的位置会差出一个车身距离。常见做法是先设 20 ms再把debug打开看配对时间差的统计用 P95 作为基准微调。# fusion_node 的参数可由 rosparam load 载入也可在 launch 里写成 param time_sync_slop_ms: 20 # 两路消息最大时间差单位毫秒 lidar_topic: /lidar_points # 激光点云话题 mmwave_topic: /mmwave_tracks # 毫米波目标话题 use_predict_to_sync: true # 用毫米波速度外推到激光帧时刻 match_distance_base: 0.8 # 关联距离阈值基础值单位米 match_distance_k: 0.025 # 距离自适应系数lidar_topic和mmwave_topic是话题名必须和 launch 里的 remap 对应。第四个参数use_predict_to_sync是进阶玩法毫米波刷新率高两帧之间目标位置可以按上一帧速度做线性外推外推到激光帧时间戳这样即使两路消息没有真正同时到达融合也不会出现同一个目标两个位置。代价是外推本身有误差在急刹、横穿这类高动态场景里要慎重源码包如果没有这个开关也可以自己加在关联前把毫米波目标状态(x, y, vx, vy)按时间差做一阶更新实现成本不大但动态场景的效果提升非常明显。帧率适配还有一个容易忽略的点两路消息频率不同融合节点最好维护一个“最近一帧缓存”而不是直接丢弃不同步的消息。常见实现是激光帧到了取时间差最接近的毫米波帧参与融合毫米波帧到了只更新缓存不触发融合输出。这样做输出频率由激光主导毫米波帧率高时不会让融合结果抖动更快。修改时注意给缓存加上互斥锁多线程回调下不加锁轻则数据错乱重则直接崩溃。4.2 关联阈值和外参的联动调整目标级融合里最核心的是关联门限毫米波目标与激光目标在什么位置、速度差异范围内算同一个目标。常见参数是match_distance和match_velocity。位置阈值设太大相邻车道的两个目标被合并成一个下游规控会以为前侧一直有障碍设太小两路各报各的融合输出目标数量直接翻倍避障模块会频繁切换决策。常用起始参数整理在下面参数推荐起始值设错的表现time_sync_slop_ms20太小配对率低太大动态目标成对误差大match_distance_base0.8 m太小目标分裂太大相邻目标合并match_distance_k0.02-0.03远距离丢目标或近距离误关联match_vel_threshold2-3 m/s太小低速目标反复换 id太大不同目标被合并毫米波的角度误差随距离增大而增大50 米外目标位置误差可能到 2 m固定阈值会把远处目标全丢掉所以常见做法是距离自适应。公式很简单threshold match_distance_base match_distance_k * range。base 取 0.8 mk 取 0.020.0330 米处阈值约 1.41.7 m60 米处约 2.02.6 m刚好贴合毫米波误差的增长曲线。这个公式没有统一标准完全取决于你的雷达型号和安装位置拿到包后先录一段 30 米、50 米、80 米各有一个目标的场景用实测分布定 base 和 k。外参的影响通常被低估。毫米波安装角偏 1 度30 米外目标位置就偏约 0.5 m偏 2 度50 米外偏 1.7 m这已经超过关联阈值。所以调关联阈值前先做一次外参精标定找一根角反射器或金属杆放在毫米波视场里多个距离把毫米波点云与激光点云叠加到 rviz看横向偏差是否收敛。收敛后再调关联阈值否则你会陷入“阈值调到极限目标还是一会儿合一会儿分”的死循环。标定完把外参写进 TF 静态变换不要写进融合节点的私有参数里因为坐标系变换是全系统共享的只有一份。4.3 用评估脚本量化融合效果不要只靠肉眼效果调优最后一步是量化评估。常见做法是录一段带真值的 bag——真值可以来自人工标注也可以来自高精度定位系统——然后统计 MOTA、ID 切换次数和位置误差。源码包里不一定带评估脚本这里给出一个可复用的轻量思路离线跑融合把/fusion_tracks写入 CSV再和真值文件做最近邻匹配统计每帧匹配比例和平均位置误差。import rosbag import csv bag rosbag.Bag(eval.bag) rows [] # 真值和融合输出按时间戳对齐后做最近邻位置匹配 for topic, msg, t in bag.read_messages( topics[/fusion_tracks, /ground_truth_tracks]): for box in msg.tracks: rows.append([topic, t.to_sec(), box.x, box.y, box.id]) with open(fusion_eval.csv, w, newline) as f: writer csv.writer(f) writer.writerow([topic, stamp, x, y, id]) writer.writerows(rows)这段脚本的关键是“先按时间对齐再按位置匹配”避免把不同时刻的目标强行拿来比。真值话题名取决于你的标注工具包里不一定会带需要自己录。真要写全还需要处理一个真值目标对应多个融合输出的情况那是多目标跟踪评估的标准做法这里只要能把你改参数前后的漏检率和 ID 切换次数统计出来就够用。评估时注意三点固定同一段 bag参数每次只改一个每个配置跑三次取中位数不要拿最好的一次上报。很多人在调参时踩的坑是“越调越兴奋最后忘了原来参数是什么”。拿一个配置文件管理工具或者在 yaml 里留注释记录每次改动的数值和对应指标一个月后回看比任何自动化调参工具都可靠。第 5 章的避坑清单里我会把几个最容易把新手绕晕的现象列出来。5. 避坑指南五个高频问题按现象、原因、解决来排查调试多传感器融合和写纯算法不一样算法可以离线反复试融合只能在真实数据上暴露问题。我自己的血泪经验是拿到 bag 先录好、每次只改一个参数、把每一次改动的效果记录在参数文件注释里。下面前五个问题我几乎每个项目都撞过按“现象、原因、解决”的顺序写出来。5.1 融合目标 id 频繁切换同一辆车一会 3 号一会 7 号现象rviz 里同一目标的编号标签在几十帧内来回变下游规控看到的是两个目标轮流出现急刹和变道决策会被带偏。原因通常是关联门限过紧和时间同步不准双重作用。毫米波帧刷新率高激光帧稀疏如果 slop 太小两路消息配对率低跟踪器经常处于“没收到观测”的状态目标生命周期被反复创建和销毁。另一个常见原因是速度阈值太小车辆低速蠕行时毫米波径向速度变化超过阈值关联器认为这是新目标。解决先把use_predict_to_sync打开或调大time_sync_slop_ms到 2030 ms再看配对率然后把match_distance_threshold改成距离自适应具体公式用第 4.2 节那个让阈值随距离略微放开。改完回放同一段 bag如果 id 切换次数从每分钟几十次降到个位数问题基本就解决了。如果还不行就要看跟踪器里有没有做目标轨迹的“未关联保留帧”保留帧至少要有 3 帧给短暂丢观测留缓冲。5.2 毫米波点云聚类分裂一个目标变成三个簇现象融合输出里一辆卡车被框成三个并排的小框速度还各不相同避障模块会认为前方有三辆车在并行路径规划频繁绕行。原因毫米波点云稀疏且分布不均近距离点云密、远距离点云稀。聚类半径固定时近距离同一个目标被切成多段远距离多个目标又会被并成一个。另外卡车侧面多点反射时几个反射点之间间距较大超过聚类半径就分家了。解决预处理聚类采用距离自适应半径比如cluster_radius 0.6 0.01 * range近距离 0.7 m 以内合并远距离放一点同时增加速度一致性校验两个候选簇的速度差小于 0.5 m/s 且距离小于阈值时强制合并。毫米波直接测速速度一致性在目标合并里非常可靠这个技巧比单纯调位置阈值更有效。注意别把合并条件放太宽否则岔路口两辆并行车的目标会被并成一个。5.3 静止目标被过滤掉融合输出里突然少了一辆车现象车辆停在路边或排队等红灯时融合输出里一会儿有它一会儿完全没有而激光点云里明明能看到。原因毫米波雷达通常会用多普勒滤波滤掉静止杂波静止目标的反射点本来就少如果融合节点里还设了min_static_points: 5之类的参数静止目标很容易被当成噪声扔掉。激光这边如果地面分割过猛也可能把低矮静止目标并进地面。解决先查毫米波雷达驱动配置里的静态目标输出开关确认有输出再把融合参数里静止目标的最小点云数降到 23并给静止目标加一个“连续 N 帧确认”的机制单帧置信度低没关系连续 3 帧有观测就先建目标再用跟踪器维持。确认机制写进跟踪生命周期而不是简单调高阈值否则动态目标的虚检也会跟着涨上来。5.4 目标位置周期性前后抖像在“呼吸”现象车在直道上匀速行驶融合目标到自车的距离每隔两三帧就前后跳 0.51 m速度也跟着正负振荡规控里看就是前方障碍物在“呼吸”。原因十有八九是时间戳不同源造成的固定偏置。激光时间戳来自传感器内部时钟毫米波时间戳来自控制器两者之间有一个几十 ms 的偏置没有补偿。偏置是固定值所以抖动是周期性的而不是随机噪声。这个特征可以用来区分它和外参误差外参误差造成的偏差一般固定在一个方向而时间偏置会让目标位置沿行进方向前后摆。解决录一段 bag统计同一目标在两路消息里时间戳差值的分布把中位数作为time_offset写入配置。如果没有现成参数就在消息回调里给毫米波header.stamp加上这个偏置量。注意time_offset和time_sync_slop是两回事前者修系统偏置后者是配对容差顺序上先修偏置再调 slop。只调 slop 不修偏置问题会一直在。5.5 编译通过但 roslaunch 启动直接段错误崩溃现象launch 文件正常加载节点一启动就 Segmentation fault终端里连话题都还没来得及注册日志里只有一行 core dump 提示。原因常见三类。消息回调里访问了未加锁的共享数据毫米波点云 size 为 0 但代码直接取了 data[0]message_filters 同步器订阅的两个话题消息类型和回调参数不匹配。第三类最隐蔽往往不是代码逻辑错而是驱动发出来的消息类型和你以为的不一样类型对不上时同步器内部直接越界。解决先rostopic type确认实际话题类型再看代码里订阅回调的模板参数是否一致。启动方式改成roslaunch ... --screen或者用 gdb 打开节点崩溃时直接看 backtrace 定位到具体行。如果没有现成改动可以在毫米波预处理节点给点云接入前做一次空数据检查if (points.empty()) { return; }这一行能挡住一多半的启动崩溃。多线程回调里的共享缓存一律加锁别指望“频率低就没事”传感器数据一来就是每秒几十帧。6. 更进一步把融合输出接进规控的四个小习惯源码包跑通只是第一步真正让融合在项目里落地是把它接进规控之后还不出问题。这里分享四个我自己一直在用的习惯都是很小但很实用的改动。第一个习惯是自定义消息里带协方差。很多源码包的输出消息只有 x、y、vx、vy、id。越往下游走规控越需要的不只是数值而是“这个值可信吗”。常见做法是自定义FusionTrackArray每个目标带covariance或confidence字段。毫米波雷达径向速度准、切向不准可以给 vx、vy 分别填上不同量级的不确定度激光位置误差小position 协方差填小值。如果消息结构不能改至少把“最近一次关联到的点数”作为字段输出下游能拿它当置信度用。第二个习惯是录 bag 做回归测试。每次调参前先把原始传感器数据录下来。改完参数后用同一条 bag 回放对比调参前后输出。听起来机械却是唯一能防止“今天调好明天回归”的办法。最好把 bag 按场景命名直道、路口、雨天、夜间做回归时分开测。有人会觉得录 bag 麻烦但一次调参至少要回放十几次实车不可能陪你反复制造同一个场景。第三个习惯是目标生命周期加滞留帧。融合跟踪器常见问题是目标被遮挡几帧后立刻删除然后又重建形成“闪烁”。常见做法是目标连续丢失 N 帧后才删除路口场景车流复杂N 取 58 帧高速或园区直线场景目标速度高、遮挡短取 23 帧即可。这个 N 要跟场景联动不要一个值跑天下。第四个习惯是把速度和几何分开看。激光目标带着完整的框毫米波目标带着可靠的速度但两者更新时间不同。我的习惯是在融合消息里保留两个时间戳geometry_stamp和velocity_stamp规控如果要外推目标位置用速度时间戳做外推几何时间戳只负责框的位置。这个小改动让下游在低帧率传感器上做预测时少走弯路。我现在拿到一套多传感器融合源码第一个动作永远是看消息定义和 TF 树再看算法主干这两样先确认后面所有调参才能落到真实问题上而不是在空转。希望帮到你。本文还有配套的精品资源点击获取