第8章-感知融合与场景理解

第8章 感知融合与场景理解

免责声明

本文档为学术研究与技术学习目的而编写，基于Autoware开源项目（Apache 2.0许可证）的源码分析。文档内容力求准确，但不保证完全无误，仅供参考。读者在实际应用时应以官方文档和源码为准。

本文档不涉及任何商业用途，所有代码示例均来自开源项目。如有侵权，请联系删除。

摘要

本章深入解析Autoware中感知融合与场景理解模块的架构设计与实现细节。感知融合负责整合来自3D目标检测、2D图像识别、跟踪等多个感知源的信息，消除冗余、补全属性、统一输出。场景理解模块在融合结果基础上提取交通场景要素，建模交通参与者关系，评估场景危险度，为规划决策提供结构化的环境认知。本章还介绍感知不确定性建模方法，确保下游模块能够合理处理感知误差。该模块位于Autoware感知系统的最后一环，是连接原始传感器数据与规划决策的关键桥梁。

目录

• 8.1 感知融合架构
• 8.2 目标级融合
  • 8.2.1 3D与2D检测融合
  • 8.2.2 冗余目标消除
  • 8.2.3 目标属性补全
• 8.3 场景理解
  • 8.3.1 场景要素提取
  • 8.3.2 交通参与者关系建模
  • 8.3.3 场景危险度评估
• 8.4 感知不确定性建模
  • 8.4.1 检测不确定性
  • 8.4.2 跟踪不确定性
  • 8.4.3 不确定性传播](#843-不确定性传播)
• 参考资料

8.1 感知融合架构

感知融合模块是Autoware感知系统的最后一级处理环节，位于3D目标检测、2D图像识别、多目标跟踪等子模块之后，负责将来自不同传感器、不同算法的检测结果统一整合为一致的环境表达。

融合模块在感知流水线中的位置

传感器数据
↓
┌─────────────────────────────────────┐
│ 预处理（点云滤波、图像去畸变） │
└─────────────────────────────────────┘
↓
┌─────────────────────────────────────┐
│ 3D检测（LiDAR点云聚类/深度学习） │
│ 2D检测（摄像头目标识别） │
└─────────────────────────────────────┘
↓
┌─────────────────────────────────────┐
│ 多目标跟踪（卡尔曼滤波、数据关联） │
└─────────────────────────────────────┘
↓
┌─────────────────────────────────────┐
│ ★ 感知融合（本章） │
│ – 3D与2D融合 │
│ – 冗余消除 │
│ – 属性补全 │
│ – 场景理解 │
└─────────────────────────────────────┘
↓
规划决策模块

融合架构设计

📁 源码路径: universe/autoware_universe/perception/

感知融合模块主要包含以下组件：

数据流与消息接口

📨 订阅Topic:

• /perception/object_recognition/detection/objects (autoware_perception_msgs/msg/DetectedObjects) – 3D检测结果
• /perception/object_recognition/detection/rois (tier4_perception_msgs/msg/DetectedObjectsWithFeature) – 2D检测结果
• /perception/object_recognition/tracking/objects (autoware_perception_msgs/msg/TrackedObjects) – 跟踪结果
• /perception/traffic_light_recognition/traffic_signals (autoware_perception_msgs/msg/TrafficSignalArray) – 交通灯识别
• /map/vector_map (autoware_map_msgs/msg/LaneletMapBin) – 高精地图

📤 发布Topic:

• /perception/object_recognition/objects (autoware_perception_msgs/msg/PredictedObjects) – 融合后的目标列表（含预测轨迹）
• /perception/scene_understanding/scene (自定义消息) – 场景理解结果

8.2 目标级融合

目标级融合是将来自不同传感器、不同算法的检测结果在目标（Object）层面进行关联与整合的过程。其核心挑战是判断不同检测结果是否对应同一个真实物体，以及如何从多个观测中提取最优估计。

8.2.1 3D与2D检测融合

3D检测（LiDAR）与2D检测（摄像头）具有互补特性：

• 3D检测优势：精确的距离、位置、尺寸估计
• 2D检测优势：精确的目标分类、颜色、纹理信息

融合策略

📁 源码路径: universe/autoware_universe/perception/detection_by_tracker/src/

投影关联法：将3D边界框投影到图像平面，与2D检测框计算IoU（Intersection over Union）

// 3D到2D投影关联伪代码
double calculate_2d_iou(const BoundingBox3D& box_3d,
const BoundingBox2D& box_2d,
const CameraInfo& camera_info) {
// 1. 将3D框的8个顶点投影到图像平面
std::vector<cv::Point2f> projected_points;
for (const auto& corner : box_3d.corners) {
cv::Point2f img_point = project_to_image(corner, camera_info);
projected_points.push_back(img_point);
}

// 2. 计算投影多边形的外接矩形
cv::Rect projected_rect = cv::boundingRect(projected_points);

// 3. 计算与2D检测框的IoU
double iou = calculate_iou(projected_rect, box_2d.rect);
return iou;
}

关联决策：

# 融合参数配置
fusion_params:
min_iou_threshold: 0.3 # 最小IoU阈值
max_distance_threshold: 50.0 # 最大关联距离（米）
use_class_constraint: true # 是否使用类别约束

属性融合规则：

多摄像头融合

对于多摄像头系统（前视、左视、右视），需要先在各自相机坐标系下完成3D-2D融合，再统一到车辆坐标系。

// 多摄像头融合流程
struct FusedObject {
DetectedObject object_3d; // 3D检测结果
std::map<std::string, DetectedObject2D> objects_2d; // 各相机的2D检测

// 融合函数
void fuse() {
// 遍历所有相机
for (const auto& [camera_id, obj_2d] : objects_2d) {
if (calculate_2d_iou(object_3d, obj_2d, camera_infos[camera_id]) > threshold) {
// 更新类别（选择置信度最高的）
if (obj_2d.classification.confidence > object_3d.classification.confidence) {
object_3d.classification = obj_2d.classification;
}
}
}
}
};

8.2.2 冗余目标消除

多传感器、多算法可能对同一物体产生多个检测结果，需要通过数据关联消除冗余。

非极大值抑制（NMS）

在同一传感器的检测结果中，使用NMS消除重叠检测：

// 3D NMS算法
std::vector<DetectedObject> nms_3d(
const std::vector<DetectedObject>& detections,
double iou_threshold = 0.5) {

std::vector<DetectedObject> result;
std::vector<bool> suppressed(detections.size(), false);

// 按置信度降序排序
auto sorted_indices = sort_by_confidence(detections);

for (size_t i = 0; i < sorted_indices.size(); ++i) {
if (suppressed[i]) continue;

result.push_back(detections[sorted_indices[i]]);

// 抑制与当前目标高度重叠的其他检测
for (size_t j = i + 1; j < sorted_indices.size(); ++j) {
if (suppressed[j]) continue;

double iou = calculate_3d_iou(
detections[sorted_indices[i]],
detections[sorted_indices[j]]
);

if (iou > iou_threshold) {
suppressed[j] = true;
}
}
}

return result;
}

跨传感器数据关联

使用匈牙利算法（Hungarian Algorithm）或全局最近邻（GNN）进行跨传感器目标关联：

// 代价矩阵构建
Eigen::MatrixXd build_cost_matrix(
const std::vector<DetectedObject>& objects_a,
const std::vector<DetectedObject>& objects_b) {

Eigen::MatrixXd cost(objects_a.size(), objects_b.size());

for (size_t i = 0; i < objects_a.size(); ++i) {
for (size_t j = 0; j < objects_b.size(); ++j) {
// 综合考虑位置、尺寸、类别差异
double pos_dist = (objects_a[i].position – objects_b[j].position).norm();
double size_diff = std::abs(objects_a[i].size.x – objects_b[j].size.x);
bool class_match = (objects_a[i].classification.label ==
objects_b[j].classification.label);

cost(i, j) = pos_dist + size_diff * 0.5 + (class_match ? 0 : 10.0);
}
}

return cost;
}

// 匈牙利算法求解最优关联
std::vector<std::pair<int, int>> hungarian_matching(const Eigen::MatrixXd& cost);

关联阈值配置：

association_params:
max_distance: 2.0 # 最大关联距离（米）
max_size_diff: 1.0 # 最大尺寸差异（米）
require_class_match: false # 是否强制类别匹配

8.2.3 目标属性补全

融合后的目标可能缺失某些属性，需要从地图、历史信息中补全。

属性来源优先级

基于地图的属性补全

对于静态目标（交通灯、标志牌），可从高精地图中获取精确属性：

// 从地图补全交通灯属性
void complete_traffic_light_attributes(
DetectedObject& object,
const LaneletMap& map) {

// 1. 在地图中搜索附近的交通灯
auto nearby_lights = map.query_traffic_lights(
object.position,
5.0 // 搜索半径
);

// 2. 匹配最近的交通灯
if (!nearby_lights.empty()) {
auto matched_light = find_nearest(nearby_lights, object.position);

// 3. 更新精确位置和朝向
object.position = matched_light.position;
object.orientation = matched_light.orientation;
object.map_id = matched_light.id; // 记录地图ID
}
}

历史信息补全

利用跟踪历史补全短暂遮挡时的目标信息：

// 基于历史轨迹预测当前状态
DetectedObject predict_from_history(
const std::vector<DetectedObject>& history,
double dt) {

if (history.size() < 2) return history.back();

// 简单线性预测
const auto& last = history[history.size() – 1];
const auto& second_last = history[history.size() – 2];

DetectedObject predicted = last;
predicted.position = last.position + last.velocity * dt;
predicted.velocity = (last.position – second_last.position) /
(last.timestamp – second_last.timestamp);

return predicted;
}

8.3 场景理解

场景理解模块在融合后的目标列表基础上，提取结构化的交通场景信息，包括场景要素、交通参与者关系、危险度评估等，为规划决策提供高层语义信息。

8.3.1 场景要素提取

车道关系分析

判断每个目标所在的车道，以及与自车的相对车道关系：

// 车道关系判断
enum LaneRelation {
SAME_LANE, // 同车道
LEFT_LANE, // 左侧车道
RIGHT_LANE, // 右侧车道
OPPOSITE_LANE, // 对向车道
UNKNOWN
};

LaneRelation get_lane_relation(
const DetectedObject& object,
const Pose& ego_pose,
const LaneletMap& map) {

// 1. 查询目标所在车道
auto object_lanelet = map.query_lanelet(object.position);
auto ego_lanelet = map.query_lanelet(ego_pose.position);

if (!object_lanelet || !ego_lanelet) return UNKNOWN;

// 2. 判断车道关系
if (object_lanelet->id == ego_lanelet->id) {
return SAME_LANE;
} else if (map.is_left_of(object_lanelet, ego_lanelet)) {
return LEFT_LANE;
} else if (map.is_right_of(object_lanelet, ego_lanelet)) {
return RIGHT_LANE;
} else if (map.is_opposite_direction(object_lanelet, ego_lanelet)) {
return OPPOSITE_LANE;
}

return UNKNOWN;
}

交叉口场景识别

识别车辆是否处于交叉口，以及交叉口类型：

// 交叉口场景信息
struct IntersectionScene {
bool in_intersection; // 是否在交叉口内
std::string intersection_type; // 类型：crosswalk/traffic_light/stop_sign
double distance_to_intersection; // 距离交叉口距离
std::vector<DetectedObject> conflicting_objects; // 冲突目标
};

IntersectionScene analyze_intersection_scene(
const Pose& ego_pose,
const std::vector<DetectedObject>& objects,
const LaneletMap& map) {

IntersectionScene scene;

// 1. 查询前方交叉口
auto upcoming_intersection = map.query_upcoming_intersection(
ego_pose,
50.0 // 前视距离
);

if (!upcoming_intersection) {
scene.in_intersection = false;
return scene;
}

scene.distance_to_intersection = upcoming_intersection->distance;
scene.intersection_type = upcoming_intersection->type;

// 2. 筛选冲突目标（可能与自车路径交叉的目标）
for (const auto& obj : objects) {
if (is_conflicting_path(obj, ego_pose, upcoming_intersection)) {
scene.conflicting_objects.push_back(obj);
}
}

return scene;
}

8.3.2 交通参与者关系建模

建模自车与其他交通参与者的空间-时间关系，预测潜在交互。

相对运动状态

计算目标相对于自车的运动状态：

// 相对运动状态
struct RelativeMotion {
double relative_velocity; // 相对速度（正值=接近，负值=远离）
double time_to_collision; // 碰撞时间（TTC）
double lateral_distance; // 横向距离
double longitudinal_distance; // 纵向距离
};

RelativeMotion calculate_relative_motion(
const DetectedObject& object,
const VehicleState& ego_state) {

RelativeMotion rel;

// 1. 计算相对位置（转换到自车坐标系）
Eigen::Vector2d rel_pos = transform_to_ego_frame(
object.position,
ego_state.pose
);

rel.longitudinal_distance = rel_pos.x();
rel.lateral_distance = rel_pos.y();

// 2. 计算相对速度
Eigen::Vector2d rel_vel = transform_to_ego_frame(
object.velocity,
ego_state.pose
) – Eigen::Vector2d(ego_state.velocity, 0);

rel.relative_velocity = rel_vel.norm();

// 3. 计算碰撞时间（TTC）
if (rel.relative_velocity > 0.1 && rel.longitudinal_distance > 0) {
rel.time_to_collision = rel.longitudinal_distance / rel.relative_velocity;
} else {
rel.time_to_collision = std::numeric_limits<double>::infinity();
}

return rel;
}

交互意图识别

基于目标的运动轨迹和位置，推断其可能的驾驶意图：

// 驾驶意图类型
enum DrivingIntent {
LANE_KEEPING, // 车道保持
LANE_CHANGE_LEFT, // 向左变道
LANE_CHANGE_RIGHT, // 向右变道
TURNING_LEFT, // 左转
TURNING_RIGHT, // 右转
STOPPING, // 停车
YIELDING, // 让行
UNKNOWN
};

DrivingIntent infer_intent(
const DetectedObject& object,
const std::vector<DetectedObject>& history,
const LaneletMap& map) {

// 1. 速度分析
if (object.velocity.norm() < 0.5) {
return STOPPING;
}

// 2. 横向运动分析
if (history.size() >= 5) {
double lateral_displacement = calculate_lateral_displacement(history);

if (lateral_displacement > 0.5) {
return (lateral_displacement > 0) ? LANE_CHANGE_LEFT : LANE_CHANGE_RIGHT;
}
}

// 3. 转向灯信号（如果检测到）
if (object.has_turn_signal) {
if (object.turn_signal == TurnSignal::LEFT) return TURNING_LEFT;
if (object.turn_signal == TurnSignal::RIGHT) return TURNING_RIGHT;
}

// 4. 地图上下文
auto current_lanelet = map.query_lanelet(object.position);
if (current_lanelet && current_lanelet->is_turn_lane) {
return (current_lanelet->turn_direction == "left") ?
TURNING_LEFT : TURNING_RIGHT;
}

return LANE_KEEPING;
}

8.3.3 场景危险度评估

量化当前场景的危险程度，为规划决策提供风险感知。

危险度评分模型

综合考虑多个危险因素计算场景危险度：

// 场景危险度评估
struct RiskAssessment {
double overall_risk; // 总体危险度 [0, 1]
double collision_risk; // 碰撞风险
double traffic_rule_risk; // 违反交规风险
double comfort_risk; // 舒适性风险
std::vector<std::string> risk_factors; // 风险因素列表
};

RiskAssessment assess_scene_risk(
const std::vector<DetectedObject>& objects,
const VehicleState& ego_state,
const IntersectionScene& intersection_scene,
const LaneletMap& map) {

RiskAssessment risk;

// 1. 碰撞风险评估
for (const auto& obj : objects) {
auto rel_motion = calculate_relative_motion(obj, ego_state);

// TTC阈值判断
if (rel_motion.time_to_collision < 3.0) {
risk.collision_risk = std::max(
risk.collision_risk,
1.0 – rel_motion.time_to_collision / 3.0
);
risk.risk_factors.push_back("Low TTC with object " + obj.id);
}

// 横向距离判断
if (std::abs(rel_motion.lateral_distance) < 1.5) {
risk.collision_risk = std::max(risk.collision_risk, 0.7);
risk.risk_factors.push_back("Close lateral distance");
}
}

// 2. 交通规则风险
if (intersection_scene.in_intersection) {
// 检查是否有冲突目标
if (!intersection_scene.conflicting_objects.empty()) {
risk.traffic_rule_risk = 0.6;
risk.risk_factors.push_back("Conflicting objects in intersection");
}

// 检查交通灯状态
auto traffic_signal = get_current_traffic_signal(ego_state, map);
if (traffic_signal == TrafficSignal::RED) {
risk.traffic_rule_risk = 0.9;
risk.risk_factors.push_back("Red traffic light");
}
}

// 3. 舒适性风险（基于加速度需求）
double required_deceleration = calculate_required_deceleration(
ego_state,
objects
);
if (required_deceleration > 3.0) { // m/s²
risk.comfort_risk = 0.5;
risk.risk_factors.push_back("High deceleration required");
}

// 4. 综合危险度（加权求和）
risk.overall_risk =
0.5 * risk.collision_risk +
0.3 * risk.traffic_rule_risk +
0.2 * risk.comfort_risk;

return risk;
}

危险场景分类

预定义典型危险场景，快速识别：

# 危险场景库配置
risk_scenarios:
– name: "cut_in"
description: "侧方车辆切入"
conditions:
– lateral_distance < 2.0
– relative_velocity > 5.0
– intent == LANE_CHANGE
risk_level: 0.8

– name: "pedestrian_crossing"
description: "行人横穿"
conditions:
– object_class == PEDESTRIAN
– longitudinal_distance < 10.0
– lateral_velocity > 0.5
risk_level: 0.9

– name: "rear_end"
description: "前车急刹"
conditions:
– same_lane == true
– ttc < 2.0
– front_vehicle_deceleration > 4.0
risk_level: 0.85

8.4 感知不确定性建模

感知结果存在固有的不确定性（传感器噪声、算法误差、遮挡等），需要量化并传播至下游模块，确保规划决策能够合理处理感知误差。

8.4.1 检测不确定性

位置不确定性

使用协方差矩阵表示目标位置的不确定性：

// 位置不确定性（协方差矩阵）
struct PositionUncertainty {
Eigen::Matrix3d covariance; // 3×3协方差矩阵 (x, y, z)

// 从检测置信度估计不确定性
static PositionUncertainty from_detection(
const DetectedObject& object,
SensorType sensor_type) {

PositionUncertainty uncertainty;

// 根据传感器类型和置信度设置协方差
double base_std = 0.0;
switch (sensor_type) {
case SensorType::LIDAR:
base_std = 0.1; // LiDAR基础标准差（米）
break;
case SensorType::CAMERA:
base_std = 0.5; // 摄像头基础标准差
break;
case SensorType::RADAR:
base_std = 0.3; // 毫米波雷达基础标准差
break;
}

// 置信度越低，不确定性越高
double std = base_std / object.classification.confidence;

uncertainty.covariance = Eigen::Matrix3d::Identity() * (std * std);

return uncertainty;
}
};

分类不确定性

使用类别概率分布表示分类不确定性：

// 分类不确定性（概率分布）
struct ClassificationUncertainty {
std::map<std::string, double> class_probabilities; // 类别概率

double entropy() const {
// 计算信息熵（衡量不确定性）
double H = 0.0;
for (const auto& [cls, prob] : class_probabilities) {
if (prob > 1e-6) {
H -= prob * std::log2(prob);
}
}
return H;
}

bool is_confident() const {
// 判断分类是否可信（最大概率>阈值）
double max_prob = 0.0;
for (const auto& [cls, prob] : class_probabilities) {
max_prob = std::max(max_prob, prob);
}
return max_prob > 0.8;
}
};

8.4.2 跟踪不确定性

跟踪器通过卡尔曼滤波等方法维护状态估计的协方差：

// 跟踪状态不确定性
struct TrackingUncertainty {
Eigen::MatrixXd state_covariance; // 状态协方差（位置、速度、加速度）
int track_age; // 跟踪帧数
double track_confidence; // 跟踪置信度

// 根据跟踪历史调整不确定性
void update_confidence() {
// 跟踪时间越长，置信度越高
track_confidence = std::min(
1.0,
static_cast<double>(track_age) / 10.0
);

// 但协方差增长会降低置信度
double trace = state_covariance.trace();
if (trace > 1.0) {
track_confidence *= std::exp(–trace);
}
}
};

遮挡情况下的不确定性增长

当目标被遮挡时，不确定性会随时间增长：

// 遮挡期间不确定性增长
void propagate_uncertainty_during_occlusion(
TrackingUncertainty& uncertainty,
double dt,
const MotionModel& model) {

// 过程噪声（与时间相关）
Eigen::MatrixXd Q = model.process_noise * dt;

// 协方差传播（无观测更新）
Eigen::MatrixXd F = model.state_transition_matrix(dt);
uncertainty.state_covariance =
F * uncertainty.state_covariance * F.transpose() + Q;

// 长时间遮挡会大幅降低置信度
uncertainty.track_confidence *= std::exp(–0.1 * dt);
}

8.4.3 不确定性传播

将感知不确定性传播至规划模块，确保规划决策考虑感知误差。

不确定性消息定义

扩展感知消息，包含不确定性信息：

# PredictedObjectWithUncertainty.msg
Header header
string object_id
geometry_msgs/PoseWithCovariance pose # 位置+协方差
geometry_msgs/TwistWithCovariance velocity # 速度+协方差
ClassificationWithProbability[] classifications # 类别+概率分布
PredictedPath[] predicted_paths # 预测轨迹
float64 existence_probability # 存在概率

规划决策中的不确定性处理

规划模块根据不确定性调整安全距离：

// 基于不确定性计算安全距离
double calculate_safe_distance(
const PredictedObject& object,
double ego_velocity) {

// 1. 提取位置不确定性（标准差）
Eigen::Vector3d pos_std =
object.pose.covariance.diagonal().cwiseSqrt();

// 2. 基础安全距离（基于速度）
double base_distance = ego_velocity * 2.0; // 2秒响应时间

// 3. 不确定性增量（3倍标准差，99.7%置信区间）
double uncertainty_margin = 3.0 * pos_std.x();

// 4. 总安全距离
return base_distance + uncertainty_margin;
}

多假设跟踪

对于高不确定性目标，维护多个假设：

// 多假设跟踪
struct MultiHypothesisTrack {
std::vector<TrackHypothesis> hypotheses;

struct TrackHypothesis {
DetectedObject object;
double probability; // 假设概率
TrackingUncertainty uncertainty;
};

// 规划时考虑所有高概率假设
std::vector<DetectedObject> get_high_probability_objects(
double prob_threshold = 0.1) const {

std::vector<DetectedObject> result;
for (const auto& hyp : hypotheses) {
if (hyp.probability > prob_threshold) {
result.push_back(hyp.object);
}
}
return result;
}
};

参考资料

官方文档

• Autoware Perception Documentation
• Autoware Perception Messages
• ROS 2 Sensor Fusion

源码仓库

• autoware_universe/perception
• tier4_perception_msgs

学术论文

• Multiple Sensor Fusion and Classification for Moving Object Detection and Tracking, IEEE Transactions on Intelligent Transportation Systems, 2016
• Uncertainty Estimation for Deep Neural Object Detectors in Safety-Critical Applications, IEEE ICRA, 2020
• Online Multi-Object Tracking with Dual Matching Attention Networks, ECCV, 2020

技术博客

• Multi-Sensor Fusion in Autonomous Driving
• Understanding Object Detection Uncertainty