1. 项目概述规划模块不是“画路线”而是给车装上实时决策大脑“自动驾驶中的《规划》在做什么”——这个问题我被问过不下两百次从高校实验室的研究生到车企新入职的算法工程师再到对技术好奇的汽车媒体编辑。很多人第一反应是“哦不就是高德地图那种路径规划吗选条最近的路开过去”——这个理解偏差恰恰是入行前最危险的认知陷阱。规划Planning在自动驾驶系统里根本不是静态导航而是一套每秒运行数十次、融合感知结果、预测意图、权衡安全与效率、并生成可执行运动指令的实时决策中枢。它处在感知Perception和控制Control之间像一个坐在驾驶座上的“数字老司机”左边眼睛盯着感知模块传来的360度障碍物位置、速度、类型右边耳朵听着预测模块对周围车辆未来3秒轨迹的判断脑子里飞速计算着“现在打多少方向盘、踩多深油门、要不要刹停、变道是否安全、跟车距离留多少才既不吓人又不被加塞”——所有这些都在毫秒级完成。它输出的不是一张地图上的红蓝线而是一组带时间戳的、平滑连续的车辆位姿序列x, y, θ, v, a直接喂给底盘控制模块去执行。所以它解决的从来不是“去哪里”而是“此刻这一帧车该以什么姿态、多快的速度、沿着哪条曲率最优的轨迹安全、舒适、合规地抵达下一个微小目标点”。这背后牵扯到运动学约束、动力学可行性、交通规则硬边界、人类驾驶习惯软约束、甚至突发状况下的博弈策略。我带过的实习生里有三个栽在同一个坑里把规划当成离线路径搜索来调参结果实车一上路就频繁急刹或犹豫不决。后来我们干脆在团队内部立下一条铁律任何规划模块的调试必须在闭环仿真中先跑满1000公里无接管再上封闭场地上路前得用真实传感器数据回放验证其对“鬼探头”、“加塞车突然减速”、“施工锥桶阵列”三类典型长尾场景的响应逻辑是否合理。这篇文章不讲公式推导也不堆砌论文名词就用我过去八年在L2到L4系统量产落地中踩过的坑、调过的参数、写过的测试用例把“规划到底在忙什么”这件事掰开揉碎说透。2. 规划模块的核心设计思路与方案选型逻辑2.1 为什么不能直接用高德/百度地图的路径规划这是最常被忽略的底层差异。地图导航的“路径规划”本质是图搜索问题把城市路网抽象成节点路口和边路段用A*或Dijkstra算法找起点到终点的最短/最快路径。它假设道路是静态的、通行规则是确定的、车辆是理想质点。但自动驾驶面对的是一个动态、不确定、充满博弈的物理世界。举个最简单的例子导航软件告诉你“前方500米右转”但规划模块在每一帧通常20-50ms都要回答“此刻我的车头距离路口还有499.8米旁边车道有辆SUV正以65km/h匀速行驶它距离我的横向距离只有2.3米且没有打灯我的车速是58km/h方向盘当前转角是1.2度如果维持当前状态2.1秒后将与SUV发生碰撞——那么我该立刻降速到50km/h并微调方向盘向左偏移0.3度还是保持车速但准备在300米处开始缓打方向右转同时预留0.8秒的加速冗余应对SUV可能的突然变道”——这种毫秒级、带物理约束、含风险预判的决策图搜索算法连输入数据都接不住。规划模块处理的是“轨迹”不是“路径”它优化的是“运动过程”不是“空间连线”。我们曾做过对比实验把高德SDK的路径点直接喂给控制模块车辆在空旷停车场能走直线但一进入真实路口遇到一辆缓慢汇入的电动车系统就因无法计算避让加速度而触发紧急制动。根本原因在于导航路径缺乏时间维度和运动学可行性校验。2.2 主流架构选型分层规划 vs. 端到端为什么我们坚持分层目前行业主要有两条技术路线一是传统分层架构Hierarchical Planning二是端到端学习End-to-End Learning。前者把规划拆解为“行为决策Behavioral Decision”和“运动规划Motion Planning”两个子模块后者试图用一个深度神经网络直接从原始传感器输入映射到方向盘转角和油门开度。我们团队在2019年深度评估过端到端方案结论很明确在L2量产车上端到端目前仍是“好看不好用”的技术玩具。原因有三第一可解释性为零。当车辆在雨天高速上突然向左猛打方向你无法追溯是哪个像素点的反光触发了错误决策这在车规级功能安全ISO 26262 ASIL-B认证中是致命伤第二长尾场景泛化能力极差。模型在99%的晴天城区数据上表现完美但遇到一次“洒水车后方形成的镜面水洼”就可能把水洼倒影识别成实体障碍物而急刹第三迭代成本高。修改一个交通规则比如某城市新增“公交车道禁止社会车辆借道”分层架构只需更新行为决策模块的一条规则引擎而端到端模型需要重新采集数百万公里对应场景数据并重训。所以我们坚持分层架构但做了关键改良把传统“决策-规划-控制”三层压缩为“行为决策轨迹生成”两层并在轨迹生成层内置了实时动力学可行性校验器。这样既保留了规则的可解释性和快速迭代能力又通过紧耦合降低了层间信息损失。实测下来这套改良架构在高速NOA功能中变道成功率从92.7%提升至98.3%且接管原因中“规划不合理”类占比下降了64%。2.3 行为决策层规则引擎与学习模型的黄金配比行为决策层是规划模块的“大脑皮层”负责回答“我要做什么”。常见动作包括跟车Follow、跟停Stop、换道Lane Change、超车Overtake、绕行Avoid、停止等待Wait。很多团队早期全靠手写规则比如“当前车距1.5秒时长且本车速度30km/h则触发跟停”。但现实远比这复杂。我们曾遇到一个经典案例在暴雨夜一辆大货车在前方缓慢行驶雷达显示其后方20米处有一辆轿车正以相同速度跟随。我们的规则引擎判定“前方无加塞风险可保持跟车”结果轿车司机突然猛打方向变道导致我们的车因未预留足够侧向冗余而差点擦碰。复盘发现规则只看了“距离”和“速度”却忽略了“天气”、“光照”、“车辆类型”货车遮挡视线这三个关键上下文。于是我们引入了轻量级学习模型一个仅128个神经元的MLP来辅助规则引擎。它的输入是当前车距、相对速度、本车速度、天气标签晴/雨/雾、光照强度来自摄像头直方图、前车类型卡车/轿车/摩托车、后方车辆数量及距离。模型不直接输出动作而是输出一个“风险权重系数”范围0.0~1.0。规则引擎的原始阈值如1.5秒会乘以这个系数动态调整。例如暴雨卡车后方有车时系数升至0.6阈值就变成0.9秒提前触发更保守的跟停策略。这个设计的关键在于规则是主干学习是枝叶规则保证底线安全学习提供情境适应性。模型训练数据全部来自真实接管日志标注员只标“此处是否应更早介入”不标具体动作极大降低了标注成本。上线后雨天高速跟车的误刹车率下降了57%而极端保守行为如无故降速反而减少了23%因为模型学会了区分“真风险”和“虚惊一场”。2.4 轨迹生成层为什么选择优化方法而非采样方法轨迹生成层是规划模块的“小脑”负责把行为决策的指令如“现在开始向左变道”转化为一条平滑、可行、舒适的车辆轨迹。主流方法有两大类基于采样的Sampling-based如RRT*和基于优化的Optimization-based如ST Graph 二次规划。我们最终选择了后者理由非常实际采样方法在开放空间如停车场表现好但在结构化道路高速、城市快速路上其随机性会导致轨迹抖动、变道切角生硬且难以硬编码交通规则。举个例子RRT*在生成变道轨迹时可能为了“最短路径”而规划出一条近乎直角的横向移动这对乘客舒适度是灾难性的也超出EPS电机的瞬时转向能力。而优化方法我们可以把所有约束显式写进目标函数最小化加加速度jerk保证平顺最小化与参考线的横向偏差保证居中最小化与前车的时间距离TTC保证安全同时硬性约束方向盘转角速率≤300°/s、横向加速度≤3m/s²、所有轨迹点必须落在合法车道内。我们采用ST图时空图作为基础框架S轴是沿车道中心线的纵向距离T轴是时间。在ST图上前车轨迹是一条曲线我们的目标是规划一条不与之相交、且满足所有运动学约束的S-T曲线。然后用二次规划QP求解器如OSQP实时求解。这套方案的代价是计算量稍大但我们通过两项工程优化解决了第一将QP问题规模从全轨迹100个点压缩到“滚动时域”只优化未来3秒内的20个点每50ms滚动更新第二预计算Hessian矩阵目标函数二阶导的稀疏结构使每次求解耗时稳定在8ms以内远低于100Hz控制周期。实车反馈乘客抱怨“晕车”的投诉率下降了89%因为所有变道、跟车加减速的jerk值都被严格控在0.5m/s³以下——这个数值接近人类老司机的平均水平。3. 核心细节解析与实操要点从理论到落地的七道坎3.1 坐标系统一为什么毫米级误差会导致规划失效规划模块是整个自动驾驶系统的“中枢神经”但它本身不产生数据所有输入都来自上游模块。而各模块使用的坐标系五花八门摄像头输出的是图像像素坐标u,v激光雷达是自身坐标系下的三维点云x,y,zGPS/IMU组合导航输出的是WGS84大地坐标经度、纬度、高度高精地图提供的是局部ENU坐标系东、北、天。如果规划模块拿到的“前车位置”是基于激光雷达坐标系而“本车位置”却是GPS转换来的ENU坐标两者原点不重合、朝向有偏差哪怕只有5厘米的系统性偏移在60km/h车速下1秒内就会导致1.7米的位置误判——这足以让规划出的避让轨迹完全失效。我们吃过这个亏。2021年某次高速测试车辆在无任何障碍物的直道上频繁触发“前方碰撞预警”查了一周才发现是激光雷达的外参标定文件版本错了导致其坐标系相对于车辆中心偏移了3.2cm。解决方案必须是系统级的第一建立全栈统一的“车辆坐标系”Vehicle Body Frame原点在后轴中心X轴指向前Y轴指向左Z轴指向上第二所有传感器数据接入规划模块前必须经过严格的“坐标变换链”校验。我们开发了一个自动化校验工具在空旷场地放置4个已知精确坐标的靶标同时用摄像头、激光雷达、GNSS分别观测工具自动计算各传感器到车辆坐标系的变换矩阵误差。要求所有平移误差1cm旋转误差0.1度。这个工具现在是我们每个新车型标定后的强制验收项。另外高精地图的坐标系必须与车辆坐标系对齐我们采用“地图匹配在线标定”双保险先用GNSS粗匹配再用激光雷达点云与地图中车道线、护栏等特征进行ICP迭代最近点精配准最后用卡尔曼滤波在线估计并补偿车辆坐标系随时间的微小漂移。这套流程下来坐标系误差被压到毫米级规划模块的输入可靠性才真正有了根基。3.2 参考线Reference Line的生成与平滑不是画条线那么简单参考线是规划模块的“虚拟轨道”所有轨迹都围绕它生成。它通常由高精地图的车道中心线生成但直接拿地图线会出大问题。高精地图的中心线是离散点序列点间距可能达1米且包含大量锐角如匝道入口的90度折线。如果规划直接用这些点生成的轨迹会在折点处产生巨大的曲率突变导致方向盘疯狂抖动。参考线的本质是一条高阶连续C2连续即位置、一阶导、二阶导都连续的参数化曲线它必须同时满足几何精度和运动学可行性。我们的实操方案是“三步平滑法”第一步用B样条B-Spline对原始地图点进行拟合控制点数量设为原始点的1/5降低自由度第二步对B样条曲线进行“曲率约束优化”目标是最小化曲率变化率dκ/ds约束是最大曲率κ_max0.02/m对应转弯半径50米适合城市道路使用梯度下降迭代求解第三步将优化后的B样条离散化为等弧长点序列点间距严格设为0.5米并计算每个点的切向量、法向量、曲率。这一步看似繁琐但效果立竿见影实车测试中车辆在S形弯道上行驶时方向盘转角波动幅度从±15度降至±3度乘客不再抱怨“像在开船”。还有一个隐藏技巧在匝道、施工区等特殊路段我们会动态注入“虚拟参考线”。比如前方500米处有锥桶占道地图线仍按原车道延伸但我们根据感知模块实时检测到的锥桶位置生成一条绕行虚拟线并赋予它更高的权重。这样规划模块会自然倾向于选择绕行轨迹而不是死守地图线导致强行压线。3.3 障碍物建模为什么要把车看成“带概率的椭圆”规划模块眼中的障碍物绝不是激光雷达点云里的一团噪点也不是摄像头框出的一个矩形。它必须是一个能反映物理属性、运动状态和不确定性的真实模型。我们采用“运动椭圆概率分布”的复合建模法。首先用卡尔曼滤波KF或交互多模型IMM跟踪每个障碍物输出其在车辆坐标系下的状态向量[x, y, vx, vy, length, width]。然后将这个矩形障碍物近似为一个中心在(x,y)、长轴沿运动方向、长短轴分别为length/2和width/2的椭圆。但这还不够因为跟踪本身有误差。KF的协方差矩阵P描述了状态估计的不确定性。我们将P投影到横向y和纵向x方向得到两个标准差σ_x和σ_y。最终障碍物在规划中的“影响区域”是一个中心在(x,y)、x方向半轴为length/2 2σ_x、y方向半轴为width/2 2σ_y的椭圆。这个“2倍标准差”的膨胀是工程经验的结晶它覆盖了95%的跟踪误差可能既不过于保守如3σ会大幅缩小可行驶空间也不过于激进如1σ可能导致漏检。更重要的是我们为每个障碍物分配一个“行为置信度”Behavior Confidence范围0.0~1.0。它由三部分组成跟踪稳定性连续跟踪帧数、运动一致性当前速度与历史均值的偏差、外观置信度摄像头分类结果的softmax概率。当一辆车刚从侧方盲区出现跟踪帧数少、速度跳变大其置信度可能只有0.3规划模块就会给它分配更大的膨胀椭圆并优先选择远离它的轨迹。而一辆已稳定跟踪30秒、速度平稳的前车置信度0.95膨胀就很小。这个机制让我们在“鬼探头”场景下的响应时间平均缩短了0.4秒——因为系统不是等到障碍物完全清晰才行动而是根据不确定性程度提前预留了安全裕度。3.4 交通规则硬编码如何让AI“懂交规”而不被规则捆死规划模块必须遵守交通法规但法规不是冷冰冰的条款而是有语境、有例外、有优先级的活规则。比如“实线禁止变道”是铁律但“前方事故导致拥堵所有车道都停了此时应急车道是否可以借用”——这需要结合实时场景判断。我们的做法是构建一个“规则知识图谱”而非简单if-else。图谱节点是原子规则如“禁止跨越实线”、“最低限速60km/h”、“公交专用道工作时间7:00-9:00”边是规则间的逻辑关系AND、OR、EXCEPT、PRIORITY。当规划模块启动时它会根据当前定位、时间、天气、车道类型从高精地图读取、感知到的交通标志从视觉识别动态激活相关规则子图。例如在早高峰7:30定位在城市快速路感知到“公交专用道”标志则激活“公交专用道禁行”规则但如果此时本车道前方200米发生事故所有车辆静止且系统检测到应急车道空闲则“EXCEPT”边会被触发允许临时借用应急车道但需满足附加条件车速20km/h、借用时长60秒、与应急车道内车辆保持50米距离。这个设计的精髓在于规则是静态的但规则的“适用性”是动态计算的。我们还加入了“规则冲突仲裁器”。当多个规则同时生效且矛盾时如“必须保持车距”vs“必须在绿灯结束前通过路口”仲裁器根据预设的优先级安全法规效率和实时风险评分基于障碍物距离、相对速度计算做出裁决。2022年某次北京测试车辆在晚高峰面临“黄灯前车缓行后方有车”的三难局面旧版规则引擎因无法仲裁而急刹新版图谱系统则计算出“以25km/h匀速通过路口的风险评分为0.12低于急刹导致后车追尾的风险0.45”于是平稳通过。这背后是把交规从“法律条文”转化为了“可计算、可权衡、可执行的工程语言”。3.5 舒适性约束为什么乘客晕车往往是规划的锅很多人以为晕车是底盘调校或悬架的问题其实规划模块的运动学输出才是根源。人体前庭系统对加速度a和加加速度jerk即加速度的变化率da/dt极其敏感。jerk超过0.7m/s³大多数人就会感到不适超过1.2m/s³几乎必然晕车。而传统规划只关注“能否到达”对jerk毫无约束。我们为此专门设立了“舒适性代价函数”。在轨迹优化的目标函数中除了安全、效率项我们加入一项λ_jerk * ∫(jerk)² dt。λ_jerk是可调权重我们通过大量用户主观评价实验邀请100名不同年龄乘客乘坐记录其晕车程度和轨迹jerk值标定了最优值0.008。这个值确保95%的乘客在常规工况下无不适感。但单纯加权重还不够因为优化器可能为了降低jerk而过度牺牲效率。所以我们采用“分段约束”在跟车、巡航等常规场景jerk上限设为0.5m/s³在紧急避让等极限场景允许短暂0.3秒升至1.0m/s³但之后必须用0.2秒内将jerk归零。实现上我们把轨迹的加速度序列作为优化变量直接约束其一阶导即jerk的绝对值。这比事后平滑轨迹更有效因为它是“源头治理”。实测数据很说明问题搭载新规划模块的测试车在连续30分钟的城市拥堵路段行驶后乘客平均晕车报告率从18%降至2.3%而在高速变道场景方向盘转角的最大变化率等效jerk从120°/s²降至35°/s²乘客普遍反馈“转向像丝般顺滑”。这提醒我们自动驾驶的终极目标不是技术炫技而是让乘客忘记技术的存在。3.6 实时性保障如何在10ms内完成一次完整规划规划模块的实时性是生命线。如果一帧计算耗时超过100ms10Hz控制模块就会收到过期指令车辆可能已驶过规划点。我们设定的硬性指标是99%的帧耗时≤8ms峰值耗时≤12ms。这听起来苛刻但通过四层优化达成第一层算法剪枝。在ST图搜索中我们预设一个“安全走廊”Safety Corridor基于当前车速和感知结果动态计算出未来3秒内所有障碍物的预测包络只在包络外的S-T区域进行搜索将搜索空间缩小70%。第二层数据结构优化。放弃通用容器自研“时空哈希网格”ST-Hash Grid将ST图划分为100x100的网格每个网格存储可能进入的障碍物ID。查询某个S-T点是否安全时只需O(1)时间定位网格再遍历其中少量障碍物比全量遍历快两个数量级。第三层求解器定制。商用QP求解器如OSQP虽鲁棒但有启动开销。我们针对规划问题的稀疏特性用C手写了一个轻量级内点法求解器代码仅2000行专解“带线性约束的二次规划”启动时间从1.2ms降至0.03ms。第四层硬件协同。规划模块部署在车载AI芯片如Orin的专用CPU核心上并绑定独立内存池避免与其他进程争抢资源。我们甚至为规划线程设置了最高调度优先级SCHED_FIFO确保其不会被Linux内核的其他任务打断。这套组合拳下来实车运行时规划模块的CPU占用率稳定在35%~45%远低于80%的警戒线为后续功能升级留足了余量。有一次芯片供应商建议我们“把规划迁移到GPU上以释放CPU”我们婉拒了——因为GPU的延迟不可控而规划要的是确定性的低延迟。3.7 仿真验证为什么说“没跑过100万公里仿真的规划不敢上路”实车测试成本高、周期长、风险大尤其对于长尾场景如“暴雨夜隧道出口遇逆行三轮车”。因此高保真仿真Simulation是规划模块验证的基石其重要性不亚于实车。我们构建的仿真平台有三大支柱第一传感器模型。不是简单渲染图片而是模拟真实物理过程摄像头要考虑镜头畸变、动态模糊、雨滴折射、LED频闪激光雷达要模拟点云密度随距离衰减、多路径反射、阳光直射导致的饱和噪点。第二交通流模型。采用“混合驱动”主车Ego Vehicle由我们的规划模块控制其他车辆Traffic一部分用基于规则的智能体如SUMO一部分用从真实接管日志中提取的“行为模板”Behavior Template能复现人类司机的犹豫、试探、加塞等非理性行为。第三场景库Scenario Library。我们不依赖人工编写而是用“数据挖掘自动合成”从PB级路测数据中用聚类算法找出高频危险场景如“跟车时前车急刹”、“无保护左转遇对向直行”再用程序自动生成数千个变体改变车速、距离、天气、光照形成覆盖10万场景的库。验证流程是每日CI持续集成自动运行全场景库统计规划模块的“成功通过率”、“接管次数”、“违规次数”对失败场景自动保存完整数据包传感器原始数据、规划中间结果、控制指令供工程师复现分析。我们有个铁律任何规划算法的代码提交必须附带其在仿真库中的性能报告且关键场景如AEB、LKA通过率不得低于99.99%。正是这套严苛的仿真体系让我们在2023年某款新车上市前发现了规划模块在“强逆光条件下对白色障碍物距离估计偏大”的致命缺陷——实车很难复现但在仿真中被自动捕获并修复避免了潜在召回风险。4. 实操过程与核心环节实现从代码到实车的完整链路4.1 开发环境搭建Ubuntu 20.04 ROS2 Foxy 的选型依据开发环境的选择直接影响团队协作效率和后期部署难度。我们最终锁定Ubuntu 20.04 LTS ROS2 Foxy是经过多轮技术论证的结果。Ubuntu 20.04提供了长期支持至2030年且是NVIDIA DRIVE OS和多数车规级AI芯片SDK的官方推荐系统驱动兼容性最好。ROS2 Foxy相比ROS1最大的优势是实时性Real-time Support和确定性Determinism。ROS1的TCPROS通信协议基于TCP存在队列阻塞和不可预测延迟而ROS2的DDSData Distribution Service中间件支持配置传输优先级、截止时间Deadline和寿命Lifespan能保证规划模块发布的轨迹消息以1ms的抖动准时送达控制模块。我们曾对比测试在ROS1下规划消息的端到端延迟从发布到控制接收平均为8.2ms标准差1.7ms在ROS2 Foxy下平均延迟降至5.1ms标准差压缩到0.3ms。这个0.3ms的确定性对高速工况下的轨迹跟踪精度至关重要。搭建步骤如下首先安装官方NVIDIA JetPack SDK含CUDA、cuDNN、TensorRT这是所有AI推理的基础其次从源码编译ROS2 Foxy官方二进制包对ARM64支持不完善特别注意启用--enable-rt选项以开启实时调度支持第三创建规划专用工作空间所有节点行为决策、轨迹生成、参考线管理均用C编写利用rclcpp::Node基类并在节点初始化时调用set_scheduler()设置SCHED_FIFO策略第四配置DDS QoS服务质量策略对轨迹Topic/planning/trajectory设置RELIABLE可靠性、TRANSIENT_LOCAL持久性确保控制模块重启后能获取最新轨迹、DEADLINE为10ms超时则丢弃旧消息。这套环境支撑了我们团队20工程师并行开发从未因环境问题导致集成失败。4.2 行为决策模块代码实现一个可扩展的状态机行为决策的核心是状态机State Machine我们采用“分层状态机”Hierarchical State Machine, HSM设计兼顾清晰性和可扩展性。顶层是驾驶模式Driving ModeLANE_FOLLOW车道居中、LANE_CHANGE_LEFT左变道、LANE_CHANGE_RIGHT右变道、STOP停车、WAIT等待。每个模式下有独立的子状态Sub-state和转移条件Transition Condition。以LANE_FOLLOW为例其子状态包括FOLLOWING正常跟随、PREPARE_TO_STOP预判前车将停、STOPPING正在刹停、STOPPED已停稳。转移条件不是硬编码在if语句里而是注册为回调函数。例如FOLLOWING - PREPARE_TO_STOP的条件是front_vehicle_distance (current_speed * 1.5) front_vehicle_acceleration -1.5前车距离小于1.5秒时距且减速度-1.5m/s²。代码结构如下简化版// BehaviorDecisionNode.cpp class BehaviorDecisionNode : public rclcpp::Node { public: BehaviorDecisionNode() : Node(behavior_decision) { // 注册所有状态转移条件 transition_conditions_[LANE_FOLLOW][FOLLOWING][PREPARE_TO_STOP] [this]() - bool { auto dist perception_-GetFrontVehicleDistance(); auto acc perception_-GetFrontVehicleAcceleration(); return (dist 0 dist current_speed_ * 1.5 acc -1.5); }; // 启动状态机 current_mode_ DRIVING_MODE::LANE_FOLLOW; current_state_ STATE::FOLLOWING; } private: void RunStateMachine() { // 每50ms执行一次状态机循环 auto next_state state_machine_.GetNextState( current_mode_, current_state_, transition_conditions_); if (next_state ! current_state_) { RCLCPP_INFO(this-get_logger(), State change: %s - %s, StateToString(current_state_).c_str(), StateToString(next_state).c_str()); OnStateExit(current_state_); current_state_ next_state; OnStateEnter(current_state_); } } // 每个状态进入时执行的动作 void OnStateEnter(STATE state) { switch(state) { case FOLLOWING: // 启动跟车PID控制器 pid_controller_-Enable(); break; case PREPARE_TO_STOP: // 提前松油门准备能量回收 control_cmd_-throttle 0.0; break; // ... 其他状态 } } };这个设计的好处是添加新行为如FREE_DRIVE自由驾驶只需新增一个顶层Mode枚举、定义其子状态和转移条件无需改动现有状态机核心逻辑。所有条件函数都可单元测试覆盖率要求100%。我们用Google Test框架为每个条件函数编写了边界值测试如距离0、加速度NaN确保其在异常输入下不崩溃。4.3 轨迹生成核心算法ST图构建与QP求解的完整流程轨迹生成是规划模块的“心脏”其实现必须精准、高效。我们以“跟车”场景为例展示从ST图构建到QP求解的完整链路。输入当前车速v₀25m/s90km/h前车在S轴距离d_s45m相对速度Δv-2m/s前车慢前车加速度a_lead-1.0m/s²。目标生成未来3秒T3s、共20个点Δt0.15s的S-T轨迹。第一步构建ST图。在S-T平面横轴S米纵轴T秒。前车轨迹为一条抛物线S_lead(T) d_s (v₀ Δv)T 0.5a_lead*T²。我们的规划轨迹S_ego(T)必须满足S_ego(T) ≥ S_lead(T) d_min(T)其中d_min(T)是动态最小安全距离我们采用“时间距离”模型d_min(T) v_ego(T) * 1.5 2.01.5秒时距2米固定缓冲。将S_ego(T)参数化为三次多项式S_ego(T) a₀ a₁T a₂T² a₃T³。约束条件包括初始条件S_ego(0)0, dS/dT(0)v₀、末端条件S_ego(3)S_target、以及全程不碰撞S_ego(T) ≥ S_lead(T) d_min(T)。第二步转化为QP问题。目标函数为最小化jerkmin ∫(d³S/dt³)² dt ≈ min Σ(a₃² * 6² * Δt)这是一个关于系数a₀,a₁,a₂,a₃的二次函数。不等式约束S_ego(T_i) ≥ S_lead(T_i) d_min(T_i)是线性的因为S_ego是T的线性函数。最终QP问题形式为min (1/2)xᵀPx qᵀxs.t. Ax ≤ b, Cx d。其中x[a₀,a₁,a₂,a₃]ᵀ。第三步调用求解器。我们封装了一个QP Solver Wrapper// QPSolverWrapper.h struct QPResult { std::vectordouble coefficients; // [a0, a1, a2, a3] bool success; double solve_time_ms; }; class QPSolverWrapper { public: QPResult Solve(const STConstraints constraints) { // 构建P, q, A, b, C, d 矩阵此处省略具体构建代码 // ... // 调用OSQP求解器 OSQPSettings* settings (OSQPSettings*)c_malloc(sizeof(OSQPSettings)); osqp_set_default_settings(settings); settings-eps_abs
