巡检车上装了两个距离传感器：MS200 激光雷达和 Aurora 930 Pro 深度相机。激光雷达 360° 扫描但前方有盲区，深度相机只看前方但近距离精度高。SLAM 和 Nav2 需要一条统一的 /scan 话题——把两个传感器的数据合成一条扫描线，就是 Scan Fusion 要干的活。
听起来简单：把深度相机的点投影到雷达坐标系，重叠区域取最近的。但实际做起来有三个坑：坐标系变换、点数不一致、和那个让 SLAM 崩溃的可变长度问题。
融合逻辑：前方 ±70° 取最近
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
| def _fuse_scan_ranges(self, raw, depth, transform):
fused = list(raw.ranges)
for r in depth.ranges:
x_d = r * math.cos(angle)
y_d = r * math.sin(angle)
x_r = c * x_d - s * y_d + tx
y_r = s * x_d + c * y_d + ty
range_r = math.hypot(x_r, y_r)
angle_r = math.atan2(y_r, x_r)
if abs(angle_r) > self._fuse_front_half_angle_rad:
continue
idx = int((angle_r - raw_angle_min) / raw_angle_inc + 0.5)
current = fused[idx]
if (not math.isfinite(current)) or (range_r < current):
fused[idx] = float(range_r)
return fused
|
五步流水线：深度点 → 坐标变换 → 角度过滤 → bin 映射 → 最近优先。
min 融合策略很关键：如果雷达说前方 1.5m 有东西，深度相机说 0.3m 有东西，那 0.3m 是对的——深度相机在近距离更准。反过来，如果雷达说有东西但深度没检测到，雷达的值保留——深度相机可能因为反光丢了数据。
坐标变换：TF2 自动查
深度相机和雷达不在同一个位置，中间有个物理偏移和旋转。ROS2 的 TF2 系统维护了所有坐标系的变换关系：
1
2
3
4
5
6
7
8
9
10
11
| try:
transform = self._tf_buffer.lookup_transform(
raw.header.frame_id,
depth.header.frame_id,
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=0.05),
)
except TransformException:
self._scan_pub.publish(self._build_output_scan(raw, list(raw.ranges), ...))
return
|
TF 不可用时不融合，直接透传原始雷达数据——降级而不是崩溃。
固定点数：SLAM 的隐性要求
这是最大的坑。SLAM 算法（slam_toolbox）在初始化时记录第一条 LaserScan 的 angle_min、angle_max、angle_increment 和 ranges 长度，后续所有帧必须跟第一帧完全一致。如果某一帧的 ranges 少了几个点，SLAM 直接崩溃。
问题是：深度相机的点数会变。相机帧率波动时，深度转激光扫描的输出点数可能从 360 变成 358 或 362。融合后的点数跟着变——SLAM 就炸了。
解法：强制固定点数，用最近邻重采样。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| def _normalize_scan_payload(self, template, ranges, intensities):
if not self._enforce_fixed_scan_size:
return ranges, intensities
if self._fixed_scan_size > 0:
target_size = self._fixed_scan_size
else:
if self._expected_scan_size <= 0:
self._expected_scan_size = max(1, len(ranges))
target_size = self._expected_scan_size
normalized_ranges = self._resample_nearest(ranges, target_size, fill=template.range_max)
normalized_intensities = self._resample_nearest(intensities, target_size, fill=0.0)
return normalized_ranges, normalized_intensities
|
最近邻重采样：
1
2
3
4
5
6
7
8
9
10
11
12
| @staticmethod
def _resample_nearest(values, target_size, fill):
src = len(values)
if src == target_size:
return list(values)
scale = float(src - 1) / float(target - 1)
out = []
for i in range(target):
idx = int(round(i * scale))
out.append(float(values[idx]))
return out
|
从 358 点到 360 点：多出来的 2 个位置通过线性插值找最近的原始点填充。从 362 到 360：多余的 2 个点被均匀丢弃。变化在 ±2 点以内时，对精度几乎没有影响。
深度超时：相机卡了就别用
深度相机偶尔会卡帧——ROS topic 几百毫秒没有新消息。如果用过期的深度数据融合，车已经走了 0.5 米了，融合出来的障碍物位置全错。
1
2
3
| if depth is None or depth_age > self._depth_timeout_sec:
self._scan_pub.publish(self._build_output_scan(raw, list(raw.ranges), ...))
return
|
0.4 秒超时。深度数据超过 0.4 秒未更新就丢弃，退化为纯雷达模式。这个阈值比雷达扫描周期（~100ms）大不少，留了余量处理网络延迟。
运行时热更新
所有融合参数都可以通过 /agri/runtime_config 话题热更新——不用重启节点就能开关融合、调整角度范围、修改最小距离。
1
2
3
4
5
6
7
8
9
| def _on_runtime_config(self, msg):
payload = json.loads(msg.data)
self._apply_runtime_config_payload(payload)
def _apply_runtime_config_payload(self, payload):
fusion_cfg = runtime_cfg.get('scan_fusion')
self._depth_fusion_enabled = bool(fusion_cfg.get('depth_fusion_enabled', ...))
self._fuse_front_half_angle_deg = max(0.0, float(fusion_cfg.get('fuse_front_half_angle_deg', ...)))
...
|
带版本号和签名去重，同一个配置不会重复应用。这在调试时特别有用——现场发现融合角度太窄导致漏检，改个配置就生效，不用 SSH 上去重启 Docker。
效果
| 指标 | 纯雷达 | 雷达 + 深度融合 |
|---|
| 前方最小检测距离 | ~0.3m | ~0.1m |
| 低矮障碍检测 | 容易漏（激光扫过去） | 能检测到 |
| 玻璃/反光表面 | 检测不到 | 看深度相机 |
| SLAM 稳定性 | 稳定 | 固定点数后稳定 |
| CPU 开销 | 0 | +3%（重采样+融合） |
3% 的 CPU 换来前方近距离检测能力的质变，在树莓派上完全可以接受。