1. 项目概述为什么我们需要一个清晰的规划控制理论体系在自动驾驶、机器人导航甚至是工业自动化领域我们常常听到“规划控制”这个词。听起来很高大上但说白了它解决的就是一个最朴素的问题如何让一个机器比如一辆车、一个机械臂从A点安全、高效、舒适地到达B点。你可能觉得这不就是设定起点和终点然后中间画条线吗如果世界是静止的、完全已知的那确实如此。但现实是路上有突然窜出的行人、旁边有加塞的车辆、地图信息可能滞后、传感器会有噪声……面对这些动态和不确定性那条“理想的线”需要被实时地、智能地“画”出来这就是规划控制的核心任务。我干了十多年机器人相关研发从实验室的轮式机器人到如今火热的智能驾驶踩过无数的坑。最大的教训之一就是没有坚实的理论体系作为地基所有的算法实现和代码优化都是空中楼阁。你可能调参调出一个在某个场景下表现不错的Demo但一旦场景稍加变化系统就可能崩溃而你甚至不知道问题出在哪里。这就是为什么我们需要系统地梳理“规划控制”的理论体系——它不是一堆数学公式的堆砌而是一套帮助我们理解问题本质、设计可靠方案、并能在出问题时快速定位的“思维框架”和“工具箱”。简单来说规划控制理论体系就是教你如何把“从A到B”这个模糊的需求拆解成一系列可定义、可建模、可求解、可验证的工程问题。它告诉你有哪些工具算法可用每种工具擅长解决什么问题它们的边界在哪里以及如何将它们组合起来构建一个鲁棒的系统。接下来我就结合自己这些年的实战经验为你拆解这个体系的核心脉络。2. 规划控制理论体系的核心框架与设计哲学规划控制不是一个单一的算法而是一个分层、分模块的复杂系统。为了便于理解我们可以把它想象成一家公司的决策与执行链。2.1 分层架构战略、战术与执行一个成熟的规划控制系统通常采用三层架构这几乎是业内的共识因为它清晰地分离了不同时间尺度和抽象层次的决策。第一层任务规划或路由规划这属于“战略层”。它的输入是全局地图和目的地输出是一条从起点到终点的宏观路径。它不关心具体的车道线只关心走哪条路、在哪个路口转弯。常用的算法是搜索算法如A*、Dijkstra或者基于图的路由算法。这一层的关键是全局最优和可行性。例如它要确保规划的路径不会让车辆驶入单行道或者无法通行的施工路段。第二层行为决策与轨迹规划这是“战术层”也是整个系统的智慧核心。它接收任务规划给出的宏观路径以及感知模块提供的周围动态障碍物信息车辆、行人、信号灯状态等然后做出具体的驾驶行为决策是跟车、换道、超车、还是停车等待基于这个决策它需要生成一条未来几秒内通常是3-8秒车辆应该遵循的、具体的运动轨迹。这条轨迹包含了每一个时间点车辆应该处于的位置x y以及对应的速度、加速度甚至朝向。这一层是理论体系中最复杂的部分涉及预测、决策、优化等多个子问题我们后面会详细展开。第三层运动控制这是“执行层”。它接收轨迹规划层给出的目标轨迹一系列期望的状态点然后通过控制算法如PID控制、模型预测控制MPC、滑模控制等计算出具体的执行指令方向盘应该转多少度转向角油门或刹车应该踩多深加速度。它的核心任务是精准跟踪和抗干扰确保车辆能够尽可能贴合规划出的理想轨迹同时克服风阻、路面不平等外部扰动。注意这三层并非严格串行而是存在频繁的交互。例如运动控制发现无法跟踪某条轨迹可能因为曲率太大需要反馈给轨迹规划层重新规划轨迹规划层发现所有选项都不可行时可能需要任务规划层重新规划全局路径。2.2 核心设计哲学在“最优”与“可行”之间权衡规划控制理论体系的背后贯穿着几个核心的设计哲学理解它们比记住几个算法更重要。1. 问题形式化把直觉变成数学这是理论体系的第一步。如何用数学语言描述“开得好”工程师们定义了一系列代价函数。例如效率代价希望尽快到达所以时间要短。舒适度代价加速度、加加速度冲击度要平滑不能急刹急加速。安全代价与障碍物、道路边界的距离要足够大。规则代价要遵守交通规则如车道中心线行驶。“规划一条好轨迹”就转化为了一个数学优化问题寻找一条轨迹使得上述所有代价的总和最小。不同的算法本质上是在用不同的方式求解这个优化问题。2. 预测与决策的耦合机器不能像人一样凭直觉预判他人行为。因此理论体系必须包含对周围动态物体交通参与者未来行为的预测。最简单的预测是假设对方匀速运动复杂的则可能用到交互式多模态预测。规划模块需要基于这些预测结果来做决策。这里有一个关键矛盾我的决策会影响他人的行为而他行为的变化又会影响我的决策。例如我想换道但如果预测到旁边车道的车会加速阻止我就应该放弃换道。处理这种强交互场景是当前理论研究和工程实践的前沿与难点。3. 不确定性的处理传感器有误差预测不准模型不完美。理论体系必须考虑这些不确定性。一种常见的方法是使用概率模型如高斯分布来描述车辆自身状态和障碍物的位置然后在规划时要求轨迹满足概率安全例如与障碍物碰撞的概率低于10^-6。另一种工程化的方法是引入安全余量比如在规划的轨迹与障碍物之间留出比理论计算更大的距离。4. 计算实时性规划控制是实时系统通常要求在几十到一百毫秒内完成一个计算周期。这意味着再优美的数学理论如果无法在有限时间内求解也无法落地。因此理论体系中包含大量关于问题简化、高效求解器如二次规划QP求解器和近似算法的研究。工程师常常需要在“最优解”和“快速可行解”之间做出妥协。3. 轨迹生成的核心方法论详解轨迹规划是规划控制体系中的“重头戏”其方法论主要分为两大类基于搜索的和基于优化的。它们各有优劣在实际系统中常常结合使用。3.1 基于搜索的规划方法在状态空间中“寻路”这类方法把规划问题看作在一个离散的“状态空间”中搜索一条路径。状态空间可以包括位置、速度、时间等维度。经典算法状态格子State Lattice搜索这种方法预先定义好一组运动基元。每个运动基元是一段短时间的、车辆从某个起始状态如特定速度和朝向到某个终止状态的可行轨迹。规划时算法像搭积木一样将这些运动基元在状态空间里连接起来从一个状态节点搜索到另一个状态节点直到到达目标区域形成一条完整的轨迹。搜索过程中每条路径都会根据代价函数长度、靠近障碍物程度等计算一个代价最终选择总代价最小的路径。优点完备性只要解存在在足够精细的离散化下搜索算法理论上能找到它。能处理复杂约束可以方便地将障碍物、交通规则作为约束条件在搜索时直接排除违反约束的节点。缺点维度灾难当状态空间维度较高如加入时间维度时离散化会导致节点数量爆炸搜索非常耗时。解的质量由于状态是离散的找到的路径通常是“折线”不够平滑需要后处理。实操心得在结构化道路如高速公路上状态空间可以设计得相对简单例如主要考虑纵向S和横向L坐标此时基于搜索的方法非常有效常用于生成换道、避障的候选轨迹。我们团队在早期项目中就用A*算法在(S L)二维空间里搜索效果直接且稳定。但要注意设计好状态离散化的分辨率太粗可能找不到安全路径太细则计算负担重。3.2 基于优化的规划方法寻找“最平滑”的曲线这是目前主流的方法尤其是对于生成舒适度要求高的轨迹。其核心思想是直接将轨迹表示为一个参数化的函数如多项式曲线、样条曲线然后通过求解一个优化问题来确定函数的参数。最常用的方法多项式轨迹优化我们通常将轨迹在横向和纵向分别规划。纵向规划负责速度profiles-t关系横向规划负责车道内的偏移d-s关系。以横向规划为例我们用一个五次多项式来描述横向偏移d关于纵向距离s的函数d(s) a0 a1*s a2*s^2 a3*s^3 a4*s^4 a5*s^5为什么是五次因为我们需要满足起点和终点的边界条件起点的横向偏移、横向速度一阶导、横向加速度二阶导终点的横向偏移、横向速度、横向加速度。六个边界条件正好可以求解五次多项式的六个系数。优化问题就是寻找一组系数a0~a5使得这条曲线满足边界条件同时让某个代价函数最小。代价函数通常包括舒适度代价曲线的二阶导加速度和三阶导加加速度的平方积分越小越平滑。中心线代价偏离车道中心线的惩罚。障碍物代价与预测的障碍物轨迹距离太近的惩罚。求解工具二次规划QP幸运的是如果代价函数是状态或多项式系数的二次型并且约束是线性的那么上述优化问题就转化为了一个二次规划问题。QP是凸优化问题有成熟、快速的求解器如OSQP qpOASES能够保证在毫秒级内找到全局最优解。这也是多项式方法在工业界如此流行的关键原因——它把复杂的轨迹规划问题转化为了一个可实时求解的标准数学问题。实操心得与避坑指南边界条件的设计是灵魂终点的横向速度、加速度设置至关重要。如果希望车辆最终平行于车道中心线行驶就应将终点的横向速度和加速度设为0。如果设置不当会导致规划出的轨迹在终点处仍然有横向运动趋势控制层很难跟踪。代价函数权重的调参安全、舒适、居中这几个代价的权重需要大量调试。例如在空旷道路可以增大“居中”权重让车开得更稳在靠近障碍物时需要临时增大“障碍物”权重优先保证安全。我们建立了一个基于场景的权重自适应模块效果比固定权重好很多。数值稳定性当规划周期很短如100ms时两次规划间的状态变化很小。如果直接用上一次规划的终点作为下一次规划的起点由于数值误差可能会导致优化问题无解。一个技巧是将起点状态做一个轻微的“平滑”或“松弛”不把它作为硬约束而是作为一个高权重的代价项。3.3 混合方法搜索优化在实际系统中纯搜索或纯优化都有局限。因此分层规划成为主流实践行为层/决策层基于搜索或规则生成几个宏观的“驾驶意图”例如“左换道”、“跟车”、“超车”。每种意图对应一个粗略的路径区域。轨迹生成层针对每一个驾驶意图使用基于优化的方法在该意图对应的路径区域内生成一条平滑、舒适、安全的具体轨迹。轨迹评价与选择对多条轨迹进行打分计算其总代价选择代价最小的一条输出给控制层。这种方法结合了搜索的“全局视野”和优化的“局部精细”既保证了能考虑多种可能性又保证了最终轨迹的质量。4. 预测模块与规划模块的交互闭环规划离不开预测。一个不考虑其他交通参与者未来行为的规划系统是危险且不实用的。预测模块为规划模块提供关键输入。4.1 预测的层次与常用方法运动学预测最简单也最常用。假设目标车辆保持当前速度、加速度或角速度运动。适用于短时距 2秒预测计算量小在大部分跟车场景下足够有效。基于地图的预测结合高精地图的车道信息假设车辆会沿车道中心线行驶或者基于车道线进行简单的换道预测。这比单纯的运动学预测更符合驾驶常识。交互式多模态预测这是前沿方向。它考虑到车辆之间的相互影响预测出多种可能的未来轨迹并为每种轨迹赋予一个概率。例如对于前方车辆它可能预测出“保持车道”、“向左换道”、“减速”等多种模态。规划模块需要为每一种可能的预测模态做好准备或者选择概率最高的一种进行响应。4.2 规划如何利用预测安全走廊与时空联合规划规划模块拿到预测轨迹后不能简单地把它们当作静态障碍物处理。因为预测是概率性的且可能有多条。1. 构建时空安全走廊这是最直观的方法。我们将时间和空间结合起来考虑。对于每一个预测的障碍物轨迹我们可以在时空S-T或L-T坐标系中画出一个“占据区域”。这个区域代表了该障碍物在未来一段时间内可能占据的空间。规划模块的任务就是生成一条自身车辆的轨迹在时空图上不与任何障碍物的占据区域相交。这相当于在时空图中为自车规划出一条“安全走廊”。2. 基于风险的规划对于多模态预测我们可以进行风险感知的规划。不是寻找一条绝对不与任何预测轨迹碰撞的路径可能不存在而是寻找一条期望风险最小的路径。期望风险是碰撞概率与碰撞严重程度的乘积。例如如果预测旁边车道车辆有10%的概率换道挤过来而直行车辆有0.1%的概率失控那么规划器可能会更倾向于防范那个10%概率的事件即使其严重性可能较低。实操中的挑战预测不准是常态尤其是对行人、自行车等弱势交通参与者其行为不确定性极大。我们的策略是对预测不确定性大的对象留出更大的安全余量。例如对于行人我们规划的安全距离通常比对于车辆更大。计算复杂度时空联合规划或考虑多模态预测会极大地增加规划问题的复杂度。工程上常采用滚动优化的方式只规划未来一个较短的时间窗口如3秒在这个窗口内精细考虑预测和交互窗口之外的则用更粗略的假设如假设障碍物消失或匀速。随着车辆前进这个时间窗口不断向前滚动。5. 控制层从轨迹到执行指令规划层输出了一条理想的时空轨迹控制层的任务就是让车辆的实际状态尽可能贴近这条理想轨迹。这本质上是一个跟踪问题。5.1 车辆模型知道自己控制的是什么设计控制器前必须有一个描述车辆运动的数学模型。最常用的是自行车模型也叫单车模型。它把四轮车辆简化为前后两轮并假设车辆只在平面运动忽略悬架和轮胎的复杂动力学。这个模型建立了车辆状态位置、朝向、速度与控制输入前轮转向角、纵向加速度之间的关系。虽然简化了很多但自行车模型在低速到中速范围内对于路径跟踪控制已经足够精确且形式简单便于控制器设计。它的状态方程是控制器如MPC设计的核心基础。5.2 主流控制算法对比PID控制经典且简单。它根据轨迹跟踪的误差横向位置误差、朝向误差的比例、积分、微分项来计算转向角。对于速度控制跟踪目标速度非常有效。但对于复杂的轨迹跟踪单纯的PID很难处理好车辆模型非线性和各种约束性能有限。纯跟踪控制一种几何跟踪方法。它在车辆前方选择一个“预瞄点”然后控制车辆转向使得车辆能够沿着一条圆弧路径到达该预瞄点。方法直观参数少主要是一个预瞄距离在低速场景下效果不错。但预瞄距离需要根据车速调整且理论上的稳定性保证较弱。线性二次型调节器这是一种最优控制方法。它首先在参考轨迹的每一个点附近对车辆模型进行线性化得到一个线性时变系统。然后设计一个控制器最小化跟踪误差和控制量的加权平方和。LQR能提供很好的理论性能保证但需要实时求解Riccati方程计算量相对较大且对模型精度比较敏感。模型预测控制这是目前业界的“明星”算法也是我个人认为最适合车辆轨迹跟踪的控制方法。MPC的核心思想可以概括为“边走边看滚动优化”预测在每个控制周期基于当前车辆状态和车辆模型预测未来一段时间预测时域内车辆在多种控制输入下的行为。优化从所有可能的未来控制序列中寻找一个最优序列使得预测的轨迹与期望轨迹的误差最小同时控制量平滑并且满足各种约束如转向角速度限制、加速度限制。执行只采用最优控制序列中的第一个控制量作用于车辆。滚动到下一个控制周期重复以上步骤基于新的状态重新进行预测和优化。MPC的优势在于它能显式地处理系统的约束如执行器的物理极限并且是一种前馈-反馈结合的控制对模型误差和干扰有一定的鲁棒性。它把控制问题也转化为了一个优化问题通常是二次规划与规划层的优化问题在数学形式上统一非常优雅。5.3 控制层实操要点接口设计规划层给控制层的不应该仅仅是一条几何路径而最好是一条带时间信息的轨迹即每个点都有对应的期望速度、加速度。这样控制层可以同时进行横向转向和纵向速度的协同控制效果远好于分开控制。抗积分饱和无论是PID还是MPC在使用积分项消除稳态误差时都必须注意积分饱和问题。当误差长期存在如车辆一直无法到达目标点积分项会累积到非常大一旦误差反向系统需要很长时间才能响应。必须加入抗积分饱和逻辑。前馈补偿对于已知的“干扰”如前轮转角对路径曲率的响应可以加入前馈控制。例如跟踪一条固定曲率的圆弧可以直接计算出一个稳态的前轮转角作为前馈量再结合反馈控制消除误差这样可以大幅提升跟踪精度和响应速度。参数标定与迭代控制器的参数如MPC的预测时域、代价函数权重需要大量实车测试来标定。不同车型质量、轴距、转向传动比不同、不同车速下的最优参数可能不同。我们建立了参数与车速、曲率的查找表实现了参数的自适应调整。6. 系统集成与工程化挑战把规划和控制算法跑在仿真器里是一回事把它部署到实车上稳定运行是另一回事。这个过程中会遇到大量理论模型覆盖不到的工程问题。6.1 时序与同步规划控制模块通常以固定的频率运行如10Hz。但感知模块的输出、定位模块的输出可能有不同的延迟和频率。如何保证规划模块使用的是时间戳对齐的、一致的全局状态这里需要一个数据同步与融合模块。我们通常采用“缓存插值”的方法。为每个数据流维护一个短时间的历史缓存当规划周期到来时将所有需要的信息统一插值或 extrapolate 到同一个时间戳上。处理不好时序会导致规划基于“过时”或“未来”的信息做出决策引发严重问题。6.2 故障诊断与降级策略系统必须能处理异常情况。例如感知失效某个传感器突然没有输出。规划模块是应该紧急停车还是基于历史信息继续行驶一段这需要设计安全的降级策略比如立即触发“最小风险状态”缓慢减速并靠边停车。规划器无解当前场景下优化问题找不到满足所有约束的可行解。此时不能直接输出空值而应该有一个“回退轨迹”比如沿当前曲率匀速减速的轨迹或者直接复制上一周期的轨迹并做减速处理同时向上层报警。控制跟踪误差过大实际轨迹与规划轨迹的偏差超过阈值。这可能是因为路面湿滑、轮胎打滑或者模型失配。此时需要触发重新规划或者切换到一个更保守的控制器。6.3 大量实车测试与Corner Case处理理论体系提供了主干但血肉来自于无数次的实车测试。你会遇到无数个“Corner Case”鬼探头静止车辆后面突然跑出行人。加塞旁边车辆突然强硬地插入你的前方。特殊交通参与者道路上的三角锥、遗落的纸箱、动物。恶劣天气大雨、大雪影响感知和车辆动力学模型。对于这些情况没有银弹算法。我们的做法是场景库建设尽可能多地收集真实和模拟的Corner Case场景形成测试用例库。参数自适应针对不同场景如高速、城区、拥堵动态调整规划算法的代价函数权重和安全距离参数。规则兜底在优化算法之上增加一层基于规则的“监控器”或“仲裁器”。当优化算法输出的轨迹在某些硬性安全指标上如与障碍物的TTC小于阈值不合格时由规则层覆盖发出紧急制动或转向指令。这相当于为智能系统加了一道保险。7. 理论体系的演进与个人思考回顾规划控制理论的发展它从最初的几何方法、基于规则的方法发展到今天以优化搜索和模型预测为主流。推动其发展的核心动力始终是对安全性、舒适性、智能性日益增长的需求与有限的计算资源、不确定的现实环境之间的矛盾。我个人在实践中最深的一点体会是不要迷信任何一个单一的算法或理论。无论是A*、Lattice、QP还是MPC它们都是工具箱里的工具。一个优秀的规划控制系统工程师应该深刻理解每个工具的原理、优势和局限性。他的核心能力体现在如何根据具体的场景、硬件条件和性能要求灵活地选择和组合这些工具并设计出稳健的故障处理和安全冗余机制。例如在计算资源极其有限的嵌入式平台上你可能不得不采用非常简化的模型和规则为主的决策而在算力充足的域控制器上你就可以运行复杂的多模态预测和时空联合优化。再比如对于结构化程度高的高速公路基于 Frenet 坐标系和多项式优化的方法非常高效而对于混乱无序的城区非结构化道路可能更需要依赖搜索和语义地图。最后这个领域仍在快速演进。端到端学习、强化学习等数据驱动的方法正在挑战传统基于模型的规划控制范式。它们有潜力处理更复杂的交互和长尾场景但其可解释性、安全性的验证仍是巨大挑战。我认为在未来很长一段时间内“基于模型的优化”与“数据驱动的学习”相结合的混合范式将是主流的发展方向。用学习的方法来提升预测的准确性、决策的智能性甚至直接生成更合理的代价函数同时用基于模型的优化来保证轨迹的平滑性、安全性和实时可控性两者相辅相成可能是通向更高阶自动驾驶的可行路径。这要求我们从业者不仅要夯实传统控制理论、优化理论的基础也要保持开放心态积极学习和理解机器学习的新进展。 自动驾驶规划控制理论体系：从A到B的智能决策与轨迹优化
1. 项目概述为什么我们需要一个清晰的规划控制理论体系在自动驾驶、机器人导航甚至是工业自动化领域我们常常听到“规划控制”这个词。听起来很高大上但说白了它解决的就是一个最朴素的问题如何让一个机器比如一辆车、一个机械臂从A点安全、高效、舒适地到达B点。你可能觉得这不就是设定起点和终点然后中间画条线吗如果世界是静止的、完全已知的那确实如此。但现实是路上有突然窜出的行人、旁边有加塞的车辆、地图信息可能滞后、传感器会有噪声……面对这些动态和不确定性那条“理想的线”需要被实时地、智能地“画”出来这就是规划控制的核心任务。我干了十多年机器人相关研发从实验室的轮式机器人到如今火热的智能驾驶踩过无数的坑。最大的教训之一就是没有坚实的理论体系作为地基所有的算法实现和代码优化都是空中楼阁。你可能调参调出一个在某个场景下表现不错的Demo但一旦场景稍加变化系统就可能崩溃而你甚至不知道问题出在哪里。这就是为什么我们需要系统地梳理“规划控制”的理论体系——它不是一堆数学公式的堆砌而是一套帮助我们理解问题本质、设计可靠方案、并能在出问题时快速定位的“思维框架”和“工具箱”。简单来说规划控制理论体系就是教你如何把“从A到B”这个模糊的需求拆解成一系列可定义、可建模、可求解、可验证的工程问题。它告诉你有哪些工具算法可用每种工具擅长解决什么问题它们的边界在哪里以及如何将它们组合起来构建一个鲁棒的系统。接下来我就结合自己这些年的实战经验为你拆解这个体系的核心脉络。2. 规划控制理论体系的核心框架与设计哲学规划控制不是一个单一的算法而是一个分层、分模块的复杂系统。为了便于理解我们可以把它想象成一家公司的决策与执行链。2.1 分层架构战略、战术与执行一个成熟的规划控制系统通常采用三层架构这几乎是业内的共识因为它清晰地分离了不同时间尺度和抽象层次的决策。第一层任务规划或路由规划这属于“战略层”。它的输入是全局地图和目的地输出是一条从起点到终点的宏观路径。它不关心具体的车道线只关心走哪条路、在哪个路口转弯。常用的算法是搜索算法如A*、Dijkstra或者基于图的路由算法。这一层的关键是全局最优和可行性。例如它要确保规划的路径不会让车辆驶入单行道或者无法通行的施工路段。第二层行为决策与轨迹规划这是“战术层”也是整个系统的智慧核心。它接收任务规划给出的宏观路径以及感知模块提供的周围动态障碍物信息车辆、行人、信号灯状态等然后做出具体的驾驶行为决策是跟车、换道、超车、还是停车等待基于这个决策它需要生成一条未来几秒内通常是3-8秒车辆应该遵循的、具体的运动轨迹。这条轨迹包含了每一个时间点车辆应该处于的位置x y以及对应的速度、加速度甚至朝向。这一层是理论体系中最复杂的部分涉及预测、决策、优化等多个子问题我们后面会详细展开。第三层运动控制这是“执行层”。它接收轨迹规划层给出的目标轨迹一系列期望的状态点然后通过控制算法如PID控制、模型预测控制MPC、滑模控制等计算出具体的执行指令方向盘应该转多少度转向角油门或刹车应该踩多深加速度。它的核心任务是精准跟踪和抗干扰确保车辆能够尽可能贴合规划出的理想轨迹同时克服风阻、路面不平等外部扰动。注意这三层并非严格串行而是存在频繁的交互。例如运动控制发现无法跟踪某条轨迹可能因为曲率太大需要反馈给轨迹规划层重新规划轨迹规划层发现所有选项都不可行时可能需要任务规划层重新规划全局路径。2.2 核心设计哲学在“最优”与“可行”之间权衡规划控制理论体系的背后贯穿着几个核心的设计哲学理解它们比记住几个算法更重要。1. 问题形式化把直觉变成数学这是理论体系的第一步。如何用数学语言描述“开得好”工程师们定义了一系列代价函数。例如效率代价希望尽快到达所以时间要短。舒适度代价加速度、加加速度冲击度要平滑不能急刹急加速。安全代价与障碍物、道路边界的距离要足够大。规则代价要遵守交通规则如车道中心线行驶。“规划一条好轨迹”就转化为了一个数学优化问题寻找一条轨迹使得上述所有代价的总和最小。不同的算法本质上是在用不同的方式求解这个优化问题。2. 预测与决策的耦合机器不能像人一样凭直觉预判他人行为。因此理论体系必须包含对周围动态物体交通参与者未来行为的预测。最简单的预测是假设对方匀速运动复杂的则可能用到交互式多模态预测。规划模块需要基于这些预测结果来做决策。这里有一个关键矛盾我的决策会影响他人的行为而他行为的变化又会影响我的决策。例如我想换道但如果预测到旁边车道的车会加速阻止我就应该放弃换道。处理这种强交互场景是当前理论研究和工程实践的前沿与难点。3. 不确定性的处理传感器有误差预测不准模型不完美。理论体系必须考虑这些不确定性。一种常见的方法是使用概率模型如高斯分布来描述车辆自身状态和障碍物的位置然后在规划时要求轨迹满足概率安全例如与障碍物碰撞的概率低于10^-6。另一种工程化的方法是引入安全余量比如在规划的轨迹与障碍物之间留出比理论计算更大的距离。4. 计算实时性规划控制是实时系统通常要求在几十到一百毫秒内完成一个计算周期。这意味着再优美的数学理论如果无法在有限时间内求解也无法落地。因此理论体系中包含大量关于问题简化、高效求解器如二次规划QP求解器和近似算法的研究。工程师常常需要在“最优解”和“快速可行解”之间做出妥协。3. 轨迹生成的核心方法论详解轨迹规划是规划控制体系中的“重头戏”其方法论主要分为两大类基于搜索的和基于优化的。它们各有优劣在实际系统中常常结合使用。3.1 基于搜索的规划方法在状态空间中“寻路”这类方法把规划问题看作在一个离散的“状态空间”中搜索一条路径。状态空间可以包括位置、速度、时间等维度。经典算法状态格子State Lattice搜索这种方法预先定义好一组运动基元。每个运动基元是一段短时间的、车辆从某个起始状态如特定速度和朝向到某个终止状态的可行轨迹。规划时算法像搭积木一样将这些运动基元在状态空间里连接起来从一个状态节点搜索到另一个状态节点直到到达目标区域形成一条完整的轨迹。搜索过程中每条路径都会根据代价函数长度、靠近障碍物程度等计算一个代价最终选择总代价最小的路径。优点完备性只要解存在在足够精细的离散化下搜索算法理论上能找到它。能处理复杂约束可以方便地将障碍物、交通规则作为约束条件在搜索时直接排除违反约束的节点。缺点维度灾难当状态空间维度较高如加入时间维度时离散化会导致节点数量爆炸搜索非常耗时。解的质量由于状态是离散的找到的路径通常是“折线”不够平滑需要后处理。实操心得在结构化道路如高速公路上状态空间可以设计得相对简单例如主要考虑纵向S和横向L坐标此时基于搜索的方法非常有效常用于生成换道、避障的候选轨迹。我们团队在早期项目中就用A*算法在(S L)二维空间里搜索效果直接且稳定。但要注意设计好状态离散化的分辨率太粗可能找不到安全路径太细则计算负担重。3.2 基于优化的规划方法寻找“最平滑”的曲线这是目前主流的方法尤其是对于生成舒适度要求高的轨迹。其核心思想是直接将轨迹表示为一个参数化的函数如多项式曲线、样条曲线然后通过求解一个优化问题来确定函数的参数。最常用的方法多项式轨迹优化我们通常将轨迹在横向和纵向分别规划。纵向规划负责速度profiles-t关系横向规划负责车道内的偏移d-s关系。以横向规划为例我们用一个五次多项式来描述横向偏移d关于纵向距离s的函数d(s) a0 a1*s a2*s^2 a3*s^3 a4*s^4 a5*s^5为什么是五次因为我们需要满足起点和终点的边界条件起点的横向偏移、横向速度一阶导、横向加速度二阶导终点的横向偏移、横向速度、横向加速度。六个边界条件正好可以求解五次多项式的六个系数。优化问题就是寻找一组系数a0~a5使得这条曲线满足边界条件同时让某个代价函数最小。代价函数通常包括舒适度代价曲线的二阶导加速度和三阶导加加速度的平方积分越小越平滑。中心线代价偏离车道中心线的惩罚。障碍物代价与预测的障碍物轨迹距离太近的惩罚。求解工具二次规划QP幸运的是如果代价函数是状态或多项式系数的二次型并且约束是线性的那么上述优化问题就转化为了一个二次规划问题。QP是凸优化问题有成熟、快速的求解器如OSQP qpOASES能够保证在毫秒级内找到全局最优解。这也是多项式方法在工业界如此流行的关键原因——它把复杂的轨迹规划问题转化为了一个可实时求解的标准数学问题。实操心得与避坑指南边界条件的设计是灵魂终点的横向速度、加速度设置至关重要。如果希望车辆最终平行于车道中心线行驶就应将终点的横向速度和加速度设为0。如果设置不当会导致规划出的轨迹在终点处仍然有横向运动趋势控制层很难跟踪。代价函数权重的调参安全、舒适、居中这几个代价的权重需要大量调试。例如在空旷道路可以增大“居中”权重让车开得更稳在靠近障碍物时需要临时增大“障碍物”权重优先保证安全。我们建立了一个基于场景的权重自适应模块效果比固定权重好很多。数值稳定性当规划周期很短如100ms时两次规划间的状态变化很小。如果直接用上一次规划的终点作为下一次规划的起点由于数值误差可能会导致优化问题无解。一个技巧是将起点状态做一个轻微的“平滑”或“松弛”不把它作为硬约束而是作为一个高权重的代价项。3.3 混合方法搜索优化在实际系统中纯搜索或纯优化都有局限。因此分层规划成为主流实践行为层/决策层基于搜索或规则生成几个宏观的“驾驶意图”例如“左换道”、“跟车”、“超车”。每种意图对应一个粗略的路径区域。轨迹生成层针对每一个驾驶意图使用基于优化的方法在该意图对应的路径区域内生成一条平滑、舒适、安全的具体轨迹。轨迹评价与选择对多条轨迹进行打分计算其总代价选择代价最小的一条输出给控制层。这种方法结合了搜索的“全局视野”和优化的“局部精细”既保证了能考虑多种可能性又保证了最终轨迹的质量。4. 预测模块与规划模块的交互闭环规划离不开预测。一个不考虑其他交通参与者未来行为的规划系统是危险且不实用的。预测模块为规划模块提供关键输入。4.1 预测的层次与常用方法运动学预测最简单也最常用。假设目标车辆保持当前速度、加速度或角速度运动。适用于短时距 2秒预测计算量小在大部分跟车场景下足够有效。基于地图的预测结合高精地图的车道信息假设车辆会沿车道中心线行驶或者基于车道线进行简单的换道预测。这比单纯的运动学预测更符合驾驶常识。交互式多模态预测这是前沿方向。它考虑到车辆之间的相互影响预测出多种可能的未来轨迹并为每种轨迹赋予一个概率。例如对于前方车辆它可能预测出“保持车道”、“向左换道”、“减速”等多种模态。规划模块需要为每一种可能的预测模态做好准备或者选择概率最高的一种进行响应。4.2 规划如何利用预测安全走廊与时空联合规划规划模块拿到预测轨迹后不能简单地把它们当作静态障碍物处理。因为预测是概率性的且可能有多条。1. 构建时空安全走廊这是最直观的方法。我们将时间和空间结合起来考虑。对于每一个预测的障碍物轨迹我们可以在时空S-T或L-T坐标系中画出一个“占据区域”。这个区域代表了该障碍物在未来一段时间内可能占据的空间。规划模块的任务就是生成一条自身车辆的轨迹在时空图上不与任何障碍物的占据区域相交。这相当于在时空图中为自车规划出一条“安全走廊”。2. 基于风险的规划对于多模态预测我们可以进行风险感知的规划。不是寻找一条绝对不与任何预测轨迹碰撞的路径可能不存在而是寻找一条期望风险最小的路径。期望风险是碰撞概率与碰撞严重程度的乘积。例如如果预测旁边车道车辆有10%的概率换道挤过来而直行车辆有0.1%的概率失控那么规划器可能会更倾向于防范那个10%概率的事件即使其严重性可能较低。实操中的挑战预测不准是常态尤其是对行人、自行车等弱势交通参与者其行为不确定性极大。我们的策略是对预测不确定性大的对象留出更大的安全余量。例如对于行人我们规划的安全距离通常比对于车辆更大。计算复杂度时空联合规划或考虑多模态预测会极大地增加规划问题的复杂度。工程上常采用滚动优化的方式只规划未来一个较短的时间窗口如3秒在这个窗口内精细考虑预测和交互窗口之外的则用更粗略的假设如假设障碍物消失或匀速。随着车辆前进这个时间窗口不断向前滚动。5. 控制层从轨迹到执行指令规划层输出了一条理想的时空轨迹控制层的任务就是让车辆的实际状态尽可能贴近这条理想轨迹。这本质上是一个跟踪问题。5.1 车辆模型知道自己控制的是什么设计控制器前必须有一个描述车辆运动的数学模型。最常用的是自行车模型也叫单车模型。它把四轮车辆简化为前后两轮并假设车辆只在平面运动忽略悬架和轮胎的复杂动力学。这个模型建立了车辆状态位置、朝向、速度与控制输入前轮转向角、纵向加速度之间的关系。虽然简化了很多但自行车模型在低速到中速范围内对于路径跟踪控制已经足够精确且形式简单便于控制器设计。它的状态方程是控制器如MPC设计的核心基础。5.2 主流控制算法对比PID控制经典且简单。它根据轨迹跟踪的误差横向位置误差、朝向误差的比例、积分、微分项来计算转向角。对于速度控制跟踪目标速度非常有效。但对于复杂的轨迹跟踪单纯的PID很难处理好车辆模型非线性和各种约束性能有限。纯跟踪控制一种几何跟踪方法。它在车辆前方选择一个“预瞄点”然后控制车辆转向使得车辆能够沿着一条圆弧路径到达该预瞄点。方法直观参数少主要是一个预瞄距离在低速场景下效果不错。但预瞄距离需要根据车速调整且理论上的稳定性保证较弱。线性二次型调节器这是一种最优控制方法。它首先在参考轨迹的每一个点附近对车辆模型进行线性化得到一个线性时变系统。然后设计一个控制器最小化跟踪误差和控制量的加权平方和。LQR能提供很好的理论性能保证但需要实时求解Riccati方程计算量相对较大且对模型精度比较敏感。模型预测控制这是目前业界的“明星”算法也是我个人认为最适合车辆轨迹跟踪的控制方法。MPC的核心思想可以概括为“边走边看滚动优化”预测在每个控制周期基于当前车辆状态和车辆模型预测未来一段时间预测时域内车辆在多种控制输入下的行为。优化从所有可能的未来控制序列中寻找一个最优序列使得预测的轨迹与期望轨迹的误差最小同时控制量平滑并且满足各种约束如转向角速度限制、加速度限制。执行只采用最优控制序列中的第一个控制量作用于车辆。滚动到下一个控制周期重复以上步骤基于新的状态重新进行预测和优化。MPC的优势在于它能显式地处理系统的约束如执行器的物理极限并且是一种前馈-反馈结合的控制对模型误差和干扰有一定的鲁棒性。它把控制问题也转化为了一个优化问题通常是二次规划与规划层的优化问题在数学形式上统一非常优雅。5.3 控制层实操要点接口设计规划层给控制层的不应该仅仅是一条几何路径而最好是一条带时间信息的轨迹即每个点都有对应的期望速度、加速度。这样控制层可以同时进行横向转向和纵向速度的协同控制效果远好于分开控制。抗积分饱和无论是PID还是MPC在使用积分项消除稳态误差时都必须注意积分饱和问题。当误差长期存在如车辆一直无法到达目标点积分项会累积到非常大一旦误差反向系统需要很长时间才能响应。必须加入抗积分饱和逻辑。前馈补偿对于已知的“干扰”如前轮转角对路径曲率的响应可以加入前馈控制。例如跟踪一条固定曲率的圆弧可以直接计算出一个稳态的前轮转角作为前馈量再结合反馈控制消除误差这样可以大幅提升跟踪精度和响应速度。参数标定与迭代控制器的参数如MPC的预测时域、代价函数权重需要大量实车测试来标定。不同车型质量、轴距、转向传动比不同、不同车速下的最优参数可能不同。我们建立了参数与车速、曲率的查找表实现了参数的自适应调整。6. 系统集成与工程化挑战把规划和控制算法跑在仿真器里是一回事把它部署到实车上稳定运行是另一回事。这个过程中会遇到大量理论模型覆盖不到的工程问题。6.1 时序与同步规划控制模块通常以固定的频率运行如10Hz。但感知模块的输出、定位模块的输出可能有不同的延迟和频率。如何保证规划模块使用的是时间戳对齐的、一致的全局状态这里需要一个数据同步与融合模块。我们通常采用“缓存插值”的方法。为每个数据流维护一个短时间的历史缓存当规划周期到来时将所有需要的信息统一插值或 extrapolate 到同一个时间戳上。处理不好时序会导致规划基于“过时”或“未来”的信息做出决策引发严重问题。6.2 故障诊断与降级策略系统必须能处理异常情况。例如感知失效某个传感器突然没有输出。规划模块是应该紧急停车还是基于历史信息继续行驶一段这需要设计安全的降级策略比如立即触发“最小风险状态”缓慢减速并靠边停车。规划器无解当前场景下优化问题找不到满足所有约束的可行解。此时不能直接输出空值而应该有一个“回退轨迹”比如沿当前曲率匀速减速的轨迹或者直接复制上一周期的轨迹并做减速处理同时向上层报警。控制跟踪误差过大实际轨迹与规划轨迹的偏差超过阈值。这可能是因为路面湿滑、轮胎打滑或者模型失配。此时需要触发重新规划或者切换到一个更保守的控制器。6.3 大量实车测试与Corner Case处理理论体系提供了主干但血肉来自于无数次的实车测试。你会遇到无数个“Corner Case”鬼探头静止车辆后面突然跑出行人。加塞旁边车辆突然强硬地插入你的前方。特殊交通参与者道路上的三角锥、遗落的纸箱、动物。恶劣天气大雨、大雪影响感知和车辆动力学模型。对于这些情况没有银弹算法。我们的做法是场景库建设尽可能多地收集真实和模拟的Corner Case场景形成测试用例库。参数自适应针对不同场景如高速、城区、拥堵动态调整规划算法的代价函数权重和安全距离参数。规则兜底在优化算法之上增加一层基于规则的“监控器”或“仲裁器”。当优化算法输出的轨迹在某些硬性安全指标上如与障碍物的TTC小于阈值不合格时由规则层覆盖发出紧急制动或转向指令。这相当于为智能系统加了一道保险。7. 理论体系的演进与个人思考回顾规划控制理论的发展它从最初的几何方法、基于规则的方法发展到今天以优化搜索和模型预测为主流。推动其发展的核心动力始终是对安全性、舒适性、智能性日益增长的需求与有限的计算资源、不确定的现实环境之间的矛盾。我个人在实践中最深的一点体会是不要迷信任何一个单一的算法或理论。无论是A*、Lattice、QP还是MPC它们都是工具箱里的工具。一个优秀的规划控制系统工程师应该深刻理解每个工具的原理、优势和局限性。他的核心能力体现在如何根据具体的场景、硬件条件和性能要求灵活地选择和组合这些工具并设计出稳健的故障处理和安全冗余机制。例如在计算资源极其有限的嵌入式平台上你可能不得不采用非常简化的模型和规则为主的决策而在算力充足的域控制器上你就可以运行复杂的多模态预测和时空联合优化。再比如对于结构化程度高的高速公路基于 Frenet 坐标系和多项式优化的方法非常高效而对于混乱无序的城区非结构化道路可能更需要依赖搜索和语义地图。最后这个领域仍在快速演进。端到端学习、强化学习等数据驱动的方法正在挑战传统基于模型的规划控制范式。它们有潜力处理更复杂的交互和长尾场景但其可解释性、安全性的验证仍是巨大挑战。我认为在未来很长一段时间内“基于模型的优化”与“数据驱动的学习”相结合的混合范式将是主流的发展方向。用学习的方法来提升预测的准确性、决策的智能性甚至直接生成更合理的代价函数同时用基于模型的优化来保证轨迹的平滑性、安全性和实时可控性两者相辅相成可能是通向更高阶自动驾驶的可行路径。这要求我们从业者不仅要夯实传统控制理论、优化理论的基础也要保持开放心态积极学习和理解机器学习的新进展。