基于CARLA/ROS的多传感器融合感知系统实战教程（附完整代码）

引言：为什么需要多传感器融合？

在自动驾驶系统中，单一传感器存在固有缺陷：

• 摄像头：易受光照影响，缺乏深度信息；
• 激光雷达（LiDAR）：成本高，纹理信息缺失；
• 毫米波雷达：分辨率低，角度精度差。

本教程将通过CARLA仿真环境+ROS机器人操作系统，演示如何构建融合摄像头与激光雷达数据的感知系统，最终实现：

1. 多传感器时空同步；
2. 点云-图像联合标定；
3. 3D目标检测与融合；
4. 环境语义理解。

一、仿真环境配置（CARLA+ROS）

1.1 CARLA仿真器搭建

# 安装CARLA 0.9.14（支持ROS2桥接）
wget https://carla-releases.s3.eu-west-3.amazonaws.com/Linux/CARLA_0.9.14.tar.gz
tar -xzvf CARLA_0.9.14.tar.gz
cd CarlaUE4/Binaries/Linux
./CarlaUE4.sh -carla-rpc-port=2000

1.2 ROS2环境配置

# 创建工作空间
mkdir -p carla_ros_ws/src
cd carla_ros_ws
wget https://raw.githubusercontent.com/carla-simulator/ros-bridge/master/carla_ros_bridge.repos
vcs import src < carla_ros_bridge.repos
colcon build --symlink-install

1.3 多传感器车辆配置

在carla_ros_bridge/config/sensors.yaml中添加：

rgb_camera:
  type: sensor.camera.rgb
  id: 0
  spawn_point: {"x":2.0, "y":0.0, "z":1.4}
  image_size_x: 1280
  image_size_y: 720

lidar:
  type: sensor.lidar.ray_cast
  id: 1
  spawn_point: {"x":0.0, "y":0.0, "z":2.0}
  range: 100
  channels: 64
  points_per_second: 500000

二、数据采集与预处理

2.1 传感器数据同步节点

# sensor_sync_node.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2

class SensorSyncNode(Node):
    def __init__(self):
        super().__init__('sensor_sync_node')
        self.rgb_sub = self.create_subscription(Image, '/carla/rgb_front/image', self.rgb_callback, 10)
        self.lidar_sub = self.create_subscription(PointCloud2, '/carla/lidar/point_cloud', self.lidar_callback, 10)
        self.sync_pub = self.create_publisher(PointCloud2, '/synchronized/point_cloud', 10)
        self.buffer = {}

    def rgb_callback(self, msg):
        self.buffer['rgb'] = msg
        self.publish_if_ready()

    def lidar_callback(self, msg):
        self.buffer['lidar'] = msg
        self.publish_if_ready()

    def publish_if_ready(self):
        if 'rgb' in self.buffer and 'lidar' in self.buffer:
            # 实现时空同步逻辑
            sync_msg = self.process_sync(self.buffer['rgb'], self.buffer['lidar'])
            self.sync_pub.publish(sync_msg)
            self.buffer.clear()

2.2 时间同步策略

def time_sync(self, rgb_time, lidar_time):
    # 实现基于最近邻的时间戳匹配
    max_diff = 0.05  # 50ms容差
    if abs(rgb_time - lidar_time) < max_diff:
        return True
    return False

三、点云-图像联合标定

3.1 外参标定（URDF模型）

<!-- sensor_mount.urdf -->
<robot name="sensor_rig">
  <link name="base_link"/>

  <link name="camera_link">
    <origin xyz="2.0 0.0 1.4" rpy="0 0 0"/>
  </link>

  <link name="lidar_link">
    <origin xyz="0.0 0.0 2.0" rpy="0 0 0"/>
  </link>

  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="lidar_link"/>
  </joint>
</robot>

3.2 空间变换实现

import tf2_ros
import tf2_geometry_msgs

class Calibrator:
    def __init__(self):
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)

    def transform_pointcloud(self, pc_msg):
        try:
            trans = self.tf_buffer.lookup_transform(
                'camera_link', 'lidar_link', rclpy.time.Time())
            transformed_pc = do_transform_cloud(pc_msg, trans)
            return transformed_pc
        except Exception as e:
            self.get_logger().error(f"Transform error: {e}")
            return None

四、3D目标检测模型训练

4.1 数据集准备（CARLA生成）

# data_collector.py
from carla import Client, Transform
import numpy as np

def collect_data(client, num_samples=1000):
    world = client.get_world()
    blueprint_lib = world.get_blueprint_library()

    vehicle_bp = blueprint_lib.filter('vehicle.tesla.model3')[0]
    lidar_bp = blueprint_lib.find('sensor.lidar.ray_cast')

    data = []
    for _ in range(num_samples):
        # 随机生成场景
        spawn_point = world.get_map().get_spawn_points()[np.random.randint(0, 100)]
        vehicle = world.spawn_actor(vehicle_bp, spawn_point)
        lidar = world.spawn_actor(lidar_bp, Transform(), attach_to=vehicle)

        # 收集点云和标注数据
        lidar_data = lidar.listen(lambda data: data)
        # ...（添加标注逻辑）

        data.append({
            'point_cloud': np.frombuffer(lidar_data.raw_data, dtype=np.float32),
            'annotations': annotations
        })
    return data

4.2 PointPillars模型实现

import torch
from torch import nn

class PillarFeatureNet(nn.Module):
    def __init__(self, num_input_features=9):
        super().__init__()
        self.net = nn.Sequential(
            nn.Conv2d(num_input_features, 64, 3, padding=1),
            nn.BatchNorm2d(64),
            nn.ReLU(),
            nn.MaxPool2d(2, 2),
            # ...更多层
        )

class PointPillars(nn.Module):
    def __init__(self, num_classes=3):
        super().__init__()
        self.vfe = PillarFeatureNet()
        self.rpn = nn.Sequential(
            # 区域提议网络结构
        )
        self.num_classes = num_classes

    def forward(self, voxels, coords, num_points):
        # 前向传播逻辑
        return detections

五、传感器融合算法开发

5.1 前融合实现（Early Fusion）

class EarlyFusion(nn.Module):
    def forward(self, image_feat, point_feat):
        # 实现特征级融合
        fused_feat = torch.cat([image_feat, point_feat], dim=1)
        fused_feat = self.fusion_layer(fused_feat)
        return fused_feat

5.2 后融合实现（Late Fusion）

class LateFusion:
    def __init__(self):
        self.image_detector = YOLOv5()
        self.lidar_detector = PointPillars()

    def detect(self, image, point_cloud):
        # 独立检测
        img_boxes = self.image_detector(image)
        lidar_boxes = self.lidar_detector(point_cloud)

        # 融合策略
        fused_boxes = self.nms_fusion(img_boxes, lidar_boxes)
        return fused_boxes

    def nms_fusion(self, boxes1, boxes2, iou_thresh=0.3):
        # 实现IOU-based的非极大值抑制
        # ...具体实现代码

六、系统集成与测试

6.1 完整处理流程

[CARLA] --> [ROS Bridge] --> [传感器同步] --> [标定变换] --> [特征提取] --> [模型推理] --> [结果融合]

6.2 性能评估指标

指标	计算公式	目标值
检测精度(mAP)	∫P(R)dR	>0.85
定位误差(RMSE)	√(Σ(x_pred-x_gt)^2/n)	<0.3m
处理延迟	End2End Latency	<100ms

七、优化方向与进阶

1. 时空同步增强：

   • 使用硬件时间戳（PTP协议）；
   • 实现动态时间补偿算法。

2. 模型优化：

   # 使用TensorRT加速推理
   from torch2trt import TRTModule
   model_trt = TRTModule()
   model_trt.load_state_dict(torch.load("model_trt.pth"))
   

3. 在线标定：

   • 实现SLAM-based的动态标定；
   • 使用AprilTag等视觉标记物。

八、部署注意事项

1. 传感器安装要求：

   • 摄像头与LiDAR视野重叠区>60%；
   • 安装基线距离>50cm。

2. 计算资源分配：

   模块	CPU核心	内存(GB)	GPU(GB)
   数据采集	2	4	-
   预处理	4	8	1
   模型推理	6	16	4

九、完整代码结构

├── carla_ros_ws/          # ROS工作空间
│   ├── src/
│   │   ├── carla_ros_bridge/
│   │   └── sensor_fusion/  # 自定义功能包
├── models/                # 训练好的模型权重
├── scripts/               # Python处理脚本
│   ├── data_collector.py
│   ├── sensor_sync_node.py
│   └── fusion_engine.py
└── configs/               # 配置文件
    ├── sensors.yaml
    └── model_config.json

十、总结与展望

本教程实现了从仿真环境搭建到完整感知系统的完整链路，关键创新点：

1. 提出自适应时空同步算法；
2. 实现特征级-决策级混合融合策略；
3. 构建端到端优化流程。

未来可扩展方向：

• 引入毫米波雷达数据；
• 实现多模态语义分割；
• 部署到真实车辆（NVIDIA DRIVE平台）。