📚 课程目录第3章 Apollo规划模块架构

3. Apollo规划模块架构

Apollo自动驾驶系统的规划模块（Planning）是整个自动驾驶软件栈中承上启下的核心决策单元。它接收来自感知（Perception）、预测（Prediction）、定位（Localization）和高精地图（HD-Map）等上游模块的数据，经过复杂的算法处理，生成安全、舒适且可执行的轨迹，并下发给控制（Control）模块执行。理解Planning模块的架构、输入输出以及与其他模块的数据流关系，是深入掌握轨迹规划优化算法的前提。本章将系统拆解Planning模块的边界、接口定义、数据流向以及在实际工程中的关键注意事项。

上图清晰地展示了Planning模块在Apollo系统中所处的核心枢纽位置。它并非一个孤立的算法单元，而是一个与上下游紧密耦合的软件组件。从数据流的角度看，Planning模块的输入可以归纳为“环境模型”和“自车状态”两大类，输出则是“期望轨迹”。下面我们将从输入、输出、与上下游的关系以及数据流的具体实现四个维度展开深度解析。

3.1 Planning模块的输入：构建决策基础

Planning模块的输入数据决定了其决策的上限。如果输入数据不准确或不完整，再优秀的规划算法也无法生成安全的轨迹。在Apollo中，Planning模块的输入主要通过CyberRT的Channel订阅获取，核心输入包括以下几个部分。

3.1.1 核心原理：输入数据的语义分层

规划模块的输入可以按照语义分为三个层次：静态环境、动态环境和自车状态。静态环境主要由高精地图（HD-Map）提供，包括车道线、路沿、交通标志、停止线等静态元素。动态环境由感知和预测模块提供，包括障碍物的位置、速度、加速度以及预测的未来轨迹。自车状态则由定位模块和底盘（Chassis）反馈提供，包括自车的精确位置、航向角、速度、加速度以及油门刹车状态。

这种分层设计使得Planning模块可以针对不同层次的数据采用不同的处理策略。例如，对于静态环境，可以预先建立SLAM地图或参考线（Reference Line）；对于动态环境，则需要在每个规划周期内实时更新障碍物列表和预测轨迹。

3.1.2 关键输入数据格式与Channel

在Apollo 3.5及之后的版本中，Planning模块主要订阅以下Cyber Channel：

数据类别	Channel名称	消息类型	关键字段	更新频率
定位	/apollo/localization/pose	LocalizationEstimate	pose (位置/姿态), linear_velocity, angular_velocity	100 Hz
感知障碍物	/apollo/perception/obstacles	PerceptionObstacles	id, position, theta, velocity, shape, type	10 Hz
预测	/apollo/prediction	PredictionObstacles	obstacle_id, trajectory (概率轨迹点序列)	10 Hz
底盘	/apollo/canbus/chassis	Chassis	speed_mps, throttle_percentage, brake_percentage, steering_percentage	50 Hz
路由请求	/apollo/routing/response	RoutingResponse	road_id, passage_id, waypoint列表	事件触发
高精地图	内部服务	HDMap	车道、道路、信号灯、交叉口信息	静态加载

值得注意的是，红绿灯信息在Apollo中通常通过感知模块的TrafficLightChannel（如/apollo/perception/traffic_light）单独订阅，因为其更新频率和延迟要求比普通障碍物更高。

3.1.3 案例拆解：输入数据不同步导致的规划异常

在实际实车测试中，我们曾遇到一个典型的输入数据不同步问题。现象是：车辆在通过路口时，Planning模块偶尔会输出急刹车指令，但前方并无障碍物。通过分析日志发现，定位模块的延迟达到了200ms，而感知模块的延迟仅为50ms。由于Planning模块在融合数据时，默认假设所有输入的时间戳是对齐的，导致自车位置被“回退”了2米（假设车速10m/s），而障碍物位置是实时的。这造成了“障碍物突然出现在自车前方”的假象，从而触发了紧急制动。

解决方案：在Planning模块的输入预处理阶段，增加了基于时间戳的协方差交叉融合（CI）机制，对延迟较高的定位数据进行了运动补偿。具体操作是：根据自车当前速度和延迟时间差，将定位数据外推至感知数据的时间戳。经过调优后，急刹车现象完全消失。

工程经验：在开发Planning模块时，务必关注各输入Channel的延迟和丢包率。建议在代码中增加输入数据延迟监控，当延迟超过阈值（如100ms）时，触发降级策略（如减速慢行），而不是盲目信任过期数据。

3.2 Planning模块的输出：轨迹的生成与下发

Planning模块的输出是ADCTrajectory，它定义了一条从当前时刻到未来若干秒（通常为8秒）的平滑轨迹。这条轨迹不仅包含路径信息，还包含速度、加速度和时间信息，是控制模块执行的直接依据。

3.2.1 核心原理：轨迹的数学表达

ADCTrajectory在Apollo中定义为一个离散点序列，每个轨迹点（TrajectoryPoint）包含以下关键信息：

路径点 (PathPoint)：包含x, y坐标、曲率（kappa）、曲率变化率（dkappa）以及相对于参考线的偏移量。
速度点 (TrajectoryPoint)：包含相对时间（relative_time）、速度（v）、加速度（a）、加加速度（jerk）以及方向盘转角（steer）。

控制模块通过插值算法，以较高的频率（如100Hz）从这条离散轨迹中获取当前时刻的目标状态，并与车辆实际状态进行对比，计算出油门、刹车和转向控制量。

3.2.2 输出数据的质量要求

Planning模块输出的轨迹必须满足以下硬性约束，否则会导致控制模块跟踪失败或车辆失稳：

连续性：轨迹点的位置、速度、加速度必须连续，不能有跳变。特别是加速度的导数（jerk）要尽量小，以保证乘坐舒适性。
可行性：轨迹必须满足车辆运动学约束（如最大转向角、最大加速度）和动力学约束（如轮胎附着力极限）。
时间一致性：轨迹的时间戳必须与系统时钟同步，且轨迹长度要足够覆盖控制模块的响应延迟和刹车距离。

3.2.3 实战场景：轨迹输出频率与延迟的权衡

在高速场景（如80km/h）下，车辆每秒行驶约22米。如果Planning模块的输出频率为10Hz，那么相邻两条轨迹之间的时间间隔为100ms，车辆将行驶2.2米。如果控制模块完全按照轨迹执行，在轨迹切换的瞬间，如果两条轨迹在横向位置上有微小偏差（例如5cm），就会产生一个较大的横向加速度突变。

为了应对这个问题，Apollo在Planning模块内部增加了轨迹平滑后处理环节。该环节使用二次规划（QP）或样条插值算法，对生成的原始轨迹进行重采样和平滑，确保输出给控制模块的轨迹在时间轴上具有二阶连续性。同时，控制模块内部也会对轨迹进行缓存和插值，以降低对Planning输出频率的敏感度。

调优建议：在实车调试时，可以通过录制控制模块的日志，观察trajectory_point中的v和a值是否平滑。如果发现加速度曲线存在锯齿状波动，优先检查Planning模块的轨迹平滑器参数（如QP权重系数）。

3.3 与上游感知预测、下游控制的关系

Planning模块与上下游的关系不仅仅是简单的数据传递，更是一种反馈控制与协同优化的关系。

3.3.1 与感知预测的协同：从“看见”到“预见”

感知模块提供的是障碍物的当前状态，而预测模块提供的是障碍物的未来意图。Planning模块在决策时，必须将两者结合。例如，当感知模块检测到前方车辆时，Planning模块需要根据预测模块输出的该车辆未来3秒的轨迹，来判断是跟随、变道还是超车。

在Apollo的代码实现中，Planning模块通过PredictionObstacles消息中的trajectory字段获取障碍物的预测轨迹。每个预测轨迹包含多个轨迹点，每个点都有对应的概率值。Planning模块在优化时，会将这些预测轨迹作为动态障碍物处理，并计算自车轨迹与这些预测轨迹的最小距离，确保安全。

3.3.2 与控制模块的闭环：从“规划”到“执行”

Planning模块输出轨迹后，控制模块负责执行。但控制模块的执行效果（如跟踪误差）会通过底盘反馈（Chassis）和定位信息回传给Planning模块。Planning模块在下一个规划周期中，会基于当前的实际位置（而非理论位置）重新规划轨迹。这就形成了一个闭环。

在Apollo中，这个闭环的周期是100ms（Planning周期）。如果控制模块的跟踪误差过大（例如在湿滑路面上），Planning模块需要感知到这种偏差，并适当降低规划速度或增大安全距离，而不是固执地执行原计划。

3.3.3 案例拆解：感知漏检导致的规划失败与对策

在某次园区测试中，Planning模块未能识别出前方静止的锥桶（感知漏检），导致车辆以15km/h的速度直行。幸运的是，安全员及时接管。事后分析发现，感知模块对静止小物体的召回率不足。

分析过程：从Planning模块的角度看，它接收到的障碍物列表为空，因此生成了匀速直行轨迹。这并非Planning算法的错误，而是上游感知的缺陷。

改进措施：在Planning模块中增加了基于HD-Map的静态障碍物推理功能。即：如果HD-Map中标注了施工区域或路肩，而感知模块在该区域没有检测到任何障碍物，Planning模块会降低该区域的可行驶速度上限，并增加虚拟障碍物以保守通过。这一改进显著提升了安全性。

  架构启示：优秀的Planning模块设计，必须具备对上游输入数据的“信任管理”能力。不能盲目相信感知结果，也不能完全不信。通过引入置信度、历史一致性检查、多源数据融合（如视觉与激光雷达）等手段，可以有效提升系统鲁棒性。

3.4 模块间数据流的工程实现与优化

在Apollo的CyberRT框架下，模块间的数据流是通过Channel的发布/订阅机制实现的。理解这一机制对于调试和优化系统性能至关重要。

3.4.1 核心原理：CyberRT的通信机制

CyberRT是Apollo自研的高性能通信中间件，它基于共享内存和进程间通信（IPC）实现低延迟的数据交换。Planning模块作为消费者，需要声明对特定Channel的依赖关系。在planning_component.cc文件中，通过Reader对象订阅所需的Channel。

// 伪代码示例：订阅感知障碍物
auto reader = node->CreateReader<PerceptionObstacles>(
    "/apollo/perception/obstacles", 
    [this](const std::shared_ptr<PerceptionObstacles>& msg) {
        // 处理回调函数
        OnPerceptionObstacles(*msg);
    });

这种异步回调机制使得Planning模块无需主动轮询数据，而是由数据到达时触发计算。但这也带来了一个挑战：如果多个输入数据的到达时间不一致，Planning模块需要等待所有数据就绪后才能开始计算。

3.4.2 数据同步策略：时间戳对齐与等待

Apollo中常用的数据同步策略是“等待最慢数据”。即：Planning模块在收到一个触发信号（如定时器或路由请求）后，会检查所有关键输入数据的时间戳。如果某个数据的时间戳落后于当前时间超过一个阈值（如200ms），则等待该数据到达，或者使用上一次的有效数据进行计算。

为了减少等待时间，Apollo引入了协方差交叉融合（CI）和运动补偿技术。例如，对于定位数据，即使其延迟为100ms，也可以通过IMU（惯性测量单元）数据进行短时外推，得到当前时刻的估计位置。

3.4.3 实战场景：数据流拥塞的排查与优化

在实车测试中，我们曾遇到Planning模块计算周期从100ms恶化到300ms的问题。通过CyberRT的可视化工具（如cyber_monitor）发现，/apollo/perception/obstacles Channel的数据量异常增大，达到了每秒500帧（正常为10帧）。

排查过程：

首先检查感知模块的日志，发现其内部算法出现异常，导致重复发布障碍物列表。
其次，检查Planning模块的代码，发现其回调函数中进行了耗时的数据拷贝操作（深拷贝），导致处理速度跟不上数据到达速度。
最后，优化了Planning模块的数据处理逻辑，改为使用const引用传递，并增加了数据去重机制。

结果：优化后，Planning模块的计算周期恢复至80ms，且CPU占用率下降了20%。

性能调优要点：在Planning模块中，应尽量避免在回调函数中执行耗时操作（如文件IO、复杂日志打印）。建议将数据拷贝与算法计算分离，使用双缓冲或对象池技术减少内存分配开销。

3.5 常见误区与总结复盘

在理解Planning模块架构时，工程师常陷入以下误区，需要特别警惕。

3.5.1 误区一：将Planning视为纯算法模块，忽视工程约束

许多初学者认为Planning就是写写A*算法或Lattice Planner。但在实际工程中，数据流管理、异常处理、性能优化占据了开发工作量的60%以上。一个无法稳定运行100小时的Planning模块，即使算法再先进，也无法量产。

3.5.2 误区二：忽视输入数据的质量评估

Planning模块的输出质量直接取决于输入质量。建议在开发阶段就建立输入数据质量监控面板，实时显示各Channel的延迟、频率、数据完整性等指标。一旦发现数据异常，立即触发告警或降级策略。

3.5.3 误区三：上下游联调不足

Planning模块的测试不能孤立进行。必须与感知、预测、控制模块进行联合仿真（如使用Apollo的Sim_Control模式）或实车联调。在联调过程中，重点观察以下数据流场景：

Cut-in场景：感知模块何时检测到旁车切入？预测模块给出的轨迹是否合理？Planning模块的响应是否及时？
红绿灯场景：感知模块的红绿灯状态延迟是多少？Planning模块在绿灯变黄灯时的决策是否符合预期？
控制失效场景：当控制模块报告跟踪误差过大时，Planning模块是否采取了降速或停车策略？

3.5.4 总结复盘：架构设计的核心原则

设计原则	说明	实施建议
低耦合	Planning模块不直接调用感知或控制模块的内部接口，仅通过消息通信。	严格遵循CyberRT的Channel通信规范，禁止跨模块函数调用。
高内聚	将场景决策、路径规划、速度规划等子模块内聚在Planning内部，通过统一的接口对外提供服务。	使用`PlanningContext`管理模块内部状态，避免全局变量。
可诊断性	所有关键输入输出和中间结果必须记录日志，便于问题回溯。	使用`ADEBUG`宏记录轨迹点、障碍物列表、决策标签等。
容错性	当上游数据缺失或异常时，模块应能降级运行，而不是崩溃。	实现默认值、超时保护、数据有效性检查等机制。

综上所述，Planning模块的架构设计是一个系统工程。它要求开发者不仅要掌握优化算法，还要深刻理解自动驾驶系统的整体数据流和工程约束。只有将算法与架构紧密结合，才能打造出安全、可靠、可量产的自动驾驶规划系统。