1. 项目概述与核心价值

最近在啃Apollo 9.0的PNC（Planning and Control）源码，特别是Planning模块里的碰撞检测部分，感触颇深。很多刚接触自动驾驶规划的朋友，一上来就想搞懂复杂的轨迹优化和决策逻辑，结果往往在第一步——判断“车会不会撞上东西”这里就卡住了。其实，碰撞检测是规划安全性的基石，它的效率和准确性直接决定了后续轨迹是否靠谱。Apollo里大量使用了两种经典的包围盒算法：AABB（轴对齐包围盒）和OBB（有向包围盒）。网上关于它们数学原理的文章不少，但真正结合像Apollo这样的大型工业级C++代码，把“为什么用”、“怎么用”、“踩过什么坑”讲透的资料却不多。这篇文章，我就以Apollo 9.0 Planning模块的源码为蓝本，带你手把手拆解AABB和OBB的实现，并分享我在学习和复现过程中总结的实战经验。无论你是正在学习自动驾驶的学生，还是希望深入理解Apollo框架的开发者，相信这篇从工程视角出发的深度解析都能让你有所收获。

简单来说，AABB和OBB是两种用于快速判断两个几何形状（比如车辆和障碍物）是否可能相交的“快速过滤器”。Planning模块需要在极短的时间内（通常是几十毫秒）处理海量的障碍物，如果对每个障碍物都用精确的几何形状进行碰撞计算，算力根本扛不住。所以，先用一个简单的“盒子”把物体包起来，进行快速的初步碰撞检测，能迅速排除大量明显不相交的物体，只有那些盒子相交的，才需要进一步进行更精细（也更耗时）的检测。Apollo的代码清晰地体现了这种分层检测的思想。

2. 碰撞检测的核心思想与方案选型

在深入代码之前，我们必须先搞清楚一个根本问题：在自动驾驶的规划上下文里，碰撞检测到底要解决什么？它不是一个纯粹的几何计算问题，而是一个融合了效率、精度和安全性的工程权衡。

2.1 为什么是包围盒？

想象一下，我们的自动驾驶车辆用一个多边形来表示轮廓，周围每个障碍物也可能是复杂的多边形。进行多边形之间的精确碰撞检测（比如分离轴定理SAT）计算量很大。当场景中有几十上百个动态、静态障碍物时，实时性无法保证。因此，工业界的普遍做法是采用分层检测（Broad Phase & Narrow Phase）：

1. Broad Phase（粗略检测）：使用包围盒进行快速筛选，找出所有可能发生碰撞的物体对。这一步必须极快，过滤掉大部分明显安全的物体。
2. Narrow Phase（精细检测）：仅对Broad Phase筛选出的物体对，进行精确的几何形状碰撞检测。

AABB和OBB就是Broad Phase中最常用的两种包围盒。它们的核心价值在于用极低的计算成本，实现了碰撞可能性的快速初筛，为后续复杂的规划算法赢得了宝贵的计算时间。

2.2 AABB vs. OBB：如何选择？

这是理解Apollo代码设计的关键。两者没有绝对的好坏，只有是否适合当前场景。

AABB (Axis-Aligned Bounding Box) - 轴对齐包围盒

• 是什么：一个各边都平行于坐标轴（通常是世界坐标系或车辆坐标系）的矩形（2D）或长方体（3D）。
• Apollo中的典型应用：在世界坐标系下，对障碍物进行快速的初步位置筛选。因为它的边界由(min_x, max_x, min_y, max_y)等值直接定义，相交测试只需要比较最大最小值，速度极快（O(1)复杂度）。
• 优点：
  • 计算速度无敌快：相交判断仅需6次比较（3D情况下）。
  • 构建和更新简单：遍历物体所有顶点，找到各个轴上的最大最小值即可。
  • 内存友好：存储只需要两个点（最小点和最大点）。
• 缺点：
  • 紧密性差：对于方向与坐标轴不一致的物体（比如斜着的车辆），AABB会包含大量空白区域，导致“误报”增多（即盒子相交但物体实际不相交），增加了Narrow Phase的无谓计算。
  • 动态物体更新开销大：物体旋转后，需要重新计算顶点并更新AABB，可能涉及全部顶点的变换。

OBB (Oriented Bounding Box) - 有向包围盒

• 是什么：一个可以根据物体自身方向进行旋转的盒子。它由中心点、三个互相垂直的轴方向（单位向量）以及在这三个轴上的半长（从中心到面的距离）来定义。
• Apollo中的典型应用：在车辆坐标系或物体自身坐标系下，表示车辆、障碍物本体的精确碰撞边界。它能紧密地包裹物体，特别适合表示有明确朝向的实体，如汽车、行人、自行车。
• 优点：
  • 紧密性好：能非常贴合物体形状，大大减少了“误报”，提高了Broad Phase的过滤精度。
  • 旋转不变性：物体旋转时，OBB的轴跟着旋转，盒子本身不需要重建，只需更新轴的方向，更适合动态物体。
• 缺点：
  • 相交检测计算更复杂：通常使用分离轴定理（SAT），需要更多的点积和比较操作，计算量比AABB大一个数量级。
  • 构建相对复杂：需要计算物体的主方向（例如，使用PCA主成分分析），以确定OBB的最佳朝向。

Apollo的混合策略：理解了上述区别，你再看Apollo的代码就豁然开朗了。它经常采用一种混合策略：先用世界坐标系下的AABB对所有障碍物进行一轮极其快速的“海选”，快速剔除掉距离很远的障碍物。然后，对于通过海选的障碍物，再将其和自车轮廓转换到统一的坐标系（比如车辆坐标系），使用OBB进行更精确的碰撞可能性判断。这种“AABB筛距离，OBB判姿态”的分层思路，在效率和精度之间取得了很好的平衡。

3. AABB的C++实现与源码解析

让我们深入到代码层面。在Apollo的代码库中，AABB的实现通常分散在几个工具类中，而不是一个单独的AABB类。核心思想是通过Box2d或AABox2d（如果有）这类基础几何结构，以及Obstacle类中的边界框成员来实现。

3.1 数据结构与构建

在Apollo中，一个障碍物（Obstacle）通常包含一个bounding_box_成员。这个边界框在很多情况下就是一个AABB（在世界坐标系下）。它的构建发生在障碍物信息更新时。

假设我们从感知模块获得了一个障碍物的多边形顶点（polygon_points），构建其世界坐标系下的AABB伪代码如下：

// 假设 points 是 std::vector<Eigen::Vector2d>，存储障碍物轮廓顶点 double min_x = std::numeric_limits<double>::max(); double max_x = -std::numeric_limits<double>::max(); double min_y = std::numeric_limits<double>::max(); double max_y = -std::numeric_limits<double>::max(); for (const auto& point : polygon_points) { min_x = std::min(min_x, point.x()); max_x = std::max(max_x, point.x()); min_y = std::min(min_y, point.y()); max_y = std::max(max_y, point.y()); } // 这样就得到了AABB的边界：{min_x, max_x, min_y, max_y} // Apollo可能会将其封装成一个 Box2d 对象，但此时这个Box2d的朝向角为0，本质上就是AABB

在modules/prediction/container/obstacles/obstacle.cc或modules/planning/reference_line/相关的代码中，你能找到类似的逻辑。Box2d类（定义在modules/common/math/box2d.h）虽然名字叫Box，但其构造函数接收中心点、朝向、长和宽。当朝向角为0时，它在世界坐标系下就是一个AABB。很多上游模块提供的初始边界框就是这种形式。

3.2 相交检测算法

AABB相交检测的逻辑简单到令人发指，这也是它速度快的根源。判断两个AABB（box1: (min_x1, max_x1, min_y1, max_y1),box2: (min_x2, max_x2, min_y2, max_y2)）是否相交：

bool AABBIntersect(const AABB& box1, const AABB& box2) { // 如果在x轴或y轴上投影区间不重叠，则一定不相交 if (box1.max_x < box2.min_x || box2.max_x < box1.min_x) { return false; // x轴分离 } if (box1.max_y < box2.min_y || box2.max_y < box1.min_y) { return false; // y轴分离 } // 两个轴上的投影都重叠，则AABB相交 return true; }

这就是著名的“分离轴定理”在轴对齐情况下的简化形式：只要找到一个轴（这里是X轴或Y轴），使得两个盒子在该轴上的投影不重叠，它们就不相交。因为轴是对齐的，我们只需要比较最大最小值。

在Apollo的规划碰撞检测中，你可能会在PathDecision或CollisionChecker相关的代码里看到这种思想的变种。例如，快速判断一条路径点是否进入某个障碍物的“危险区域”时，首先就会用AABB检查该点是否在边界框内。

3.3 实战技巧与注意事项

1. 缓存与更新：障碍物的AABB不是一成不变的。对于动态障碍物，每一帧都需要根据其预测轨迹更新AABB。这里有个优化点：如果障碍物只是平移，AABB只需做同样的平移；但如果障碍物发生了旋转，则必须重新计算所有顶点并更新AABB，因为旋转后的AABB会变大。在代码中要注意区分这两种情况，避免不必要的重算。
2. 加入安全余量（Padding）：在自动驾驶中，安全是第一位的。直接使用感知给出的物体轮廓构建AABB是不够的。通常会在AABB的每个边上增加一个安全余量（例如0.5米），形成一个更大的“缓冲框”。这样可以在规划阶段更早地预警潜在风险。这个padding的值是一个重要的调参项，需要在误报率和安全性之间权衡。

   // 构建带安全余量的AABB double padding = 0.5; // 单位：米 aabb_with_padding.min_x = min_x - padding; aabb_with_padding.max_x = max_x + padding; // y轴同理

3. 用于空间索引：AABB因其简单的结构，常与空间索引数据结构结合使用，如四叉树（Quadtree）或网格（Grid），用于快速检索某个区域内的所有障碍物。Apollo的ReferenceLineInfo可能会将道路上的障碍物按其AABB插入到空间索引中，当规划一条候选轨迹时，可以快速查询轨迹周围可能发生碰撞的障碍物列表，这是大规模场景下保证效率的关键。

4. OBB的C++实现与分离轴定理（SAT）详解

OBB是Apollo中用于精确表示自车和障碍物碰撞边界的主力。其核心是Box2d类。我们重点看如何用SAT算法判断两个OBB是否相交。

4.1 Box2d数据结构

在modules/common/math/box2d.h中，Box2d类的核心成员通常包括：

class Box2d { public: // ... private: Eigen::Vector2d center_; // 盒子中心点 double length_; // 长度（通常对应车身方向） double width_; // 宽度 double half_length_; // 半长 double width_; // 半宽 double heading_; // 朝向角（弧度），从x轴逆时针旋转 Eigen::Vector2d axes_[2]; // 两个单位轴向量，axes_[0]指向长度方向(cos, sin), axes_[1]指向宽度方向(-sin, cos) std::vector<Eigen::Vector2d> corners_; // 四个角点，缓存用于计算 };

axes_是两个正交的单位向量，定义了OBB的本地坐标系。heading_是axes_[0]与世界坐标系x轴的夹角。

4.2 分离轴定理（SAT）原理与实现

SAT是判断两个凸多边形（OBB是特殊的凸多边形）是否相交的经典算法。其核心思想：如果能找到一条直线（轴），使得两个多边形在该直线上的投影不重叠，则它们一定不相交。如果对于所有可能的候选轴，投影都重叠，则它们相交。

对于两个OBB，我们需要检查的候选轴包括：

• 每个OBB的两个本地轴（共4条轴）。

所以总共是4条轴。对于每条轴，我们需要：

1. 将两个OBB的所有顶点投影到该轴上，得到两个投影区间[min1, max1]和[min2, max2]。
2. 判断两个区间是否重叠。如果不重叠，则在此轴上分离，两个OBB不相交。
3. 如果检查完所有4条轴，都未发现分离，则两个OBB相交。

下面是SAT检测两个Box2d是否相交的简化代码逻辑：

bool Box2d::HasOverlap(const Box2d& other_box) const { // 获取当前盒子（this）的两个轴 const Eigen::Vector2d& axis0 = axes_[0]; const Eigen::Vector2d& axis1 = axes_[1]; // 获取另一个盒子的两个轴 const Eigen::Vector2d& other_axis0 = other_box.axes()[0]; const Eigen::Vector2d& other_axis1 = other_box.axes()[1]; // 需要检查的分离轴：this的axis0, axis1; other的axis0, axis1 std::array<Eigen::Vector2d, 4> test_axes = {axis0, axis1, other_axis0, other_axis1}; for (const auto& axis : test_axes) { // 1. 投影当前盒子(this)到axis上 double this_proj_min = std::numeric_limits<double>::max(); double this_proj_max = -std::numeric_limits<double>::max(); for (const auto& corner : corners_) { double proj = corner.dot(axis); // 点积即投影长度 this_proj_min = std::min(this_proj_min, proj); this_proj_max = std::max(this_proj_max, proj); } // 2. 投影另一个盒子(other_box)到axis上 double other_proj_min = std::numeric_limits<double>::max(); double other_proj_max = -std::numeric_limits<double>::max(); for (const auto& corner : other_box.corners()) { double proj = corner.dot(axis); other_proj_min = std::min(other_proj_min, proj); other_proj_max = std::max(other_proj_max, proj); } // 3. 判断投影区间是否分离 // 如果一个区间的最大值小于另一个区间的最小值，则分离 if (this_proj_max < other_proj_min || other_proj_max < this_proj_min) { return false; // 找到分离轴，不相交 } } // 所有轴上都未分离，则相交 return true; }

在Apollo的实际代码中（如modules/planning/common/obstacle.cc中的碰撞检查函数），你会看到高度优化过的SAT实现。它可能不会每次都重新计算角点投影，而是利用OBB的中心、半长和轴向量，直接计算投影区间，公式如下：

投影区间半长 = |(半长 * 轴_x) · 本地方向轴0| + |(半宽 * 轴_y) · 本地方向轴1| 投影区间中心 = 中心点 · 轴 区间 = [中心 - 半长, 中心 + 半长]

这种方式完全避免了顶点遍历，效率更高。这也是阅读工业级代码和学术Demo的区别——处处充满了性能优化。

4.3 OBB使用的工程实践要点

1. 坐标系统一：这是最容易出错的地方！自车的OBB通常在车辆坐标系（后轴中心为原点，车头方向为x轴）下定义。而障碍物的OBB可能来自感知模块，其坐标可能是世界坐标系或传感器坐标系。在进行SAT检测前，必须将所有OBB变换到同一个坐标系下。通常的选择是都变换到车辆坐标系。Apollo中大量的坐标变换（WorldCoord->VehicleCoord）就服务于这个目的。
2. 角点缓存与更新：Box2d的corners_是缓存起来的四个角点。当center_或heading_改变时，需要调用InitCorners()之类的函数重新计算角点。在动态场景中，如果障碍物在运动，需要及时更新其OBB并重新计算角点。
3. 不只是相交，还要计算深度（Penetration Depth）：对于规划模块，仅仅知道“撞了”还不够，还需要知道“撞进去多深”，以便评估碰撞的严重程度和设计惩罚函数。SAT算法可以扩展来计算穿透深度和最小平移向量（MTV），用于轨迹优化中的碰撞代价计算。这在Apollo的DualVariable优化或类似框架中可能会用到。
4. 与轨迹采样的结合：在Apollo的EM Planner或Lattice Planner中，会生成一系列候选轨迹。对每条轨迹，会在每个时间点或路径点上，将自车轮廓（表示为OBB）放置在该位姿，然后与静态/动态障碍物的OBB进行碰撞检查。这是一个批量操作，对OBB检测的效率要求极高。

5. Apollo Planning模块中的碰撞检测流程实战

现在，我们把AABB和OBB放到完整的Planning模块碰撞检测流程中看。这个过程通常发生在CollisionChecker类或PathDecision相关的函数中。

5.1 分层检测流程拆解

一个典型的碰撞检测流程如下：

1. 数据准备：

   • 获取自车状态（位置、朝向、速度）。
   • 获取感知融合后的障碍物列表，每个障碍物包含其预测轨迹（一系列时间点上的状态）。
   • 获取当前规划周期生成的候选轨迹（一系列路径点）。

2. Broad Phase 1: 基于AABB的快速空间筛选：

   • 为自车在当前规划周期内的可能活动范围，计算一个大的“搜索AABB”。这个范围可以根据自车速度和规划时长估算。
   • 遍历所有障碍物，获取它们在世界坐标系下的AABB（通常由感知模块提供或可快速计算）。
   • 使用简单的AABB相交测试，快速筛选出与自车搜索范围相交的障碍物。这一步可能过滤掉80%以上的远距离障碍物。

3. Broad Phase 2: 基于OBB和时间维度的精细筛选：

   • 对于筛选出的障碍物，进行更精细的检查。这里通常涉及时间维度。
   • 动态障碍物处理：对于有预测轨迹的障碍物，我们不是只检查一个时刻，而是检查规划周期内的一系列离散时间点（例如，每0.1秒一个点）。
   • 在每个时间点t： a. 将自车根据候选轨迹插值到时间t的位姿，生成自车在该时刻的OBB（ego_box_at_t）。 b. 将障碍物根据其预测轨迹插值到时间t的位姿，生成障碍物在该时刻的OBB（obs_box_at_t）。 c. 将两者变换到同一坐标系（通常是车辆坐标系在t时刻的坐标，或一个固定的世界坐标系）。 d. 使用SAT算法检查ego_box_at_t和obs_box_at_t是否相交。 e. 如果任何一个时间点相交，则标记该候选轨迹与障碍物在该时间段内存在碰撞风险。

4. Narrow Phase (如果必要)：

   • 如果OBB检测发现相交，但规划模块需要更精确的结果（例如，对于非常规形状的障碍物），可能会进一步进行多边形与多边形的精确碰撞检测。但在Apollo的多数规划场景中，OBB的精度已经足够。

5.2 关键代码逻辑定位

在Apollo源码中，你可以沿着以下路径寻找碰撞检测的核心代码：

• modules/planning/common/obstacle.cc：Obstacle类，其中可能有IsCollision()或类似方法，内部会调用Box2d的HasOverlap。
• modules/planning/common/trajectory_evaluator.cc或modules/planning/scenarios/...：在评估轨迹代价的函数中，会遍历障碍物进行碰撞检查。
• modules/planning/constraint_checker/collision_checker.cc：可能存在专门的碰撞检查器类。
• modules/common/math/box2d.h/.cc：Box2d类的定义和HasOverlap、DistanceTo等方法的实现，这是所有几何运算的基础。

阅读这些代码时，注意观察：

• 坐标变换在哪里发生？（common::math::Vec2d的变换，或使用Transform类）
• 安全余量（padding）是如何添加的？（可能在构建OBB时，也可能在检测时膨胀盒子）
• 如何处理障碍物的不确定性？（有时会使用比实际尺寸更大的OBB来包容预测误差）

6. 常见问题、调试技巧与性能优化

在实际实现和调试碰撞检测时，你会遇到各种各样的问题。下面是我从实践中总结的一些典型问题和解决思路。

6.1 常见问题排查表

6.2 调试与可视化技巧

碰撞检测算法光看代码和日志很难调试，可视化是关键。

1. 绘制包围盒：在仿真环境或日志回放工具中，将每个障碍物的AABB和OBB用不同颜色的矩形绘制出来。同时绘制出自车轮廓的OBB。一眼就能看出盒子是否贴合，以及碰撞判断是否合理。
2. 绘制分离轴：在怀疑有问题的碰撞帧，将SAT算法检查的4条分离轴也画出来，并画出两个OBB在这些轴上的投影区间。这能帮你直观理解为什么算法认为它们相交或分离。
3. 关键数据打印：在碰撞判断的代码分支里，打印出此时的自车位姿、障碍物ID、位姿、OBB参数、投影区间值等。将这些数据与可视化结果对照。
4. 单元测试：为Box2d::HasOverlap函数编写全面的单元测试，覆盖各种边界情况：完全分离、刚好相切、部分重叠、完全包含、边平行等。确保基础几何计算的正确性。

6.3 高级优化思路

当系统需要处理成百上千的障碍物时，进一步的优化是必要的：

1. 空间索引（Spatial Indexing）：如前所述，使用四叉树或网格对所有障碍物的AABB进行管理。查询与自车搜索范围相交的障碍物时，时间复杂度可以从O(N)降到O(log N)或O(1)。
2. 增量更新：对于动态障碍物，如果其运动是连续的，可以增量式地更新其AABB/OBB在空间索引中的位置，而不是每帧重新插入。
3. 并行计算：不同候选轨迹之间的碰撞检测是相互独立的，可以并行进行。同样，一条轨迹与多个障碍物的检测也可以并行化。利用多核CPU加速。
4. 近似计算：在某些对实时性要求极高的阶段（如紧急制动），可以使用更激进的近似，比如用圆形包围球（Bounding Sphere）代替OBB，相交测试只需比较圆心距离和半径之和，速度更快。

理解Apollo中AABB和OBB的实现，不仅仅是学会两个几何算法，更是掌握了自动驾驶规划系统中处理“安全”与“效率”这对核心矛盾的一种经典工程范式。从快速的AABB海选，到精确的OBB判定，再到与时空轨迹的结合，每一步都体现了系统设计的权衡。在你自己动手实现或调试相关功能时，希望这些从源码和实践中提炼出的细节与心得，能帮你更稳地走好每一步。