课程导入：自动驾驶规划算法的定义、在自动驾驶系统中的地位、课程学习路径与实战环境搭建

自动驾驶系统是一个高度复杂的软硬件耦合体，其核心目标是在动态交通环境中实现安全、高效、舒适的自主行驶。在感知、决策、规划、控制四大模块中，规划算法承担着从"理解环境"到"生成可执行轨迹"的关键桥梁作用。它接收感知模块输出的障碍物信息、预测模块给出的未来意图、定位模块提供的自车位姿，以及高精地图提供的道路拓扑，经过一系列约束优化与决策推理，输出一条满足动力学约束、避障要求、交通规则和乘员舒适性的轨迹序列。规划算法并非单一算法，而是一个分层递阶的算法族，涵盖全局路径规划、行为决策、运动规划（含轨迹生成与轨迹优化）等不同层级。本章作为课程的第一讲，将系统梳理规划算法的定义与边界、在自动驾驶系统中的定位、课程的整体学习路径，并带领读者完成实战环境的搭建，为后续章节的算法实现与工程落地奠定坚实基础。

1.1 规划算法的定义与核心内涵

规划算法在自动驾驶语境下，是指将感知、预测、定位等上游信息转化为车辆可执行的时空轨迹的计算过程。从数学本质上看，规划问题可以形式化为一个带约束的最优化问题：在给定的状态空间和时间区间内，寻找一条从当前状态到目标状态的轨迹，使得某个代价函数最小化，同时满足车辆运动学/动力学约束、环境避障约束、交通规则约束以及乘员舒适性约束。然而，实际工程中的规划问题远比教科书中的最优控制问题复杂——环境是部分可观测的、障碍物行为是不确定的、计算资源是有限的、安全冗余是必须的。因此，工程落地的规划算法往往采用分层递阶架构，将复杂问题分解为不同抽象层次、不同时间尺度的子问题，逐层精化。

从功能维度看，规划算法包含三个核心层次：全局路径规划（Route Planning）负责在道路网络层面寻找从起点到终点的宏观路径，通常基于高精地图的拓扑结构，使用A*、Dijkstra等图搜索算法，输出一系列道路级路径点；行为决策（Behavior Decision）负责在局部场景中做出驾驶行为选择，如跟车、变道、超车、停车让行、通过路口等，常用方法包括有限状态机（FSM）、决策树、马尔可夫决策过程（MDP）以及基于学习的决策模型；运动规划（Motion Planning）则进一步生成满足车辆运动学约束的平滑轨迹，包含路径（几何形状）和速度（时间分配）两个维度，常用方法包括采样法（RRT、Lattice Planner）、插值法（多项式曲线、样条曲线）、优化法（二次规划、模型预测控制）以及近年来兴起的端到端学习方法。

值得注意的是，规划算法与决策算法在工程实践中经常被合并讨论，但二者存在明确边界：决策回答"做什么"（What to do），规划回答"怎么做"（How to do it）。例如，行为决策层决定"当前需要变道超车"，而运动规划层则具体生成一条从当前车道平滑过渡到目标车道的轨迹，包含每个时刻的位置、速度、加速度和转向角。这种分层设计使得系统具备良好的模块化特性，便于独立开发、测试和调试，也便于在某一层引入新的算法而无需重构整个系统。

1.2 规划算法在自动驾驶系统中的定位与接口关系

自动驾驶系统通常被划分为感知（Perception）、预测（Prediction）、规划（Planning）、控制（Control）四大核心模块，外加定位（Localization）与高精地图（HD Map）作为基础支撑。规划算法处于系统的"决策大脑"位置，其输入输出接口直接决定了整个系统的数据流架构。理解规划算法在系统中的定位，不能仅停留在"规划在感知和控制之间"这种粗浅认知，而需要深入理解各模块之间的数据契约、时序关系和耦合方式。

从数据流角度看，规划算法的输入包括：自车状态（位姿、速度、加速度、航向角，通常来自定位模块，频率50-100Hz）；环境感知结果（障碍物的位置、尺寸、类别、速度，来自感知模块，频率10-30Hz）；障碍物预测轨迹（未来3-8秒的预测轨迹集合，来自预测模块，频率10Hz）；高精地图信息（车道线、路沿、交通标志、限速、道路拓扑，来自地图引擎）；路由指令（全局路径的宏观指引，来自路由模块）。规划算法的输出则是一条或多条候选轨迹，每条轨迹由一系列带时间戳的状态点组成，通常包含位置（x, y）、航向角θ、速度v、加速度a、曲率κ等参数，输出频率通常为10-50Hz，控制模块接收后通过转向、驱动、制动执行器进行跟踪。

在工程实践中，规划模块与上下游模块的接口设计往往决定了系统的鲁棒性。例如，感知模块输出的障碍物列表可能存在漏检、误检和延迟，规划算法必须设计相应的容错机制；预测模块给出的轨迹可能带有概率分布，规划算法需要在高置信度与低置信度预测之间做出合理权衡；控制模块的执行能力存在延迟和误差，规划算法输出的轨迹必须预留控制裕量。此外，坐标系变换也是接口设计中的关键环节——感知结果通常在自车坐标系（Vehicle Frame）下表达，高精地图在全局坐标系（UTM或经纬度）下表达，规划算法需要在不同坐标系之间进行精确变换，并处理时间同步问题。

从系统架构视角看，规划算法还承担着"安全兜底"的职责。当感知或预测模块出现异常时，规划算法需要具备降级策略——例如，当感知置信度降低时，规划应主动减速并扩大安全距离；当预测模块失效时，规划应基于最坏情况假设进行保守决策。这种安全冗余设计是自动驾驶系统通过功能安全认证（如ISO 26262）的必要条件，也是规划算法工程落地中区别于纯学术研究的重要方面。

1.3 课程学习路径与知识体系架构

本课程共30章，按照"基础-核心-进阶-工程"四个阶段递进设计，旨在帮助学员从零基础构建起自动驾驶规划算法的完整知识体系，并具备工程落地能力。课程学习路径的设计遵循"理论够用、实践为主、工程导向"的原则，每一章都包含核心原理讲解、代码实现、仿真验证和常见问题复盘。

第一阶段（第1-5章）：基础与工具链。在完成本章环境搭建后，第2章深入讲解自动驾驶坐标系与坐标变换，这是规划算法实现的基础设施；第3章介绍车辆运动学与动力学模型，包括自行车模型、阿克曼转向模型等，为轨迹生成提供车辆约束；第4章讲解Frenet坐标系及其与笛卡尔坐标系的转换，这是运动规划中最常用的工具；第5章聚焦高精地图的数据结构与车道模型，为全局路径规划提供数据基础。

第二阶段（第6-15章）：核心规划算法。第6-7章分别讲解全局路径规划中的图搜索算法（Dijkstra、A*、Hybrid A*）与采样方法（PRM、RRT及其变种）；第8-9章深入行为决策层，覆盖有限状态机、决策树、马尔可夫决策过程以及基于学习的决策方法；第10-12章是运动规划的核心，分别讲解路径-速度解耦规划（Path-Velocity Decomposition）、Lattice Planner、多项式轨迹生成与优化方法；第13章介绍模型预测控制（MPC）在轨迹规划中的应用；第14章讨论时空联合规划（Spatio-Temporal Planning）这一前沿方向；第15章对各类规划算法进行横向对比与选型分析。

第三阶段（第16-22章）：工程化与系统集成。第16章讲解规划算法的仿真环境搭建，包括CARLA、SUMO、Autoware等开源仿真器的使用；第17章深入规划算法的性能优化，涵盖代码级优化、并行计算、GPU加速等；第18章讨论规划算法的功能安全与冗余设计；第19章讲解规划算法的测试与验证方法论，包括场景库构建、回归测试、故障注入等；第20章介绍规划算法与上下游模块的联调方法；第21章聚焦实车部署中的标定、调试与排障；第22章复盘真实路测中常见的规划失败案例。

第四阶段（第23-30章）：前沿拓展与综合实战。第23-25章分别介绍基于强化学习的规划、基于模仿学习的规划以及端到端自动驾驶中的规划模块；第26章讨论多车协同规划与车路协同；第27章讲解规划算法在特定场景（泊车、匝道汇入、无保护左转）中的工程实现；第28章介绍规划算法的云端仿真与大规模评测；第29章进行综合实战项目——从零搭建一个完整的规划系统；第30章总结规划算法的行业趋势与职业发展路径。

1.4 实战环境搭建：从零构建规划算法开发环境

规划算法的开发与调试高度依赖仿真环境。一个完整的规划算法开发环境应包含以下组件：操作系统与基础工具链（推荐Ubuntu 20.04/22.04 LTS，配置CMake、GCC/G++、Python 3.8+）；编程语言与核心库（C++17/20作为性能关键路径的实现语言，Python作为算法原型与数据分析工具；依赖Eigen（线性代数）、OpenCV（图像处理）、PCL（点云处理，可选）、Boost（C++基础库））；可视化与调试工具（Matplotlib/PyPlot用于Python可视化，PlotJuggler用于时间序列数据可视化，RViz（ROS生态）用于三维场景可视化）；仿真平台（推荐从轻量级的CARLA或SUMO开始，进阶可使用Autoware或Apollo平台）；版本控制与协作工具（Git、GitLab/GitHub、Docker用于环境一致性管理）。

下面给出一个经过验证的环境搭建流程，适用于Ubuntu 22.04系统：

# 1. 更新系统与安装基础依赖
sudo apt update && sudo apt upgrade -y
sudo apt install -y build-essential cmake git python3-dev python3-pip \
    libeigen3-dev libboost-all-dev libyaml-cpp-dev libopencv-dev

# 2. 安装Python依赖（用于算法原型与可视化）
pip3 install numpy scipy matplotlib pandas jupyter notebook \
    shapely pyyaml rospkg

# 3. 安装PlotJuggler（时间序列数据可视化）
sudo apt install -y ros-humble-plotjuggler  # 如果使用ROS 2 Humble
# 或从源码安装：https://github.com/facontidavide/PlotJuggler

# 4. 安装CARLA仿真器（0.9.15版本）
mkdir -p ~/carla && cd ~/carla
wget https://carla-releases.s3.us-east-005.backblazeb2.com/Linux/CARLA_0.9.15.tar.gz
tar -xzf CARLA_0.9.15.tar.gz
# 安装CARLA的Python API
pip3 install carla==0.9.15

# 5. 克隆课程配套代码仓库
mkdir -p ~/autonomous_planning_ws && cd ~/autonomous_planning_ws
git clone https://github.com/your-course-repo/planning_course.git
cd planning_course && mkdir build && cd build
cmake .. && make -j$(nproc)

# 6. 验证环境
./bin/hello_planning  # 输出 "Planning Environment OK!"

环境搭建完成后，建议学员立即运行一个简单的规划算法示例——例如，在CARLA仿真环境中实现一个基于A*的全局路径规划，并在仿真地图上可视化规划结果。这一练习虽然简单，但能够帮助学员快速熟悉整个工具链的运作方式，包括：如何获取自车位置、如何获取地图信息、如何发布规划轨迹、如何在仿真器中可视化结果。后续章节的实战项目都将基于这套环境展开。

1.5 案例拆解：从零搭建一个最小规划系统原型

为了帮助学员建立对规划算法工程落地的整体认知，本节通过一个最小规划系统原型的搭建案例，展示规划算法在实际系统中的工作流程。该案例不涉及复杂算法，而是聚焦于数据流打通和系统集成，为后续章节的算法实现提供"骨架"。

案例背景与目标：在CARLA仿真环境的Town01地图中，实现一个最简单的规划系统：车辆从指定起点出发，沿道路中心线行驶到终点。系统仅包含全局路径规划（使用A*算法在车道级拓扑图上搜索）和轨迹跟踪（使用纯跟踪Pure Pursuit算法），不涉及行为决策和复杂避障。目标是打通"地图读取→路径搜索→轨迹生成→控制执行"的完整链路。

关键输入数据：CARLA Town01地图的高精路网（包含车道中心线、车道宽度、连接关系）；自车初始位置（x=100, y=100, θ=0°）；目标位置（x=800, y=600）。

实现步骤：第一步，通过CARLA Python API获取地图的路网拓扑，提取所有车道中心线采样点，构建拓扑图（节点为车道段，边为车道连接关系）；第二步，实现A*搜索算法，在拓扑图上搜索从起点车道到终点车道的最短路径，输出一系列车道段ID；第三步，将车道段中心线采样点作为全局路径点，使用三次样条插值生成平滑路径；第四步，实现纯跟踪控制器，根据当前车辆位置和路径点计算前轮转角，驱动车辆沿路径行驶；第五步，在CARLA中运行仿真，记录车辆实际行驶轨迹与规划路径的偏差。

结果与分析：在仿真中，车辆能够从起点沿规划路径行驶到终点，横向跟踪误差在直线段小于0.3米，在弯道段小于0.8米。通过PlotJuggler可视化可以看到，跟踪误差主要出现在弯道处，原因是纯跟踪控制器在弯道处的前视距离选择不够合理。调整前视距离参数后，弯道跟踪误差降低至0.5米以内。这一案例虽然简单，但完整展示了规划系统的基本工作流程，也暴露了工程中常见的参数调优问题。

案例复盘与经验总结：（1）数据接口是系统集成的最大成本——CARLA的地图数据格式与规划算法所需的数据格式存在差异，需要编写转换层；（2）坐标系一致性至关重要——CARLA使用UE坐标系（左手系），而规划算法通常使用右手系，需要仔细处理坐标变换；（3）参数调优需要系统化方法——前视距离、路径插值密度、控制频率等参数相互影响，建议使用仿真批量测试进行参数扫描；（4）可视化是调试的利器——将规划路径、实际轨迹、跟踪误差在同一坐标系下叠加显示，能够快速定位问题。

1.6 常见误区与学习建议

在规划算法的学习与工程实践中，初学者常陷入以下误区，需要特别警惕：

• 误区一：重算法轻工程。许多学习者热衷于研究最新的规划算法，却忽视了数据接口、坐标系变换、时间同步、异常处理等工程细节。实际上，在工业级自动驾驶系统中，工程问题往往比算法本身更耗时、更影响系统稳定性。建议在课程学习中，每实现一个算法，都要思考其工程落地的完整链路。
• 误区二：忽视车辆约束。规划算法输出的轨迹必须满足车辆的运动学/动力学约束，否则控制模块无法跟踪。例如，不考虑最大转向角限制的路径规划，在实车中必然导致跟踪失败。建议在学习运动规划时，始终将车辆模型作为核心约束纳入考虑。
• 误区三：过度依赖仿真。仿真环境无法完全模拟真实世界的传感器噪声、通信延迟、执行器误差和极端天气。建议在仿真验证通过后，尽早进行硬件在环测试和实车测试，发现仿真中无法暴露的问题。
• 误区四：忽略安全冗余。规划算法必须考虑感知失效、预测错误、控制偏差等异常情况，设计降级策略和紧急停车机制。安全冗余不是"锦上添花"，而是自动驾驶系统的"生命线"。
• 误区五：盲目追求最优。在实时系统中，规划算法必须在有限时间内给出可行解，而非最优解。工程实践中，通常采用"快速可行解+后台优化"的策略，保证系统在任何情况下都有输出。

最后，需要强调的是，规划算法是一个快速演进的领域。从早期的基于规则的方法，到后来的优化方法，再到如今深度学习与强化学习的引入，规划算法的技术栈在不断扩展。但无论技术如何变化，其核心目标——在复杂动态环境中生成安全、高效、舒适的轨迹——始终不变。掌握扎实的算法基础、工程实践能力和系统思维，是应对技术变革的根本之道。本课程将陪伴学员完成从理论到实践的完整跨越，为在自动驾驶领域深入发展奠定坚实基础。