1. 项目概述与核心价值最近在啃Apollo 9.0的PNCPlanning 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 PhaseBroad Phase粗略检测使用包围盒进行快速筛选找出所有可能发生碰撞的物体对。这一步必须极快过滤掉大部分明显安全的物体。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::vectorEigen::Vector2d存储障碍物轮廓顶点 double min_x std::numeric_limitsdouble::max(); double max_x -std::numeric_limitsdouble::max(); double min_y std::numeric_limitsdouble::max(); double max_y -std::numeric_limitsdouble::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相交检测的逻辑简单到令人发指这也是它速度快的根源。判断两个AABBbox1: (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 实战技巧与注意事项缓存与更新障碍物的AABB不是一成不变的。对于动态障碍物每一帧都需要根据其预测轨迹更新AABB。这里有个优化点如果障碍物只是平移AABB只需做同样的平移但如果障碍物发生了旋转则必须重新计算所有顶点并更新AABB因为旋转后的AABB会变大。在代码中要注意区分这两种情况避免不必要的重算。加入安全余量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轴同理用于空间索引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::vectorEigen::Vector2d corners_; // 四个角点缓存用于计算 };axes_是两个正交的单位向量定义了OBB的本地坐标系。heading_是axes_[0]与世界坐标系x轴的夹角。4.2 分离轴定理SAT原理与实现SAT是判断两个凸多边形OBB是特殊的凸多边形是否相交的经典算法。其核心思想如果能找到一条直线轴使得两个多边形在该直线上的投影不重叠则它们一定不相交。如果对于所有可能的候选轴投影都重叠则它们相交。对于两个OBB我们需要检查的候选轴包括每个OBB的两个本地轴共4条轴。所以总共是4条轴。对于每条轴我们需要将两个OBB的所有顶点投影到该轴上得到两个投影区间[min1, max1]和[min2, max2]。判断两个区间是否重叠。如果不重叠则在此轴上分离两个OBB不相交。如果检查完所有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::arrayEigen::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_limitsdouble::max(); double this_proj_max -std::numeric_limitsdouble::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_limitsdouble::max(); double other_proj_max -std::numeric_limitsdouble::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使用的工程实践要点坐标系统一这是最容易出错的地方自车的OBB通常在车辆坐标系后轴中心为原点车头方向为x轴下定义。而障碍物的OBB可能来自感知模块其坐标可能是世界坐标系或传感器坐标系。在进行SAT检测前必须将所有OBB变换到同一个坐标系下。通常的选择是都变换到车辆坐标系。Apollo中大量的坐标变换WorldCoord-VehicleCoord就服务于这个目的。角点缓存与更新Box2d的corners_是缓存起来的四个角点。当center_或heading_改变时需要调用InitCorners()之类的函数重新计算角点。在动态场景中如果障碍物在运动需要及时更新其OBB并重新计算角点。不只是相交还要计算深度Penetration Depth对于规划模块仅仅知道“撞了”还不够还需要知道“撞进去多深”以便评估碰撞的严重程度和设计惩罚函数。SAT算法可以扩展来计算穿透深度和最小平移向量MTV用于轨迹优化中的碰撞代价计算。这在Apollo的DualVariable优化或类似框架中可能会用到。与轨迹采样的结合在Apollo的EM Planner或Lattice Planner中会生成一系列候选轨迹。对每条轨迹会在每个时间点或路径点上将自车轮廓表示为OBB放置在该位姿然后与静态/动态障碍物的OBB进行碰撞检查。这是一个批量操作对OBB检测的效率要求极高。5. Apollo Planning模块中的碰撞检测流程实战现在我们把AABB和OBB放到完整的Planning模块碰撞检测流程中看。这个过程通常发生在CollisionChecker类或PathDecision相关的函数中。5.1 分层检测流程拆解一个典型的碰撞检测流程如下数据准备获取自车状态位置、朝向、速度。获取感知融合后的障碍物列表每个障碍物包含其预测轨迹一系列时间点上的状态。获取当前规划周期生成的候选轨迹一系列路径点。Broad Phase 1: 基于AABB的快速空间筛选为自车在当前规划周期内的可能活动范围计算一个大的“搜索AABB”。这个范围可以根据自车速度和规划时长估算。遍历所有障碍物获取它们在世界坐标系下的AABB通常由感知模块提供或可快速计算。使用简单的AABB相交测试快速筛选出与自车搜索范围相交的障碍物。这一步可能过滤掉80%以上的远距离障碍物。Broad Phase 2: 基于OBB和时间维度的精细筛选对于筛选出的障碍物进行更精细的检查。这里通常涉及时间维度。动态障碍物处理对于有预测轨迹的障碍物我们不是只检查一个时刻而是检查规划周期内的一系列离散时间点例如每0.1秒一个点。在每个时间点t a. 将自车根据候选轨迹插值到时间t的位姿生成自车在该时刻的OBBego_box_at_t。 b. 将障碍物根据其预测轨迹插值到时间t的位姿生成障碍物在该时刻的OBBobs_box_at_t。 c. 将两者变换到同一坐标系通常是车辆坐标系在t时刻的坐标或一个固定的世界坐标系。 d. 使用SAT算法检查ego_box_at_t和obs_box_at_t是否相交。 e. 如果任何一个时间点相交则标记该候选轨迹与障碍物在该时间段内存在碰撞风险。Narrow Phase (如果必要)如果OBB检测发现相交但规划模块需要更精确的结果例如对于非常规形状的障碍物可能会进一步进行多边形与多边形的精确碰撞检测。但在Apollo的多数规划场景中OBB的精度已经足够。5.2 关键代码逻辑定位在Apollo源码中你可以沿着以下路径寻找碰撞检测的核心代码modules/planning/common/obstacle.ccObstacle类其中可能有IsCollision()或类似方法内部会调用Box2d的HasOverlap。modules/planning/common/trajectory_evaluator.cc或modules/planning/scenarios/...在评估轨迹代价的函数中会遍历障碍物进行碰撞检查。modules/planning/constraint_checker/collision_checker.cc可能存在专门的碰撞检查器类。modules/common/math/box2d.h/.ccBox2d类的定义和HasOverlap、DistanceTo等方法的实现这是所有几何运算的基础。阅读这些代码时注意观察坐标变换在哪里发生common::math::Vec2d的变换或使用Transform类安全余量padding是如何添加的可能在构建OBB时也可能在检测时膨胀盒子如何处理障碍物的不确定性有时会使用比实际尺寸更大的OBB来包容预测误差6. 常见问题、调试技巧与性能优化在实际实现和调试碰撞检测时你会遇到各种各样的问题。下面是我从实践中总结的一些典型问题和解决思路。6.1 常见问题排查表问题现象可能原因排查思路与解决方案误报太多明明离得很远却检测为碰撞1. AABB/OBB的安全余量padding设置过大。2. 使用了世界坐标系下的AABB进行最终判断而物体方向与坐标轴夹角大AABB过于松散。3.坐标系统一错误导致盒子位置计算完全错误。1. 逐步减小padding值观察效果。通常0.2~0.5米是合理范围。2. 确认在精细检测阶段是否切换到了OBB。检查OBB的朝向角计算是否正确。3.重点检查打印出自车和障碍物在检测时的中心点坐标、朝向角确认它们是否在同一个合理的坐标系下。可视化是终极手段。漏报实际很近却没检测到1. 安全余量过小或为0。2.时间未对齐自车和障碍物的轨迹插值时间点不一致或过于稀疏。3. 动态障碍物预测轨迹不准确实际位置与预测偏差大。1. 适当增加padding特别是横向的padding对安全至关重要。2. 增加轨迹插值的时间分辨率例如从0.2s提高到0.05s。检查时间戳同步逻辑。3. 引入不确定性模型使用“概率占据栅格”或放大障碍物OBB来包容预测不确定性。检测结果不稳定时而碰撞时而不碰撞1. 浮点数精度问题在边界情况下判断不一致。2. 障碍物状态更新或坐标变换存在竞态条件不同线程读到不同时刻的数据。3. OBB角点缓存未及时更新。1. 在SAT比较投影区间时引入一个小的容差epsilon如1e-6将改为 -epsilon。2. 检查数据流确保用于碰撞检测的自车状态和障碍物状态是同一帧、原子性的数据。3. 确认在更新center_或heading_后立即调用了角点更新函数。性能瓶颈1. 未使用AABB进行快速筛选直接对所有障碍物进行OBB/SAT检测。2. 动态障碍物轨迹插值点过多。3. SAT实现未优化每次检测都重新计算投影。1.强制加入AABB Broad Phase筛选层。2. 根据障碍物距离和速度自适应调整轨迹检查的时间分辨率。远处的、低速的障碍物可以降低检查频率。3. 优化SAT使用4.2节提到的基于中心和半长的公式避免循环计算角点投影。将轴向量归一化等计算提前。6.2 调试与可视化技巧碰撞检测算法光看代码和日志很难调试可视化是关键。绘制包围盒在仿真环境或日志回放工具中将每个障碍物的AABB和OBB用不同颜色的矩形绘制出来。同时绘制出自车轮廓的OBB。一眼就能看出盒子是否贴合以及碰撞判断是否合理。绘制分离轴在怀疑有问题的碰撞帧将SAT算法检查的4条分离轴也画出来并画出两个OBB在这些轴上的投影区间。这能帮你直观理解为什么算法认为它们相交或分离。关键数据打印在碰撞判断的代码分支里打印出此时的自车位姿、障碍物ID、位姿、OBB参数、投影区间值等。将这些数据与可视化结果对照。单元测试为Box2d::HasOverlap函数编写全面的单元测试覆盖各种边界情况完全分离、刚好相切、部分重叠、完全包含、边平行等。确保基础几何计算的正确性。6.3 高级优化思路当系统需要处理成百上千的障碍物时进一步的优化是必要的空间索引Spatial Indexing如前所述使用四叉树或网格对所有障碍物的AABB进行管理。查询与自车搜索范围相交的障碍物时时间复杂度可以从O(N)降到O(log N)或O(1)。增量更新对于动态障碍物如果其运动是连续的可以增量式地更新其AABB/OBB在空间索引中的位置而不是每帧重新插入。并行计算不同候选轨迹之间的碰撞检测是相互独立的可以并行进行。同样一条轨迹与多个障碍物的检测也可以并行化。利用多核CPU加速。近似计算在某些对实时性要求极高的阶段如紧急制动可以使用更激进的近似比如用圆形包围球Bounding Sphere代替OBB相交测试只需比较圆心距离和半径之和速度更快。理解Apollo中AABB和OBB的实现不仅仅是学会两个几何算法更是掌握了自动驾驶规划系统中处理“安全”与“效率”这对核心矛盾的一种经典工程范式。从快速的AABB海选到精确的OBB判定再到与时空轨迹的结合每一步都体现了系统设计的权衡。在你自己动手实现或调试相关功能时希望这些从源码和实践中提炼出的细节与心得能帮你更稳地走好每一步。
