03.Localization&Prediction模块详解
Apollo 10.0自动驾驶平台的预测模块采用四层架构设计，包含容器层、场景层、评估层和预测器层。容器层负责存储和管理障碍物、自车轨迹及定位数据，其中ObstaclesContainer通过卡尔曼滤波器（6维状态向量）实现障碍物状态追踪。场景层基于距离阈值（默认20米）区分巡航和路口场景。评估层包含9种评估器，包括MLP、LSTM和VectorNet等深度学习模型。预测器层提供9种预测算法，覆盖
·
第三部分：预测与定位算法详解
免责声明
本文档仅供学习和技术研究使用,内容基于 Apollo 开源项目的公开资料和代码分析整理而成。文档中的技术实现细节、架构设计等内容可能随 Apollo 项目更新而变化。使用本文档所述技术方案时,请以 Apollo 官方最新文档为准。声明事项:
- 本文档不构成任何商业使用建议
- 涉及自动驾驶技术的应用需遵守当地法律法规
- 作者不对因使用本文档内容产生的任何后果承担责任
- Apollo 为百度公司注册商标,本文档为非官方技术解析
1. 预测模块架构
1.1 系统概述
Prediction模块负责预测其他道路参与者（车辆、行人、自行车等）的未来轨迹，为规划模块提供决策依据。
核心组件:
- 位置:
modules/prediction/prediction_component.h - 主类:
PredictionComponent - 基类:
cyber::Component<>
预测时长: 8秒（默认）
预测频率: 10Hz
时间分辨率: 0.1秒（80个轨迹点）
1.2 四层架构
┌─────────────────────────────────────────────────┐
│ Layer 1: Container（容器层） │
│ ├─ ObstaclesContainer（障碍物容器） │
│ ├─ ADCTrajectoryContainer（自车轨迹容器） │
│ └─ PoseContainer（位姿容器） │
└─────────────────┬───────────────────────────────┘
│
┌─────────────────▼───────────────────────────────┐
│ Layer 2: Scenario（场景层） │
│ ├─ ScenarioManager（场景管理器） │
│ ├─ Cruise（巡航场景） │
│ ├─ Junction（路口场景） │
│ └─ Prioritization（优先级分类） │
│ ├─ IGNORE（忽略） │
│ ├─ CAUTION（警告） │
│ └─ NORMAL（普通） │
└─────────────────┬───────────────────────────────┘
│
┌─────────────────▼───────────────────────────────┐
│ Layer 3: Evaluator（评估层） │
│ 9种评估器： │
│ ├─ Cost Evaluator（成本评估） │
│ ├─ MLP Evaluator（多层感知机） │
│ ├─ Cruise MLP Evaluator（巡航MLP） │
│ ├─ Junction MLP Evaluator（路口MLP） │
│ ├─ Junction Map Evaluator（路口地图） │
│ ├─ Semantic LSTM Evaluator（语义LSTM）⭐ │
│ ├─ VectorNet Evaluator（向量网络） │
│ ├─ Jointly Prediction Planning Evaluator │
│ └─ Multi-Agent Evaluator（多智能体） │
└─────────────────┬───────────────────────────────┘
│
┌─────────────────▼───────────────────────────────┐
│ Layer 4: Predictor（预测器层） │
│ 9种预测器： │
│ ├─ Lane Sequence Predictor（车道序列） │
│ ├─ Move Sequence Predictor（运动序列） │
│ ├─ Free Movement Predictor（自由移动） │
│ ├─ Regional Movement Predictor（区域移动） │
│ ├─ Junction Predictor（路口） │
│ ├─ Interaction Predictor（交互） │
│ ├─ Extrapolation Predictor（外推） │
│ ├─ Single Lane Predictor（单车道） │
│ └─ Empty Predictor（无预测） │
└──────────────────────────────────────────────────┘
1.3 容器层（Container）
1.3.1 ObstaclesContainer
位置: modules/prediction/container/obstacles/obstacles_container.h
功能:
- 存储和管理感知模块输出的障碍物数据
- 维护障碍物历史信息（位置、速度、加速度）
- 提供障碍物查询接口
Obstacle类 (container/obstacles/obstacle.h):
class Obstacle {
public:
// 基本信息
int id() const; // 障碍物ID
PerceptionObstacle::Type type() const; // 类型（车辆/行人/自行车）
// 特征访问
const Feature& latest_feature() const; // 最新特征
Feature* mutable_latest_feature(); // 可修改的最新特征
int history_size() const; // 历史帧数
const Feature& feature(int i) const; // 第i帧特征
// 状态判断
bool IsOnLane() const; // 是否在车道上
bool IsNearJunction() const; // 是否接近路口
bool IsInJunction() const; // 是否在路口内
bool IsStill() const; // 是否静止
bool IsPedestrian() const; // 是否行人
bool IsVehicle() const; // 是否车辆
// 卡尔曼滤波
const KalmanFilter<double, 6, 2, 0>& kf() const; // 获取卡尔曼滤波器
// 预测结果
int NumOfPredictedTrajectory() const; // 预测轨迹数量
const Trajectory& trajectory(int i) const; // 第i条轨迹
private:
int id_;
std::deque<Feature> feature_history_; // 特征历史队列
KalmanFilter<double, 6, 2, 0> kf_; // 卡尔曼滤波器
};
卡尔曼滤波器配置:
// 状态向量 [x, y, vx, vy, ax, ay]
// 观测向量 [x, y]
KalmanFilter<double, 6, 2, 0> kf_;
// 参数配置（prediction_gflags.h）
FLAGS_enable_kf_tracking: true
FLAGS_q_var: 0.01 // 过程噪声方差
FLAGS_r_var: 0.1 // 观测噪声方差
FLAGS_p_var: 0.1 // 初始协方差
1.3.2 ADCTrajectoryContainer
位置: modules/prediction/container/adc_trajectory/adc_trajectory_container.h
功能:
- 存储自动驾驶车辆（ADC）的规划轨迹
- 用于交互预测（Jointly Prediction Planning）
1.3.3 PoseContainer
位置: modules/prediction/container/pose/pose_container.h
功能:
- 存储车辆定位信息
- 提供坐标转换服务
1.4 场景层（Scenario）
1.4.1 场景管理器
ScenarioManager (scenario/scenario_manager.h):
支持场景:
enum ScenarioType {
CRUISE, // 巡航场景（车道保持、跟车）
JUNCTION // 路口场景（左转、右转、直行）
};
场景判断逻辑:
// 判断是否接近路口
if (distance_to_junction < FLAGS_junction_distance_threshold) {
return ScenarioType::JUNCTION;
} else {
return ScenarioType::CRUISE;
}
// 典型参数
FLAGS_junction_distance_threshold: 20.0米
1.4.2 优先级分类
ObstaclesPrioritizer (scenario/prioritization/obstacles_prioritizer.h):
三种优先级:
enum PriorityType {
IGNORE, // 忽略 - 不影响主车轨迹
CAUTION, // 警告 - 可能与主车交互
NORMAL // 普通 - 默认优先级
};
分类规则:
IGNORE:
- 距离主车 > caution_distance_threshold
- 在主车后方且速度慢于主车
CAUTION:
- 距离主车 < caution_distance_threshold
- 在主车前方或侧方
- 速度与主车相近
- 可能变道到主车车道
NORMAL:
- 其他情况
典型参数:
FLAGS_caution_distance_threshold: 50.0米
FLAGS_interaction_distance_threshold: 100.0米
2. 评估器算法详解
2.1 评估器管理器
EvaluatorManager (evaluator/evaluator_manager.h):
功能:
- 根据障碍物类型、状态、场景选择合适的评估器
- 管理模型加载和推理
- 协调多个评估器的执行
2.2 九种评估器详解
2.2.1 Cost Evaluator（成本评估器）
位置: modules/prediction/evaluator/vehicle/cost_evaluator.h
算法: 基于成本函数的路径概率计算
成本函数:
Cost = w1 * CostToLane +
w2 * CostToLaneChange +
w3 * CostToCollision +
w4 * CostToCenterLine
// 概率计算
Probability = exp(-Cost / temperature) / Z
其中:
Z: 归一化因子
temperature: 温度参数（控制概率分布尖锐度）
特点:
- 计算简单快速
- 适合实时性要求高的场景
- 不依赖深度学习模型
2.2.2 MLP Evaluator（多层感知机评估器）
位置: modules/prediction/evaluator/vehicle/mlp_evaluator.h
特征维度:
障碍物特征: 22维
- 位置 (x, y)
- 速度 (vx, vy, v)
- 加速度 (ax, ay, a)
- 航向角、航向角速度
- 尺寸 (length, width, height)
- 距离车道中心线距离
- 相对主车位置
- ...
车道特征: 40维
- 左车道特征 (20维)
- 右车道特征 (20维)
- 每个车道：
- 车道类型
- 车道宽度
- 车道曲率
- 到车道中心线距离
- ...
网络结构:
输入层: 62维 (22 + 40)
隐藏层1: 128节点 + ReLU
隐藏层2: 64节点 + ReLU
输出层: 12维 (3个候选车道 × 4个概率)
- 保持当前车道概率
- 左变道概率
- 右变道概率
- 其他概率
模型文件: data/mlp_vehicle_model.bin
2.2.3 Cruise MLP Evaluator（巡航MLP评估器）
位置: modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.h
特征规模:
障碍物特征: 23 + 5×9 = 68维
- 基础特征: 23维
- 车道序列特征: 5条车道 × 9维/车道
交互特征: 8维
- 与周围障碍物的相对位置
- 相对速度
- 时间碰撞时间（TTC）
单车道特征: 4维 × 20个车道点 = 80维
- 每个点: (s, l, heading, curvature)
模型:
Go模型: data/cruise_go_vehicle_model.pt
- 用于判断是否继续前进
- 输出: 前进概率
Cut-in模型: data/cruise_cutin_vehicle_model.pt
- 用于判断是否会切入主车车道
- 输出: 切入概率
框架: PyTorch JIT模型
2.2.4 Junction MLP Evaluator（路口MLP评估器）
位置: modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h
专用场景: 交叉路口场景
特点:
- 考虑多条可能路径（左转、右转、直行）
- 结合地图拓扑信息
- 输出每条路径的概率
模型: data/junction_mlp_vehicle_model.pt
2.2.5 Junction Map Evaluator（路口地图评估器）
位置: modules/prediction/evaluator/vehicle/junction_map_evaluator.h
算法: 基于语义地图的CNN模型
输入:
语义地图: 224×224×3
- 道路: 灰色
- 车道线: 白色/黄色
- 人行横道: 蓝色
- 路口: 绿色
网络: ResNet-18 + LSTM
优先级: 仅处理CAUTION级别障碍物
模型: data/junction_map_vehicle_model.pt
3. LSTM轨迹预测算法
3.1 Semantic LSTM Evaluator
位置: modules/prediction/evaluator/vehicle/semantic_lstm_evaluator.h
核心算法: LSTM + 语义地图
3.1.1 算法流程
代码位置: semantic_lstm_evaluator.cc 行45-150
bool SemanticLSTMEvaluator::Evaluate(
Obstacle* obstacle_ptr,
ObstaclesContainer* obstacles_container) {
// ===== 步骤1: 获取语义地图 (224×224×3) =====
cv::Mat feature_map;
if (!semantic_map_->GetMapById(id, &feature_map)) {
return false;
}
// ===== 步骤2: 图像预处理 =====
// 2.1 BGR → RGB转换
cv::cvtColor(feature_map, feature_map, cv::COLOR_BGR2RGB);
// 2.2 归一化到[0, 1]
cv::Mat img_float;
feature_map.convertTo(img_float, CV_32F, 1.0 / 255);
// 2.3 转换为PyTorch张量
torch::Tensor img_tensor = torch::from_blob(
img_float.data, {1, 224, 224, 3});
img_tensor = img_tensor.permute({0, 3, 1, 2}); // NHWC → NCHW
// 2.4 ImageNet标准化
img_tensor[0][0] = img_tensor[0][0].sub(0.485).div(0.229); // R通道
img_tensor[0][1] = img_tensor[0][1].sub(0.456).div(0.224); // G通道
img_tensor[0][2] = img_tensor[0][2].sub(0.406).div(0.225); // B通道
// ===== 步骤3: 提取历史轨迹 (20帧) =====
std::vector<std::pair<double, double>> pos_history(20, {0.0, 0.0});
if (!ExtractObstacleHistory(obstacle_ptr, &pos_history)) {
return false;
}
// ===== 步骤4: 构建LSTM输入 =====
torch::Tensor obstacle_pos = torch::zeros({1, 20, 2});
torch::Tensor obstacle_pos_step = torch::zeros({1, 20, 2});
for (int i = 0; i < 20; ++i) {
// 位置（相对于当前位置）
obstacle_pos[0][19 - i][0] = pos_history[i].first;
obstacle_pos[0][19 - i][1] = pos_history[i].second;
// 位置步长（速度）
if (i < 19 && pos_history[i].first != 0.0) {
obstacle_pos_step[0][19 - i][0] =
pos_history[i].first - pos_history[i + 1].first;
obstacle_pos_step[0][19 - i][1] =
pos_history[i].second - pos_history[i + 1].second;
}
}
// ===== 步骤5: 模型推理 =====
std::vector<void*> input_buffers{
img_tensor.data_ptr<float>(),
obstacle_pos.data_ptr<float>(),
obstacle_pos_step.data_ptr<float>()
};
std::vector<void*> output_buffers{
torch_default_output_tensor_.data_ptr<float>()
};
// 根据障碍物类型选择模型
if (obstacle_ptr->IsPedestrian()) {
model_manager_.SelectModel(
device_, ObstacleConf::SEMANTIC_LSTM_EVALUATOR,
PerceptionObstacle::PEDESTRIAN)->Inference(
input_buffers, 3, &output_buffers, 1);
} else {
model_manager_.SelectModel(
device_, ObstacleConf::SEMANTIC_LSTM_EVALUATOR,
PerceptionObstacle::VEHICLE)->Inference(
input_buffers, 3, &output_buffers, 1);
}
// 输出张量: {1, 30, 2}
at::Tensor torch_output_tensor = torch::from_blob(
output_buffers[0], {1, 30, 2});
// ===== 步骤6: 坐标变换（相对坐标 → 世界坐标） =====
double pos_x = latest_feature_ptr->position().x();
double pos_y = latest_feature_ptr->position().y();
double heading = latest_feature_ptr->velocity_heading();
Trajectory* trajectory = latest_feature_ptr->add_predicted_trajectory();
trajectory->set_probability(1.0);
for (int i = 0; i < 30; ++i) {
double prev_x = pos_x;
double prev_y = pos_y;
if (i > 0) {
const auto& last_point =
trajectory->trajectory_point(i - 1).path_point();
prev_x = last_point.x();
prev_y = last_point.y();
}
// 获取相对偏移
double dx = torch_output[0][i][0];
double dy = torch_output[0][i][1];
// 旋转到世界坐标系
Vec2d offset(dx, dy);
Vec2d rotated_offset = offset.rotate(heading);
// 计算世界坐标
double point_x = prev_x + rotated_offset.x();
double point_y = prev_y + rotated_offset.y();
// 添加到轨迹
TrajectoryPoint* point = trajectory->add_trajectory_point();
point->mutable_path_point()->set_x(point_x);
point->mutable_path_point()->set_y(point_y);
point->set_relative_time(i * 0.1); // 0.1秒间隔
}
return true;
}
3.1.2 网络结构
LSTM网络架构:
┌─────────────────────────────────────────────────┐
│ 输入层 │
├─────────────────────────────────────────────────┤
│ 1. 语义地图: [1, 3, 224, 224] │
│ └─ CNN特征提取器(ResNet-18) │
│ └─ 输出: [1, 512] │
│ │
│ 2. 历史位置: [1, 20, 2] │
│ └─ 位置编码层 │
│ └─ 输出: [1, 20, 64] │
│ │
│ 3. 历史位置步长: [1, 20, 2] │
│ └─ 速度编码层 │
│ └─ 输出: [1, 20, 64] │
└─────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────┐
│ 特征融合层 │
│ └─ 拼接: [1, 512 + 20×64 + 20×64] │
│ └─ 全连接: [1, 256] │
└─────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────┐
│ LSTM层 │
│ ├─ LSTM单元数: 128 │
│ ├─ 层数: 2 │
│ ├─ Dropout: 0.3 │
│ └─ 输出: [1, 128](隐藏状态) │
└─────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────┐
│ 解码器层 │
│ ├─ 全连接1: [1, 128] → [1, 256] │
│ ├─ ReLU激活 │
│ ├─ 全连接2: [1, 256] → [1, 60] │
│ └─ Reshape: [1, 30, 2] │
└─────────────────────────────────────────────────┘
│
▼
输出: 30个轨迹点
每个点: (Δx, Δy)
3.1.2.1 LSTM网络深度解析
LSTM单元内部结构
Long Short-Term Memory (LSTM)通过门控机制解决RNN的梯度消失问题:
LSTM单元包含3个门 + 1个细胞状态:
┌─────────────────────────────────┐
│ 输入门 (Input Gate) │
│ i_t = σ(W_i[h_{t-1}, x_t] + b_i)│
└─────────────────────────────────┘
┌─────────────────────────────────┐
│ 遗忘门 (Forget Gate) │
│ f_t = σ(W_f[h_{t-1}, x_t] + b_f)│
└─────────────────────────────────┘
┌─────────────────────────────────┐
│ 输出门 (Output Gate) │
│ o_t = σ(W_o[h_{t-1}, x_t] + b_o)│
└─────────────────────────────────┘
┌─────────────────────────────────┐
│ 候选细胞状态 │
│ C̃_t = tanh(W_C[h_{t-1}, x_t]+b_C)│
└─────────────────────────────────┘
细胞状态更新:
C_t = f_t ⊙ C_{t-1} + i_t ⊙ C̃_t
隐藏状态更新:
h_t = o_t ⊙ tanh(C_t)
其中:
σ: sigmoid函数 (0-1范围,作为门控信号)
⊙: 逐元素乘法
[h_{t-1}, x_t]: 拼接操作
Apollo LSTM配置
# PyTorch模型定义
class SemanticLSTMModel(nn.Module):
def __init__(self):
super(SemanticLSTMModel, self).__init__()
# CNN特征提取器 (ResNet-18预训练)
self.cnn = models.resnet18(pretrained=True)
self.cnn.fc = nn.Identity() # 移除最后的分类层
# 输出: [batch, 512]
# 位置编码器
self.pos_encoder = nn.Linear(2, 64)
# 输入: [batch, seq_len=20, 2]
# 输出: [batch, 20, 64]
# 速度编码器
self.vel_encoder = nn.Linear(2, 64)
# 输入: [batch, seq_len=20, 2]
# 输出: [batch, 20, 64]
# 特征融合层
self.fusion = nn.Linear(512 + 20*64 + 20*64, 256)
# 输入: [batch, 512 + 1280 + 1280] = [batch, 3072]
# 输出: [batch, 256]
# LSTM层
self.lstm = nn.LSTM(
input_size=256,
hidden_size=128,
num_layers=2,
batch_first=True,
dropout=0.3
)
# 输入: [batch, seq_len=1, 256]
# 输出: [batch, 1, 128]
# 解码器
self.decoder = nn.Sequential(
nn.Linear(128, 256),
nn.ReLU(),
nn.Linear(256, 60) # 30个点 × 2个坐标
)
# 输出: [batch, 60] → reshape → [batch, 30, 2]
def forward(self, img, pos_history, vel_history):
# Step 1: CNN特征提取
cnn_feat = self.cnn(img) # [batch, 512]
# Step 2: 位置和速度编码
pos_feat = self.pos_encoder(pos_history) # [batch, 20, 64]
vel_feat = self.vel_encoder(vel_history) # [batch, 20, 64]
# Step 3: 特征展平和拼接
pos_feat_flat = pos_feat.view(batch, -1) # [batch, 1280]
vel_feat_flat = vel_feat.view(batch, -1) # [batch, 1280]
combined = torch.cat([cnn_feat, pos_feat_flat, vel_feat_flat], dim=1)
# [batch, 3072]
# Step 4: 特征融合
fused_feat = self.fusion(combined) # [batch, 256]
fused_feat = fused_feat.unsqueeze(1) # [batch, 1, 256]
# Step 5: LSTM编码
lstm_out, (h_n, c_n) = self.lstm(fused_feat)
lstm_out = lstm_out[:, -1, :] # 取最后时刻的输出 [batch, 128]
# Step 6: 解码为轨迹
trajectory = self.decoder(lstm_out) # [batch, 60]
trajectory = trajectory.view(batch, 30, 2) # [batch, 30, 2]
return trajectory
训练损失函数
Apollo采用**加权均方误差(Weighted MSE)**作为损失函数:
def weighted_mse_loss(pred, target, time_weights):
"""
Args:
pred: 预测轨迹 [batch, 30, 2]
target: 真实轨迹 [batch, 30, 2]
time_weights: 时间权重 [30]
Returns:
loss: 标量损失值
"""
# 计算每个点的MSE
mse_per_point = torch.sum((pred - target) ** 2, dim=2) # [batch, 30]
# 应用时间权重 (越远的点权重越大)
# time_weights = [0.5, 0.6, 0.7, ..., 3.0]
weighted_mse = mse_per_point * time_weights # [batch, 30]
# 平均损失
loss = torch.mean(weighted_mse)
return loss
# 时间权重设计
time_weights = torch.linspace(0.5, 3.0, 30)
# 原理: 短期预测(0.5-1.0秒)相对容易,给予较低权重
# 长期预测(2.0-3.0秒)更难但更重要,给予较高权重
完整训练代码示例
# 训练配置
optimizer = torch.optim.Adam(model.parameters(), lr=0.001)
scheduler = torch.optim.lr_scheduler.StepLR(optimizer, step_size=10, gamma=0.5)
num_epochs = 100
batch_size = 32
# 数据增强
augmentation = A.Compose([
A.RandomBrightnessContrast(p=0.5),
A.GaussianBlur(p=0.3),
A.RandomRotation(degrees=5)
])
# 训练循环
for epoch in range(num_epochs):
model.train()
total_loss = 0.0
for batch_idx, (img, pos_hist, vel_hist, target_traj) in enumerate(train_loader):
# 数据移到GPU
img = img.to(device)
pos_hist = pos_hist.to(device)
vel_hist = vel_hist.to(device)
target_traj = target_traj.to(device)
# 前向传播
pred_traj = model(img, pos_hist, vel_hist)
# 计算损失
loss = weighted_mse_loss(pred_traj, target_traj, time_weights)
# 反向传播
optimizer.zero_grad()
loss.backward()
# 梯度裁剪 (防止梯度爆炸)
torch.nn.utils.clip_grad_norm_(model.parameters(), max_norm=5.0)
# 参数更新
optimizer.step()
total_loss += loss.item()
# 学习率衰减
scheduler.step()
# 打印训练信息
avg_loss = total_loss / len(train_loader)
print(f"Epoch {epoch+1}/{num_epochs}, Loss: {avg_loss:.4f}")
# 验证
if (epoch + 1) % 5 == 0:
model.eval()
val_ade, val_fde = evaluate(model, val_loader)
print(f"Validation ADE: {val_ade:.2f}m, FDE: {val_fde:.2f}m")
评价指标
def evaluate(model, dataloader):
"""
计算平均位移误差(ADE)和最终位移误差(FDE)
"""
model.eval()
total_ade = 0.0
total_fde = 0.0
num_samples = 0
with torch.no_grad():
for img, pos_hist, vel_hist, target_traj in dataloader:
img = img.to(device)
pos_hist = pos_hist.to(device)
vel_hist = vel_hist.to(device)
target_traj = target_traj.to(device)
# 预测
pred_traj = model(img, pos_hist, vel_hist)
# ADE: 所有时间步的平均欧氏距离
# ADE = (1/T) * Σ||pred_t - gt_t||
diff = pred_traj - target_traj # [batch, 30, 2]
dist = torch.sqrt(diff[:, :, 0]**2 + diff[:, :, 1]**2) # [batch, 30]
ade = torch.mean(dist) # 标量
# FDE: 最后一个时间步的欧氏距离
# FDE = ||pred_T - gt_T||
fde = torch.sqrt(
(pred_traj[:, -1, 0] - target_traj[:, -1, 0])**2 +
(pred_traj[:, -1, 1] - target_traj[:, -1, 1])**2
)
fde = torch.mean(fde)
total_ade += ade.item() * img.size(0)
total_fde += fde.item() * img.size(0)
num_samples += img.size(0)
avg_ade = total_ade / num_samples
avg_fde = total_fde / num_samples
return avg_ade, avg_fde
# 典型性能指标 (在验证集上):
# - 车辆预测:
# ADE: 1.2-1.8米
# FDE: 2.5-4.0米
# - 行人预测:
# ADE: 0.8-1.2米
# FDE: 1.5-2.5米
数据集准备
# 数据格式
{
"obstacle_id": 12345,
"timestamp": 1234567890.123,
"type": "VEHICLE", # or "PEDESTRIAN"
# 历史轨迹 (2秒,20帧,每帧0.1秒)
"history": [
{"x": 100.0, "y": 50.0, "t": -2.0},
{"x": 100.5, "y": 50.1, "t": -1.9},
...
{"x": 110.0, "y": 52.0, "t": 0.0}
],
# 未来轨迹 (3秒,30帧)
"future": [
{"x": 110.5, "y": 52.1, "t": 0.1},
{"x": 111.0, "y": 52.2, "t": 0.2},
...
{"x": 125.0, "y": 55.0, "t": 3.0}
],
# 语义地图 (224×224 RGB图像)
"semantic_map_path": "/path/to/map_12345.png"
}
# 语义地图生成
def generate_semantic_map(obstacle_pos, map_range=50.0, resolution=224):
"""
生成以障碍物为中心的语义地图
Args:
obstacle_pos: (x, y) 障碍物位置
map_range: 地图范围 (±50米)
resolution: 图像分辨率 224×224
Returns:
semantic_map: RGB图像 [224, 224, 3]
"""
img = np.zeros((resolution, resolution, 3), dtype=np.uint8)
# 颜色编码
ROAD_COLOR = [128, 128, 128] # 灰色
LANE_LINE_COLOR = [255, 255, 255] # 白色
CROSSWALK_COLOR = [255, 255, 0] # 黄色
VEHICLE_COLOR = [0, 0, 255] # 蓝色
PEDESTRIAN_COLOR = [255, 0, 0] # 红色
# 查询HD地图,绘制道路元素
lanes = hd_map.query_lanes(obstacle_pos, map_range)
for lane in lanes:
draw_lane(img, lane, ROAD_COLOR)
draw_lane_boundary(img, lane, LANE_LINE_COLOR)
crosswalks = hd_map.query_crosswalks(obstacle_pos, map_range)
for cw in crosswalks:
draw_crosswalk(img, cw, CROSSWALK_COLOR)
# 绘制周围障碍物
nearby_obs = perception.query_obstacles(obstacle_pos, map_range)
for obs in nearby_obs:
if obs.type == "VEHICLE":
draw_obstacle(img, obs, VEHICLE_COLOR)
elif obs.type == "PEDESTRIAN":
draw_obstacle(img, obs, PEDESTRIAN_COLOR)
return img
模型推理优化
// C++推理代码优化技巧
// modules/prediction/evaluator/vehicle/semantic_lstm_evaluator.cc
// 技巧1: 批处理推理
std::vector<Obstacle*> batch_obstacles;
batch_obstacles.reserve(32); // 批大小32
for (auto& obs : obstacles) {
batch_obstacles.push_back(&obs);
if (batch_obstacles.size() == 32) {
// 批量推理
BatchInference(batch_obstacles);
batch_obstacles.clear();
}
}
// 处理剩余障碍物
if (!batch_obstacles.empty()) {
BatchInference(batch_obstacles);
}
// 技巧2: 模型量化 (FP32 → FP16)
// 在模型转换时使用:
// torch.jit.save(model, "model.pt", {"use_fp16": True})
// 技巧3: CUDA流并发
cudaStream_t stream1, stream2;
cudaStreamCreate(&stream1);
cudaStreamCreate(&stream2);
// 交替使用两个流
InferenceAsync(obstacles_batch1, stream1);
InferenceAsync(obstacles_batch2, stream2);
cudaStreamSynchronize(stream1);
cudaStreamSynchronize(stream2);
// 性能对比:
// - 单个推理: 15-20ms
// - 批处理(batch=32): 平均2-3ms/样本
// - 量化+批处理: 平均1-2ms/样本
3.1.3 模型文件
车辆模型:
GPU版本: data/semantic_lstm_vehicle_model.pt
CPU版本: data/semantic_lstm_vehicle_cpu_model.pt
行人模型:
GPU版本: data/semantic_lstm_pedestrian_model.pt
CPU版本: data/semantic_lstm_pedestrian_cpu_model.pt
模型大小: ~20MB
推理时间: 10-20ms (GPU) / 50-100ms (CPU)
3.1.4 坐标系统
相对坐标系定义:
↑ y (左侧)
│
│
──────────●────────── → x (前方)
(当前障碍物位置)
坐标变换:
// 1. 世界坐标 → 障碍物相对坐标
std::pair<double, double> WorldCoordToObjCoord(
std::pair<double, double> input_world_coord,
std::pair<double, double> obj_world_coord,
double obj_world_angle) {
// 计算相对位移
double x_diff = input_world_coord.first - obj_world_coord.first;
double y_diff = input_world_coord.second - obj_world_coord.second;
// 转换到极坐标
double rho = sqrt(x_diff*x_diff + y_diff*y_diff);
double theta = atan2(y_diff, x_diff) - obj_world_angle;
// 转回直角坐标（障碍物坐标系）
return {cos(theta)*rho, sin(theta)*rho};
}
// 2. 相对偏移 → 世界坐标
Vec2d offset(dx, dy);
Vec2d rotated_offset = offset.rotate(heading);
world_x = current_x + rotated_offset.x();
world_y = current_y + rotated_offset.y();
3.2 VectorNet Evaluator
位置: modules/prediction/evaluator/vehicle/vectornet_evaluator.h
核心思想: 将地图元素表示为向量集合
3.2.1 VectorNet处理
VectorNet类 (pipeline/vector_net.h):
class VectorNet {
public:
// 地图元素类型
enum ATTRIBUTE_TYPE {
ROAD = 0, // 道路
LANE_DOTTED_YELLOW = 1, // 黄虚线
LANE_DOTTED_WHITE = 2, // 白虚线
LANE_SOLID_YELLOW = 3, // 黄实线
LANE_SOLID_WHITE = 4, // 白实线
JUNCTION = 5, // 路口
CROSSWALK = 6, // 人行横道
BOUNDARY = 7, // 边界
STOP_SIGN = 8, // 停止标志
UNKNOWN = -1
};
// 查询地图向量
bool query(
const PointENU& center_point, // 中心点
const double obstacle_phi, // 障碍物航向
FeatureVector* feature_ptr, // 输出特征向量
PidVector* p_id_ptr); // 输出多段线ID
// 获取特定类型元素
void GetRoads(const PointENU& center_point, ...);
void GetLanes(const PointENU& center_point, ...);
void GetJunctions(const PointENU& center_point, ...);
void GetCrosswalks(const PointENU& center_point, ...);
};
向量表示:
每个地图元素表示为一组向量:
向量 = (start_point, end_point, attribute)
例如车道线:
向量1: (P0, P1, LANE_SOLID_WHITE)
向量2: (P1, P2, LANE_SOLID_WHITE)
向量3: (P2, P3, LANE_SOLID_WHITE)
...
网络结构:
VectorNet = 子图网络 + 全局交互网络
子图网络:
- 处理每个地图元素的向量序列
- 使用GRU聚合向量特征
全局交互网络:
- 处理所有子图特征
- 使用Self-Attention捕获交互
3.3 Jointly Prediction Planning Evaluator
位置: modules/prediction/evaluator/vehicle/jointly_prediction_planning_evaluator.h
创新点: 同时考虑ADC规划轨迹和障碍物预测
输入:
1. 障碍物历史轨迹: [1, 20, 2]
2. ADC规划轨迹: [1, 80, 2]
3. VectorNet地图特征: [1, N, 128]
网络结构:
障碍物编码器 → LSTM → [1, 128]
↓
ADC轨迹编码器 → LSTM → [1, 128] → 拼接 → 解码器 → 轨迹
↓
地图特征编码器 → GNN → [1, 128]
优势:
- 考虑主车意图
- 更准确的交互预测
- 适合复杂场景
模型: data/jointly_prediction_planning_vehicle_model.pt
4. 定位模块架构
4.1 系统概述
Localization模块提供车辆的精确位姿估计，包括位置、速度、加速度、航向角等信息。
定位精度要求:
横向误差: < 0.1m (RMS)
纵向误差: < 0.2m (RMS)
航向误差: < 0.5° (RMS)
三种定位方法:
| 方法 | 传感器 | 精度 | 适用场景 |
|---|---|---|---|
| RTK | GPS + IMU | 厘米级 | 开阔场景、GPS信号良好 |
| MSF | GPS + IMU + LiDAR | 厘米级 | 全场景、高精度要求 |
| NDT | LiDAR | 分米级 | GPS失效、隧道/地库 |
4.2 定位消息格式
LocalizationEstimate (localization.proto):
message LocalizationEstimate {
Header header = 1;
Pose pose = 2;
Uncertainty uncertainty = 3;
double measurement_time = 4; // 测量时间戳
MeasureState msf_status = 5; // MSF状态
MeasureState lidar_status = 6; // LiDAR状态
MeasureState fusion_status = 7; // 融合状态
}
message Pose {
// 位置（地图坐标系）
Point3D position = 1; // (x, y, z)
// 姿态（四元数）
Quaternion orientation = 2; // (qw, qx, qy, qz)
// 线速度（地图坐标系）
Point3D linear_velocity = 3; // (vx, vy, vz)
// 线加速度（地图坐标系）
Point3D linear_acceleration = 4; // (ax, ay, az)
// 角速度（地图坐标系）
Point3D angular_velocity = 5; // (ωx, ωy, ωz)
// 航向角（弧度）
double heading = 6;
// 线加速度（车体坐标系）
Point3D linear_acceleration_vrf = 7;
// 角速度（车体坐标系）
Point3D angular_velocity_vrf = 8;
// 欧拉角（roll, pitch, yaw）
EulerAngles euler_angles = 9;
}
message Uncertainty {
// 位置不确定性
Point3D position_std_dev = 1; // 标准差
// 姿态不确定性
Point3D orientation_std_dev = 2;
// 速度不确定性
Point3D linear_velocity_std_dev = 3;
// 加速度不确定性
Point3D linear_acceleration_std_dev = 4;
// 角速度不确定性
Point3D angular_velocity_std_dev = 5;
}
5. RTK定位算法
5.1 RTK定位原理
RTK (Real-Time Kinematic): 实时动态差分定位
原理:
基准站 (已知精确坐标)
↓ 差分改正数
GPS卫星 → 移动站 (车辆) → 高精度定位结果
↓
接收原始观测值
核心组件: modules/localization/rtk/rtk_localization.h
5.2 算法流程
RTKLocalization类:
class RTKLocalization {
public:
// 主要回调函数
void GpsCallback(const Gps& gps_msg);
void ImuCallback(const Imu& imu_msg);
private:
// IMU数据缓冲区
std::list<ImuData> imu_list_;
int imu_list_max_size_ = 50;
// GPS状态缓冲区
std::list<GpsStatus> gps_status_list_;
int gps_status_list_max_size_ = 10;
// 地图偏移
double map_offset_[3]; // [x, y, z]
};
GPS回调处理流程:
void RTKLocalization::GpsCallback(const Gps& gps_msg) {
// ===== 步骤1: 查找匹配的IMU消息 =====
double gps_timestamp = gps_msg.header().timestamp_sec();
ImuData imu_msg;
if (!FindMatchingIMU(gps_timestamp, &imu_msg)) {
AERROR << "Cannot find matching IMU for GPS at " << gps_timestamp;
return;
}
// ===== 步骤2: IMU时间插值 =====
// 如果GPS时间不完全匹配IMU时间，进行线性插值
ImuData interpolated_imu;
if (NeedInterpolation(gps_timestamp, imu_msg)) {
ImuData imu1, imu2;
FindNearestIMU(gps_timestamp, &imu1, &imu2);
InterpolateIMU(imu1, imu2, gps_timestamp, &interpolated_imu);
} else {
interpolated_imu = imu_msg;
}
// ===== 步骤3: 组合定位消息 =====
LocalizationEstimate localization;
ComposeLocalizationMsg(gps_msg, interpolated_imu, &localization);
// ===== 步骤4: 发布定位结果 =====
PublishLocalization(localization);
}
IMU时间插值:
bool InterpolateIMU(const ImuData& imu1, const ImuData& imu2,
double timestamp, ImuData* output) {
// 计算插值权重
double total_delta = imu2.timestamp - imu1.timestamp;
if (total_delta <= 0) return false;
double frac1 = (timestamp - imu1.timestamp) / total_delta;
double frac2 = 1.0 - frac1;
// 线性插值 - 加速度
output->linear_acceleration.x =
imu1.linear_acceleration.x * frac2 +
imu2.linear_acceleration.x * frac1;
output->linear_acceleration.y =
imu1.linear_acceleration.y * frac2 +
imu2.linear_acceleration.y * frac1;
output->linear_acceleration.z =
imu1.linear_acceleration.z * frac2 +
imu2.linear_acceleration.z * frac1;
// 线性插值 - 角速度
output->angular_velocity.x =
imu1.angular_velocity.x * frac2 +
imu2.angular_velocity.x * frac1;
output->angular_velocity.y =
imu1.angular_velocity.y * frac2 +
imu2.angular_velocity.y * frac1;
output->angular_velocity.z =
imu1.angular_velocity.z * frac2 +
imu2.angular_velocity.z * frac1;
output->timestamp = timestamp;
return true;
}
5.3 坐标转换
世界坐标系 → 地图坐标系:
void ComposeLocalizationMsg(const Gps& gps_msg,
const ImuData& imu_msg,
LocalizationEstimate* localization) {
// 位置转换
localization->mutable_pose()->mutable_position()->set_x(
gps_msg.position().x() - map_offset_[0]);
localization->mutable_pose()->mutable_position()->set_y(
gps_msg.position().y() - map_offset_[1]);
localization->mutable_pose()->mutable_position()->set_z(
gps_msg.position().z() - map_offset_[2]);
// 姿态（四元数）
localization->mutable_pose()->mutable_orientation()->CopyFrom(
gps_msg.orientation());
// 航向角计算
double heading = QuaternionToHeading(
gps_msg.orientation().qw(),
gps_msg.orientation().qx(),
gps_msg.orientation().qy(),
gps_msg.orientation().qz());
localization->mutable_pose()->set_heading(heading);
// 速度（地图坐标系）
localization->mutable_pose()->mutable_linear_velocity()->CopyFrom(
gps_msg.linear_velocity());
// 加速度转换（车体坐标系 → 地图坐标系）
Vector3d acc_vehicle(
imu_msg.linear_acceleration.x,
imu_msg.linear_acceleration.y,
imu_msg.linear_acceleration.z);
Vector3d acc_map = QuaternionRotate(
gps_msg.orientation(), acc_vehicle);
localization->mutable_pose()->mutable_linear_acceleration()->set_x(
acc_map.x());
localization->mutable_pose()->mutable_linear_acceleration()->set_y(
acc_map.y());
localization->mutable_pose()->mutable_linear_acceleration()->set_z(
acc_map.z());
// 角速度转换（车体坐标系 → 地图坐标系）
Vector3d omega_vehicle(
imu_msg.angular_velocity.x,
imu_msg.angular_velocity.y,
imu_msg.angular_velocity.z);
Vector3d omega_map = QuaternionRotate(
gps_msg.orientation(), omega_vehicle);
localization->mutable_pose()->mutable_angular_velocity()->set_x(
omega_map.x());
localization->mutable_pose()->mutable_angular_velocity()->set_y(
omega_map.y());
localization->mutable_pose()->mutable_angular_velocity()->set_z(
omega_map.z());
}
5.4 配置参数
配置文件: modules/localization/conf/rtk_localization.pb.txt
localization_topic: "/apollo/localization/pose"
imu_topic: "/apollo/sensor/gnss/imu"
gps_topic: "/apollo/sensor/gnss/best_pose"
imu_list_max_size: 50
gps_imu_time_diff_threshold: 0.02 # 20ms
gps_time_delay_tolerance: 1.0 # 1秒
map_offset_x: 0.0
map_offset_y: 0.0
map_offset_z: 0.0
enable_tf2_transform: false
6. MSF多传感器融合定位
6.1 MSF系统概述
MSF (Multi-Sensor Fusion): 融合GPS、IMU、LiDAR实现高精度定位
核心组件: modules/localization/msf/local_integ/localization_integ.h
6.2 LocalizationInteg接口
代码位置: localization_integ.h 行1-90
class LocalizationInteg {
public:
// 初始化
common::Status Init(const LocalizationIntegParam ¶ms);
// ===== 点云处理 =====
void PcdProcess(const drivers::PointCloud &message);
// ===== IMU处理 =====
// FLU坐标系: Front-Left-Up (前左上)
void RawImuProcessFlu(const drivers::gnss::Imu &imu_msg);
// RFU坐标系: Right-Front-Up (右前上)
void RawImuProcessRfu(const drivers::gnss::Imu &imu_msg);
// ===== GNSS处理 =====
// 原始观测值
void RawObservationProcess(
const drivers::gnss::EpochObservation &raw_obs_msg);
// 星历数据
void RawEphemerisProcess(
const drivers::gnss::GnssEphemeris &gnss_orbit_msg);
// GNSS最佳位姿
void GnssBestPoseProcess(
const drivers::gnss::GnssBestPose &bestgnsspos_msg);
// GNSS航向
void GnssHeadingProcess(
const drivers::gnss::Heading &gnss_heading_msg);
// ===== 获取结果 =====
const LocalizationResult &GetLastestLidarLocalization() const;
const LocalizationResult &GetLastestIntegLocalization() const;
const LocalizationResult &GetLastestGnssLocalization() const;
};
6.3 四种工作模式
配置参数 (localization_msf/integ_sins_pva.h):
// GNSS模式
gnss_mode: 0 或 1
0: 使用BestGNSS (RTK定位结果)
1: 使用Local-GNSS (原始观测值计算定位)
// GNSS初始化模式
gnss_only_init: true 或 false
false: GNSS持续参与融合
true: GNSS仅用于初始化SINS对齐
四种模式:
模式1: 三系统 (BestGNSS + LiDAR + SINS)
gnss_mode=0, gnss_only_init=false
- GNSS持续提供位置更新
- LiDAR提供位姿校正
- SINS积分平滑
模式2: 三系统 (Local-GNSS + LiDAR + SINS)
gnss_mode=1, gnss_only_init=false
- 使用原始GNSS观测值
- 更高的定位精度
- 计算复杂度更高
模式3: 二系统 (BestGNSS + LiDAR + SINS)
gnss_mode=0, gnss_only_init=true
- GNSS仅初始化
- 主要依赖LiDAR和SINS
- 适合GPS信号不稳定场景
模式4: 二系统 (Local-GNSS + LiDAR + SINS)
gnss_mode=1, gnss_only_init=true
- 原始GNSS观测仅初始化
- 最高精度但计算量大
6.4 LiDAR定位
LocalizationLidar (localization_lidar.h):
class LocalizationLidar {
public:
// 三种定位模式
enum LocalizationMode {
INTENSITY, // 反射强度匹配
ALTITUDE, // 高程匹配
FUSION // 融合模式（推荐）
};
// 初始化地图
bool Init(const string& map_path,
unsigned int search_range_x,
unsigned int search_range_y,
int zone_id);
// 更新定位
int Update(unsigned int frame_idx,
const Affine3d& pose,
const Vector3d velocity,
const LidarFrame& lidar_frame,
bool use_avx = false);
// 获取结果
void GetResult(Affine3d* location,
Matrix3d* covariance,
double* location_score);
};
金字塔地图 (pyramid_map/pyramid_map.h):
class PyramidMap {
// 地图节点
struct MapNode {
float intensity_avg; // 反射强度均值
float intensity_var; // 反射强度方差
float altitude; // 高程
int count; // 点数统计
};
// 分辨率
float resolution_ = 0.125; // 默认0.125米
// 地图数据
std::map<MapNodeIndex, MapNode> nodes_;
};
7. NDT点云配准算法
7.1 NDT算法原理
NDT (Normal Distributions Transform): 正态分布变换
核心思想: 将点云表示为正态分布的集合
算法步骤:
1. 体素化
└─ 将目标点云划分为3D体素网格
2. 统计建模
└─ 每个体素内的点拟合正态分布
├─ 均值: μ = (1/N) Σ p_i
└─ 协方差: Σ = (1/N) Σ (p_i - μ)(p_i - μ)ᵀ
3. 概率匹配
└─ 源点云变换后，计算每个点落在目标分布的概率
4. 优化
└─ 最大化总概率 = 最小化负对数似然
├─ 使用牛顿法迭代
└─ 计算梯度和Hessian矩阵
5. 收敛检查
└─ 变换增量 < 阈值
7.2 NDT实现
核心类: NormalDistributionsTransform
位置: modules/localization/ndt/ndt_locator/ndt_solver.h
template <typename PointSource, typename PointTarget>
class NormalDistributionsTransform {
public:
// ===== 参数设置 =====
void SetResolution(float resolution) {
resolution_ = resolution;
}
void SetStepSize(double step_size) {
step_size_ = step_size;
}
void SetMaximumIterations(int max_iterations) {
max_iterations_ = max_iterations;
}
void SetTransformationEpsilon(double epsilon) {
transformation_epsilon_ = epsilon;
}
void SetOutlierRatio(double ratio) {
outlier_ratio_ = ratio;
}
// ===== 输入设置 =====
void SetInputTarget(const PointCloudTargetPtr& cloud);
void SetInputSource(const PointCloudSourcePtr& cloud);
// ===== 配准执行 =====
void Align(PointCloudSourcePtr output,
const Matrix4f& guess);
// ===== 结果获取 =====
Matrix4f GetFinalTransformation() const {
return final_transformation_;
}
Matrix3d GetLocationCovariance() const {
return location_covariance_;
}
double GetFitnessScore() const {
return fitness_score_;
}
private:
// 核心计算方法
void ComputeTransformation(PointCloudSourcePtr output,
const Matrix4f& guess);
double ComputeDerivatives(Matrix<6,1>* score_gradient,
Matrix<6,6>* hessian,
PointCloudSourcePtr trans_cloud,
Matrix<6,1>* p);
void ComputeStepLengthMt(const Matrix<6,1>& x,
Matrix<6,1>* delta_x,
double step_init,
double step_max,
double step_min,
...);
void UpdateDerivatives(Matrix<6,1>* score_gradient,
Matrix<6,6>* hessian,
const Vector3d& x_trans,
const Matrix3d& cov_inv);
// 成员变量
float resolution_; // 体素分辨率
double step_size_; // 牛顿法步长
double outlier_ratio_; // 离群点比例
int max_iterations_; // 最大迭代次数
double transformation_epsilon_; // 收敛阈值
Matrix4f final_transformation_; // 最终变换
Matrix3d location_covariance_; // 位置协方差
double fitness_score_; // 配准分数
};
7.3 ComputeTransformation方法
牛顿法优化:
void ComputeTransformation(PointCloudSourcePtr output,
const Matrix4f& guess) {
// 初始化变换
transformation_ = guess;
Matrix<6,1> x; // 6DOF位姿参数 [x, y, z, roll, pitch, yaw]
TransformationToVector(transformation_, &x);
// 迭代优化
for (int iter = 0; iter < max_iterations_; ++iter) {
// 1. 应用当前变换
PointCloudSourcePtr trans_cloud(new PointCloudSource);
TransformPointCloud(*input_source_, *trans_cloud, transformation_);
// 2. 计算梯度和Hessian矩阵
Matrix<6,1> score_gradient;
Matrix<6,6> hessian;
double score = ComputeDerivatives(
&score_gradient, &hessian, trans_cloud, &x);
// 3. 检查是否需要继续
if (score_gradient.norm() < 1e-4) {
AINFO << "Gradient too small, converged.";
break;
}
// 4. 牛顿法更新
Matrix<6,1> delta_x = -hessian.inverse() * score_gradient;
// 5. 线搜索优化步长
double step_init = 1.0;
double step_max = 1.0;
double step_min = 0.0;
ComputeStepLengthMt(x, &delta_x, step_init,
step_max, step_min, ...);
// 6. 更新参数
x = x + step_size_ * delta_x;
VectorToTransformation(x, &transformation_);
// 7. 检查收敛
if (delta_x.norm() < transformation_epsilon_) {
AINFO << "Transformation change too small, converged at iter "
<< iter;
break;
}
ADEBUG << "Iteration " << iter << ", score: " << score
<< ", delta_x norm: " << delta_x.norm();
}
// 保存最终结果
final_transformation_ = transformation_;
TransformPointCloud(*input_source_, *output, final_transformation_);
// 计算配准分数
fitness_score_ = ComputeFitnessScore(output);
}
7.4 ComputeDerivatives方法
梯度和Hessian计算:
double ComputeDerivatives(Matrix<6,1>* score_gradient,
Matrix<6,6>* hessian,
PointCloudSourcePtr trans_cloud,
Matrix<6,1>* p) {
score_gradient->setZero();
hessian->setZero();
double score = 0.0;
// 遍历所有源点
for (size_t i = 0; i < trans_cloud->size(); ++i) {
const PointSource& point = trans_cloud->points[i];
// 查找对应的目标体素
Vector3d x(point.x, point.y, point.z);
Leaf* cell = target_cells_.GetLeaf(x);
if (!cell || cell->GetPointCount() < min_points_per_voxel_) {
continue; // 跳过无效体素
}
// 获取正态分布参数
Vector3d mean = cell->GetMean();
Matrix3d cov = cell->GetCovariance();
Matrix3d cov_inv = cov.inverse();
// 计算点到分布的距离
Vector3d diff = x - mean;
double exponent = -0.5 * diff.transpose() * cov_inv * diff;
double prob = exp(exponent);
// 更新分数（负对数似然）
score -= prob;
// 计算雅可比矩阵 J = ∂x/∂p
Matrix<3,6> jacobian;
ComputePointJacobian(x, p, &jacobian);
// 更新梯度: g = J^T * Σ^-1 * (x - μ) * prob
Vector3d grad_xyz = cov_inv * diff * prob;
*score_gradient += jacobian.transpose() * grad_xyz;
// 更新Hessian: H = J^T * Σ^-1 * J * prob
*hessian += jacobian.transpose() * cov_inv * jacobian * prob;
}
return score;
}
概率密度函数:
P(x|μ,Σ) = exp(-0.5 * (x-μ)ᵀΣ⁻¹(x-μ)) / √((2π)³|Σ|)
负对数似然:
-log P(x) = 0.5 * (x-μ)ᵀΣ⁻¹(x-μ) + const
最小化负对数似然 = 最大化概率
7.5 NDT参数配置
典型参数:
// 体素分辨率
resolution: 1.0 // 米
// 牛顿法步长
step_size: 0.1
// 最大迭代次数
max_iterations: 10
// 收敛阈值
transformation_epsilon: 0.01
// 离群点比例
outlier_ratio: 0.55 // 55%的点可能是离群点
8. 卡尔曼滤波实现
8.1 卡尔曼滤波基础
位置: modules/common/math/kalman_filter.h
模板类定义 (行49-252):
template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
class KalmanFilter {
public:
// ===== 构造函数 =====
KalmanFilter() {
F_.setIdentity(); // 状态转移矩阵
Q_.setZero(); // 过程噪声协方差
H_.setIdentity(); // 观测矩阵
R_.setZero(); // 观测噪声协方差
B_.setZero(); // 控制矩阵
x_.setZero(); // 状态向量
P_.setZero(); // 状态协方差
}
// ===== 初始化 =====
void SetStateEstimate(const Eigen::Matrix<T, XN, 1> &x,
const Eigen::Matrix<T, XN, XN> &P) {
x_ = x;
P_ = P;
is_initialized_ = true;
}
// ===== 预测步骤 =====
void Predict(const Eigen::Matrix<T, UN, 1> &u =
Eigen::Matrix<T, UN, 1>::Zero());
// ===== 更新步骤 =====
void Correct(const Eigen::Matrix<T, ZN, 1> &z);
// ===== 获取结果 =====
Eigen::Matrix<T, XN, 1> GetStateEstimate() const { return x_; }
Eigen::Matrix<T, XN, XN> GetStateCovariance() const { return P_; }
private:
// 状态
Eigen::Matrix<T, XN, 1> x_; // 状态向量
Eigen::Matrix<T, XN, XN> P_; // 状态协方差
// 系统模型
Eigen::Matrix<T, XN, XN> F_; // 状态转移矩阵
Eigen::Matrix<T, XN, XN> Q_; // 过程噪声协方差
Eigen::Matrix<T, XN, UN> B_; // 控制矩阵
// 观测模型
Eigen::Matrix<T, ZN, XN> H_; // 观测矩阵
Eigen::Matrix<T, ZN, ZN> R_; // 观测噪声协方差
// 中间变量
Eigen::Matrix<T, ZN, 1> y_; // 新息
Eigen::Matrix<T, ZN, ZN> S_; // 新息协方差
Eigen::Matrix<T, XN, ZN> K_; // 卡尔曼增益
bool is_initialized_ = false;
};
8.2 预测步骤
代码位置: kalman_filter.h 行255-262
template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Predict(
const Eigen::Matrix<T, UN, 1> &u) {
ACHECK(is_initialized_);
// 状态预测
x_ = F_ * x_ + B_ * u;
// 协方差预测
P_ = F_ * P_ * F_.transpose() + Q_;
}
预测方程:
x̂⁻ = F × x̂⁺ + B × u
P⁻ = F × P⁺ × Fᵀ + Q
其中:
x̂⁻: 先验状态估计
x̂⁺: 后验状态估计
P⁻: 先验协方差
P⁺: 后验协方差
F: 状态转移矩阵
B: 控制矩阵
u: 控制输入
Q: 过程噪声协方差
8.3 更新步骤
代码位置: kalman_filter.h 行264-279
template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Correct(
const Eigen::Matrix<T, ZN, 1> &z) {
ACHECK(is_initialized_);
// 新息（innovation）
y_ = z - H_ * x_;
// 新息协方差
S_ = static_cast<Eigen::Matrix<T, ZN, ZN>>(
H_ * P_ * H_.transpose() + R_);
// 卡尔曼增益
K_ = static_cast<Eigen::Matrix<T, XN, ZN>>(
P_ * H_.transpose() * PseudoInverse<T, ZN>(S_));
// 状态更新
x_ = x_ + K_ * y_;
// 协方差更新
P_ = static_cast<Eigen::Matrix<T, XN, XN>>(
(Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_);
}
更新方程:
y = z - H × x̂⁻ (新息)
S = H × P⁻ × Hᵀ + R (新息协方差)
K = P⁻ × Hᵀ × S⁻¹ (卡尔曼增益)
x̂⁺ = x̂⁻ + K × y (状态更新)
P⁺ = (I - K × H) × P⁻ (协方差更新)
其中:
z: 观测值
H: 观测矩阵
R: 观测噪声协方差
K: 卡尔曼增益（核心!）
I: 单位矩阵
8.3.1 扩展卡尔曼滤波(EKF)深度解析
扩展卡尔曼滤波 (Extended Kalman Filter, EKF)
标准卡尔曼滤波要求系统是线性的,但实际定位系统往往是非线性的。EKF通过一阶泰勒展开(线性化)将非线性系统近似为线性系统。
非线性系统模型:
状态方程: x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}
观测方程: z_k = h(x_k) + v_k
其中:
f(): 非线性状态转移函数
h(): 非线性观测函数
w_k ~ N(0, Q): 过程噪声
v_k ~ N(0, R): 观测噪声
EKF线性化
对非线性函数在当前估计点进行一阶泰勒展开:
f(x) ≈ f(x̂) + F × (x - x̂)
h(x) ≈ h(x̂) + H × (x - x̂)
其中雅可比矩阵:
F = ∂f/∂x |_{x=x̂} (状态转移雅可比)
H = ∂h/∂x |_{x=x̂} (观测雅可比)
Apollo EKF实现 (modules/localization/msf/local_integ/sins_struct.h):
// 15维状态向量
struct SinsPva {
// 位置 (3维)
Eigen::Vector3d pos; // [latitude, longitude, altitude]
// 速度 (3维)
Eigen::Vector3d vel; // [v_north, v_east, v_down]
// 姿态四元数 (4维)
Eigen::Quaterniond qbn; // 车体系到导航系的旋转
// IMU偏差 (6维)
Eigen::Vector3d gyro_bias; // 陀螺仪偏差
Eigen::Vector3d acc_bias; // 加速度计偏差
// 总维度: 3 + 3 + 4 + 3 + 3 = 16维
// 实际使用15维 (四元数3维误差 + 其他12维 = 15维)
};
// 状态误差向量 (15维)
// δx = [δp, δv, δθ, δba, δbg]'
// δp: 位置误差 (3维)
// δv: 速度误差 (3维)
// δθ: 姿态误差 (3维,四元数误差用旋转向量表示)
// δba: 加速度计偏差 (3维)
// δbg: 陀螺仪偏差 (3维)
EKF预测步骤
// modules/localization/msf/local_integ/localization_integ_impl.cc
void LocalizationIntegImpl::ImuMeasureUpdate(const ImuData& imu) {
// Step 1: 状态传播 (非线性状态方程)
SinsPva sins_pva_new;
// 1.1 姿态更新 (四元数微分方程)
Eigen::Vector3d omega_b = imu.angular_velocity - sins_pva_.gyro_bias;
Eigen::Quaterniond dq = Eigen::Quaterniond(
1.0, 0.5*omega_b.x()*dt, 0.5*omega_b.y()*dt, 0.5*omega_b.z()*dt);
sins_pva_new.qbn = sins_pva_.qbn * dq;
sins_pva_new.qbn.normalize();
// 1.2 速度更新
Eigen::Vector3d f_b = imu.linear_acceleration - sins_pva_.acc_bias;
Eigen::Vector3d f_n = sins_pva_.qbn.toRotationMatrix() * f_b; // 转到导航系
Eigen::Vector3d g_n(0, 0, -9.80665); // 重力加速度
sins_pva_new.vel = sins_pva_.vel + (f_n + g_n) * dt;
// 1.3 位置更新
sins_pva_new.pos = sins_pva_.pos + sins_pva_.vel * dt +
0.5 * (f_n + g_n) * dt * dt;
// 1.4 IMU偏差保持不变 (一阶马尔科夫过程)
sins_pva_new.gyro_bias = sins_pva_.gyro_bias;
sins_pva_new.acc_bias = sins_pva_.acc_bias;
// Step 2: 误差协方差传播 (线性化)
// P⁻ = F × P⁺ × Fᵀ + Q
// 2.1 计算状态转移雅可比矩阵 F (15×15)
Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Zero();
// F的分块结构:
// ┌─────────────────────────────────────┐
// │ I I*dt 0 0 0 │ 位置行
// │ 0 I -C*[f_b×] 0 -C │ 速度行
// │ 0 0 I 0 -I │ 姿态行
// │ 0 0 0 I 0 │ 加速度计偏差行
// │ 0 0 0 0 I │ 陀螺仪偏差行
// └─────────────────────────────────────┘
// 位置行
F.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
F.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity() * dt;
// 速度行
F.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity();
Eigen::Matrix3d C_bn = sins_pva_.qbn.toRotationMatrix();
F.block<3, 3>(3, 6) = -C_bn * SkewSymmetric(f_b);
F.block<3, 3>(3, 12) = -C_bn * dt;
// 姿态行
F.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity();
F.block<3, 3>(6, 9) = -Eigen::Matrix3d::Identity() * dt;
// IMU偏差行
F.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity();
F.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity();
// 2.2 离散化 (一阶近似)
Eigen::Matrix<double, 15, 15> Phi = Eigen::Matrix<double, 15, 15>::Identity() + F * dt;
// 2.3 过程噪声协方差 Q (15×15)
Eigen::Matrix<double, 15, 15> Q = Eigen::Matrix<double, 15, 15>::Zero();
// 加速度计噪声
double sigma_acc = 0.01; // m/s²
Q.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * sigma_acc * sigma_acc * dt * dt;
// 陀螺仪噪声
double sigma_gyro = 0.001; // rad/s
Q.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * sigma_gyro * sigma_gyro * dt * dt;
// 加速度计偏差随机游走
double sigma_acc_bias = 0.0001;
Q.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * sigma_acc_bias * sigma_acc_bias * dt;
// 陀螺仪偏差随机游走
double sigma_gyro_bias = 0.00001;
Q.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity() * sigma_gyro_bias * sigma_gyro_bias * dt;
// 2.4 协方差更新
P_ = Phi * P_ * Phi.transpose() + Q;
// 更新状态
sins_pva_ = sins_pva_new;
}
辅助函数:
// 反对称矩阵 (用于叉乘)
Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d& v) {
Eigen::Matrix3d m;
m << 0, -v.z(), v.y(),
v.z(), 0, -v.x(),
-v.y(), v.x(), 0;
return m;
}
// 示例:
// v = [vx, vy, vz]'
// v × w = SkewSymmetric(v) * w
EKF更新步骤 (GPS观测)
void LocalizationIntegImpl::GnssMeasureUpdate(const GnssBestPose& gnss) {
// Step 1: 观测向量 z (3维位置)
Eigen::Vector3d z_gnss(gnss.latitude, gnss.longitude, gnss.altitude);
// Step 2: 计算观测预测值 (从当前状态预测观测)
Eigen::Vector3d z_pred = sins_pva_.pos; // h(x) = x.pos
// Step 3: 新息 y
Eigen::Vector3d y = z_gnss - z_pred;
// Step 4: 观测雅可比矩阵 H (3×15)
// h(x) = x.pos, 所以 H = [I_3×3, 0_3×12]
Eigen::Matrix<double, 3, 15> H = Eigen::Matrix<double, 3, 15>::Zero();
H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
// Step 5: 观测噪声协方差 R (3×3)
Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
R(0, 0) = gnss.latitude_std * gnss.latitude_std;
R(1, 1) = gnss.longitude_std * gnss.longitude_std;
R(2, 2) = gnss.altitude_std * gnss.altitude_std;
// Step 6: 新息协方差 S
Eigen::Matrix3d S = H * P_ * H.transpose() + R;
// Step 7: 卡尔曼增益 K (15×3)
Eigen::Matrix<double, 15, 3> K = P_ * H.transpose() * S.inverse();
// Step 8: 状态误差更新
Eigen::Matrix<double, 15, 1> delta_x = K * y;
// Step 9: 状态修正
// 位置修正
sins_pva_.pos += delta_x.segment<3>(0);
// 速度修正
sins_pva_.vel += delta_x.segment<3>(3);
// 姿态修正 (小角度近似)
Eigen::Vector3d delta_theta = delta_x.segment<3>(6);
Eigen::Quaterniond dq_corr(1.0, 0.5*delta_theta.x(),
0.5*delta_theta.y(), 0.5*delta_theta.z());
sins_pva_.qbn = sins_pva_.qbn * dq_corr;
sins_pva_.qbn.normalize();
// IMU偏差修正
sins_pva_.acc_bias += delta_x.segment<3>(9);
sins_pva_.gyro_bias += delta_x.segment<3>(12);
// Step 10: 协方差更新
Eigen::Matrix<double, 15, 15> I_KH =
Eigen::Matrix<double, 15, 15>::Identity() - K * H;
P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
// 使用Joseph形式保证数值稳定性
}
卡尔曼增益的物理意义
K = P⁻ × Hᵀ × (H × P⁻ × Hᵀ + R)⁻¹
分析:
1. 如果R很大(观测不可靠):
K → 0, 更相信预测, 忽略观测
2. 如果P⁻很小(预测很可靠):
K → 0, 更相信预测
3. 如果P⁻很大(预测不可靠):
K → Hᵀ × (H × H' + R)⁻¹, 更相信观测
4. 最优情况:
K自动权衡预测和观测的可靠性
完整使用示例
// 定位系统中的EKF应用
class LocalizationEKF {
public:
void Init(const Eigen::Vector3d& init_pos,
const Eigen::Vector3d& init_vel,
const Eigen::Quaterniond& init_quat) {
// 初始化状态
sins_pva_.pos = init_pos;
sins_pva_.vel = init_vel;
sins_pva_.qbn = init_quat;
sins_pva_.acc_bias.setZero();
sins_pva_.gyro_bias.setZero();
// 初始化协方差
P_.setIdentity();
P_.block<3, 3>(0, 0) *= 1.0; // 位置初始不确定性 1m²
P_.block<3, 3>(3, 3) *= 0.1; // 速度初始不确定性 0.1(m/s)²
P_.block<3, 3>(6, 6) *= 0.01; // 姿态初始不确定性 0.01rad²
P_.block<3, 3>(9, 9) *= 0.001; // 加速度计偏差
P_.block<3, 3>(12, 12) *= 0.0001; // 陀螺仪偏差
}
void Process(const ImuData& imu,
const std::optional<GnssBestPose>& gnss) {
// IMU预测 (100Hz)
ImuMeasureUpdate(imu);
// GNSS更新 (1Hz)
if (gnss.has_value()) {
GnssMeasureUpdate(gnss.value());
}
}
LocalizationEstimate GetEstimate() const {
LocalizationEstimate est;
est.pose.position = sins_pva_.pos;
est.pose.linear_velocity = sins_pva_.vel;
est.pose.orientation = sins_pva_.qbn;
est.pose.linear_acceleration_vrf = sins_pva_.acc_bias;
est.pose.angular_velocity_vrf = sins_pva_.gyro_bias;
// 不确定性
est.uncertainty.position_std_dev.x = std::sqrt(P_(0, 0));
est.uncertainty.position_std_dev.y = std::sqrt(P_(1, 1));
est.uncertainty.position_std_dev.z = std::sqrt(P_(2, 2));
return est;
}
private:
SinsPva sins_pva_; // 状态
Eigen::Matrix<double, 15, 15> P_; // 协方差
};
// 使用
LocalizationEKF ekf;
ekf.Init(init_pos, init_vel, init_quat);
while (true) {
ImuData imu = ReadImuData(); // 100Hz
std::optional<GnssBestPose> gnss = ReadGnssData(); // 1Hz
ekf.Process(imu, gnss);
LocalizationEstimate est = ekf.GetEstimate();
PublishLocalization(est);
}
性能指标
典型精度 (在良好GNSS条件下):
- 位置精度: 0.05-0.10米 (RMS)
- 速度精度: 0.02-0.05m/s (RMS)
- 姿态精度: 0.1-0.2度 (RMS)
计算性能:
- 预测步骤: 0.1-0.2ms
- 更新步骤: 0.5-1.0ms
- 总延迟: < 2ms
优势:
- 高频IMU预测保证平滑性
- 低频GNSS更新消除累积误差
- 协方差自动权衡传感器可靠性
S = H × P⁻ × Hᵀ + R (新息协方差)
K = P⁻ × Hᵀ × S⁻¹ (卡尔曼增益)
x̂⁺ = x̂⁻ + K × y (状态更新)
P⁺ = (I - K × H) × P⁻ (协方差更新)
其中:
z: 观测值
H: 观测矩阵
R: 观测噪声协方差
K: 卡尔曼增益
y: 新息（实际观测与预测观测的差）
### 8.4 在预测模块中的应用
**Obstacle类中的卡尔曼滤波器**:
```cpp
class Obstacle {
private:
// 卡尔曼滤波器
// 状态维度: 6 [x, y, vx, vy, ax, ay]
// 观测维度: 2 [x, y]
// 控制维度: 0 (无控制输入)
KalmanFilter<double, 6, 2, 0> kf_;
};
状态向量定义:
x = [x, y, vx, vy, ax, ay]ᵀ
其中:
x, y: 位置
vx, vy: 速度
ax, ay: 加速度
状态转移矩阵F (常速度模型):
F = [1 0 Δt 0 0.5Δt² 0 ]
[0 1 0 Δt 0 0.5Δt²]
[0 0 1 0 Δt 0 ]
[0 0 0 1 0 Δt ]
[0 0 0 0 1 0 ]
[0 0 0 0 0 1 ]
其中 Δt 为时间间隔（0.1秒）
观测矩阵H:
H = [1 0 0 0 0 0]
[0 1 0 0 0 0]
只观测位置，不直接观测速度和加速度
参数配置:
// prediction_gflags.h
FLAGS_enable_kf_tracking: true
FLAGS_q_var: 0.01 // 过程噪声方差
FLAGS_r_var: 0.1 // 观测噪声方差
FLAGS_p_var: 0.1 // 初始协方差
9. 总结
9.1 预测模块核心要点
- 四层架构: Container → Scenario → Evaluator → Predictor
- 9种评估器: 覆盖各种场景和障碍物类型
- LSTM预测: 结合语义地图和历史轨迹，预测3秒轨迹
- VectorNet: 向量化地图表示，高效编码环境信息
- 交互预测: 考虑主车规划轨迹，实现联合预测规划
9.2 定位模块核心要点
- 三种方法: RTK、MSF、NDT，适应不同场景
- RTK定位: GPS+IMU，厘米级精度，适合开阔场景
- MSF融合: GPS+IMU+LiDAR，全场景高精度
- NDT配准: 正态分布变换，GPS失效时的备选方案
- 卡尔曼滤波: 最优状态估计，融合多源观测
9.3 关键算法总结
LSTM轨迹预测:
输入: 224×224×3语义地图 + 20帧历史轨迹
网络: CNN特征提取 + LSTM时序建模
输出: 30个未来轨迹点（3秒，0.1秒间隔）
性能: 10-20ms (GPU)
NDT点云配准:
原理: 正态分布变换
算法: 牛顿法优化
参数: 分辨率1.0m, 最大迭代10次
性能: 50-100ms
精度: 分米级
卡尔曼滤波:
状态: 6维 [x, y, vx, vy, ax, ay]
观测: 2维 [x, y]
模型: 常速度模型
应用: 轨迹平滑、速度估计
9.4 性能指标
预测模块:
预测时长: 8秒
预测频率: 10Hz
时间分辨率: 0.1秒
推理时间: 10-50ms (取决于模型)
定位模块:
RTK精度: < 0.05m (横向)
MSF精度: < 0.1m (横向)
NDT精度: < 0.2m (横向)
更新频率: 100Hz (IMU主频)
定位延迟: < 50ms
下一部分预告:第三部分: 规划控制 - 场景化规划、Piecewise Jerk、LQR/PID/MPC
参考资料与引用
官方资源
-
Apollo 官方 GitHub 仓库
https://github.com/ApolloAuto/apollo- Apollo 开源项目主仓库,包含完整源代码
-
Apollo 官方文档
https://apollo.baidu.com/docs- 官方技术文档和开发指南
-
Apollo 开发者社区
https://apollo.baidu.com/community- 官方开发者论坛和技术交流平台
技术规范与标准
-
ISO 26262 - 道路车辆功能安全标准
https://www.iso.org/standard/68383.html -
ISO 21448 (SOTIF) - 预期功能安全标准
https://www.iso.org/standard/77490.html
学术论文与技术资源
-
CenterPoint: Center-based 3D Object Detection and Tracking
Yin, T., Zhou, X., & Krähenbühl, P. (2021)
https://arxiv.org/abs/2006.11275 -
BEVFormer: Learning Bird’s-Eye-View Representation from Multi-Camera Images
Li, Z., et al. (2022)
https://arxiv.org/abs/2203.17270 -
OpenDRIVE 地图标准
https://www.asam.net/standards/detail/opendrive/
开源工具与库
-
Bazel 构建系统
https://bazel.build/ -
Fast-DDS (eProsima)
https://www.eprosima.com/index.php/products-all/eprosima-fast-dds -
PROJ 坐标转换库
https://proj.org/ -
TensorRT 开发指南
https://docs.nvidia.com/deeplearning/tensorrt/ -
PCL 点云库文档
https://pointclouds.org/ -
IPOPT 优化求解器
https://coin-or.github.io/Ipopt/
说明
本文档内容整理自上述官方资料、开源代码以及相关技术文档。所有代码示例和技术细节均基于 Apollo 9.0/10.0 版本。如需获取最新信息,请访问 Apollo 官方网站和 GitHub 仓库。
版权说明
Apollo® 是百度公司的注册商标。本文档为基于开源项目的非官方技术研究文档,仅供学习参考使用。
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
18
0- 0
已为社区贡献5条内容
所有评论(0)