第二部分：地图模块详解
免责声明
本文档仅供学习和技术研究使用,内容基于 Apollo 开源项目的公开资料和代码分析整理而成。文档中的技术实现细节、架构设计等内容可能随 Apollo 项目更新而变化。使用本文档所述技术方案时,请以 Apollo 官方最新文档为准。
声明事项:
- 本文档不构成任何商业使用建议
- 涉及自动驾驶技术的应用需遵守当地法律法规
- 作者不对因使用本文档内容产生的任何后果承担责任
- Apollo 为百度公司注册商标,本文档为非官方技术解析
1. 地图模块概述
1.1 地图模块架构
Apollo的地图模块是自动驾驶系统的基础设施，提供三层地图体系：
┌──────────────────────────────────────────────┐
│ Application Layer 应用层 │
│ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ Planning │ │ Control │ │ Perception│ │
│ └────┬─────┘ └────┬─────┘ └────┬─────┘ │
└───────┼─────────────┼─────────────┼──────────┘
│ │ │
┌───────▼─────────────▼─────────────▼──────────┐
│ Map Module 地图模块 │
│ ┌──────────┐ ┌──────────┐ ┌──────────┐ │
│ │ HDMap │ │ PncMap │ │RelativeMap│ │
│ │高精度地图 │ │规划地图 │ │ 相对地图 │ │
│ └────┬─────┘ └────┬─────┘ └────┬─────┘ │
└───────┼─────────────┼─────────────┼──────────┘
│ │ │
┌───────▼─────────────▼─────────────▼──────────┐
│ Spatial Index 空间索引 │
│ ┌──────────────────────────────────────┐ │
│ │ KD-Tree Based Fast Query System │ │
│ │ (亚毫秒级查询性能) │ │
│ └──────────────────────────────────────┘ │
└───────────────────────────────────────────────┘1.2 三大地图子系统
| 子系统 | 功能 | 精度 | 应用场景 |
|---|---|---|---|
| HDMap | 高精度静态地图 | 厘米级 | 城市道路、高速公路 |
| PncMap | 规划控制地图 | 厘米级 | 路径规划、轨迹控制 |
| RelativeMap | 实时相对地图 | 分米级 | 导航模式、感知融合 |
1.3 核心模块路径
modules/map/
├── hdmap/ # 高精度地图
│ ├── hdmap_common.h # 地图元素定义
│ ├── hdmap_impl.cc # 地图加载与查询
│ └── adapter/ # 格式适配器
│ └── opendrive_adapter.h
├── pnc_map/ # 规划控制地图
│ ├── path.h # 路径表示
│ └── route_segments.h # 路由段
├── relative_map/ # 相对地图
│ ├── relative_map.cc # 实时地图生成
│ └── navigation_lane.h # 导航车道
└── proto/ # 地图数据定义
├── map.proto
└── map_geometry.proto2. HDMap高精度地图系统
2.1 地图元素体系
HDMap包含14种核心地图元素，形成完整的道路语义网络：
2.1.1 核心类层次结构
cpp
// 文件: modules/map/hdmap/hdmap_common.h
// 1. 车道信息类 - 最核心的地图元素
class LaneInfo {
public:
explicit LaneInfo(const Lane& lane);
// 基本属性
const Id& id() const { return lane_.id(); }
const Lane& lane() const { return lane_; }
double total_length() const { return total_length_; }
// 几何查询接口
double Heading(const double s) const; // 航向角
double Curvature(const double s) const; // 曲率
void GetWidth(const double s, double* left_width, // 车道宽度
double* right_width) const;
double GetEffectiveWidth(const double s) const; // 有效宽度
// 空间查询接口
bool IsOnLane(const Vec2d& point) const; // 点是否在车道上
bool IsOnLane(const Box2d& box) const; // 框是否在车道上
// 投影与距离计算
bool GetProjection(const Vec2d& point, // 获取投影点
double* accumulate_s, // 沿车道的累积距离
double* lateral) const; // 横向偏移
double DistanceTo(const Vec2d& point) const; // 到点的距离
PointENU GetNearestPoint(const Vec2d& point, // 最近点
double* distance) const;
// 拓扑关系
const std::vector<Id>& successor_ids() const; // 后继车道
const std::vector<Id>& predecessor_ids() const; // 前驱车道
const std::vector<Id>& left_neighbor_forward_lane_ids() const; // 左邻车道
const std::vector<Id>& right_neighbor_forward_lane_ids() const; // 右邻车道
// Overlap重叠关系
const std::vector<OverlapInfoConstPtr>& signals() const; // 信号灯
const std::vector<OverlapInfoConstPtr>& crosswalks() const; // 人行横道
const std::vector<OverlapInfoConstPtr>& stop_signs() const; // 停止标志
const std::vector<OverlapInfoConstPtr>& junctions() const; // 路口
private:
// 内部数据结构
const Lane& lane_; // Protobuf数据
std::vector<Vec2d> points_; // 中心线点序列
std::vector<Vec2d> unit_directions_; // 单位方向向量
std::vector<double> headings_; // 航向角序列
std::vector<LineSegment2d> segments_; // 线段序列
std::vector<double> accumulated_s_; // 累积距离
// 空间索引 - KD树加速查询
std::vector<LaneSegmentBox> segment_box_list_;
std::unique_ptr<LaneSegmentKDTree> lane_segment_kdtree_;
double total_length_ = 0.0;
std::vector<SampledWidth> sampled_left_width_; // 采样左宽度
std::vector<SampledWidth> sampled_right_width_; // 采样右宽度
};2.1.2 路口信息类
cpp
// 2. 路口信息类
class JunctionInfo {
public:
explicit JunctionInfo(const Junction& junction);
const Id& id() const { return junction_.id(); }
const Junction& junction() const { return junction_; }
const Polygon2d& polygon() const { return polygon_; } // 路口多边形
const std::vector<Id>& OverlapStopSignIds() const; // 关联停止标志
private:
const Junction& junction_;
Polygon2d polygon_; // 路口边界多边形
std::vector<Id> overlap_stop_sign_ids_;
};
// KD树类型定义
using JunctionPolygonBox = ObjectWithAABox<JunctionInfo, Polygon2d>;
using JunctionPolygonKDTree = AABoxKDTree2d<JunctionPolygonBox>;2.1.3 交通信号灯
cpp
// 3. 信号灯信息类
class SignalInfo {
public:
explicit SignalInfo(const Signal& signal);
const Id& id() const { return signal_.id(); }
const Signal& signal() const { return signal_; }
const std::vector<LineSegment2d>& segments() const { return segments_; }
private:
const Signal& signal_;
std::vector<LineSegment2d> segments_; // 信号灯停止线
};
using SignalSegmentBox = ObjectWithAABox<SignalInfo, LineSegment2d>;
using SignalSegmentKDTree = AABoxKDTree2d<SignalSegmentBox>;2.1.4 人行横道
cpp
// 4. 人行横道信息类
class CrosswalkInfo {
public:
explicit CrosswalkInfo(const Crosswalk& crosswalk);
const Id& id() const { return crosswalk_.id(); }
const Crosswalk& crosswalk() const { return crosswalk_; }
const Polygon2d& polygon() const { return polygon_; }
private:
const Crosswalk& crosswalk_;
Polygon2d polygon_; // 人行横道多边形区域
};2.1.5 其他地图元素
cpp
// 5. 停止标志
class StopSignInfo { /* ... */ };
// 6. 让行标志
class YieldSignInfo { /* ... */ };
// 7. 禁行区域
class ClearAreaInfo { /* ... */ };
// 8. 减速带
class SpeedBumpInfo { /* ... */ };
// 9. 停车位
class ParkingSpaceInfo { /* ... */ };
// 10. 栅栏门
class BarrierGateInfo { /* ... */ };
// 11. 道路
class RoadInfo { /* ... */ };
// 12. 重叠区域
class OverlapInfo { /* ... */ };
// 13. RSU路侧单元
class RSUInfo { /* ... */ };
// 14. 规划路口
class PNCJunctionInfo { /* ... */ };2.2 HDMap核心实现
2.2.1 地图加载流程
cpp
// 文件: modules/map/hdmap/hdmap_impl.cc
int HDMapImpl::LoadMapFromFile(const std::string& map_filename) {
Clear();
// 1. 根据文件扩展名选择加载器
if (absl::EndsWith(map_filename, ".xml")) {
// OpenDRIVE格式 (行业标准)
if (!adapter::OpendriveAdapter::LoadData(map_filename, &map_)) {
return -1;
}
} else {
// Protobuf格式 (Apollo原生)
if (!cyber::common::GetProtoFromFile(map_filename, &map_)) {
return -1;
}
}
// 2. 从Protobuf加载到内存结构
return LoadMapFromProto(map_);
}
int HDMapImpl::LoadMapFromProto(const Map& map_proto) {
// 1. 构建哈希表索引
for (const auto& lane : map_.lane()) {
lane_table_[lane.id().id()].reset(new LaneInfo(lane));
}
for (const auto& junction : map_.junction()) {
junction_table_[junction.id().id()].reset(new JunctionInfo(junction));
}
for (const auto& signal : map_.signal()) {
signal_table_[signal.id().id()].reset(new SignalInfo(signal));
}
for (const auto& crosswalk : map_.crosswalk()) {
crosswalk_table_[crosswalk.id().id()].reset(new CrosswalkInfo(crosswalk));
}
// ... 其他元素类似加载
// 2. 建立车道与道路的关联
for (const auto& road_ptr_pair : road_table_) {
for (const auto& road_section : road_ptr_pair.second->sections()) {
for (const auto& lane_id : road_section.lane_id()) {
auto iter = lane_table_.find(lane_id.id());
if (iter != lane_table_.end()) {
iter->second->set_road_id(road_id);
iter->second->set_section_id(section_id);
}
}
}
}
// 3. 后处理 - 建立Overlap关系
for (const auto& lane_ptr_pair : lane_table_) {
lane_ptr_pair.second->PostProcess(*this);
}
// 4. 构建空间索引 - KD树
BuildLaneSegmentKDTree();
BuildJunctionPolygonKDTree();
BuildSignalSegmentKDTree();
BuildCrosswalkPolygonKDTree();
BuildStopSignSegmentKDTree();
BuildYieldSignSegmentKDTree();
BuildClearAreaPolygonKDTree();
BuildSpeedBumpSegmentKDTree();
BuildParkingSpacePolygonKDTree();
BuildPNCJunctionPolygonKDTree();
BuildAreaPolygonKDTree();
BuildBarrierGateSegmentKDTree();
return 0;
}关键步骤解析：
-
双格式支持：
- OpenDRIVE XML：工业标准，与其他系统互操作
- Protobuf：Apollo原生，性能更高
-
哈希表索引：O(1)时间复杂度的ID查询
-
拓扑关系建立：
- 车道连接关系（前驱/后继）
- Overlap重叠关系（信号灯、人行横道等）
-
空间索引构建：
- 为所有地图元素构建KD树
- 支持快速空间查询（见5.1节）
2.2.2 车道查询接口
cpp
// 根据坐标点查询附近车道
int HDMapImpl::GetLanes(const Vec2d& point, double distance,
std::vector<LaneInfoConstPtr>* lanes) const {
if (lanes == nullptr || lane_segment_kdtree_ == nullptr) {
return -1;
}
lanes->clear();
std::vector<std::string> ids;
// 使用KD树查询距离distance内的车道
const int status = SearchObjects(point, distance,
*lane_segment_kdtree_, &ids);
if (status < 0) {
return status;
}
// 通过ID获取车道对象
for (const auto& id : ids) {
lanes->emplace_back(GetLaneById(CreateHDMapId(id)));
}
return 0;
}
// 根据坐标点查询所属道路
int HDMapImpl::GetRoads(const Vec2d& point, double distance,
std::vector<RoadInfoConstPtr>* roads) const {
// 1. 先查询车道
std::vector<LaneInfoConstPtr> lanes;
if (GetLanes(point, distance, &lanes) != 0) {
return -1;
}
// 2. 通过车道找到所属道路
std::unordered_set<std::string> road_ids;
for (auto& lane : lanes) {
if (!lane->road_id().id().empty()) {
road_ids.insert(lane->road_id().id());
}
}
// 3. 获取道路对象
for (auto& road_id : road_ids) {
RoadInfoConstPtr road = GetRoadById(CreateHDMapId(road_id));
roads->push_back(road);
}
return 0;
}查询性能：
- KD树查询：O(log N + K)，N为总元素数，K为结果数
- 典型场景：查询10m范围内车道，耗时 < 1ms
3. PncMap规划控制地图
3.1 PncMap核心数据结构
PncMap在HDMap基础上，提供面向规划控制的专用接口。
3.1.1 路径点表示
cpp
// 文件: modules/map/pnc_map/path.h
// 车道路径点 - Frenet坐标系
struct LaneWaypoint {
LaneInfoConstPtr lane = nullptr; // 所属车道
double s = 0.0; // 沿车道中心线的纵向距离 (m)
double l = 0.0; // 相对中心线的横向偏移 (m)
// 坐标转换
PointENU ToPointENU() const {
if (lane == nullptr) {
return PointENU();
}
// s-l坐标 → 世界坐标
PointENU point = lane->GetSmoothPoint(s);
Vec2d direction = lane->GetUnitDirection(s);
Vec2d lateral_offset = direction.rotate(M_PI / 2) * l;
point.set_x(point.x() + lateral_offset.x());
point.set_y(point.y() + lateral_offset.y());
return point;
}
};Frenet坐标系优势：
- s坐标：沿道路中心线，便于纵向控制
- l坐标：横向偏移，便于横向控制
- 解耦：纵向和横向规划独立进行
3.1.2 车道段表示
cpp
// 车道段 - 表示车道的一部分
struct LaneSegment {
LaneInfoConstPtr lane = nullptr;
double start_s = 0.0; // 起点s坐标
double end_s = 0.0; // 终点s坐标
double Length() const {
return end_s - start_s;
}
};
// 路由段 - 从起点到终点的完整路径
struct RouteSegments {
std::vector<LaneSegment> lane_segments; // 车道段序列
// 判断点是否在路由上
bool IsOnSegment(const LaneWaypoint& waypoint) const {
for (const auto& segment : lane_segments) {
if (segment.lane->id() == waypoint.lane->id() &&
waypoint.s >= segment.start_s &&
waypoint.s <= segment.end_s) {
return true;
}
}
return false;
}
// 计算总长度
double TotalLength() const {
double length = 0.0;
for (const auto& segment : lane_segments) {
length += segment.Length();
}
return length;
}
};3.2 关键算法：路径拼接
cpp
// 伪代码：将多个车道段拼接成连续路径
std::vector<PathPoint> StitchPath(const RouteSegments& route_segments) {
std::vector<PathPoint> path_points;
double accumulated_s = 0.0;
for (const auto& segment : route_segments.lane_segments) {
// 采样当前车道段
double sample_step = 0.1; // 10cm采样间隔
for (double s = segment.start_s; s <= segment.end_s; s += sample_step) {
PathPoint point;
// 获取位置
PointENU enu_point = segment.lane->GetSmoothPoint(s);
point.set_x(enu_point.x());
point.set_y(enu_point.y());
// 获取航向角
point.set_theta(segment.lane->Heading(s));
// 获取曲率
point.set_kappa(segment.lane->Curvature(s));
// 累积距离
point.set_s(accumulated_s);
accumulated_s += sample_step;
path_points.push_back(point);
}
}
return path_points;
}3.3 坐标系转换
世界坐标系 (UTM) Frenet坐标系 (s-l)
y↑ l↑
| |
| P(x,y) | P(s,l)
| / | /
| / | /
| / | /
|/________→x |/________→s
O O(车道中心线)
转换公式：
给定世界坐标P(x,y)，求Frenet坐标P(s,l)：
1. 投影到车道中心线，找到最近点Q
2. s = Q点沿中心线的累积距离
3. l = 带符号的垂直距离（左正右负）
lane->GetProjection(Vec2d(x, y), &s, &l);4. RelativeMap相对地图
4.1 RelativeMap设计理念
RelativeMap是为导航模式设计的轻量级地图方案：
应用场景：
- 高速公路/城市快速路
- 没有HDMap覆盖的区域
- 需要快速部署的场景
数据来源：
- 导航信息：GPS导航路线
- 感知车道线：摄像头检测的车道线
- 定位信息：车辆位姿
4.2 RelativeMap核心实现
cpp
// 文件: modules/map/relative_map/relative_map.cc
bool RelativeMap::CreateMapFromNavigationLane(MapMsg* map_msg) {
CHECK_NOTNULL(map_msg);
// 1. 更新车辆状态 (定位 + 底盘)
const LocalizationEstimate& localization = localization_;
const Chassis& chassis = chassis_;
vehicle_state_provider_->Update(localization, chassis);
map_msg->mutable_localization()->CopyFrom(localization_);
// 2. 更新导航车道 (融合感知车道线)
const PerceptionObstacles& perception = perception_obstacles_;
navigation_lane_.UpdatePerception(perception);
map_msg->mutable_lane_marker()->CopyFrom(perception.lane_marker());
// 3. 生成导航路径
if (!navigation_lane_.GeneratePath()) {
LogErrorStatus(map_msg, "Failed to generate a navigation path.");
return false;
}
// 4. 检查路径有效性
if (navigation_lane_.Path().path().path_point().empty()) {
LogErrorStatus(map_msg,
"There is no path point in current navigation path.");
return false;
}
// 5. 创建地图Protobuf消息
if (!navigation_lane_.CreateMap(config_.map_param(), map_msg)) {
LogErrorStatus(map_msg,
"Failed to create map from current navigation path.");
return false;
}
ADEBUG << "There is/are " << map_msg->navigation_path().size()
<< " navigation path(s) in the current relative map.";
return true;
}4.3 导航车道融合算法
cpp
// 伪代码：融合导航线和感知车道线
void NavigationLane::UpdatePerception(const PerceptionObstacles& perception) {
// 1. 提取感知车道线
std::vector<LaneMarker> perceived_lanes;
for (const auto& marker : perception.lane_marker()) {
perceived_lanes.push_back(marker);
}
// 2. 与导航线对齐
AlignWithNavigationLine(perceived_lanes);
// 3. 加权融合
double nav_weight = config_.lane_marker_weight(); // 默认0.1
double percep_weight = 1.0 - nav_weight;
for (size_t i = 0; i < navigation_points_.size(); ++i) {
if (i < perceived_lanes.size()) {
// 融合位置
navigation_points_[i].x =
nav_weight * navigation_points_[i].x +
percep_weight * perceived_lanes[i].x;
navigation_points_[i].y =
nav_weight * navigation_points_[i].y +
percep_weight * perceived_lanes[i].y;
}
}
}融合策略：
- 导航线权重小（0.1）：提供长期路径引导
- 感知线权重大（0.9）：提供短期精确定位
- 互补优势：导航稳定，感知精确
5. 空间索引与查询算法
5.1 KD树空间索引
Apollo使用轴对齐包围盒KD树（AABoxKDTree2d）加速地图查询。
5.1.1 KD树数据结构
cpp
// 文件: modules/common/math/aaboxkdtree2d.h
// KD树参数
struct AABoxKDTreeParams {
int max_depth = -1; // 最大深度 (-1表示无限制)
int max_leaf_size = -1; // 叶节点最大对象数
double max_leaf_dimension = -1.0; // 叶节点最大尺寸 (m)
};
// KD树节点
template <class ObjectType>
class AABoxKDTree2dNode {
public:
using ObjectPtr = const ObjectType*;
// 构造函数 - 递归构建KD树
AABoxKDTree2dNode(const std::vector<ObjectPtr>& objects,
const AABoxKDTreeParams& params, int depth)
: depth_(depth) {
// 1. 计算边界
ComputeBoundary(objects);
// 2. 确定分割轴 (X或Y)
ComputePartition();
// 3. 判断是否需要分裂
if (SplitToSubNodes(objects, params)) {
std::vector<ObjectPtr> left_subnode_objects;
std::vector<ObjectPtr> right_subnode_objects;
// 4. 分割对象
PartitionObjects(objects, &left_subnode_objects,
&right_subnode_objects);
// 5. 递归构建子树
if (!left_subnode_objects.empty()) {
left_subnode_.reset(new AABoxKDTree2dNode<ObjectType>(
left_subnode_objects, params, depth + 1));
}
if (!right_subnode_objects.empty()) {
right_subnode_.reset(new AABoxKDTree2dNode<ObjectType>(
right_subnode_objects, params, depth + 1));
}
} else {
// 叶节点 - 存储对象
InitObjects(objects);
}
}
// 最近邻查询
ObjectPtr GetNearestObject(const Vec2d& point) const {
ObjectPtr nearest_object = nullptr;
double min_distance_sqr = std::numeric_limits<double>::infinity();
GetNearestObjectInternal(point, &min_distance_sqr, &nearest_object);
return nearest_object;
}
// 范围查询
std::vector<ObjectPtr> GetObjects(const Vec2d& point,
const double distance) const {
std::vector<ObjectPtr> result_objects;
GetObjectsInternal(point, distance, Square(distance), &result_objects);
return result_objects;
}
// 获取包围盒
AABox2d GetBoundingBox() const {
return AABox2d({min_x_, min_y_}, {max_x_, max_y_});
}
private:
// 节点数据
int depth_; // 深度
double min_x_, max_x_, min_y_, max_y_; // 包围盒
enum { PARTITION_X = 0, PARTITION_Y = 1 };
int partition_; // 分割轴
double partition_position_; // 分割位置
// 子节点
std::unique_ptr<AABoxKDTree2dNode> left_subnode_;
std::unique_ptr<AABoxKDTree2dNode> right_subnode_;
// 叶节点对象
int num_objects_ = 0;
std::vector<ObjectPtr> objects_sorted_by_min_;
std::vector<ObjectPtr> objects_sorted_by_max_;
};5.1.2 分割策略
cpp
void ComputePartition() {
double x_range = max_x_ - min_x_;
double y_range = max_y_ - min_y_;
// 选择较长的轴进行分割
if (x_range >= y_range) {
partition_ = PARTITION_X;
partition_position_ = (min_x_ + max_x_) / 2.0;
} else {
partition_ = PARTITION_Y;
partition_position_ = (min_y_ + max_y_) / 2.0;
}
}
bool SplitToSubNodes(const std::vector<ObjectPtr>& objects,
const AABoxKDTreeParams& params) {
// 终止条件1: 达到最大深度
if (params.max_depth >= 0 && depth_ >= params.max_depth) {
return false;
}
// 终止条件2: 对象数量足够少
if (params.max_leaf_size >= 0 &&
static_cast<int>(objects.size()) <= params.max_leaf_size) {
return false;
}
// 终止条件3: 节点尺寸足够小
if (params.max_leaf_dimension > 0.0) {
double x_range = max_x_ - min_x_;
double y_range = max_y_ - min_y_;
if (x_range <= params.max_leaf_dimension &&
y_range <= params.max_leaf_dimension) {
return false;
}
}
return true;
}5.1.3 范围查询算法
cpp
void GetObjectsInternal(const Vec2d& point,
const double distance,
const double distance_sqr,
std::vector<ObjectPtr>* result) const {
// 叶节点 - 直接检查所有对象
if (left_subnode_ == nullptr && right_subnode_ == nullptr) {
for (ObjectPtr object : objects_sorted_by_min_) {
if (object->DistanceSquaredTo(point) <= distance_sqr) {
result->push_back(object);
}
}
return;
}
// 内部节点 - 递归搜索
double partition_value = (partition_ == PARTITION_X) ? point.x() : point.y();
// 判断左子树是否可能包含结果
if (partition_value - distance <= partition_position_) {
if (left_subnode_ != nullptr) {
left_subnode_->GetObjectsInternal(point, distance,
distance_sqr, result);
}
}
// 判断右子树是否可能包含结果
if (partition_value + distance >= partition_position_) {
if (right_subnode_ != nullptr) {
right_subnode_->GetObjectsInternal(point, distance,
distance_sqr, result);
}
}
}算法复杂度：
- 构建：O(N log N)
- 查询：O(log N + K)，K为结果数
- 空间：O(N)
5.2 车道KD树构建
cpp
void HDMapImpl::BuildLaneSegmentKDTree() {
AABoxKDTreeParams params;
params.max_leaf_dimension = 5.0; // 叶节点最大5m
params.max_leaf_size = 16; // 叶节点最多16个对象
std::vector<LaneSegmentBox> lane_segment_boxes;
// 为每个车道的每个线段创建包围盒
for (const auto& lane_pair : lane_table_) {
const auto& lane_info = lane_pair.second;
// 获取车道线段
const auto& segments = lane_info->segments();
for (size_t i = 0; i < segments.size(); ++i) {
LaneSegmentBox box;
box.object_id = lane_info->id().id();
box.segment_index = i;
box.aabox = segments[i].AABoundingBox(); // 轴对齐包围盒
lane_segment_boxes.push_back(box);
}
}
// 构建KD树
lane_segment_kdtree_.reset(
new LaneSegmentKDTree(lane_segment_boxes, params));
AINFO << "Built lane segment KD-tree with "
<< lane_segment_boxes.size() << " segments.";
}5.3 查询性能优化
Apollo的优化策略：
-
多级索引：
ID哈希表 (O(1)) → 快速精确查找 KD树 (O(log N)) → 快速空间查询 -
包围盒预计算：
- 构建时预计算所有包围盒
- 避免查询时重复计算
-
对象池：
- 使用shared_ptr管理内存
- 避免重复创建对象
-
查询缓存（用户可选）：
- 缓存最近查询结果
- LRU淘汰策略
典型性能数据：
- 地图大小：100 km² 城市区域
- 车道数量：~50,000条
- KD树深度：~12层
- 10m范围查询：< 0.5 ms
- 最近车道查询：< 0.2 ms
6. 地图数据格式与加载
6.1 OpenDRIVE格式
OpenDRIVE是自动驾驶行业的标准地图格式。
6.1.1 OpenDRIVE XML结构
xml
<?xml version="1.0" encoding="UTF-8"?>
<OpenDRIVE>
<header revMajor="1" revMinor="4" name="SampleMap">
<geoReference><![CDATA[+proj=utm +zone=50 +datum=WGS84]]></geoReference>
</header>
<!-- 道路定义 -->
<road name="Road1" length="500.0" id="1" junction="-1">
<!-- 道路几何 -->
<planView>
<geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="200.0">
<line/> <!-- 直线段 -->
</geometry>
<geometry s="200.0" x="200.0" y="0.0" hdg="0.0" length="300.0">
<arc curvature="0.01"/> <!-- 圆弧段 -->
</geometry>
</planView>
<!-- 车道定义 -->
<lanes>
<laneSection s="0.0">
<!-- 左侧车道 -->
<left>
<lane id="1" type="driving" level="false">
<width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0"/>
<roadMark sOffset="0.0" type="solid" weight="standard" color="white"/>
</lane>
</left>
<!-- 中心线 -->
<center>
<lane id="0" type="none" level="false"/>
</center>
<!-- 右侧车道 -->
<right>
<lane id="-1" type="driving" level="false">
<width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0"/>
<roadMark sOffset="0.0" type="broken" weight="standard" color="white"/>
</lane>
</right>
</laneSection>
</lanes>
<!-- 信号灯 -->
<signals>
<signal s="150.0" id="Signal1" name="TrafficLight1"
dynamic="yes" orientation="+" country="CN" type="1000001">
<validity fromLane="-1" toLane="-1"/>
</signal>
</signals>
</road>
<!-- 路口定义 -->
<junction name="Junction1" id="100">
<connection id="0" incomingRoad="1" connectingRoad="10" contactPoint="start">
<laneLink from="-1" to="-1"/>
</connection>
</junction>
</OpenDRIVE>6.1.2 OpenDRIVE适配器
cpp
// 文件: modules/map/hdmap/adapter/opendrive_adapter.h
class OpendriveAdapter {
public:
static bool LoadData(const std::string& filename, Map* map);
private:
// 解析道路
static void ParseRoad(const tinyxml2::XMLElement* road_elem, Road* road);
// 解析几何
static void ParseGeometry(const tinyxml2::XMLElement* geometry_elem,
Geometry* geometry);
// 解析车道
static void ParseLanes(const tinyxml2::XMLElement* lanes_elem, Lanes* lanes);
// 解析信号灯
static void ParseSignals(const tinyxml2::XMLElement* signals_elem,
std::vector<Signal>* signals);
};6.2 Protobuf格式
Apollo原生地图格式，性能更高。
6.2.1 地图Protobuf定义
protobuf
// 文件: modules/map/proto/map.proto
message Map {
optional Header header = 1;
repeated Lane lane = 2;
repeated Junction junction = 3;
repeated Signal signal = 4;
repeated Crosswalk crosswalk = 5;
repeated StopSign stop_sign = 6;
repeated YieldSign yield_sign = 7;
repeated Overlap overlap = 8;
repeated ClearArea clear_area = 9;
repeated SpeedBump speed_bump = 10;
repeated Road road = 11;
repeated ParkingSpace parking_space = 12;
repeated PNCJunction pnc_junction = 13;
repeated BarrierGate barrier_gate = 14;
}
message Lane {
optional Id id = 1;
optional Curve central_curve = 2; // 中心线
// 左边界
optional LaneBoundary left_boundary = 3;
// 右边界
optional LaneBoundary right_boundary = 4;
// 长度
optional double length = 5;
// 速度限制
optional double speed_limit = 6;
// 拓扑关系
repeated Id predecessor_id = 7;
repeated Id successor_id = 8;
repeated Id left_neighbor_forward_lane_id = 9;
repeated Id right_neighbor_forward_lane_id = 10;
// 车道类型
enum LaneType {
NONE = 1;
CITY_DRIVING = 2;
BIKING = 3;
SIDEWALK = 4;
PARKING = 5;
}
optional LaneType type = 11;
// 转向
enum LaneTurn {
NO_TURN = 1;
LEFT_TURN = 2;
RIGHT_TURN = 3;
U_TURN = 4;
}
optional LaneTurn turn = 12;
// Overlap关联
repeated Id overlap_id = 13;
// 方向
enum LaneDirection {
FORWARD = 1;
BACKWARD = 2;
BIDIRECTION = 3;
}
optional LaneDirection direction = 14;
}
message Curve {
repeated CurveSegment segment = 1;
}
message CurveSegment {
oneof curve_type {
LineSegment line_segment = 1;
ArcSegment arc_segment = 2;
BezierSegment bezier_segment = 3;
}
optional double s = 4; // 起点累积距离
optional double length = 5;
}6.3 地图格式对比
| 特性 | OpenDRIVE XML | Apollo Protobuf |
|---|---|---|
| 可读性 | 高（文本格式） | 低（二进制） |
| 文件大小 | 大（~10MB） | 小（~2MB） |
| 加载速度 | 慢（XML解析） | 快（直接反序列化） |
| 互操作性 | 高（行业标准） | 低（Apollo专用） |
| 扩展性 | 中 | 高（易于扩展字段） |
| 验证 | 严格（XSD校验） | 灵活 |
使用建议：
- 开发调试：使用OpenDRIVE XML（便于查看和编辑）
- 生产部署：使用Protobuf（性能更高）
7. 关键算法实现
7.1 投影算法：世界坐标→Frenet坐标
cpp
// 文件: modules/map/hdmap/hdmap_common.cc
bool LaneInfo::GetProjection(const Vec2d& point,
double* accumulate_s,
double* lateral) const {
// 1. 使用KD树找到最近线段
if (lane_segment_kdtree_ == nullptr) {
return false;
}
std::vector<double> distances;
std::vector<int> segment_indices;
lane_segment_kdtree_->GetNearestSegment(point, &segment_indices, &distances);
if (segment_indices.empty()) {
return false;
}
int nearest_seg_index = segment_indices[0];
const LineSegment2d& nearest_segment = segments_[nearest_seg_index];
// 2. 投影到最近线段
double segment_s = 0.0;
double segment_l = 0.0;
Vec2d foot_point = nearest_segment.ProjectOntoUnit(point, &segment_s);
segment_l = (point - foot_point).CrossProd(nearest_segment.unit_direction());
// 3. 计算累积s坐标
*accumulate_s = accumulated_s_[nearest_seg_index] + segment_s;
*lateral = segment_l;
// 4. 边界检查
if (*accumulate_s < 0.0 || *accumulate_s > total_length_) {
return false;
}
return true;
}算法流程：
输入: 世界坐标点 P(x, y)
输出: Frenet坐标 (s, l)
1. KD树查询最近线段
├─ 时间复杂度: O(log N)
└─ 返回: segment_index
2. 投影到线段
├─ foot_point = P投影到线段
├─ segment_s = foot_point在线段上的位置
└─ segment_l = 带符号距离
3. 计算全局s坐标
└─ s = accumulated_s[segment_index] + segment_s
4. 返回: (s, l)7.2 插值算法:获取任意s处的属性
cpp
// 获取s处的航向角
double LaneInfo::Heading(const double s) const {
// 1. 查找s所在的线段
auto it = std::upper_bound(accumulated_s_.begin(),
accumulated_s_.end(), s);
if (it == accumulated_s_.begin()) {
return headings_[0];
}
if (it == accumulated_s_.end()) {
return headings_.back();
}
// 2. 线性插值
int index = std::distance(accumulated_s_.begin(), it) - 1;
double s0 = accumulated_s_[index];
double s1 = accumulated_s_[index + 1];
double h0 = headings_[index];
double h1 = headings_[index + 1];
// 处理角度跳变(-π到π)
double delta_h = h1 - h0;
if (delta_h > M_PI) {
delta_h -= 2 * M_PI;
} else if (delta_h < -M_PI) {
delta_h += 2 * M_PI;
}
double ratio = (s - s0) / (s1 - s0);
return NormalizeAngle(h0 + ratio * delta_h);
}
// 获取s处的曲率
double LaneInfo::Curvature(const double s) const {
// 1. 查找线段
auto it = std::upper_bound(accumulated_s_.begin(),
accumulated_s_.end(), s);
if (it == accumulated_s_.begin()) {
return 0.0; // 起点曲率为0
}
if (it == accumulated_s_.end()) {
return 0.0; // 终点曲率为0
}
// 2. 计算曲率 κ = dθ/ds
int index = std::distance(accumulated_s_.begin(), it) - 1;
double s0 = accumulated_s_[index];
double s1 = accumulated_s_[index + 1];
double h0 = headings_[index];
double h1 = headings_[index + 1];
double delta_h = NormalizeDeltaAngle(h1 - h0);
double delta_s = s1 - s0;
if (delta_s < kMathEpsilon) {
return 0.0;
}
return delta_h / delta_s;
}曲率计算公式:
κ(s)=dθds \kappa(s) = \frac{d\theta}{ds} κ(s)=dsdθ
其中:
- κ\kappaκ: 曲率 (1/m)
- θ\thetaθ: 航向角 (rad)
- sss: 沿车道中心线的距离 (m)
物理意义:
- κ>0\kappa > 0κ>0: 左转
- κ=0\kappa = 0κ=0: 直线
- κ<0\kappa < 0κ<0: 右转
- ∣κ∣|\kappa|∣κ∣越大,转弯越急
7.3 A*路由搜索算法深度解析
Apollo使用A*算法进行全局路径规划,从起点车道搜索到终点车道的最优车道序列。
7.3.1 路由搜索问题定义
输入:
- 起点: start_lane_id, start_s
- 终点: end_lane_id, end_s
- 路由图: 车道连接关系
输出:
- 车道序列: [lane1, lane2, ..., laneN]
- 总代价: total_cost
目标: 最小化总代价(通常是距离)
7.3.2 A*算法核心实现
cpp
// 文件: modules/routing/graph/a_star_strategy.h
class AStarStrategy {
public:
bool Search(const TopoNode* start_node,
const TopoNode* end_node,
std::vector<const TopoNode*>* result_nodes) {
// 数据结构
std::priority_queue<NodeWithCost> open_set; // 开放集(最小堆)
std::unordered_set<const TopoNode*> closed_set; // 关闭集
std::unordered_map<const TopoNode*, const TopoNode*> came_from; // 路径追踪
std::unordered_map<const TopoNode*, double> g_score; // 从起点到节点的实际代价
std::unordered_map<const TopoNode*, double> f_score; // 估计总代价 f = g + h
// 初始化
g_score[start_node] = 0.0;
f_score[start_node] = Heuristic(start_node, end_node); // h(n)
open_set.push({start_node, f_score[start_node]});
// A*主循环
while (!open_set.empty()) {
// 1. 取出f值最小的节点
NodeWithCost current = open_set.top();
open_set.pop();
const TopoNode* current_node = current.node;
// 2. 到达终点?
if (current_node == end_node) {
ReconstructPath(came_from, current_node, result_nodes);
return true;
}
// 3. 标记为已访问
if (closed_set.count(current_node) > 0) {
continue; // 已访问过,跳过
}
closed_set.insert(current_node);
// 4. 扩展邻居节点
for (const auto& edge : current_node->OutEdges()) {
const TopoNode* neighbor = edge.to_node;
// 跳过已访问节点
if (closed_set.count(neighbor) > 0) {
continue;
}
// 计算新的g值
double tentative_g = g_score[current_node] + edge.cost;
// 如果找到更短路径,或者首次访问
if (g_score.find(neighbor) == g_score.end() ||
tentative_g < g_score[neighbor]) {
// 更新路径
came_from[neighbor] = current_node;
g_score[neighbor] = tentative_g;
f_score[neighbor] = tentative_g + Heuristic(neighbor, end_node);
// 加入开放集
open_set.push({neighbor, f_score[neighbor]});
}
}
}
// 搜索失败
return false;
}
private:
// 启发函数 h(n) - 从节点n到终点的估计代价
double Heuristic(const TopoNode* from, const TopoNode* to) const {
// 使用欧氏距离作为启发函数
const PointENU& from_point = from->Centroid();
const PointENU& to_point = to->Centroid();
double dx = to_point.x() - from_point.x();
double dy = to_point.y() - from_point.y();
return std::sqrt(dx * dx + dy * dy);
}
// 重构路径
void ReconstructPath(
const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
const TopoNode* current,
std::vector<const TopoNode*>* path) const {
path->clear();
path->push_back(current);
// 回溯路径
while (came_from.find(current) != came_from.end()) {
current = came_from.at(current);
path->push_back(current);
}
// 反转路径(起点→终点)
std::reverse(path->begin(), path->end());
}
// 节点+代价(用于优先队列)
struct NodeWithCost {
const TopoNode* node;
double cost;
// 最小堆(f值小的优先)
bool operator<(const NodeWithCost& other) const {
return cost > other.cost; // 注意:大于号用于最小堆
}
};
};7.3.3 A*算法详解
算法流程图:
1. 初始化
├─ open_set = {start}
├─ closed_set = {}
├─ g[start] = 0
└─ f[start] = h(start)
2. 主循环 (while open_set不为空)
├─ current = pop节点(f值最小)
│
├─ if current == goal:
│ return 重构路径
│
├─ closed_set.add(current)
│
└─ for each neighbor of current:
├─ if neighbor in closed_set: continue
├─ tentative_g = g[current] + cost(current, neighbor)
├─ if tentative_g < g[neighbor]:
│ ├─ came_from[neighbor] = current
│ ├─ g[neighbor] = tentative_g
│ ├─ f[neighbor] = g[neighbor] + h(neighbor)
│ └─ open_set.add(neighbor, f[neighbor])
3. return 搜索失败关键概念:
- g(n): 从起点到节点n的实际代价
- h(n): 从节点n到终点的启发式估计代价
- f(n) = g(n) + h(n): 通过节点n的总估计代价
启发函数要求:
可采纳性 (Admissible): h(n) ≤ h*(n)
h*(n): 从n到目标的真实最短距离
如果h(n)满足可采纳性,A*保证找到最优解
一致性 (Consistent): h(n) ≤ c(n, n') + h(n')
c(n, n'): 从n到n'的边代价
如果h(n)满足一致性,则自动满足可采纳性Apollo的启发函数:
cpp
// 欧氏距离 (Euclidean Distance)
h(n) = ||n.position - goal.position||
性质:
1. 可采纳性: ✓ (直线距离 ≤ 道路距离)
2. 一致性: ✓ (三角不等式)
3. 计算简单: O(1)代价函数设计:
cpp
// 边的代价 = 基础距离 + 惩罚项
double EdgeCost(const TopoEdge& edge) const {
double cost = edge.length; // 基础距离
// 惩罚1: 变道惩罚
if (edge.type == EdgeType::LANE_CHANGE) {
cost += lane_change_penalty_; // 例如 5.0米
}
// 惩罚2: 掉头惩罚
if (edge.type == EdgeType::U_TURN) {
cost += u_turn_penalty_; // 例如 20.0米
}
// 惩罚3: 路口惩罚
if (edge.to_node->IsInJunction()) {
cost += junction_penalty_; // 例如 10.0米
}
// 惩罚4: 速度限制低的道路
if (edge.to_node->SpeedLimit() < 5.0) { // 低于5m/s
cost += low_speed_penalty_; // 例如 15.0米
}
return cost;
}性能分析:
时间复杂度: O((V + E) log V)
V: 节点数(车道数)
E: 边数(车道连接数)
空间复杂度: O(V)
典型场景:
- 城市道路: 1000个车道, 3000条连接
- 搜索时间: 5-20ms
- 路径长度: 10-50个车道优化技巧:
cpp
// 1. 双向A* (Bidirectional A*)
class BidirectionalAStarStrategy {
// 同时从起点和终点搜索,直到两个搜索树相遇
// 时间复杂度降低到 O((V/2 + E) log (V/2)) ≈ O(V/2 * log V)
};
// 2. 限制搜索范围
if (f_score[current] > max_search_cost_) {
continue; // 超出范围,放弃该路径
}
// 3. 缓存启发函数值
std::unordered_map<NodePair, double> heuristic_cache_;7.3.4 路由结果数据结构
cpp
// 文件: modules/routing/proto/routing.proto
message RoutingResponse {
optional apollo.common.Header header = 1;
repeated RoadSegment road = 2; // 路由结果(道路段序列)
optional Measurement measurement = 3;
optional RoutingRequest routing_request = 4;
optional bytes map_version = 5;
optional apollo.common.StatusPb status = 6;
}
message RoadSegment {
optional string id = 1;
repeated Passage passage = 2; // 通道(车道选择)
}
message Passage {
repeated LaneSegment segment = 1; // 车道段序列
optional bool can_exit = 2; // 是否可以退出该通道
optional ChangeLaneType change_lane_type = 3;
}
message LaneSegment {
optional string id = 1; // 车道ID
optional double start_s = 2; // 起点s坐标
optional double end_s = 3; // 终点s坐标
}
enum ChangeLaneType {
FORWARD = 0; // 直行
LEFT = 1; // 左变道
RIGHT = 2; // 右变道
LEFT_FORWARD = 3; // 左前方
RIGHT_FORWARD = 4; // 右前方
}使用示例:
cpp
// 发送路由请求
RoutingRequest request;
request.add_waypoint()->CopyFrom(start_point);
request.add_waypoint()->CopyFrom(end_point);
// A*搜索
AStarStrategy a_star;
std::vector<const TopoNode*> result_nodes;
bool success = a_star.Search(start_node, end_node, &result_nodes);
if (success) {
// 构造路由响应
RoutingResponse response;
for (const auto& node : result_nodes) {
RoadSegment* road_segment = response.add_road();
road_segment->set_id(node->RoadId());
Passage* passage = road_segment->add_passage();
LaneSegment* lane_segment = passage->add_segment();
lane_segment->set_id(node->LaneId());
lane_segment->set_start_s(0.0);
lane_segment->set_end_s(node->Length());
}
// 发布路由结果
routing_writer_->Write(response);
}7.3 车道宽度查询
cpp
void LaneInfo::GetWidth(const double s,
double* left_width,
double* right_width) const {
*left_width = GetWidthFromSample(sampled_left_width_, s);
*right_width = GetWidthFromSample(sampled_right_width_, s);
}
double LaneInfo::GetWidthFromSample(
const std::vector<SampledWidth>& samples, const double s) const {
if (samples.empty()) {
return 0.0;
}
// 二分查找
auto it = std::lower_bound(samples.begin(), samples.end(), s,
[](const SampledWidth& sample, double s) {
return sample.first < s;
});
if (it == samples.begin()) {
return it->second;
}
if (it == samples.end()) {
return samples.back().second;
}
// 线性插值
auto prev_it = std::prev(it);
double s0 = prev_it->first;
double s1 = it->first;
double w0 = prev_it->second;
double w1 = it->second;
double ratio = (s - s0) / (s1 - s0);
return w0 + ratio * (w1 - w0);
}采样策略：
- 每隔1m采样一次宽度
- 预计算并存储采样值
- 查询时线性插值
7.4 路径平滑算法：Douglas-Peucker
用于简化路径点序列，减少存储和计算量。
cpp
// Douglas-Peucker算法 - 递归简化
std::vector<Vec2d> DouglasPeuckerSimplify(
const std::vector<Vec2d>& points, double epsilon) {
if (points.size() <= 2) {
return points;
}
// 1. 找到距离起点-终点连线最远的点
double max_distance = 0.0;
int max_index = 0;
LineSegment2d baseline(points.front(), points.back());
for (size_t i = 1; i < points.size() - 1; ++i) {
double distance = baseline.DistanceTo(points[i]);
if (distance > max_distance) {
max_distance = distance;
max_index = i;
}
}
// 2. 如果最大距离大于阈值，递归简化
if (max_distance > epsilon) {
// 递归简化左段
std::vector<Vec2d> left_points(points.begin(),
points.begin() + max_index + 1);
std::vector<Vec2d> left_result = DouglasPeuckerSimplify(left_points,
epsilon);
// 递归简化右段
std::vector<Vec2d> right_points(points.begin() + max_index,
points.end());
std::vector<Vec2d> right_result = DouglasPeuckerSimplify(right_points,
epsilon);
// 合并结果
std::vector<Vec2d> result = left_result;
result.insert(result.end(), right_result.begin() + 1, right_result.end());
return result;
} else {
// 3. 否则，只保留起点和终点
return {points.front(), points.back()};
}
}算法复杂度：
- 最坏情况：O(N²)
- 平均情况：O(N log N)
- 空间：O(log N)（递归栈）
典型参数：
- epsilon = 0.1m：高精度保留
- epsilon = 0.5m：平衡精度与性能
- epsilon = 1.0m：低精度快速处理
8. 配置参数详解
8.1 HDMap配置
protobuf
// 文件: modules/map/proto/map_config.proto
message HDMapConfig {
// 地图文件路径
optional string map_file_path = 1;
// KD树参数
message KDTreeParams {
optional int32 max_depth = 1 [default = -1]; // 最大深度
optional int32 max_leaf_size = 2 [default = 16]; // 叶节点最大对象数
optional double max_leaf_dimension = 3 [default = 5.0]; // 叶节点最大尺寸(m)
}
optional KDTreeParams kdtree_params = 2;
// 查询参数
optional double default_lane_search_range = 3 [default = 10.0]; // 默认车道搜索范围(m)
optional double default_signal_search_range = 4 [default = 50.0]; // 默认信号灯搜索范围(m)
}典型配置：
hdmap_config {
map_file_path: "/apollo/modules/map/data/sunnyvale_big_loop/base_map.bin"
kdtree_params {
max_depth: -1 # 无限制深度
max_leaf_size: 16 # 每个叶节点最多16个对象
max_leaf_dimension: 5.0 # 叶节点最大5m×5m
}
default_lane_search_range: 10.0 # 默认搜索10m范围内车道
default_signal_search_range: 50.0 # 默认搜索50m范围内信号灯
}8.2 RelativeMap配置
protobuf
// 文件: modules/map/relative_map/proto/relative_map_config.proto
message RelativeMapConfig {
// 地图参数
message MapParam {
optional double default_left_width = 1 [default = 1.875]; // 默认左宽度(m)
optional double default_right_width = 2 [default = 1.875]; // 默认右宽度(m)
optional double default_speed_limit = 3 [default = 29.06]; // 默认限速(m/s, 65mph)
}
optional MapParam map_param = 1;
// 导航车道配置
message NavigationLaneConfig {
// 车道来源
enum LaneSource {
OFFLINE_GENERATED = 0; // 离线生成
CAMERA_PERCEPTION = 1; // 摄像头感知
LIDAR_PERCEPTION = 2; // 激光雷达感知
}
optional LaneSource lane_source = 1 [default = OFFLINE_GENERATED];
// 导航线最大长度
optional double max_len_from_navigation_line = 2 [default = 250.0]; // (m)
// 车道线标记权重（融合时）
optional double lane_marker_weight = 3 [default = 0.1];
// 车道宽度
optional double min_lane_width = 4 [default = 2.5]; // 最小车道宽度(m)
optional double max_lane_width = 5 [default = 4.5]; // 最大车道宽度(m)
}
optional NavigationLaneConfig navigation_lane = 2;
}完整配置示例：
protobuf
# 文件: modules/map/relative_map/conf/relative_map_config.pb.txt
map_param {
default_left_width: 1.875 # 默认左车道宽度 3.75m/2
default_right_width: 1.875 # 默认右车道宽度
default_speed_limit: 29.06 # 默认限速 65mph = 29.06m/s
}
navigation_lane {
lane_source: OFFLINE_GENERATED
# 从导航线提取250m长度的车道
max_len_from_navigation_line: 250.0
# 导航线权重0.1，感知车道线权重0.9
lane_marker_weight: 0.1
# 车道宽度范围
min_lane_width: 2.5
max_lane_width: 4.5
# 车道采样
lane_sample_resolution: 0.1 # 10cm采样间隔
# 平滑参数
smooth_window_size: 5 # 5点移动平均平滑
}8.3 配置参数调优指南
8.3.1 KD树参数调优
| 参数 | 推荐值 | 影响 |
|---|---|---|
max_depth |
-1（无限制） | 深度越大，查询越快，但构建时间越长 |
max_leaf_size |
16 | 越小查询越快，但树越深 |
max_leaf_dimension |
5.0m | 越小查询越精确，但树越深 |
调优建议：
- 城市密集区域 ：
max_leaf_dimension = 3.0m（车道密集） - 高速公路 ：
max_leaf_dimension = 10.0m（车道稀疏） - 内存受限 ：增大
max_leaf_size到32（减少节点数）
8.3.2 RelativeMap参数调优
| 参数 | 推荐值 | 场景 |
|---|---|---|
lane_marker_weight |
0.1 | 标准场景 |
lane_marker_weight |
0.05 | GPS信号差（更依赖感知） |
lane_marker_weight |
0.2 | 车道线模糊（更依赖导航） |
max_len_from_navigation_line |
250m | 高速公路 |
max_len_from_navigation_line |
150m | 城市道路 |
调优原则：
- 导航线权重：GPS信号好时增大，信号差时减小
- 感知权重：车道线清晰时增大，模糊时减小
- 前瞻距离：速度越快，前瞻距离越大
9. 地图模块使用示例
9.1 查询车辆当前车道
cpp
#include "modules/map/hdmap/hdmap.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
// 获取车辆当前所在车道
std::vector<LaneInfoConstPtr> GetCurrentLanes(
const HDMap& hdmap,
const VehicleStateProvider& vehicle_state) {
// 1. 获取车辆位置
Vec2d vehicle_position(vehicle_state.x(), vehicle_state.y());
// 2. 查询附近车道（3m范围）
std::vector<LaneInfoConstPtr> lanes;
hdmap.GetLanes(vehicle_position, 3.0, &lanes);
// 3. 筛选车辆真正所在的车道
std::vector<LaneInfoConstPtr> current_lanes;
for (const auto& lane : lanes) {
if (lane->IsOnLane(vehicle_position)) {
current_lanes.push_back(lane);
}
}
return current_lanes;
}9.2 规划路径投影到地图
cpp
// 将规划路径投影到Frenet坐标系
bool ProjectPathToLane(const std::vector<PathPoint>& path_points,
const LaneInfoConstPtr& lane,
std::vector<FrenetPoint>* frenet_path) {
frenet_path->clear();
for (const auto& path_point : path_points) {
FrenetPoint frenet_point;
// 投影到车道
Vec2d xy_point(path_point.x(), path_point.y());
double s = 0.0, l = 0.0;
if (!lane->GetProjection(xy_point, &s, &l)) {
AERROR << "Failed to project point to lane";
return false;
}
// 填充Frenet点
frenet_point.set_s(s);
frenet_point.set_l(l);
frenet_point.set_dl(l); // 横向速度（简化为0）
frenet_point.set_ddl(0.0); // 横向加速度
frenet_path->push_back(frenet_point);
}
return true;
}9.3 查询前方信号灯
cpp
// 查询前方50m内的信号灯
std::vector<SignalInfoConstPtr> GetForwardSignals(
const HDMap& hdmap,
const LaneInfoConstPtr& current_lane,
double current_s,
double search_distance) {
std::vector<SignalInfoConstPtr> forward_signals;
// 1. 获取当前车道的Overlap信号灯
const auto& lane_signals = current_lane->signals();
for (const auto& overlap : lane_signals) {
// 2. 检查信号灯是否在前方
double signal_s = overlap->GetObjectOverlapInfo(
current_lane->id())->lane_overlap_info().start_s();
if (signal_s > current_s &&
signal_s - current_s <= search_distance) {
// 3. 获取信号灯对象
auto signal = hdmap.GetSignalById(overlap->signal()->id());
if (signal != nullptr) {
forward_signals.push_back(signal);
}
}
}
// 4. 按距离排序
std::sort(forward_signals.begin(), forward_signals.end(),
[&](const SignalInfoConstPtr& a, const SignalInfoConstPtr& b) {
double a_s = GetSignalS(current_lane, a);
double b_s = GetSignalS(current_lane, b);
return a_s < b_s;
});
return forward_signals;
}10. 总结
10.1 地图模块核心要点
三层地图体系：
- HDMap：厘米级高精度地图，城市道路静态场景
- PncMap：规划控制专用接口，Frenet坐标系
- RelativeMap：实时相对地图，导航模式动态场景
关键技术：
- KD树空间索引：亚毫秒级查询性能
- Frenet坐标系：解耦纵向和横向控制
- Overlap机制：表达地图元素间的空间关系
- 多格式支持：OpenDRIVE标准与Protobuf性能
算法实现：
- 投影算法：世界坐标 ↔ Frenet坐标转换
- 插值算法：航向角、曲率、宽度查询
- 路径平滑：Douglas-Peucker简化
- 范围查询：KD树高效空间搜索
10.2 性能特征
| 指标 | 数值 |
|---|---|
| 地图大小 | 100 km² 城市区域 |
| 车道数量 | ~50,000条 |
| KD树深度 | ~12层 |
| 加载时间 | < 2秒 (Protobuf) |
| 查询延迟 | < 1ms (10m范围) |
| 内存占用 | ~500MB |
10.3 与其他模块的交互
┌─────────────┐
│ Localization│ → 提供车辆位姿
└──────┬──────┘
│
▼
┌─────────────┐
│ Map │ ← 核心：提供道路语义
└──────┬──────┘
│
├───→ Planning (路径规划)
├───→ Prediction (轨迹预测)
├───→ Control (车辆控制)
└───→ Perception (感知融合)典型调用流程：
- 定位模块 → 获取车辆位置 (x, y)
- 地图模块 → 查询当前车道 + 前方道路
- 规划模块 → 基于车道生成路径
- 控制模块 → 跟踪路径执行控制
10.4 未来演进方向
技术趋势：
-
高动态地图：
- 实时更新道路施工、临时管制
- 融合V2X路侧信息
- 众包更新机制
-
语义增强：
- 更丰富的道路语义（路面材质、坡度、天气影响）
- 驾驶经验数据（困难路段标注）
-
轻量化部署：
- 云端-车端协同
- 按需加载（只加载当前区域）
- 压缩算法（减少存储和传输）
-
AI生成地图：
- 基于感知数据自动生成局部地图
- 减少对预制高精度地图的依赖
附录
A. 关键文件索引
| 文件路径 | 功能 | 行数 |
|---|---|---|
modules/map/hdmap/hdmap_common.h |
地图元素类定义 | 1-400 |
modules/map/hdmap/hdmap_impl.cc |
地图加载与查询 | 1-300 |
modules/map/pnc_map/path.h |
路径表示结构 | 1-150 |
modules/map/relative_map/relative_map.cc |
相对地图生成 | 1-156 |
modules/common/math/aaboxkdtree2d.h |
KD树实现 | 1-150 |
B. 重要概念术语表
| 术语 | 英文 | 说明 |
|---|---|---|
| 高精度地图 | HDMap | 厘米级精度的静态地图 |
| Frenet坐标 | Frenet Frame | 沿道路中心线的s-l坐标系 |
| Overlap | Overlap | 地图元素间的空间重叠关系 |
| KD树 | KD-Tree | k维空间分割树，用于快速空间查询 |
| 投影 | Projection | 世界坐标到车道坐标的转换 |
| 曲率 | Curvature | 曲线弯曲程度，单位1/m |
| 累积距离 | Accumulated S | 沿车道中心线的总距离 |
| OpenDRIVE | OpenDRIVE | 自动驾驶地图数据交换标准 |
C. 参考资料
- OpenDRIVE官方规范：https://www.asam.net/standards/detail/opendrive/
- Apollo地图模块文档：https://github.com/ApolloAuto/apollo/tree/master/modules/map
- KD树算法论文：Bentley, J. L. (1975). "Multidimensional binary search trees used for associative searching"
下一部分预告：第二部分：感知模块详解
参考资料与引用
官方资源
-
Apollo 官方 GitHub 仓库
https://github.com/ApolloAuto/apollo
- Apollo 开源项目主仓库,包含完整源代码
-
Apollo 官方文档
- 官方技术文档和开发指南
-
Apollo 开发者社区
https://apollo.baidu.com/community
- 官方开发者论坛和技术交流平台
技术规范与标准
-
ISO 26262 - 道路车辆功能安全标准
-
ISO 21448 (SOTIF) - 预期功能安全标准
学术论文与技术资源
-
CenterPoint: Center-based 3D Object Detection and Tracking
Yin, T., Zhou, X., & Krähenbühl, P. (2021)
-
BEVFormer: Learning Bird's-Eye-View Representation from Multi-Camera Images
Li, Z., et al. (2022)
-
OpenDRIVE 地图标准
开源工具与库
-
Bazel 构建系统
-
Fast-DDS (eProsima)
https://www.eprosima.com/index.php/products-all/eprosima-fast-dds
-
PROJ 坐标转换库
-
TensorRT 开发指南
-
PCL 点云库文档
-
IPOPT 优化求解器
说明
本文档内容整理自上述官方资料、开源代码以及相关技术文档。所有代码示例和技术细节均基于 Apollo 9.0/10.0 版本。如需获取最新信息,请访问 Apollo 官方网站和 GitHub 仓库。
版权说明
Apollo® 是百度公司的注册商标。本文档为基于开源项目的非官方技术研究文档,仅供学习参考使用。