简介本资源面向机器人、无人机与自动驾驶领域的开发者及学习者提供一套基于DBSCAN点云聚类与视觉检测的多传感器融合避障项目源码用于解决复杂环境下障碍物识别与实时路径规划问题。包内共27个文件涵盖cpp核心算法实现、launch启动配置、world仿真场景、xacro与urdf机器人模型、py控制脚本及names类别标签等压缩包约71KB结构围绕obstacle_avoidance_test功能包组织便于在ROS环境中直接编译运行。项目将激光雷达点云聚类与摄像头视觉目标识别相结合配合超声波与GPS等多源信息实现障碍物检测、融合决策与安全路径规划并附带说明文档与附赠资料辅助理解。目前已有93人学习下载适合希望掌握多传感器融合避障完整实现思路、算法模块划分与仿真验证流程的中高级读者参考借鉴。1. 从一份多传感器融合避障源码包说起它到底能跑出什么结果如果你正在做机器人、无人机或者自动驾驶小车的避障模块大概率遇到过这种局面激光雷达点云丢进来一大片视觉检测框也在跳两边各说各话最后决策层不知道该信谁。这份资源包就是冲着这个痛点来的——它把 DBSCAN 点云聚类、视觉目标识别、多传感器融合决策和实时路径规划串成了一条完整链路不是单点 demo而是能直接跑通「感知→聚类→融合→避障」的工程骨架。它适合三类人一是刚接触 ROS 或 ROS2、想找一个能落地的多传感器融合参考实现的学生和初级工程师二是手里有雷达和相机、但融合层一直用阈值硬凑的从业者三是需要快速验证动态避障小车路径规划思路的团队。资源包里通常包含节点源码、配置文件、launch 启动脚本和仿真环境描述拿到手就能在现有工作空间里编译运行省掉从零搭框架的时间。下面我按实际拆包和复现的顺序把关键参数、代码逻辑和踩过的坑一条条讲清楚。2. DBSCAN 点云聚类参数怎么定、簇怎么分、噪声怎么处理2.1 为什么避障场景优先选 DBSCAN 而不是 K-Means点云聚类在避障里的核心诉求不是「分得漂亮」而是「不能把障碍物切碎也不能把两个靠近的障碍物粘成一个」。K-Means 需要预先指定簇数量而机器人前方有几辆车、几个人是未知的这个前提就不成立。DBSCAN 基于密度不需要簇数还能把稀疏的噪声点单独标出来正好匹配「障碍物密集、背景稀疏」的典型场景。另一个实际原因是 DBSCAN 对簇形状不敏感。激光雷达扫出来的车辆轮廓是弧形行人是不规则团块K-Means 用欧氏距离硬切容易把一辆车的车头和车尾分成两簇后续融合时视觉框和点云簇对不上决策层就会收到重复障碍物。DBSCAN 只要密度够就连成一片轮廓完整性更好。但 DBSCAN 不是没有代价。它的两个参数 eps 和 minPts 对结果影响极大而且不同距离上的点云密度天然不均匀——近处密、远处稀。常见做法是先做体素降采样把密度拉平一些再调参。我一般会先把 eps 设成略大于体素边长minPts 设成 10 到 15然后拿实际场景的 bag 包回放看聚类效果而不是在代码里拍脑袋。2.2 核心代码拆解从 PointCloud2 到聚类簇下面这段是典型的 ROS 节点里 DBSCAN 聚类的核心逻辑基于 PCL 的pcl::EuclideanClusterExtraction做密度聚类实际项目里也常用dbscan的独立实现思路一致。# 订阅激光雷达点云做体素降采样后执行 DBSCAN 聚类 import rospy import numpy as np from sensor_msgs.msg import PointCloud2 from sklearn.cluster import DBSCAN import sensor_msgs.point_cloud2 as pc2 class LidarClusterNode: def __init__(self): # eps: 邻域半径单位米min_samples: 核心点最小邻居数 self.eps 0.35 # 略大于体素边长 0.3避免同一障碍物被切碎 self.min_samples 12 # 近处车辆约 15-20 点行人约 8-12 点 self.sub rospy.Subscriber(/velodyne_points, PointCloud2, self.callback) self.pub rospy.Publisher(/clusters, PointCloud2, queue_size1) def callback(self, msg): # 只取 xyz忽略强度减少计算量 points np.array([[p[0], p[1], p[2]] for p in pc2.read_points( msg, field_names(x, y, z), skip_nansTrue)]) if len(points) self.min_samples: return # 体素降采样把 0.3m 立方体内的点合并成一个拉平远近密度差 voxel np.round(points / 0.3).astype(np.int32) _, idx np.unique(voxel, axis0, return_indexTrue) points points[idx] # DBSCAN 聚类n_jobs-1 用满 CPU labels DBSCAN(epsself.eps, min_samplesself.min_samples, n_jobs-1).fit_predict(points) # label-1 是噪声点避障时直接丢弃不参与融合 clusters [points[labels i] for i in set(labels) if i ! -1] rospy.loginfo(clusters: %d, noise: %d, len(clusters), int(np.sum(labels -1)))逻辑说明先做体素降采样把 0.3 米立方体内的多个点合并成一个代表点这一步既降计算量又缓解远处点稀疏导致 DBSCAN 漏检的问题。然后对降采样后的点做 DBSCANeps0.35比体素边长略大保证同一障碍物降采样后的点还能连成一片。min_samples12是经验值太小会把噪声当障碍物太大会把远处行人漏掉。最后把label-1的噪声点丢掉只保留有效簇。参数说明eps每增大 0.1远处障碍物更容易连成簇但相邻两个障碍物也更容易被合并min_samples每增大 5噪声抑制更强但小目标召回下降。建议用 rosbag 回放把这两个参数做成动态配置边看 RViz 边调不要一次写死。2.3 聚类结果怎么和视觉检测对齐点云簇出来之后下一步是把它和视觉检测框做关联。常见做法是把点云簇投影到图像平面计算簇的二维包围盒与检测框的 IoU超过阈值就认为匹配。这里有个容易翻车的点点云和相机的时间戳不同步车一动投影位置就偏了。我一般会在融合节点里做时间对齐用message_filters的ApproximateTime策略允许 50ms 以内的偏差超过就丢弃这一帧宁可少融合也不融错。3. 视觉检测与多传感器融合从检测框到障碍物列表3.1 视觉检测输出怎么接入融合层视觉检测部分通常用 YOLO 系列或 SSD 输出[x_min, y_min, x_max, y_max, class_id, score]。接入融合层时不要直接把检测框当障碍物用因为单目视觉没有深度框的位置在三维空间里是不确定的。正确做法是把检测框作为「类别和存在性」的证据把点云簇作为「位置和尺寸」的证据两者互补。具体流程是先把点云簇投影到图像找到与检测框 IoU 最大的簇如果匹配成功就用点云簇的三维质心和尺寸作为障碍物的位置和大小用检测框的类别作为障碍物类型如果某个检测框没有匹配到点云簇说明可能是远处小目标或误检先挂起连续三帧都出现再纳入障碍物列表。这个「连续帧确认」机制能过滤掉大部分视觉误检。3.2 融合节点代码关联、确认与障碍物发布# 将点云簇与视觉检测框关联输出融合后的障碍物列表 import numpy as np class FusionNode: def __init__(self): self.iou_thresh 0.3 # 簇投影框与检测框的最小 IoU self.confirm_frames 3 # 视觉独有目标连续确认帧数 self.pending {} # 暂存未匹配的检测框 def project_cluster(self, cluster, K, T_lidar_cam): # 点云簇投影到图像先转到相机坐标系再用内参投影 pts_cam (T_lidar_cam np.hstack( [cluster, np.ones((len(cluster), 1))]).T).T[:, :3] uv (K pts_cam.T).T uv uv[:, :2] / uv[:, 2:3] return [uv[:, 0].min(), uv[:, 1].min(), uv[:, 0].max(), uv[:, 1].max()] def iou(self, box_a, box_b): # 计算两个二维框的 IoU xa, ya max(box_a[0], box_b[0]), max(box_a[1], box_b[1]) xb, yb min(box_a[2], box_b[2]), min(box_a[3], box_b[3]) inter max(0, xb - xa) * max(0, yb - ya) area_a (box_a[2] - box_a[0]) * (box_a[3] - box_a[1]) area_b (box_b[2] - box_b[0]) * (box_b[3] - box_b[1]) return inter / (area_a area_b - inter 1e-6) def fuse(self, clusters, detections, K, T_lidar_cam): obstacles [] matched_det set() for c in clusters: proj self.project_cluster(c, K, T_lidar_cam) best_iou, best_j 0, -1 for j, d in enumerate(detections): score self.iou(proj, d[:4]) if score best_iou: best_iou, best_j score, j if best_iou self.iou_thresh: # 点云给位置视觉给类别 obstacles.append({center: c.mean(axis0), size: c.max(axis0) - c.min(axis0), type: detections[best_j][4]}) matched_det.add(best_j) else: # 无视觉匹配的簇按未知障碍物处理保守避让 obstacles.append({center: c.mean(axis0), size: c.max(axis0) - c.min(axis0), type: unknown}) # 视觉独有目标做连续帧确认 for j, d in enumerate(detections): if j in matched_det: continue key (d[4], int(d[0] // 20), int(d[1] // 20)) self.pending[key] self.pending.get(key, 0) 1 if self.pending[key] self.confirm_frames: obstacles.append({center: None, size: None, type: d[4], image_box: d[:4]}) return obstacles逻辑说明project_cluster把点云簇从雷达坐标系转到相机坐标系再投影到图像得到二维包围盒。iou计算投影框与检测框的重叠度。fuse里对每个点云簇找最佳匹配检测框匹配上就用点云的位置和视觉的类别没匹配上的簇按未知障碍物处理决策层会保守避让。视觉独有目标用pending字典做连续帧计数达到confirm_frames才纳入避免误检导致急刹。参数说明iou_thresh设 0.3 是平衡漏匹配和误匹配的经验值相机和雷达外参标定不准时可以降到 0.2但会引入更多错误关联。confirm_frames设 3 在 10Hz 相机下约 0.3 秒延迟对低速机器人可接受高速车辆要降到 2 甚至 1但误检率会上升。3.3 外参标定融合成败的隐形前提多传感器融合最容易被低估的环节是外参标定。点云投影到图像偏几厘米IoU 就掉一大截融合直接失效。常见做法是找一块标定板同时录雷达和相机数据用autoware_camera_lidar_calibrator或lidar_camera_calibration工具做联合标定。标定完不要只看重投影误差要在实际场景里把点云投影到图像上看车辆轮廓和点云边缘是否贴合。我见过重投影误差很小但实际投影偏移的情况原因是标定时的距离和实际使用距离差太多镜头畸变模型没覆盖到。4. 避坑与排查融合避障里最容易翻车的五个点4.1 现象点云簇数量忽多忽少RViz 里障碍物闪烁原因DBSCAN 的eps和min_samples固定但点云密度随距离和反射率变化远处车辆点少偶尔低于min_samples就被当噪声丢掉下一帧又够数导致簇时有时无。解决引入距离自适应参数近处eps小、远处eps大或者先做体素降采样拉平密度。更稳妥的做法是对聚类结果做帧间跟踪用卡尔曼滤波平滑障碍物位置单帧丢失不立即删除障碍物。4.2 现象视觉检测框和点云簇明明对应同一个物体IoU 却很低原因时间戳不同步。雷达和相机频率不同直接取最近帧做融合车辆运动 0.1 秒就能偏移几十厘米投影框和检测框错开。解决用message_filters做时间同步允许 50ms 偏差如果硬件支持用 PTP 或 GPS 秒脉冲做硬同步。软件同步只能缓解不能根治高速场景。4.3 现象融合后障碍物列表里同一个物体出现两次原因一个真实障碍物被 DBSCAN 分成两个簇或者视觉检测框和点云簇匹配失败后各自成了一条障碍物记录。解决在融合输出前做一次 NMS非极大值抑制按三维距离合并中心点相近的障碍物。同时检查 DBSCAN 参数eps太小是簇分裂的主因。4.4 现象避障决策频繁急停机器人走走停停原因视觉独有目标没有做连续帧确认单帧误检直接进入障碍物列表决策层看到突然出现的障碍物就急停。解决加上confirm_frames机制视觉独有目标连续多帧出现才纳入。同时给障碍物列表加生命周期管理超过一定时间没更新的障碍物自动移除避免幽灵障碍物。4.5 现象仿真里跑得好好的上实车就撞原因仿真点云是理想模型没有噪声、没有反射率变化、没有运动畸变。实车雷达点云有拖尾、有地面点干扰DBSCAN 会把地面和障碍物连成一片。解决上实车前先做地面分割用 RANSAC 或pcl::SACSegmentation把地面点滤掉再对非地面点做聚类。另外仿真里的外参是精确值实车标定有误差融合阈值要适当放宽。5. 动态避障决策与路径规划从障碍物列表到可执行轨迹5.1 局部路径规划怎么用融合后的障碍物列表融合层输出的是障碍物列表每个障碍物有位置、尺寸和类型。局部路径规划器常见的是 DWA、TEB 或 MPC需要的是障碍物在代价地图里的表达。我一般会把障碍物列表转成局部代价地图的膨胀层已知尺寸的障碍物按实际尺寸加安全半径膨胀未知类型的障碍物按最大可能尺寸膨胀视觉独有目标按行人尺寸膨胀。这样规划器不用关心障碍物来自雷达还是相机只处理统一的代价地图。动态避障小车路径规划的关键是预测。静态障碍物只需要膨胀动态障碍物要估计速度。如果资源包里带了跟踪模块可以用跟踪输出的速度做前向预测把障碍物未来 1 到 2 秒的位置也标进代价地图。没有跟踪模块的话至少要用连续两帧的位置差估算速度否则规划器只能做反应式避让遇到横穿的行人容易来不及。5.2 避障参数配置与验证方法下面是一个典型的局部代价地图配置片段用于把融合障碍物接入规划器# local_costmap_params.yaml local_costmap: plugins: - {name: obstacles, type: costmap_2d::ObstacleLayer} - {name: inflation, type: costmap_2d::InflationLayer} obstacles: observation_sources: fused_obstacles fused_obstacles: topic: /fused_obstacles data_type: PointCloud2 marking: true clearing: true obstacle_range: 6.0 # 超过 6 米不标记避免远处噪声 raytrace_range: 8.0 # 清除范围略大于标记范围 inflation: inflation_radius: 0.55 # 机器人半径 0.3 安全余量 0.25 cost_scaling_factor: 3.0 # 越大代价衰减越快路径越贴障碍物参数说明obstacle_range决定多远内的障碍物进入代价地图设太大远处噪声会拖慢规划设太小高速时来不及避让。inflation_radius至少是机器人内切圆半径加安全余量太小会擦碰太大会导致窄通道无法通过。cost_scaling_factor影响路径与障碍物的距离值越大路径越贴近障碍物值越小越保守。验证方法先在仿真里用静态障碍物验证代价地图膨胀是否正确再加动态障碍物验证预测是否生效最后用 rosbag 回放实际场景数据看规划轨迹是否平滑、有没有震荡。震荡通常是膨胀半径和代价衰减不匹配或者障碍物列表更新频率太低。5.3 一个容易被忽略的技巧给融合结果加置信度融合后的障碍物不应该只有「有」和「无」两种状态应该带置信度。点云和视觉都检测到的障碍物置信度最高只有点云检测到的次之只有视觉检测到的最低。规划器可以根据置信度调整避让策略高置信度障碍物正常膨胀低置信度障碍物只做减速不绕行。这个技巧能显著减少误检导致的过度避让我后来每次做融合避障都会在障碍物消息里加一个confidence字段规划器里按置信度分档处理。希望帮到你。本文还有配套的精品资源点击获取 多传感器融合避障实战：DBSCAN点云聚类与视觉融合的路径规划
简介本资源面向机器人、无人机与自动驾驶领域的开发者及学习者提供一套基于DBSCAN点云聚类与视觉检测的多传感器融合避障项目源码用于解决复杂环境下障碍物识别与实时路径规划问题。包内共27个文件涵盖cpp核心算法实现、launch启动配置、world仿真场景、xacro与urdf机器人模型、py控制脚本及names类别标签等压缩包约71KB结构围绕obstacle_avoidance_test功能包组织便于在ROS环境中直接编译运行。项目将激光雷达点云聚类与摄像头视觉目标识别相结合配合超声波与GPS等多源信息实现障碍物检测、融合决策与安全路径规划并附带说明文档与附赠资料辅助理解。目前已有93人学习下载适合希望掌握多传感器融合避障完整实现思路、算法模块划分与仿真验证流程的中高级读者参考借鉴。1. 从一份多传感器融合避障源码包说起它到底能跑出什么结果如果你正在做机器人、无人机或者自动驾驶小车的避障模块大概率遇到过这种局面激光雷达点云丢进来一大片视觉检测框也在跳两边各说各话最后决策层不知道该信谁。这份资源包就是冲着这个痛点来的——它把 DBSCAN 点云聚类、视觉目标识别、多传感器融合决策和实时路径规划串成了一条完整链路不是单点 demo而是能直接跑通「感知→聚类→融合→避障」的工程骨架。它适合三类人一是刚接触 ROS 或 ROS2、想找一个能落地的多传感器融合参考实现的学生和初级工程师二是手里有雷达和相机、但融合层一直用阈值硬凑的从业者三是需要快速验证动态避障小车路径规划思路的团队。资源包里通常包含节点源码、配置文件、launch 启动脚本和仿真环境描述拿到手就能在现有工作空间里编译运行省掉从零搭框架的时间。下面我按实际拆包和复现的顺序把关键参数、代码逻辑和踩过的坑一条条讲清楚。2. DBSCAN 点云聚类参数怎么定、簇怎么分、噪声怎么处理2.1 为什么避障场景优先选 DBSCAN 而不是 K-Means点云聚类在避障里的核心诉求不是「分得漂亮」而是「不能把障碍物切碎也不能把两个靠近的障碍物粘成一个」。K-Means 需要预先指定簇数量而机器人前方有几辆车、几个人是未知的这个前提就不成立。DBSCAN 基于密度不需要簇数还能把稀疏的噪声点单独标出来正好匹配「障碍物密集、背景稀疏」的典型场景。另一个实际原因是 DBSCAN 对簇形状不敏感。激光雷达扫出来的车辆轮廓是弧形行人是不规则团块K-Means 用欧氏距离硬切容易把一辆车的车头和车尾分成两簇后续融合时视觉框和点云簇对不上决策层就会收到重复障碍物。DBSCAN 只要密度够就连成一片轮廓完整性更好。但 DBSCAN 不是没有代价。它的两个参数 eps 和 minPts 对结果影响极大而且不同距离上的点云密度天然不均匀——近处密、远处稀。常见做法是先做体素降采样把密度拉平一些再调参。我一般会先把 eps 设成略大于体素边长minPts 设成 10 到 15然后拿实际场景的 bag 包回放看聚类效果而不是在代码里拍脑袋。2.2 核心代码拆解从 PointCloud2 到聚类簇下面这段是典型的 ROS 节点里 DBSCAN 聚类的核心逻辑基于 PCL 的pcl::EuclideanClusterExtraction做密度聚类实际项目里也常用dbscan的独立实现思路一致。# 订阅激光雷达点云做体素降采样后执行 DBSCAN 聚类 import rospy import numpy as np from sensor_msgs.msg import PointCloud2 from sklearn.cluster import DBSCAN import sensor_msgs.point_cloud2 as pc2 class LidarClusterNode: def __init__(self): # eps: 邻域半径单位米min_samples: 核心点最小邻居数 self.eps 0.35 # 略大于体素边长 0.3避免同一障碍物被切碎 self.min_samples 12 # 近处车辆约 15-20 点行人约 8-12 点 self.sub rospy.Subscriber(/velodyne_points, PointCloud2, self.callback) self.pub rospy.Publisher(/clusters, PointCloud2, queue_size1) def callback(self, msg): # 只取 xyz忽略强度减少计算量 points np.array([[p[0], p[1], p[2]] for p in pc2.read_points( msg, field_names(x, y, z), skip_nansTrue)]) if len(points) self.min_samples: return # 体素降采样把 0.3m 立方体内的点合并成一个拉平远近密度差 voxel np.round(points / 0.3).astype(np.int32) _, idx np.unique(voxel, axis0, return_indexTrue) points points[idx] # DBSCAN 聚类n_jobs-1 用满 CPU labels DBSCAN(epsself.eps, min_samplesself.min_samples, n_jobs-1).fit_predict(points) # label-1 是噪声点避障时直接丢弃不参与融合 clusters [points[labels i] for i in set(labels) if i ! -1] rospy.loginfo(clusters: %d, noise: %d, len(clusters), int(np.sum(labels -1)))逻辑说明先做体素降采样把 0.3 米立方体内的多个点合并成一个代表点这一步既降计算量又缓解远处点稀疏导致 DBSCAN 漏检的问题。然后对降采样后的点做 DBSCANeps0.35比体素边长略大保证同一障碍物降采样后的点还能连成一片。min_samples12是经验值太小会把噪声当障碍物太大会把远处行人漏掉。最后把label-1的噪声点丢掉只保留有效簇。参数说明eps每增大 0.1远处障碍物更容易连成簇但相邻两个障碍物也更容易被合并min_samples每增大 5噪声抑制更强但小目标召回下降。建议用 rosbag 回放把这两个参数做成动态配置边看 RViz 边调不要一次写死。2.3 聚类结果怎么和视觉检测对齐点云簇出来之后下一步是把它和视觉检测框做关联。常见做法是把点云簇投影到图像平面计算簇的二维包围盒与检测框的 IoU超过阈值就认为匹配。这里有个容易翻车的点点云和相机的时间戳不同步车一动投影位置就偏了。我一般会在融合节点里做时间对齐用message_filters的ApproximateTime策略允许 50ms 以内的偏差超过就丢弃这一帧宁可少融合也不融错。3. 视觉检测与多传感器融合从检测框到障碍物列表3.1 视觉检测输出怎么接入融合层视觉检测部分通常用 YOLO 系列或 SSD 输出[x_min, y_min, x_max, y_max, class_id, score]。接入融合层时不要直接把检测框当障碍物用因为单目视觉没有深度框的位置在三维空间里是不确定的。正确做法是把检测框作为「类别和存在性」的证据把点云簇作为「位置和尺寸」的证据两者互补。具体流程是先把点云簇投影到图像找到与检测框 IoU 最大的簇如果匹配成功就用点云簇的三维质心和尺寸作为障碍物的位置和大小用检测框的类别作为障碍物类型如果某个检测框没有匹配到点云簇说明可能是远处小目标或误检先挂起连续三帧都出现再纳入障碍物列表。这个「连续帧确认」机制能过滤掉大部分视觉误检。3.2 融合节点代码关联、确认与障碍物发布# 将点云簇与视觉检测框关联输出融合后的障碍物列表 import numpy as np class FusionNode: def __init__(self): self.iou_thresh 0.3 # 簇投影框与检测框的最小 IoU self.confirm_frames 3 # 视觉独有目标连续确认帧数 self.pending {} # 暂存未匹配的检测框 def project_cluster(self, cluster, K, T_lidar_cam): # 点云簇投影到图像先转到相机坐标系再用内参投影 pts_cam (T_lidar_cam np.hstack( [cluster, np.ones((len(cluster), 1))]).T).T[:, :3] uv (K pts_cam.T).T uv uv[:, :2] / uv[:, 2:3] return [uv[:, 0].min(), uv[:, 1].min(), uv[:, 0].max(), uv[:, 1].max()] def iou(self, box_a, box_b): # 计算两个二维框的 IoU xa, ya max(box_a[0], box_b[0]), max(box_a[1], box_b[1]) xb, yb min(box_a[2], box_b[2]), min(box_a[3], box_b[3]) inter max(0, xb - xa) * max(0, yb - ya) area_a (box_a[2] - box_a[0]) * (box_a[3] - box_a[1]) area_b (box_b[2] - box_b[0]) * (box_b[3] - box_b[1]) return inter / (area_a area_b - inter 1e-6) def fuse(self, clusters, detections, K, T_lidar_cam): obstacles [] matched_det set() for c in clusters: proj self.project_cluster(c, K, T_lidar_cam) best_iou, best_j 0, -1 for j, d in enumerate(detections): score self.iou(proj, d[:4]) if score best_iou: best_iou, best_j score, j if best_iou self.iou_thresh: # 点云给位置视觉给类别 obstacles.append({center: c.mean(axis0), size: c.max(axis0) - c.min(axis0), type: detections[best_j][4]}) matched_det.add(best_j) else: # 无视觉匹配的簇按未知障碍物处理保守避让 obstacles.append({center: c.mean(axis0), size: c.max(axis0) - c.min(axis0), type: unknown}) # 视觉独有目标做连续帧确认 for j, d in enumerate(detections): if j in matched_det: continue key (d[4], int(d[0] // 20), int(d[1] // 20)) self.pending[key] self.pending.get(key, 0) 1 if self.pending[key] self.confirm_frames: obstacles.append({center: None, size: None, type: d[4], image_box: d[:4]}) return obstacles逻辑说明project_cluster把点云簇从雷达坐标系转到相机坐标系再投影到图像得到二维包围盒。iou计算投影框与检测框的重叠度。fuse里对每个点云簇找最佳匹配检测框匹配上就用点云的位置和视觉的类别没匹配上的簇按未知障碍物处理决策层会保守避让。视觉独有目标用pending字典做连续帧计数达到confirm_frames才纳入避免误检导致急刹。参数说明iou_thresh设 0.3 是平衡漏匹配和误匹配的经验值相机和雷达外参标定不准时可以降到 0.2但会引入更多错误关联。confirm_frames设 3 在 10Hz 相机下约 0.3 秒延迟对低速机器人可接受高速车辆要降到 2 甚至 1但误检率会上升。3.3 外参标定融合成败的隐形前提多传感器融合最容易被低估的环节是外参标定。点云投影到图像偏几厘米IoU 就掉一大截融合直接失效。常见做法是找一块标定板同时录雷达和相机数据用autoware_camera_lidar_calibrator或lidar_camera_calibration工具做联合标定。标定完不要只看重投影误差要在实际场景里把点云投影到图像上看车辆轮廓和点云边缘是否贴合。我见过重投影误差很小但实际投影偏移的情况原因是标定时的距离和实际使用距离差太多镜头畸变模型没覆盖到。4. 避坑与排查融合避障里最容易翻车的五个点4.1 现象点云簇数量忽多忽少RViz 里障碍物闪烁原因DBSCAN 的eps和min_samples固定但点云密度随距离和反射率变化远处车辆点少偶尔低于min_samples就被当噪声丢掉下一帧又够数导致簇时有时无。解决引入距离自适应参数近处eps小、远处eps大或者先做体素降采样拉平密度。更稳妥的做法是对聚类结果做帧间跟踪用卡尔曼滤波平滑障碍物位置单帧丢失不立即删除障碍物。4.2 现象视觉检测框和点云簇明明对应同一个物体IoU 却很低原因时间戳不同步。雷达和相机频率不同直接取最近帧做融合车辆运动 0.1 秒就能偏移几十厘米投影框和检测框错开。解决用message_filters做时间同步允许 50ms 偏差如果硬件支持用 PTP 或 GPS 秒脉冲做硬同步。软件同步只能缓解不能根治高速场景。4.3 现象融合后障碍物列表里同一个物体出现两次原因一个真实障碍物被 DBSCAN 分成两个簇或者视觉检测框和点云簇匹配失败后各自成了一条障碍物记录。解决在融合输出前做一次 NMS非极大值抑制按三维距离合并中心点相近的障碍物。同时检查 DBSCAN 参数eps太小是簇分裂的主因。4.4 现象避障决策频繁急停机器人走走停停原因视觉独有目标没有做连续帧确认单帧误检直接进入障碍物列表决策层看到突然出现的障碍物就急停。解决加上confirm_frames机制视觉独有目标连续多帧出现才纳入。同时给障碍物列表加生命周期管理超过一定时间没更新的障碍物自动移除避免幽灵障碍物。4.5 现象仿真里跑得好好的上实车就撞原因仿真点云是理想模型没有噪声、没有反射率变化、没有运动畸变。实车雷达点云有拖尾、有地面点干扰DBSCAN 会把地面和障碍物连成一片。解决上实车前先做地面分割用 RANSAC 或pcl::SACSegmentation把地面点滤掉再对非地面点做聚类。另外仿真里的外参是精确值实车标定有误差融合阈值要适当放宽。5. 动态避障决策与路径规划从障碍物列表到可执行轨迹5.1 局部路径规划怎么用融合后的障碍物列表融合层输出的是障碍物列表每个障碍物有位置、尺寸和类型。局部路径规划器常见的是 DWA、TEB 或 MPC需要的是障碍物在代价地图里的表达。我一般会把障碍物列表转成局部代价地图的膨胀层已知尺寸的障碍物按实际尺寸加安全半径膨胀未知类型的障碍物按最大可能尺寸膨胀视觉独有目标按行人尺寸膨胀。这样规划器不用关心障碍物来自雷达还是相机只处理统一的代价地图。动态避障小车路径规划的关键是预测。静态障碍物只需要膨胀动态障碍物要估计速度。如果资源包里带了跟踪模块可以用跟踪输出的速度做前向预测把障碍物未来 1 到 2 秒的位置也标进代价地图。没有跟踪模块的话至少要用连续两帧的位置差估算速度否则规划器只能做反应式避让遇到横穿的行人容易来不及。5.2 避障参数配置与验证方法下面是一个典型的局部代价地图配置片段用于把融合障碍物接入规划器# local_costmap_params.yaml local_costmap: plugins: - {name: obstacles, type: costmap_2d::ObstacleLayer} - {name: inflation, type: costmap_2d::InflationLayer} obstacles: observation_sources: fused_obstacles fused_obstacles: topic: /fused_obstacles data_type: PointCloud2 marking: true clearing: true obstacle_range: 6.0 # 超过 6 米不标记避免远处噪声 raytrace_range: 8.0 # 清除范围略大于标记范围 inflation: inflation_radius: 0.55 # 机器人半径 0.3 安全余量 0.25 cost_scaling_factor: 3.0 # 越大代价衰减越快路径越贴障碍物参数说明obstacle_range决定多远内的障碍物进入代价地图设太大远处噪声会拖慢规划设太小高速时来不及避让。inflation_radius至少是机器人内切圆半径加安全余量太小会擦碰太大会导致窄通道无法通过。cost_scaling_factor影响路径与障碍物的距离值越大路径越贴近障碍物值越小越保守。验证方法先在仿真里用静态障碍物验证代价地图膨胀是否正确再加动态障碍物验证预测是否生效最后用 rosbag 回放实际场景数据看规划轨迹是否平滑、有没有震荡。震荡通常是膨胀半径和代价衰减不匹配或者障碍物列表更新频率太低。5.3 一个容易被忽略的技巧给融合结果加置信度融合后的障碍物不应该只有「有」和「无」两种状态应该带置信度。点云和视觉都检测到的障碍物置信度最高只有点云检测到的次之只有视觉检测到的最低。规划器可以根据置信度调整避让策略高置信度障碍物正常膨胀低置信度障碍物只做减速不绕行。这个技巧能显著减少误检导致的过度避让我后来每次做融合避障都会在障碍物消息里加一个confidence字段规划器里按置信度分档处理。希望帮到你。本文还有配套的精品资源点击获取