AI自动驾驶与智能交通完全教程
1. 自动驾驶技术概述与分级（L0-L5）
自动驾驶是指通过计算机系统实现对车辆的操控，使车辆在无需人类干预或少量干预的情况下完成行驶任务。SAE International（国际汽车工程师学会）将自动驾驶划分为六个等级：
| 等级 | 名称 | 描述 |
|---|---|---|
| L0 | 无自动化 | 完全由人类驾驶，系统仅提供警告 |
| L1 | 驾驶辅助 | 系统可辅助转向或加减速（如ACC自适应巡航） |
| L2 | 部分自动化 | 系统同时控制转向和加减速，人类需监控环境 |
| L3 | 有条件自动化 | 特定场景下系统完全接管，但需人类随时准备接管 |
| L4 | 高度自动化 | 特定区域/条件下完全自动驾驶，无需人类干预 |
| L5 | 完全自动化 | 任何场景下均可自动驾驶，无方向盘设计 |
当前行业主流聚焦于L2+到L4级别。特斯拉FSD、华为ADS、小鹏XNGP等系统处于L2+阶段，Waymo、百度Apollo、小马智行等则在特定区域实现了L4级运营。
自动驾驶系统的核心架构通常包含以下模块：
感知层 → 决策层 → 规划层 → 控制层 → 执行层
↑ |
└──────── 反馈回路 ─────────────────────┘
2. 感知系统（摄像头/激光雷达/毫米波雷达融合）
感知系统是自动驾驶的"眼睛"，负责获取周围环境信息。主流传感器包括：
摄像头（Camera）：获取丰富的颜色和纹理信息，成本低，但受光照和天气影响大。
激光雷达（LiDAR）：通过发射激光脉冲获取精确的三维点云数据，测距精度高，但成本较高。
毫米波雷达（Radar）：全天候工作，可直接测量速度，但分辨率较低。
多传感器融合是提升感知鲁棒性的关键。常见的融合策略有：
- 前融合（Early Fusion）：在原始数据层面融合，信息保留完整但计算量大
- 后融合（Late Fusion）：各传感器独立处理后融合结果，灵活但可能丢失关联信息
- 特征融合（Feature Fusion）：在特征提取后进行融合，兼顾信息量和计算效率
下面展示一个基于 BEV（鸟瞰图）的多传感器融合示例：
import numpy as np
import torch
import torch.nn as nn
class BEVFusion(nn.Module):
"""BEV视角下的多传感器融合网络"""
def __init__(self, camera_channels=256, lidar_channels=128, bev_size=(200, 200)):
super().__init__()
self.bev_size = bev_size
# 相机特征提取分支
self.camera_encoder = nn.Sequential(
nn.Conv2d(3, 64, kernel_size=7, stride=2, padding=3),
nn.BatchNorm2d(64),
nn.ReLU(inplace=True),
nn.Conv2d(64, 128, kernel_size=3, stride=2, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(inplace=True),
nn.Conv2d(128, camera_channels, kernel_size=3, stride=2, padding=1),
nn.BatchNorm2d(camera_channels),
nn.ReLU(inplace=True),
)
# 激光雷达BEV特征提取
self.lidar_encoder = nn.Sequential(
nn.Conv2d(lidar_channels, 128, kernel_size=3, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(inplace=True),
nn.Conv2d(128, lidar_channels, kernel_size=3, padding=1),
nn.BatchNorm2d(lidar_channels),
nn.ReLU(inplace=True),
)
# 特征融合层
self.fusion_conv = nn.Sequential(
nn.Conv2d(camera_channels + lidar_channels, 256, kernel_size=3, padding=1),
nn.BatchNorm2d(256),
nn.ReLU(inplace=True),
nn.Conv2d(256, 256, kernel_size=1),
)
def camera_to_bev(self, camera_features, projection_matrix):
"""将相机特征投影到BEV空间"""
B, C, H, W = camera_features.shape
# 生成BEV网格坐标
bev_grid = self._create_bev_grid(B, self.bev_size, projection_matrix)
# 通过网格采样将相机特征映射到BEV
bev_features = torch.nn.functional.grid_sample(
camera_features, bev_grid, align_corners=True
)
return bev_features
def _create_bev_grid(self, batch_size, bev_size, proj_matrix):
"""生成BEV空间的采样网格"""
H, W = bev_size
# 创建BEV平面坐标
x = torch.linspace(-50, 50, W, device=proj_matrix.device)
y = torch.linspace(-50, 50, H, device=proj_matrix.device)
grid_y, grid_x = torch.meshgrid(y, x, indexing='ij')
# 利用投影矩阵转换到图像坐标
bev_points = torch.stack([grid_x, grid_y, torch.zeros_like(grid_x)], dim=-1)
bev_points = bev_points.reshape(1, -1, 3).expand(batch_size, -1, -1)
# 投影到相机平面
img_points = torch.bmm(bev_points, proj_matrix[:, :3, :3].transpose(1, 2))
img_points += proj_matrix[:, :3, 3].unsqueeze(1)
img_points = img_points[:, :, :2] / (img_points[:, :, 2:3] + 1e-6)
# 归一化到[-1, 1]
img_points[..., 0] = img_points[..., 0] / W * 2 - 1
img_points[..., 1] = img_points[..., 1] / H * 2 - 1
return img_points.reshape(batch_size, H, W, 2)
def forward(self, camera_input, lidar_bev, projection_matrix):
# 分别提取特征
cam_feat = self.camera_encoder(camera_input)
cam_bev = self.camera_to_bev(cam_feat, projection_matrix)
lidar_feat = self.lidar_encoder(lidar_bev)
# 特征拼接与融合
fused = torch.cat([cam_bev, lidar_feat], dim=1)
output = self.fusion_conv(fused)
return output
3. 目标检测与跟踪
目标检测用于识别道路上的车辆、行人、骑行者等障碍物。3D目标检测是自动驾驶的核心任务之一。
3.1 基于图像的检测：YOLO系列
YOLO（You Only Look Once）是单阶段目标检测的经典算法。以YOLOv8为例，其核心思想是将检测任务转化为回归问题：
import torch
import torch.nn as nn
class YOLODetectionHead(nn.Module):
"""简化的YOLO检测头"""
def __init__(self, in_channels=256, num_classes=3, num_anchors=3):
super().__init__()
self.num_classes = num_classes
self.num_anchors = num_anchors
# 检测头：预测边界框 + 类别 + 置信度
self.detect_conv = nn.Sequential(
nn.Conv2d(in_channels, in_channels, 3, padding=1),
nn.BatchNorm2d(in_channels),
nn.SiLU(),
nn.Conv2d(in_channels, num_anchors * (5 + num_classes), 1),
)
# 分类分支
self.cls_conv = nn.Sequential(
nn.Conv2d(in_channels, in_channels, 3, padding=1),
nn.BatchNorm2d(in_channels),
nn.SiLU(),
nn.Conv2d(in_channels, num_classes, 1),
nn.Sigmoid(),
)
def forward(self, feature_map):
B, C, H, W = feature_map.shape
# 检测输出: [B, anchors*(5+num_classes), H, W]
detection = self.detect_conv(feature_map)
detection = detection.reshape(B, self.num_anchors, 5 + self.num_classes, H, W)
detection = detection.permute(0, 1, 3, 4, 2)
# 解码边界框
pred_xy = torch.sigmoid(detection[..., :2]) # 中心偏移
pred_wh = detection[..., 2:4] # 宽高
pred_conf = torch.sigmoid(detection[..., 4:5]) # 置信度
pred_cls = torch.sigmoid(detection[..., 5:]) # 类别概率
return torch.cat([pred_xy, pred_wh, pred_conf, pred_cls], dim=-1)
def non_max_suppression(self, predictions, conf_threshold=0.5, iou_threshold=0.45):
"""非极大值抑制后处理"""
results = []
for pred in predictions:
# 过滤低置信度
mask = pred[..., 4] > conf_threshold
pred = pred[mask]
if len(pred) == 0:
results.append(None)
continue
# 按置信度排序
sorted_indices = torch.argsort(pred[..., 4], descending=True)
pred = pred[sorted_indices]
keep = []
while len(pred) > 0:
keep.append(pred[0])
if len(pred) == 1:
break
ious = self._compute_iou(pred[0:1, :4], pred[1:, :4])
remaining = ious[0] < iou_threshold
pred = pred[1:][remaining]
results.append(torch.stack(keep) if keep else None)
return results
def _compute_iou(self, box1, box2):
"""计算IoU"""
x1 = torch.max(box1[:, 0], box2[:, 0])
y1 = torch.max(box1[:, 1], box2[:, 1])
x2 = torch.min(box1[:, 2], box2[:, 2])
y2 = torch.min(box1[:, 3], box2[:, 3])
intersection = torch.clamp(x2 - x1, min=0) * torch.clamp(y2 - y1, min=0)
area1 = (box1[:, 2] - box1[:, 0]) * (box1[:, 3] - box1[:, 1])
area2 = (box2[:, 2] - box2[:, 0]) * (box2[:, 3] - box2[:, 1])
union = area1 + area2 - intersection
return intersection / (union + 1e-6)
3.2 基于点云的检测：PointPillar
PointPillar将3D点云转化为伪图像，然后使用2D卷积网络进行检测，兼顾了精度和速度：
import torch
import torch.nn as nn
class PillarFeatureNet(nn.Module):
"""PointPillar的柱体特征提取网络"""
def __init__(self, in_channels=4, out_channels=64):
super().__init__()
self.conv = nn.Conv1d(in_channels, out_channels, 1, bias=False)
self.bn = nn.BatchNorm1d(out_channels)
def forward(self, pillars, indices, num_points):
"""
pillars: 柱体内的点云数据 [N_pillars, max_points, 4]
indices: 柱体在BEV网格中的索引 [N_pillars, 2]
num_points: 每个柱体内的实际点数 [N_pillars]
"""
# 为每个点添加柱体中心偏移
centers = pillars[:, :, :3].mean(dim=1, keepdim=True) # [N, 1, 3]
offsets = pillars[:, :, :3] - centers # 点相对于柱体中心的偏移
features = torch.cat([pillars[:, :, :3], offsets], dim=-1) # [N, P, 6]
# 注意：简化示例仅使用前4个通道
features = features[:, :, :4]
features = features.permute(0, 2, 1) # [N, C, P]
features = self.conv(features)
features = self.bn(features)
# 使用max pooling聚合柱体内特征
mask = torch.arange(features.shape[2], device=features.device).unsqueeze(0)
mask = mask < num_points.unsqueeze(1)
features = features * mask.unsqueeze(1).float()
features = torch.max(features, dim=2)[0] # [N, C]
return features, indices
class PointPillarBackbone(nn.Module):
"""PointPillar的主干网络"""
def __init__(self, in_channels=64):
super().__init__()
# 简化的2D卷积主干
self.block1 = nn.Sequential(
nn.Conv2d(in_channels, 64, 3, stride=2, padding=1),
nn.BatchNorm2d(64),
nn.ReLU(inplace=True),
nn.Conv2d(64, 64, 3, padding=1),
nn.BatchNorm2d(64),
nn.ReLU(inplace=True),
)
self.block2 = nn.Sequential(
nn.Conv2d(64, 128, 3, stride=2, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(inplace=True),
nn.Conv2d(128, 128, 3, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(inplace=True),
)
self.block3 = nn.Sequential(
nn.Conv2d(128, 256, 3, stride=2, padding=1),
nn.BatchNorm2d(256),
nn.ReLU(inplace=True),
nn.Conv2d(256, 256, 3, padding=1),
nn.BatchNorm2d(256),
nn.ReLU(inplace=True),
)
# 多尺度特征融合
self.upsample1 = nn.ConvTranspose2d(128, 128, 2, stride=2)
self.upsample2 = nn.ConvTranspose2d(256, 256, 4, stride=4)
def forward(self, bev_features):
x1 = self.block1(bev_features)
x2 = self.block2(x1)
x3 = self.block3(x2)
# 特征金字塔融合
x2_up = self.upsample1(x2)
x3_up = self.upsample2(x3)
fused = torch.cat([x1, x2_up, x3_up], dim=1)
return fused
3.3 目标跟踪
多目标跟踪（MOT）使用跟踪-检测范式，常用DeepSORT算法：
from collections import defaultdict
import numpy as np
from scipy.optimize import linear_sum_assignment
class Track:
"""单个跟踪目标的状态"""
_next_id = 0
def __init__(self, bbox, feature):
self.track_id = Track._next_id
Track._next_id += 1
self.bbox = bbox # [x1, y1, x2, y2]
self.feature = feature # 外观特征向量
self.hits = 1 # 连续匹配次数
self.miss = 0 # 连续丢失次数
self.state = 'tentative' # tentative / confirmed / deleted
self.history = [bbox]
def predict(self):
"""卡尔曼滤波预测下一帧位置（简化版：匀速模型）"""
if len(self.history) >= 2:
velocity = np.array(self.history[-1]) - np.array(self.history[-2])
self.bbox = (np.array(self.bbox) + velocity).tolist()
def update(self, bbox, feature):
self.bbox = bbox
self.feature = feature
self.hits += 1
self.miss = 0
self.history.append(bbox)
if self.hits >= 3:
self.state = 'confirmed'
class SimpleTracker:
"""简化的多目标跟踪器"""
def __init__(self, max_miss=5, iou_threshold=0.3, appearance_weight=0.5):
self.tracks = []
self.max_miss = max_miss
self.iou_threshold = iou_threshold
self.appearance_weight = appearance_weight
def update(self, detections, features):
"""
detections: [[x1,y1,x2,y2], ...]
features: 外观特征 [[feat_vec], ...]
"""
# 预测现有轨迹
for track in self.tracks:
track.predict()
if not self.tracks or not detections:
# 无匹配：为每个检测创建新轨迹
for det, feat in zip(detections, features):
self.tracks.append(Track(det, feat))
self._cleanup()
return self._get_results()
# 计算代价矩阵（IoU距离 + 外观距离）
cost_matrix = self._compute_cost(detections, features)
# 匈牙利算法匹配
row_indices, col_indices = linear_sum_assignment(cost_matrix)
matched_tracks = set()
matched_dets = set()
for r, c in zip(row_indices, col_indices):
if cost_matrix[r, c] < 1.0 - self.iou_threshold:
self.tracks[r].update(detections[c], features[c])
matched_tracks.add(r)
matched_dets.add(c)
# 未匹配的轨迹增加丢失计数
for i, track in enumerate(self.tracks):
if i not in matched_tracks:
track.miss += 1
# 未匹配的检测创建新轨迹
for j, (det, feat) in enumerate(zip(detections, features)):
if j not in matched_dets:
self.tracks.append(Track(det, feat))
self._cleanup()
return self._get_results()
def _compute_cost(self, detections, features):
"""计算跟踪-检测代价矩阵"""
n_tracks = len(self.tracks)
n_dets = len(detections)
cost = np.zeros((n_tracks, n_dets))
for i, track in enumerate(self.tracks):
for j, det in enumerate(detections):
iou = self._iou(track.bbox, det)
# 外观特征余弦距离
feat_dist = 1 - np.dot(track.feature, features[j]) / (
np.linalg.norm(track.feature) * np.linalg.norm(features[j]) + 1e-6
)
cost[i, j] = (1 - self.appearance_weight) * (1 - iou) + \
self.appearance_weight * feat_dist
return cost
def _iou(self, box1, box2):
x1 = max(box1[0], box2[0])
y1 = max(box1[1], box2[1])
x2 = min(box1[2], box2[2])
y2 = min(box1[3], box2[3])
inter = max(0, x2-x1) * max(0, y2-y1)
area1 = (box1[2]-box1[0]) * (box1[3]-box1[1])
area2 = (box2[2]-box2[0]) * (box2[3]-box2[1])
return inter / (area1 + area2 - inter + 1e-6)
def _cleanup(self):
self.tracks = [t for t in self.tracks if t.miss <= self.max_miss]
def _get_results(self):
return [{
'track_id': t.track_id,
'bbox': t.bbox,
'state': t.state
} for t in self.tracks if t.state == 'confirmed']
4. 语义分割与车道线检测
语义分割将图像中的每个像素分配到对应的类别（道路、车辆、行人、建筑物等）。在自动驾驶中，车道线检测尤为重要。
4.1 语义分割网络
import torch
import torch.nn as nn
import torch.nn.functional as F
class SegmentationDecoder(nn.Module):
"""语义分割解码器（简化版DeepLabV3+）"""
def __init__(self, in_channels=256, num_classes=19):
super().__init__()
# ASPP模块（空洞空间金字塔池化）
self.aspp = nn.ModuleList([
nn.Conv2d(in_channels, 256, 1),
nn.Conv2d(in_channels, 256, 3, padding=6, dilation=6),
nn.Conv2d(in_channels, 256, 3, padding=12, dilation=12),
nn.Conv2d(in_channels, 256, 3, padding=18, dilation=18),
])
self.aspp_bn = nn.ModuleList([nn.BatchNorm2d(256) for _ in range(4)])
self.global_pool = nn.AdaptiveAvgPool2d(1)
self.global_conv = nn.Sequential(
nn.Conv2d(in_channels, 256, 1),
nn.BatchNorm2d(256),
nn.ReLU(inplace=True),
)
# 输出层
self.classifier = nn.Sequential(
nn.Conv2d(256 * 5, 256, 3, padding=1),
nn.BatchNorm2d(256),
nn.ReLU(inplace=True),
nn.Conv2d(256, num_classes, 1),
)
def forward(self, features):
aspp_outs = []
for conv, bn in zip(self.aspp, self.aspp_bn):
out = bn(F.relu(conv(features)))
aspp_outs.append(out)
# 全局特征
global_feat = self.global_pool(features)
global_feat = self.global_conv(global_feat)
global_feat = F.interpolate(global_feat, size=features.shape[2:], mode='bilinear', align_corners=True)
aspp_outs.append(global_feat)
# 拼接并分类
x = torch.cat(aspp_outs, dim=1)
x = self.classifier(x)
return x
class LaneDetector(nn.Module):
"""车道线检测网络"""
def __init__(self, in_channels=256, num_lanes=4):
super().__init__()
self.num_lanes = num_lanes
# 车道线存在性预测
self.exist_head = nn.Sequential(
nn.AdaptiveAvgPool2d(1),
nn.Flatten(),
nn.Linear(in_channels, 128),
nn.ReLU(inplace=True),
nn.Linear(128, num_lanes),
nn.Sigmoid(),
)
# 车道线位置回归（逐行分类）
self.loc_head = nn.Sequential(
nn.Conv2d(in_channels, 128, 3, padding=1),
nn.BatchNorm2d(128),
nn.ReLU(inplace=True),
nn.Conv2d(128, num_lanes, 1),
)
def forward(self, features):
# 预测每条车道线是否存在
exist_pred = self.exist_head(features)
# 预测每个像素行上车道线的水平位置
loc_pred = self.loc_head(features)
loc_pred = torch.softmax(loc_pred, dim=3) # 沿宽度方向softmax
# 计算每条车道线在每行的期望位置
W = loc_pred.shape[3]
positions = torch.arange(W, device=loc_pred.device, dtype=torch.float32)
lane_positions = torch.sum(loc_pred * positions, dim=3) # [B, num_lanes, H]
return exist_pred, lane_positions
5. 定位与地图（高精地图/SLAM）
精确定位是自动驾驶的基础。高精地图（HD Map）提供厘米级精度的道路信息，SLAM（同步定位与建图）则用于在未知环境中实时定位。
5.1 高精地图数据结构
from dataclasses import dataclass, field
from typing import List, Tuple
import numpy as np
@dataclass
class LaneCenterLine:
"""车道中心线"""
id: str
points: np.ndarray # 形状 [N, 3]，三维坐标序列
predecessors: List[str] = field(default_factory=list)
successors: List[str] = field(default_factory=list)
lane_type: str = "driving" # driving, shoulder, bus
speed_limit: float = 60.0 # km/h
@dataclass
class LaneBoundary:
"""车道边界"""
id: str
points: np.ndarray # 形状 [N, 3]
boundary_type: str = "solid" # solid, dashed, double
@dataclass
class TrafficLight:
"""交通灯"""
id: str
position: np.ndarray # [x, y, z]
orientation: float # 朝向角
associated_lane_ids: List[str] = field(default_factory=list)
class HDMap:
"""高精地图管理器"""
def __init__(self):
self.lanes: dict[str, LaneCenterLine] = {}
self.boundaries: dict[str, LaneBoundary] = {}
self.traffic_lights: dict[str, TrafficLight] = {}
def add_lane(self, lane: LaneCenterLine):
self.lanes[lane.id] = lane
def get_nearby_lanes(self, position: np.ndarray, radius: float = 50.0) -> List[LaneCenterLine]:
"""获取指定位置附近的车道"""
nearby = []
for lane in self.lanes.values():
distances = np.linalg.norm(lane.points[:, :2] - position[:2], axis=1)
if np.min(distances) < radius:
nearby.append(lane)
return nearby
def get_route(self, start_pos: np.ndarray, end_pos: np.ndarray) -> List[str]:
"""使用Dijkstra算法规划车道级路径"""
start_lane = self._find_nearest_lane(start_pos)
end_lane = self._find_nearest_lane(end_pos)
if not start_lane or not end_lane:
return []
# BFS寻找路径
visited = set()
queue = [(start_lane.id, [start_lane.id])]
while queue:
current_id, path = queue.pop(0)
if current_id == end_lane.id:
return path
if current_id in visited:
continue
visited.add(current_id)
lane = self.lanes[current_id]
for successor_id in lane.successors:
if successor_id not in visited:
queue.append((successor_id, path + [successor_id]))
return []
def _find_nearest_lane(self, position: np.ndarray) -> LaneCenterLine:
nearest_lane = None
min_dist = float('inf')
for lane in self.lanes.values():
dists = np.linalg.norm(lane.points[:, :2] - position[:2], axis=1)
min_d = np.min(dists)
if min_d < min_dist:
min_dist = min_d
nearest_lane = lane
return nearest_lane
# 使用示例
hd_map = HDMap()
hd_map.add_lane(LaneCenterLine(
id="lane_001",
points=np.array([[0, 0, 0], [10, 0, 0], [20, 0, 0], [30, 0, 0]], dtype=np.float32),
successors=["lane_002"],
speed_limit=60.0
))
5.2 简化版ICP配准
import numpy as np
def icp(source: np.ndarray, target: np.ndarray, max_iterations=50, tolerance=1e-6):
"""
Iterative Closest Point (ICP) 算法
source: 源点云 [N, 3]
target: 目标点云 [M, 3]
返回: 旋转矩阵R和平移向量t
"""
src = source.copy()
R_total = np.eye(3)
t_total = np.zeros(3)
for i in range(max_iterations):
# 最近点匹配
distances = np.linalg.norm(
src[:, np.newaxis, :] - target[np.newaxis, :, :], axis=2
) # [N, M]
nearest_indices = np.argmin(distances, axis=1)
matched_target = target[nearest_indices]
# 计算质心
src_centroid = np.mean(src, axis=0)
tgt_centroid = np.mean(matched_target, axis=0)
# 去质心
src_centered = src - src_centroid
tgt_centered = matched_target - tgt_centroid
# SVD求解最优旋转
H = src_centered.T @ tgt_centered
U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
# 处理反射情况
if np.linalg.det(R) < 0:
Vt[-1, :] *= -1
R = Vt.T @ U.T
t = tgt_centroid - R @ src_centroid
# 更新
src = (R @ src.T).T + t
R_total = R @ R_total
t_total = R @ t_total + t
# 收敛检查
mean_error = np.mean(np.linalg.norm(src - matched_target, axis=1))
if mean_error < tolerance:
print(f"ICP收敛于第{i+1}次迭代, 误差: {mean_error:.6f}")
break
return R_total, t_total
6. 路径规划与决策（A*/RRT/强化学习）
6.1 A*算法
import heapq
import numpy as np
def astar(grid, start, goal):
"""
A*路径规划算法
grid: 二维栅格地图，0为可通行，1为障碍物
start: (row, col) 起点
goal: (row, col) 终点
"""
rows, cols = grid.shape
open_set = [(0, start)]
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
directions = [(-1,0), (1,0), (0,-1), (0,1), (-1,-1), (-1,1), (1,-1), (1,1)]
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
return reconstruct_path(came_from, current)
for dr, dc in directions:
neighbor = (current[0] + dr, current[1] + dc)
if not (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols):
continue
if grid[neighbor[0], neighbor[1]] == 1:
continue
move_cost = 1.414 if dr != 0 and dc != 0 else 1.0
tentative_g = g_score[current] + move_cost
if tentative_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None # 无路径
def heuristic(a, b):
"""欧几里得距离启发式"""
return np.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path[::-1]
6.2 RRT*算法
import numpy as np
class RRTStar:
"""RRT*快速扩展随机树"""
def __init__(self, start, goal, obstacles,
x_range=(0, 100), y_range=(0, 100),
step_size=2.0, goal_threshold=2.0,
max_iter=5000, search_radius=5.0):
self.start = np.array(start)
self.goal = np.array(goal)
self.obstacles = obstacles # [(cx, cy, r), ...]
self.x_range = x_range
self.y_range = y_range
self.step_size = step_size
self.goal_threshold = goal_threshold
self.max_iter = max_iter
self.search_radius = search_radius
self.nodes = [self.start]
self.parents = {0: -1}
self.costs = {0: 0.0}
def plan(self):
for i in range(self.max_iter):
# 随机采样（10%概率采样目标点）
if np.random.random() < 0.1:
q_rand = self.goal
else:
q_rand = np.array([
np.random.uniform(*self.x_range),
np.random.uniform(*self.y_range)
])
# 找最近节点
nearest_idx = self._find_nearest(q_rand)
q_near = self.nodes[nearest_idx]
# 扩展新节点
direction = q_rand - q_near
dist = np.linalg.norm(direction)
if dist < 1e-6:
continue
q_new = q_near + (direction / dist) * min(self.step_size, dist)
# 碰撞检测
if self._collision(q_near, q_new):
continue
# 选择最优父节点
new_idx = len(self.nodes)
best_parent = nearest_idx
best_cost = self.costs[nearest_idx] + np.linalg.norm(q_new - q_near)
# 搜索邻近节点
near_indices = self._find_near(q_new)
for idx in near_indices:
node = self.nodes[idx]
cost = self.costs[idx] + np.linalg.norm(q_new - node)
if cost < best_cost and not self._collision(node, q_new):
best_parent = idx
best_cost = cost
# 添加节点
self.nodes.append(q_new)
self.parents[new_idx] = best_parent
self.costs[new_idx] = best_cost
# 重布线
for idx in near_indices:
node = self.nodes[idx]
new_cost = best_cost + np.linalg.norm(node - q_new)
if new_cost < self.costs[idx] and not self._collision(q_new, node):
self.parents[idx] = new_idx
self.costs[idx] = new_cost
# 检查是否到达目标
if np.linalg.norm(q_new - self.goal) < self.goal_threshold:
return self._extract_path(new_idx)
return None
def _find_nearest(self, point):
distances = [np.linalg.norm(np.array(n) - point) for n in self.nodes]
return np.argmin(distances)
def _find_near(self, point):
indices = []
for i, node in enumerate(self.nodes):
if np.linalg.norm(np.array(node) - point) < self.search_radius:
indices.append(i)
return indices
def _collision(self, p1, p2):
for (cx, cy, r) in self.obstacles:
# 线段到圆心的最小距离
d = p2 - p1
f = p1 - np.array([cx, cy])
a = np.dot(d, d)
b = 2 * np.dot(f, d)
c = np.dot(f, f) - r**2
discriminant = b**2 - 4*a*c
if discriminant >= 0:
discriminant = np.sqrt(discriminant)
t1 = (-b - discriminant) / (2*a)
t2 = (-b + discriminant) / (2*a)
if (0 <= t1 <= 1) or (0 <= t2 <= 1):
return True
return False
def _extract_path(self, idx):
path = []
while idx != -1:
path.append(self.nodes[idx])
idx = self.parents[idx]
return path[::-1]
# 使用示例
rrt = RRTStar(
start=(5, 5), goal=(95, 95),
obstacles=[(30, 30, 8), (50, 50, 10), (70, 30, 6), (40, 70, 8)]
)
path = rrt.plan()
if path:
print(f"找到路径，共{len(path)}个路径点")
7. 控制系统（PID/MPC）
7.1 PID控制器
import time
class PIDController:
"""PID横向/纵向控制器"""
def __init__(self, kp, ki, kd, output_limits=(-1.0, 1.0)):
self.kp = kp
self.ki = ki
self.kd = kd
self.output_limits = output_limits
self.prev_error = 0.0
self.integral = 0.0
self.last_time = None
def compute(self, error, current_time=None):
if current_time is None:
current_time = time.time()
if self.last_time is None:
dt = 0.01
else:
dt = current_time - self.last_time
dt = max(dt, 1e-6)
# 积分项（带抗饱和）
self.integral += error * dt
self.integral = max(min(self.integral,
self.output_limits[1] / max(self.ki, 1e-6)),
self.output_limits[0] / max(self.ki, 1e-6))
# 微分项
derivative = (error - self.prev_error) / dt
# PID输出
output = self.kp * error + self.ki * self.integral + self.kd * derivative
# 输出限幅
output = max(min(output, self.output_limits[1]), self.output_limits[0])
self.prev_error = error
self.last_time = current_time
return output
def reset(self):
self.prev_error = 0.0
self.integral = 0.0
self.last_time = None
class LateralController:
"""横向控制（转向角）"""
def __init__(self, wheelbase=2.7):
self.wheelbase = wheelbase
self.pid = PIDController(kp=0.5, ki=0.01, kd=0.1, output_limits=(-0.6, 0.6))
def compute_steering(self, current_pose, reference_path, speed):
"""
current_pose: (x, y, theta) 当前位姿
reference_path: [(x, y), ...] 参考路径
speed: 当前车速 m/s
"""
# 找到最近路径点
x, y, theta = current_pose
min_dist = float('inf')
nearest_idx = 0
for i, (rx, ry) in enumerate(reference_path):
dist = (x - rx)**2 + (y - ry)**2
if dist < min_dist:
min_dist = dist
nearest_idx = i
# 计算横向误差
rx, ry = reference_path[nearest_idx]
dx = rx - x
dy = ry - y
# 横向误差（车辆坐标系下）
lateral_error = -dx * np.sin(theta) + dy * np.cos(theta)
# PID计算转向角
steering = self.pid.compute(lateral_error)
# 低速时增大增益
if speed < 2.0:
steering *= 1.5
return steering
7.2 MPC控制器
import numpy as np
from scipy.optimize import minimize
class MPCController:
"""模型预测控制器"""
def __init__(self, horizon=20, dt=0.1, wheelbase=2.7):
self.horizon = horizon
self.dt = dt
self.wheelbase = wheelbase
def bicycle_model(self, state, control):
"""自行车运动模型"""
x, y, theta, v = state
accel, delta = control
# 限幅
delta = np.clip(delta, -0.6, 0.6)
accel = np.clip(accel, -3.0, 2.0)
# 运动学更新
x_new = x + v * np.cos(theta) * self.dt
y_new = y + v * np.sin(theta) * self.dt
theta_new = theta + (v / self.wheelbase) * np.tan(delta) * self.dt
v_new = v + accel * self.dt
v_new = max(0, v_new) # 不允许倒车
return np.array([x_new, y_new, theta_new, v_new])
def compute(self, current_state, reference_trajectory, target_speed=15.0):
"""
current_state: [x, y, theta, v]
reference_trajectory: [(x, y), ...] 参考轨迹
target_speed: 目标速度 m/s
"""
n_controls = 2
# 初始控制序列
u0 = np.zeros(self.horizon * n_controls)
# 代价函数
def cost_function(u):
state = current_state.copy()
total_cost = 0.0
for k in range(self.horizon):
accel = u[k * n_controls]
delta = u[k * n_controls + 1]
control = np.array([accel, delta])
state = self.bicycle_model(state, control)
# 跟踪误差
ref_idx = min(k, len(reference_trajectory) - 1)
ref_x, ref_y = reference_trajectory[ref_idx]
track_cost = (state[0] - ref_x)**2 + (state[1] - ref_y)**2
# 速度误差
speed_cost = (state[3] - target_speed)**2
# 控制量惩罚
control_cost = 0.1 * accel**2 + 1.0 * delta**2
# 控制变化率惩罚
if k > 0:
prev_delta = u[(k-1) * n_controls + 1]
smooth_cost = 5.0 * (delta - prev_delta)**2
else:
smooth_cost = 0
total_cost += track_cost + 0.5 * speed_cost + control_cost + smooth_cost
return total_cost
# 约束
bounds = []
for _ in range(self.horizon):
bounds.extend([(-3.0, 2.0), (-0.6, 0.6)]) # 加速度和转向角范围
# 优化求解
result = minimize(cost_function, u0, method='SLSQP', bounds=bounds)
# 返回第一个控制量
optimal_u = result.x
return optimal_u[0], optimal_u[1] # (acceleration, steering)
8. V2X车路协同技术
V2X（Vehicle-to-Everything）是指车辆与周围一切事物的通信技术：
- V2V（车对车）：车辆间直接通信，共享位置、速度、方向信息
- V2I（车对基础设施）：与红绿灯、路侧单元通信
- V2P（车对行人）：检测行人位置，发出碰撞预警
- V2N（车对网络）：通过蜂窝网络获取云端信息
import json
import time
from dataclasses import dataclass, asdict
from typing import List
@dataclass
class V2XMessage:
"""V2X消息格式"""
msg_type: str # BSM(基本安全消息), SPAT(信号灯), MAP(地图)
timestamp: float
sender_id: str
position: List[float] # [lat, lon, alt]
speed: float # m/s
heading: float # 度
acceleration: float
payload: dict # 类型特定数据
class V2XCommunicationStack:
"""V2X通信协议栈"""
def __init__(self, vehicle_id):
self.vehicle_id = vehicle_id
self.message_buffer = []
self.received_messages = []
def create_bsm(self, position, speed, heading, acceleration):
"""创建基本安全消息 (BSM)"""
return V2XMessage(
msg_type="BSM",
timestamp=time.time(),
sender_id=self.vehicle_id,
position=position,
speed=speed,
heading=heading,
acceleration=acceleration,
payload={
"brake_status": "off",
"vehicle_length": 4.5,
"vehicle_width": 1.8,
}
)
def broadcast(self, message: V2XMessage):
"""广播消息（模拟）"""
self.message_buffer.append(asdict(message))
return message
def receive(self, raw_message: dict):
"""接收并解析消息"""
message = V2XMessage(**raw_message)
self.received_messages.append(message)
return message
def compute_ttc(self, ego_state, other_state):
"""
计算碰撞时间 TTC (Time To Collision)
ego_state: {'position': [x,y], 'speed': float, 'heading': float}
other_state: 同上
"""
# 相对位置和速度
dx = other_state['position'][0] - ego_state['position'][0]
dy = other_state['position'][1] - ego_state['position'][1]
ego_vx = ego_state['speed'] * np.cos(np.radians(ego_state['heading']))
ego_vy = ego_state['speed'] * np.sin(np.radians(ego_state['heading']))
other_vx = other_state['speed'] * np.cos(np.radians(other_state['heading']))
other_vy = other_state['speed'] * np.sin(np.radians(other_state['heading']))
dvx = other_vx - ego_vx
dvy = other_vy - ego_vy
# TTC计算
dv_dot_dr = dvx * dx + dvy * dy
dv_dv = dvx**2 + dvy**2
if dv_dv < 1e-6:
return float('inf')
ttc = -dv_dot_dr / dv_dv
return max(0, ttc)
def forward_collision_warning(self, ttc, threshold=3.0):
"""前碰撞预警"""
if ttc < threshold:
return {
"warning_level": "CRITICAL" if ttc < 1.5 else "WARNING",
"ttc": ttc,
"action": "BRAKE" if ttc < 1.5 else "ALERT"
}
return None
9. 仿真测试（CARLA/SUMO）
自动驾驶系统在上路前必须经过大量仿真测试。两个主流仿真平台：
CARLA：基于Unreal Engine的高保真仿真器，支持传感器模拟、天气变化、交通流生成，适合感知和控制算法验证。
SUMO：微观交通流仿真器，适合大规模交通场景和V2X通信测试。
CARLA基础API使用
import carla
import numpy as np
class CARLASimulation:
"""CARLA仿真环境封装"""
def __init__(self, host='localhost', port=2000):
self.client = carla.Client(host, port)
self.client.set_timeout(10.0)
self.world = self.client.get_world()
self.blueprint_library = self.world.get_blueprint_library()
def spawn_ego_vehicle(self, spawn_point=None):
"""生成主车"""
vehicle_bp = self.blueprint_library.find('vehicle.tesla.model3')
if spawn_point is None:
spawn_points = self.world.get_map().get_spawn_points()
spawn_point = spawn_points[0]
ego_vehicle = self.world.spawn_actor(vehicle_bp, spawn_point)
return ego_vehicle
def attach_sensors(self, vehicle):
"""挂载传感器"""
sensors = {}
# 相机
camera_bp = self.blueprint_library.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '1920')
camera_bp.set_attribute('image_size_y', '1080')
camera_bp.set_attribute('fov', '90')
camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4))
camera = self.world.spawn_actor(
camera_bp, camera_transform, attach_to=vehicle
)
sensors['camera'] = camera
# LiDAR
lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')
lidar_bp.set_attribute('channels', '32')
lidar_bp.set_attribute('points_per_second', '56000')
lidar_bp.set_attribute('range', '50')
lidar_transform = carla.Transform(carla.Location(x=0, z=2.4))
lidar = self.world.spawn_actor(
lidar_bp, lidar_transform, attach_to=vehicle
)
sensors['lidar'] = lidar
# 毫米波雷达
radar_bp = self.blueprint_library.find('sensor.other.radar')
radar_bp.set_attribute('horizontal_fov', '30')
radar_bp.set_attribute('vertical_fov', '10')
radar_bp.set_attribute('range', '100')
radar_transform = carla.Transform(carla.Location(x=2.0, z=1.0))
radar = self.world.spawn_actor(
radar_bp, radar_transform, attach_to=vehicle
)
sensors['radar'] = radar
return sensors
def set_weather(self, weather_preset='clear'):
"""设置天气"""
weather_presets = {
'clear': carla.WeatherParameters.ClearNoon,
'rain': carla.WeatherParameters.HardRainNoon,
'fog': carla.WeatherParameters.SoftRainSunset,
}
self.world.set_weather(weather_presets.get(
weather_preset, carla.WeatherParameters.ClearNoon
))
def spawn_traffic(self, num_vehicles=20, num_pedestrians=10):
"""生成交通流"""
spawn_points = self.world.get_map().get_spawn_points()
# 生成车辆
vehicle_bp = self.blueprint_library.filter('vehicle.*')
vehicles = []
for i in range(min(num_vehicles, len(spawn_points))):
bp = np.random.choice(vehicle_bp)
try:
vehicle = self.world.try_spawn_actor(bp, spawn_points[i])
if vehicle:
vehicle.set_autopilot(True)
vehicles.append(vehicle)
except:
continue
return vehicles
10. 实战案例：基于CARLA的自动驾驶仿真
综合前面所有模块，构建一个完整的自动驾驶仿真流程：
import numpy as np
class AutonomousDrivingPipeline:
"""自动驾驶完整Pipeline"""
def __init__(self):
self.perception = PerceptionModule()
self.planner = PathPlanner()
self.controller = MPCController(horizon=20, dt=0.1)
self.state = {
'x': 0, 'y': 0, 'theta': 0, 'v': 0
}
def run_step(self, sensor_data):
"""单步仿真"""
# 1. 感知
detections = self.perception.detect_objects(sensor_data['camera'])
lanes = self.perception.detect_lanes(sensor_data['camera'])
# 2. 定位
pose = self.localize(sensor_data)
self.state.update(pose)
# 3. 规划
reference_path = self.planner.plan(
start=(self.state['x'], self.state['y']),
goal=self.target,
obstacles=detections
)
# 4. 控制
accel, steering = self.controller.compute(
current_state=[self.state['x'], self.state['y'],
self.state['theta'], self.state['v']],
reference_trajectory=reference_path
)
return {
'throttle': max(0, accel),
'brake': max(0, -accel),
'steering': steering,
'detections': detections,
'lanes': lanes,
'path': reference_path
}
class PerceptionModule:
"""感知模块集成"""
def __init__(self):
self.detector = None # YOLO或其他检测器
self.segmentor = None # 语义分割网络
self.tracker = SimpleTracker()
def detect_objects(self, camera_image):
"""目标检测"""
# 模拟检测结果
detections = [
{'class': 'vehicle', 'bbox': [100, 200, 300, 400], 'confidence': 0.92},
{'class': 'pedestrian', 'bbox': [500, 250, 550, 400], 'confidence': 0.87},
]
return detections
def detect_lanes(self, camera_image):
"""车道线检测"""
lanes = {
'left_lane': [(0, 400), (100, 380), (200, 360)],
'right_lane': [(0, 600), (100, 620), (200, 640)],
'ego_lane_center': [(0, 500), (100, 500), (200, 500)],
}
return lanes
class PathPlanner:
"""路径规划器"""
def __init__(self):
self.rrt = None
def plan(self, start, goal, obstacles):
"""生成局部路径"""
# 简化的路径生成：直线+贝塞尔曲线
path = []
num_points = 50
for i in range(num_points):
t = i / (num_points - 1)
x = start[0] + t * (goal[0] - start[0])
y = start[1] + t * (goal[1] - start[1])
path.append((x, y))
return path
# 完整仿真主循环
def main():
sim = CARLASimulation()
pipeline = AutonomousDrivingPipeline()
try:
# 初始化
ego = sim.spawn_ego_vehicle()
sensors = sim.attach_sensors(ego)
sim.set_weather('clear')
sim.spawn_traffic(num_vehicles=15)
# 仿真循环
target = (100, 200)
pipeline.target = target
for step in range(1000):
# 获取传感器数据（简化）
sensor_data = {
'camera': np.zeros((1080, 1920, 3), dtype=np.uint8),
'lidar': np.random.randn(1000, 4),
'radar': [],
}
# 执行一步
control = pipeline.run_step(sensor_data)
# 应用控制指令到CARLA
carla_control = carla.VehicleControl(
throttle=control['throttle'],
brake=control['brake'],
steer=control['steering']
)
ego.apply_control(carla_control)
# 推进仿真
sim.world.tick()
if step % 100 == 0:
print(f"Step {step}: throttle={control['throttle']:.2f}, "
f"steering={control['steering']:.2f}")
finally:
# 清理
ego.destroy()
for sensor in sensors.values():
sensor.destroy()
if __name__ == '__main__':
main()
11. 安全挑战与法规
自动驾驶面临的安全挑战包括：
技术挑战：
- 长尾问题：罕见场景（如道路施工、异常天气）的处理能力不足
- 传感器退化：恶劣天气下感知精度下降
- 对抗攻击：对传感器的欺骗攻击（如对抗样本干扰摄像头、GPS欺骗）
- 系统冗余：关键部件的故障容错设计
伦理与法规：
- 责任归属：事故责任由制造商还是车主承担
- 数据隐私：大量传感器数据的采集和存储
- 道德决策：不可避免事故时的决策逻辑（电车难题）
各国法规进展：
- 中国：《智能网联汽车准入和上路通行试点》
- 美国：各州法规不一，联邦层面正在推进
- 欧盟：UN R157法规允许L3级自动驾驶
安全测试框架示例：
class SafetyValidator:
"""自动驾驶安全验证框架"""
def __init__(self):
self.test_scenarios = []
self.results = []
def add_scenario(self, name, scenario_type, params):
self.test_scenarios.append({
'name': name,
'type': scenario_type,
'params': params
})
def run_ncap_tests(self):
"""执行NCAP标准测试"""
# AEB（自动紧急制动）测试
self.add_scenario("AEB静止行人", "aeb", {
'target_speed': 40, # km/h
'pedestrian_crossing': True,
'expected_stop': True
})
# LKA（车道保持）测试
self.add_scenario("LKA车道偏离", "lka", {
'drift_angle': 3.0, # 度
'expected_correction': True
})
# ACC（自适应巡航）测试
self.add_scenario("ACC跟车", "acc", {
'lead_vehicle_speed': 60,
'gap_distance': 20,
'expected_gap_maintained': True
})
def evaluate(self, system_response, scenario):
"""评估系统响应"""
metrics = {
'collision_occurred': False,
'lane_departure': False,
'comfort_score': 0.0, # 0-1
'response_time': 0.0, # 秒
'success': False
}
if scenario['type'] == 'aeb':
metrics['success'] = not metrics['collision_occurred'] and \
metrics['response_time'] < 1.5
elif scenario['type'] == 'lka':
metrics['success'] = not metrics['lane_departure']
return metrics
自动驾驶是一项跨学科的复杂系统工程，涉及计算机视觉、机器人学、控制理论、通信技术等多个领域。随着传感器成本下降、算力提升和法规完善，L4级自动驾驶正在从测试走向商业化运营。掌握上述技术栈，是进入这一领域的坚实基础。
