传感器融合与感知系统
智能无人车核心课程 · 高中进阶篇
第一章：多传感器系统概述
1.1 为什么需要多传感器融合？
单一传感器无法应对所有场景。想象一个自动驾驶车辆进入隧道：
| 传感器 | 隧道内表现 | 局限性 |
| 摄像头 | 光线骤变，图像过曝或过暗 | 依赖光照条件 |
| 激光雷达 | 正常工作，但缺少语义信息 | 无法识别交通标志颜色 |
| GPS | 信号丢失，无法定位 | 室内/隧道/高楼区失效 |
| IMU | 短期可用，但累积误差 | 长时间漂移严重 |
| 毫米波雷达 | 正常工作 | 角分辨率低，无法识别行人姿态 |
核心洞察：每种传感器都有"能力边界"，融合的目的就是让系统在任何场景下都能找到"可靠的信息源"。
1.2 传感器特性深度对比
摄像头（Camera）
工作原理：光子 → 镜头聚焦 → CMOS 感光阵列 → 电信号 → ISP 处理 → 数字图像
关键参数：
├── 分辨率：1280×720 / 1920×1080 / 2880×1860（Tesla 使用 120 万像素摄像头）
├── 帧率：30fps / 60fps（高速场景需更高帧率）
├── 动态范围：HDR 可达 120dB（人眼约 90dB）
├── 视场角（FOV）：30°（长焦）~ 120°（鱼眼）
└── 探测距离：200m~500m（取决于焦距）
实际应用中的挑战：
- 逆光场景：太阳直射镜头时，图像出现光晕。解决方案：HDR 多帧合成 + 偏振滤镜
- 雨雾天气：水滴附着镜头。解决方案：疏水涂层 + 摄像头清洗系统 + 算法去雾
- 夜间场景：光照不足。解决方案：大光圈镜头 + 高感光度 CMOS + 红外补光
激光雷达（LiDAR）
工作原理：激光发射 → 遇到物体反射 → 接收器接收 → 计算飞行时间（ToF）→ 生成点云
关键参数：
├── 线数：16线 / 32线 / 64线 / 128线（Velodyne VLS-128）
├── 测距精度：±2cm（典型值）
├── 角分辨率：0.1°~0.4°
├── 探测距离：100m~300m
├── 点云密度：每秒 100 万~260 万个点
└── 转速：600~1200 RPM（旋转式）
点云数据结构：
# 一个典型的 LiDAR 点云数据结构
class PointCloud:
def __init__(self):
self.points = [] # N×3 数组：(x, y, z)
self.intensity = [] # N×1 数组：反射强度
self.timestamp = [] # N×1 数组：每个点的时间戳
def to_bev_image(self, resolution=0.1):
"""将 3D 点云转换为鸟瞰图（BEV）"""
x_min, x_max = -50, 50
y_min, y_max = -50, 50
width = int((x_max - x_min) / resolution)
height = int((y_max - y_min) / resolution)
bev = np.zeros((height, width), dtype=np.uint8)
for point in self.points:
x, y, z = point
if x_min <= x <= x_max and y_min <= y <= y_max:
col = int((x - x_min) / resolution)
row = int((y - y_min) / resolution)
bev[row, col] = min(255, int(z * 10 + 128))
return bev
毫米波雷达（Radar）
工作原理：电磁波发射 → 遇到目标反射 → 接收天线接收 → 多普勒频移计算 → 目标距离与速度
关键参数：
├── 频段：77GHz（主流）/ 79GHz（新一代）
├── 探测距离：前方 200m+，侧方 80m+
├── 距离分辨率：3.75cm（79GHz 4GHz 带宽）
├── 速度分辨率：0.1m/s
├── 角分辨率：10°~15°（4D 成像雷达可达 1°）
└── 优势：全天候工作，直接测量径向速度
1.3 传感器标定
多传感器融合的前提是精确标定——知道每个传感器在车辆坐标系中的精确位置和姿态。
外参标定流程：
步骤 1：安装标定
├── 使用高精度全站仪测量各传感器的安装位置
├── 获得初始的平移向量 T 和旋转矩阵 R
└── 精度：±1cm 平移，±0.5° 旋转
步骤 2：在线自标定
├── 利用行驶中的自然特征（车道线、路沿、其他车辆）
├── 通过优化算法不断微调外参
├── 典型方法：基于互信息的 LiDAR-Camera 标定
└── 精度：±0.5cm 平移，±0.1° 旋转
第二章：点云处理与 3D 目标检测
2.1 点云预处理
原始点云包含大量噪声，需要预处理：
def preprocess_point_cloud(cloud):
"""点云预处理管线"""
# 1. 去除地面点
# 使用 RANSAC 拟合地面平面方程 ax + by + cz + d = 0
ground_model = RANSAC(max_iterations=1000, distance_threshold=0.2)
ground_model.fit(cloud.points)
non_ground = cloud.remove_points(ground_model.inliers)
# 2. 统计滤波去除离群点
# 计算每个点到其 k 近邻的平均距离，剔除距离过大的点
filtered = statistical_outlier_removal(non_ground, k=20, std_threshold=2.0)
# 3. 体素化降采样
# 将空间划分为 0.1m×0.1m×0.1m 的体素，每个体素保留一个代表点
downsampled = voxel_grid_filter(filtered, voxel_size=0.1)
return downsampled
2.2 PointPillars：实时 3D 目标检测
PointPillars 是工业界广泛使用的点云检测算法，兼顾精度与速度。
核心思想：将 3D 点云转换为 2D 伪图像，然后用成熟的 2D 卷积网络检测。
处理流程：
原始点云 (N × 4) [x, y, z, intensity]
↓
[Feature Encoder]
├── 空间划分：将 XY 平面划分为 pillars（柱体）
├── 每个 pillar 内：
│ ├── 最多保留 P 个点（如 100 个）
│ ├── 对每个点计算扩展特征：
│ │ ├── (x, y, z) - 点坐标
│ │ ├── intensity - 反射强度
│ │ ├── (x_c, y_c, z_c) - 与 pillar 中心的偏移
│ │ └── (x_p, y_p) - 与 pillar 几何中心的偏移
│ └── 通过 PointNet 提取 pillar 特征 → 1 个特征向量
├── 输出：D × H × W 的伪图像
│ ├── D = 特征维度（如 64）
│ ├── H = Y 方向的 pillar 数量
│ └── W = X 方向的 pillar 数量
↓
[2D Backbone Network]
├── 多层 2D 卷积 + BatchNorm + ReLU
├── 多尺度特征融合（FPN 结构）
└── 输出：多通道特征图
↓
[Detection Head]
├── 分类分支：每个位置预测属于各类别的概率
├── 回归分支：预测 (x, y, w, l, θ, z, h)
│ ├── (x, y) - 中心点偏移
│ ├── (w, l) - 宽高
│ ├── θ - 朝向角
│ └── (z, h) - 高度信息
└── 方向分类：消除朝向角的 180° 歧义
2.3 BEVFusion：多模态融合
BEVFusion 是当前最先进的多传感器融合框架之一。
融合策略：在鸟瞰图（BEV）空间进行特征融合
摄像头图像 (6 个视角) LiDAR 点云
↓ ↓
[Image Backbone] [Point Encoder]
├── ResNet/Swin Transformer ├── VoxelNet/PointPillars
├── 输出：多尺度特征图 └── 输出：BEV 特征
↓ ↓
[View Transform] [BEV Project]
├── LSS / BEVFormer ├── 直接投影到 BEV
├── 将图像特征转换到 BEV 空间 └── 输出：BEV 特征
↓ ↓
└───────────┬───────────────┘
↓
[BEV Feature Fusion]
├── 拼接（Concat）或逐元素相加
├── Transformer 注意力融合
└── 输出：融合后的 BEV 特征
↓
[Prediction Head]
├── 3D 目标检测
├── BEV 语义分割（车道线、可行驶区域）
└── 运动预测（速度、航向变化率）
第三章：SLAM 与定位
3.1 为什么定位如此重要？
自动驾驶需要厘米级定位精度。GPS 的精度（3~5 米）远远不够——想象车辆在高速公路上，3 米的误差可能意味着不知道自己在哪条车道。
3.2 多源融合定位系统
定位系统架构：
┌─────────────────────────────────────────────────────────┐
│ 融合定位引擎 │
│ ┌─────────────────────────────────────────────────┐ │
│ │ 扩展卡尔曼滤波器（EKF） │ │
│ │ 状态向量：[x, y, z, roll, pitch, yaw, vx, vy, vz] │
│ └─────────────────────────────────────────────────┘ │
│ ↑ ↑ ↑ ↑ │
│ ┌────┴───┐ ┌───┴────┐ ┌──┴───┐ ┌───┴────┐ │
│ │GNSS/RTK│ │ IMU │ │ LiDAR│ │ 轮速计 │ │
│ │ 定位 │ │ 积分 │ │SLAM │ │ 里程计 │ │
│ │±2cm │ │短期精确│ │±5cm │ │±0.5% │ │
│ │但会跳变│ │但会漂移│ │全局一致│ │长期稳定│ │
│ └────────┘ └────────┘ └──────┘ └────────┘ │
└─────────────────────────────────────────────────────────┘
3.3 LiDAR SLAM 原理
核心思想：通过匹配连续帧的点云，估计车辆的运动轨迹，同时构建环境地图。
LiDAR SLAM 工作流程：
帧 N 的点云 P_n 帧 N+1 的点云 P_{n+1}
↓ ↓
[特征提取] [特征提取]
├── 边缘特征 ├── 边缘特征
│ （曲率大的点） │
└── 平面特征 └── 平面特征
（曲率小的点） （曲率小的点）
↓ ↓
[点云匹配] ←──────────── [初始位姿预测]
├── 边缘特征 ↔ 边缘线 （来自 IMU/里程计）
├── 平面特征 ↔ 平面
└── ICP / NDT 优化
↓
[位姿更新] → 输出 T_n（车辆在世界坐标系中的位姿）
↓
[地图更新] → 将 P_n 转换到世界坐标系，加入全局地图
NDT（正态分布变换）匹配：
def ndt_matching(source_cloud, target_map, initial_pose):
"""
NDT 匹配算法
核心思想：将目标点云空间划分为体素，
每个体素内用多元高斯分布 N(μ, Σ) 建模。
优化源点云的变换，使变换后的点落在高斯分布概率最大的位置。
"""
# 1. 构建目标地图的 NDT 网格
voxel_size = 1.0 # 1m × 1m × 1m
ndt_map = NDTMap(voxel_size)
for voxel in target_map:
points = voxel.get_points()
mean = np.mean(points, axis=0) # 均值 μ
cov = np.cov(points.T) # 协方差 Σ
ndt_map.add_voxel(mean, cov)
# 2. 优化变换参数
pose = initial_pose.copy()
for iteration in range(30):
score = 0
H = np.zeros((6, 6)) # Hessian 矩阵
g = np.zeros(6) # 梯度向量
for point in source_cloud:
# 将点变换到目标坐标系
transformed = transform_point(point, pose)
voxel = ndt_map.get_voxel(transformed)
if voxel is None:
continue
# 计算概率密度
diff = transformed - voxel.mean
prob = np.exp(-0.5 * diff.T @ np.linalg.inv(voxel.cov) @ diff)
# 累积梯度和 Hessian
J = compute_jacobian(point, pose) # 6×3 雅可比矩阵
score += -np.log(prob + 1e-10)
g += J.T @ (voxel.cov_inv @ diff) * prob
H += J.T @ voxel.cov_inv @ J * prob
# 牛顿法更新
delta = -np.linalg.solve(H, g)
pose = apply_delta(pose, delta)
if np.linalg.norm(delta) < 1e-6:
break
return pose
第四章：行为规划与决策
4.1 分层规划架构
高层决策（Mission Planner）
├── 输入：起点、终点、路网拓扑
├── 任务：全局路径规划
├── 算法：A* / Dijkstra / 路网搜索
└── 输出：全局参考路径（由一系列路段组成）
中层规划（Behavior Planner）
├── 输入：全局路径 + 感知结果 + 交通规则
├── 任务：行为决策
│ ├── 跟车（Car Following）
│ ├── 变道（Lane Change）
│ ├── 超车（Overtaking）
│ ├── 路口通行（Intersection）
│ ├── 让行（Yielding）
│ └── 紧急避让（Emergency Avoidance）
├── 算法：有限状态机（FSM）/ 决策树 / 强化学习
└── 输出：行为指令（如"向左变道"、"减速让行"）
底层规划（Motion Planner）
├── 输入：行为指令 + 障碍物位置 + 车辆运动学约束
├── 任务：生成可执行的轨迹
├── 算法：
│ ├── 基于采样的方法（Lattice Planner）
│ ├── 基于优化的方法（QP / NLP）
│ └── 基于搜索的方法（Hybrid A*）
└── 输出：时空轨迹 (x(t), y(t), θ(t), v(t), a(t))
4.2 有限状态机（FSM）实现
class AutonomousDrivingFSM:
"""自动驾驶行为决策有限状态机"""
def __init__(self):
self.current_state = "LANE_KEEPING"
self.transitions = {
"LANE_KEEPING": {
"前方有慢车": "FOLLOWING",
"左侧快车道空出": "CHANGE_LANE_LEFT",
"右侧慢车道空出": "CHANGE_LANE_RIGHT",
"前方路口": "APPROACHING_INTERSECTION",
"行人横穿": "EMERGENCY_STOP",
},
"FOLLOWING": {
"前车加速/变道离开": "LANE_KEEPING",
"跟车距离足够，左侧空出": "CHANGE_LANE_LEFT",
"前车急刹": "EMERGENCY_STOP",
},
"CHANGE_LANE_LEFT": {
"变道完成": "LANE_KEEPING",
"左侧突然出现车辆": "ABORT_LANE_CHANGE",
},
"APPROACHING_INTERSECTION": {
"绿灯，无冲突": "CLEAR_INTERSECTION",
"红灯": "STOP_AT_LIGHT",
"有横向来车": "YIELD_AT_INTERSECTION",
},
# ... 更多状态
}
def update(self, perception, planning_context):
"""每个规划周期调用"""
# 评估当前状态下的所有可能转移
for condition, next_state in self.transitions[self.current_state].items():
if self.evaluate_condition(condition, perception, planning_context):
print(f"状态转移：{self.current_state} → {next_state}")
self.current_state = next_state
return next_state
return self.current_state
第五章：实战项目
项目 1：点云可视化与分析
使用 Python + Open3D 处理真实 LiDAR 数据：
import open3d as o3d
# 加载点云
pcd = o3d.io.read_point_cloud("kitti_sample.pcd")
# 地面分割
_, inliers = pcd.segment_plane(distance_threshold=0.2, ransac_n=3, num_iterations=100)
ground = pcd.select_by_index(inliers)
objects = pcd.select_by_index(inliers, invert=True)
# 聚类
labels = np.array(objects.cluster_dbscan(eps=0.3, min_points=10))
colors = plt.cm.tab20(labels.astype(float) / (labels.max() + 1))
objects.colors = o3d.utility.Vector3dVector(colors[:, :3])
# 可视化
o3d.visualization.draw_geometries([ground, objects])
项目 2：简单的 PID 横向控制
class LateralPIDController:
"""横向控制：让车辆保持在车道中心"""
def __init__(self, kp=1.0, ki=0.01, kd=0.1):
self.kp = kp # 比例系数
self.ki = ki # 积分系数
self.kd = kd # 微分系数
self.integral = 0
self.prev_error = 0
def compute_steering(self, lateral_error, dt):
"""
lateral_error: 车辆中心与车道中心的横向偏差（米）
正值表示偏右，负值表示偏左
"""
self.integral += lateral_error * dt
derivative = (lateral_error - self.prev_error) / dt
self.prev_error = lateral_error
steering_angle = (
self.kp * lateral_error +
self.ki * self.integral +
self.kd * derivative
)
# 限制转向角度
return np.clip(steering_angle, -0.5, 0.5) # ±0.5 弧度
课后思考
- 为什么特斯拉选择"纯视觉"方案（只用摄像头），而大多数自动驾驶公司选择"激光雷达+摄像头"融合方案？各自的优劣是什么？
- 在暴雨天气下，哪些传感器会受到影响？如何设计冗余策略？
- 如果 GPS 信号完全丢失（如长隧道），车辆如何维持定位精度？
- PointPillars 为什么选择将 3D 点云转为 2D 伪图像，而不是直接在 3D 空间处理？
参考资源：
- Apollo 自动驾驶平台文档
- KITTI Vision Benchmark Suite
- nuScenes 数据集
- PointPillars 论文：Lang et al., "PointPillars: Fast Encoders for Object Detection from Point Clouds", CVPR 2019
- BEVFusion 论文：Liu et al., "BEVFusion: Multi-Task Multi-Sensor Fusion with Unified Bird's-Eye View Representation", ICRA 2023