从第一行代码跑通仿真的那一刻起你会真正理解“决策-规划-控制”这三件事在自动驾驶里是怎么分工的。这篇文章想分享的就是一套完全基于Matlab的决策规划控制代码实战方法不需要昂贵的自动驾驶实车平台也不要求你具备深厚的C工程基础只要有基本的Matlab操作能力就可以把一辆虚拟的车从A点开到B点并在途中完成跟车、变道、超车这些经典动作。我用这套思路带过几个零基础的同学也自己在上面反复调过参数踩过的坑和验证过有效的做法都会写出来希望能帮想进入自动驾驶控制算法领域的你少走一段弯路。1. 为什么我劝你先用Matlab把决策规划控制这条线跑通而不是一上来就啃源码很多人一接触自动驾驶第一反应就是去翻开源自动驾驶项目的庞大代码库看到几十个功能包、上百万行代码直接就懵了。或者去安装重型仿真环境显卡驱动、依赖库、编译环境折腾一两个星期还没看到车动起来就放弃。这两条路我都走过最后发现最合适的学习路径反而是用Matlab先自己手写一套简化的决策规划控制系统。1.1 Matlab在这场学习里的核心定位我要先说清楚一件事Matlab这套东西不是用来做量产自动驾驶系统开发的业界生产环境基本是C加中间件通信没人拿着Matlab上实车。但学习自动驾驶算法Matlab恰恰是我用过的最合适的“手术台”。原因有三个。第一它把所有中间过程都可视化了。你可以一边跑仿真一边在坐标轴上看到参考线、规划出来的轨迹、车辆实际行驶路径这三者的关系调起参数来非常直观。第二矩阵和向量运算是默认操作而自动驾驶规划控制里最核心的坐标变换、路径点计算、状态更新本质上都是矩阵运算。第三你不需要处理复杂的工程编译环境打开脚本改一个数字重新运行结果马上就出来这个调试反馈速度是学习阶段最宝贵的。1.2 我对Matlab基础配置的建议如果你已经装了Matlab我建议先确认一下你的版本自带哪些工具箱。做这套实战最理想的是有自动驾驶工具箱Automated Driving Toolbox但说实话我下面写的这套代码核心算法部分完全没有用到它而是用纯手写的方式实现车辆运动学模型和轨迹规划。这反而对学习更有好处因为你能看到每一个公式是怎么变成代码的而不是调别人封装好的接口。提示建议选择配合SIMULINK环境但先把脚本式代码跑熟再做模型层面的集成这样分两步走会更容易理解。我自己就是这么做的——先纯脚本出结果再考虑封装。1.3 这套实战帮你打通的三个能力层级我把这套代码实战要训练的能力分成三层。最底层是运动学建模能力你要能把一个车辆在平面上的位姿变化用状态方程描述出来包括位置、航向角、前轮转角的关系。中间层是规划算法能力在给定起点和终点的约束下你能够生成一条满足曲率、边界条件约束的可执行轨迹。最上层是决策逻辑能力汽车面临多车道、多障碍物时怎么根据规则判断当前该巡航、减速还是变道。说实话这三个能力分层正好对应了自动驾驶软件架构里的经典三层。你把这三层用Matlab分别实现再组装起来之后再去看任何成熟的自动驾驶框架都会有一种豁然开朗的感觉。因为所有复杂系统本质上都是在这三层的框架里做更细粒度的处理。2. 决策层实战状态机设计从if-else堆砌到结构化实现决策层是整个自动驾驶车辆的大脑。在真实的自动驾驶系统里决策模块会接收感知、定位、预测等多个模块的信息然后输出行为级的指令比如“向左变道”、“保持车道巡航”、“在目标点前停车”。在Matlab实战里我们没有真实的感知模块所以决策层的输入是经过简化的虚拟环境信息比如车道中心线位置、前方障碍物距离和速度。2.1 有限状态机的状态定义与转移条件我建议第一版决策层用有限状态机FSM来实现。这个方案虽然不是最新的深度学习端到端路线但它逻辑清晰、可解释性强而且对于理解自动驾驶行为决策的本质非常有帮助。在我的实战Demo里定义的状态包括巡航状态Cruise车道内正常行驶前方无有效障碍物跟车状态Follow前方存在障碍物但距离满足安全跟车范围变道状态ChangeLane满足变道条件车辆执行变道动作减速停车状态Stop前方障碍物过近需要减速至停车状态的转移条件我用的是距离阈值和速度阈值组合判断。举个例子从巡航切到跟车判断条件就是前方障碍物距离小于某个值比如50米且障碍物速度低于本车速度。从跟车回到巡航需要障碍物距离重新拉开到另一个更大的值比如60米。这里有一个非常关键的设计细节——滞回区间。如果你用同一个阈值做进入和退出的判断车辆会在临界位置反复切换状态表现出来就是车辆一耸一耸的非常不自然。所以我现在习惯给每个状态转移设置两个不同的阈值进入条件严格一些退出条件宽松一些。这个设计思想在工程上叫滞回控制它不复杂但对驾驶平顺性的提升是立竿见影的。2.2 决策模块的代码结构怎么组织才不变成一团乱麻我看到很多同学写决策代码最后都是一个几百行的if-else嵌套改一个条件要理半天逻辑。我推荐的结构是把决策模块写成一个独立的函数输入是当前自车状态和环境感知结果输出是一个结构体里面包含决策状态和期望的驾驶指令。function decision fsm_update(ego_state, env_info, params) % ego_state: 自车状态结构体包含速度、位置、航向 % env_info: 环境信息结构体包含前方最近障碍物的距离和速度 % params: 参数字典包含各状态切换阈值和期望速度系数 % decision: 决策输出结构体包含当前状态和期望指令 % 初始化状态 if isempty(params.current_state) params.current_state Cruise; end % 状态转移判断 switch params.current_state case Cruise if env_info.front_dist params.follow_enter_dist ... env_info.front_vel ego_state.speed params.current_state Follow; end case Follow if env_info.front_dist params.follow_exit_dist params.current_state Cruise; elseif env_info.front_dist params.stop_enter_dist params.current_state Stop; end case ChangeLane % 变道完成或中止条件 if abs(ego_state.lateral_offset) params.lane_offset_eps params.current_state Cruise; end case Stop if env_info.front_dist params.stop_exit_dist params.current_state Cruise; end otherwise params.current_state Cruise; end % 根据状态生成驾驶指令 switch params.current_state case Cruise decision.target_speed params.cruise_speed; decision.request_change_lane false; decision.need_stop false; case Follow % 跟车速度按间距比例调整配合间距误差PID decision.target_speed env_info.front_vel ... params.k_gap * (env_info.front_dist - params.desired_gap); decision.target_speed max(decision.target_speed, 0); decision.request_change_lane false; decision.need_stop false; case ChangeLane decision.target_speed params.cruise_speed; decision.request_change_lane true; decision.need_stop false; case Stop decision.target_speed 0; decision.request_change_lane false; decision.need_stop true; end decision.current_state params.current_state; end这段代码的逻辑很直白但它包含了几个值得琢磨的工程考量。判断条件里用了“且前车速度低于自己”这个要求这是为了避免前方车辆虽然近但速度比你快时触发不必要的跟车减速。变道状态的退出条件用了横向偏移量来判断是否到达目标车道中心线而不是用时间来判断这样更准确。2.3 决策输出与规划层的接口协议设计决策层输出的期望指令是规划层的输入约束。我这里的接口协议是这样设计的决策层告诉你“目标速度是多少”、“要不要变道”、“要不要停车”然后规划层负责把这个指令变成一条具体的、可执行的轨迹。这里有个很容易踩的坑决策层的指令变化太突兀了。如果巡航状态下期望速度是60km/h决策突然切到跟车状态期望速度直接变成前车的速度30km/h你直接把这个突变的目标速度扔给控制层车就会猛烈减速乘客体验极差。所以我在决策层和规划层之间增加了一个过渡逻辑当期望速度发生跳变时我不会直接把新目标速度传给规划层而是设定一个平滑过渡的目标速度序列让车辆用3秒钟左右的时间逐渐过渡到新的期望速度。这个细节虽然只是几行代码但它决定了你的仿真效果是“像玩具”还是“像一辆车”。3. 规划层拆解路径与速度分离式做法的数学直觉决策层给出了行为指令规划层的任务就是回答两个问题走哪条线怎么走传统而有效的方法是把这两个问题拆开处理——先规划一条满足几何约束的路径再沿这条路径规划每个点的速度。这种解耦方案的数学处理更简单而且调试时可以先看路径合不合理再看速度切换平顺不平顺。3.1 参考线与局部路径的坐标变换在规划层的内部我先定义了一条参考线通常就是车道中心线。车辆在实际驾驶中不可能完美贴合参考线尤其是在变道和避让场景下。所以局部路径规划的本质是相对于参考线计算一条横向偏移曲线。这里需要做坐标变换把车辆当前位置转换到以参考线为纵轴的Frenet坐标系下。用大白话解释Frenet坐标系就是“沿着路走多远纵向距离”和“偏离路中心多远横向距离”这两个维度。变道这个动作在Frenet坐标系下表达非常清爽纵向保持前进横向从0平移到相邻车道的偏移量比如3.5米。3.2 用五次多项式生成变道路径的核心逻辑横向偏移曲线用什么函数来描述我推荐五次多项式。为什么是五次而不是三次因为五次多项式有6个系数正好可以满足6个边界条件起点横向位置、对于弧长的横向速度、对于弧长的横向加速度以及终点的这三个量。这样规划出来的路径横向位置、横摆角、曲率在起点和终点都是连续的车辆执行起来不会出现折线感。function lateral_offset calc_fifth_polyline(s, s0, s1, l0, l0_d, l0_dd, l1, l1_d, l1_dd) % s: 当前弧长 % s0,s1: 变道起始和结束弧长 % l0~: 起点横向位置/一阶导/二阶导 % l1~: 终点横向位置/一阶导/二阶导 % 归一化弧长 u (s - s0) / (s1 - s0); % 五次多项式系数计算 A [1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0; 1, u, u^2, u^3, u^4, u^5; 0, 1, 2*u, 3*u^2, 4*u^3, 5*u^4; 0, 0, 2, 6*u, 12*u^2, 20*u^3]; b [l0; l0_d; l0_dd; l1; l1_d; l1_dd]; coeffs A \ b; % 计算对应弧长的横向偏移 lateral_offset coeffs(1) coeffs(2)*u coeffs(3)*u^2 ... coeffs(4)*u^3 coeffs(5)*u^4 coeffs(6)*u^5; end这段代码的核心就是解一个6阶线性方程组。我在实际调试中发现变道长度s1-s0的选择非常关键它取决于自车速度和变道时间的预期。如果变道距离给短了横向加速度很大路径会显得很“锐利”给长了车辆会长时间占用两个车道中间线影响安全。我常用的经验值变道持续时间2.5到3.5秒变道距离等于车速乘以上述时间你可以在这个基础上调整。3.3 速度规划梯形速度曲线怎么处理各种约束路径定好了就要规划速度。我采用的是梯形速度曲线策略也就是加速段、匀速段、减速段三段式。它的好处是物理意义清晰每段都有对应的驾驶场景加速段是车辆在追目标速度匀速段是稳定巡航减速段是在接近障碍物或停车点时的制动准备。梯形速度曲线的输入是起始速度、终点速度、期望最大速度、最大加速度、最大减速度。输出是每个轨迹点对应的时间戳和目标速度。规划时我会做两遍判断——先用正常情况生成速度曲线再检查任意点的速度是否会导致碰撞风险如果会导致就把期望速度降下来重新规划一段减速区。下面是我用的速度规划核心片段function speed_profile plan_trapezoid_speed(v_start, v_target, v_max, a_max, a_min, total_len) % 根据运动学公式计算三阶段分界位置 % 加速段长度估算 t_acc (v_target - v_start) / a_max; s_acc v_start * t_acc 0.5 * a_max * t_acc^2; % 减速段长度估算 t_dec (v_target - v_end) / a_min; s_dec v_end * t_dec 0.5 * a_min * t_dec^2; % 若加速段减速段超过总长度需要重新规划目标速度 if s_acc s_dec total_len v_target sqrt(v_start^2 2*a_max*(total_len * a_min/(a_min-a_max)) - 2*a_min*a_max*...); % 这里按实际场景修正目标速度 end % 据此分段生成每个s对应的目标速度限幅在[v_min, v_max] end这个函数在边界情况下还有一个容易被忽视的修正逻辑当加速段加减速段超过总长度时说明没有匀速段甚至目标速度根本达不到这时需要反解一个实际可达到的最高速度。我在纸上推算过一次这个反解公式其实就是匀加速直线运动的位移公式组合但每次写出来都很容易乱所以我在代码里加了详细注释强烈建议你也这么做。4. 控制层落地横向纯跟踪与纵向PID的调参与避坑规划层给出了带时间戳的期望轨迹之后控制层要做的就是让车实际跟着这条轨迹跑。控制层的输入是期望轨迹点输出是方向盘转角、油门和刹车开度。在Matlab仿真环境下我不会去建模复杂的转向执行器而是直接输出前轮转角和加速度指令给车辆运动学模型。4.1 横向控制三连问纯跟踪为什么会成为第一选择横向控制算法里纯跟踪Pure Pursuit是我最推荐给初学者的因为它直观得像开卡丁车。算法假设车辆后轴中心有一根虚拟的竿子竿子末端有一个“目标点”这个点取在期望轨迹前方距离为Ld的位置。控制目标就是不断打方向让后轴中心沿着指向目标点的圆弧移动过去。前轮转角计算公式是delta atan(2 * L * sin(alpha) / Ld)其中L是轴距alpha是车辆航向与目标点方向之间的夹角Ld是预瞄距离。这个公式的逻辑其实是圆弧几何车辆后轴绕着一个瞬时圆心转弯转弯半径由预瞄点和航向夹角共同决定。你不需要去背公式理解了圆弧关系自然就记住了。function delta pure_pursuit_control(ego_state, ref_path, Ld) % ego_state: 当前车辆状态[x, y, yaw, v] % ref_path: 规划轨迹点集 [px, py] % Ld: 预瞄距离 % 找到距离当前位置Ld处的前方目标点 distances sqrt((ref_path(:,1) - ego_state(1)).^2 ... (ref_path(:,2) - ego_state(2)).^2); [~, idx_target] min(abs(distances - Ld)); target_x ref_path(idx_target, 1); target_y ref_path(idx_target, 2); % 计算目标点相对于车辆坐标系的角度 alpha atan2(target_y - ego_state(2), target_x - ego_state(1)) - ego_state(3); alpha wrapToPi(alpha); % 纯跟踪前轮转角 L 2.7; % 轴距按常见车型选取 delta atan2(2 * L * sin(alpha), Ld); end这个代码跑起来之后你会立刻感受到预瞄距离Ld是整辆车横向控制品质的命门。Ld太小车会像喝醉了一样左右画龙因为系统对角度误差过于灵敏Ld太大车会“抄近路”在弯道内侧切弯严重。我在调参时的经验是让Ld与车速成正比基础值取0.5到1.0倍车速数值比如车速10m/s时预瞄距离取5到10米。4.2 纯跟踪与Stanley控制的选择差异很多同学会问横向控制为什么不用Stanley算法我也在这套Demo里实现了Stanley做过对比。Stanley的公式是delta atan2(k * e_ya, v) phi_e其中e_ya是车辆前轴到参考轨迹的横向偏差phi_e是航向角偏差。实测下来Stanley的稳态跟踪精度更好在大曲率弯道中横向偏差能控制得更小但它的缺点是对噪声和参考轨迹的平滑度非常敏感。一旦规划轨迹存在微小抖动Stanley输出的转角就容易出现振荡。纯跟踪虽然在某些场景下会存在稳态误差沿曲线行驶时会有固定的横向偏差但它胜在鲁棒性高、调参手感好。我的建议是第一版控制层先用纯跟踪把闭环跑通等你看到了车辆的完整运动过程再引入Stanley或者LQR控制做精度提升。这个循序渐进的过程会让你对不同控制算法有更真切的理解。4.3 纵向PID控制油门刹车合并输出还是分开建模纵向控制我用的是带限幅的PID控制器输入为期望速度与实际速度的误差输出为加速度指令。在实际调试中我把加速指令分为正值和负值两部分——正值对应油门负值对应刹车这样在可视化阶段可以看到清晰的加速与制动阶段。PID三个参数我总结了一套调试顺序先只上P当车速稳定在期望速度附近且没有明显等幅振荡时再加I消除稳态误差最后加D抑制超调。在速度控制里我最终用的参数大概在P0.8、I0.1、D0.05左右但需要说明的是这个参数和车辆模型的质量、速度范围强相关你的Demo里很可能需要重新标定。一个值得提醒的坑是Matlab自带的斜坡输入或正弦输入测试很常见但不代表PID参数在阶跃输入下也表现良好。我在测试时习惯给目标速度做一个阶梯变化从零加速到巡航速度再突然给一个减速指令观察车辆响应。如果速度曲线出现明显的超调和反复穿越目标速度线说明D偏小或I过大优先降低I。5. 闭环仿真的搭建顺序先跑通、再调稳、后加戏决策、规划、控制三块代码都写完之后真正的挑战开始了——把三块组装成一个完整的闭环仿真。很多人在这一步卡住不是某个模块单独有问题而是三个模块之间的数据接口和时间步长对不上导致整套系统跑不起来。我在这一部分分享一套亲测有效的搭建顺序。5.1 车辆运动学模型你只需要一个自行车模型仿真环境的车辆模型选择上第一版强烈建议使用自行车模型Bicycle Model。它把车辆简化为前后两个轮子忽略了横向载荷转移和轮胎侧偏特性对低速场景而言已经够用。车辆运动学模型的状态量为state [x, y, yaw, v]控制输入是前轮转角delta和加速度a。离散化更新公式如下function state_new update_vehicle_state(state, delta, a, dt) % 自行车模型离散更新 L 2.7; % 轴距 state_new zeros(4,1); state_new(1) state(1) state(4) * cos(state(3)) * dt; state_new(2) state(2) state(4) * sin(state(3)) * dt; state_new(3) state(3) state(4) / L * tan(delta) * dt; state_new(4) state(4) a * dt; end注意第三步更新航向角时前轮转角delta必须提前做限幅处理否则在低速下转向过度车辆会在原地打转。我的经验是把前轮转角限制在正负45度以内同时把加速度限制在正负3m/s²以内前者防止原地画圈后者防止仿真画面出现“不真实”的急加速。5.2 主循环里的时间同步与数据流整套闭环仿真用一个大循环来驱动。每步执行更新环境虚拟障碍物按设定逻辑移动、调用决策模块、调用规划模块、调用控制模块、更新车辆状态、记录数据。这里需要特别说的是控制频率和规划频率的解耦。常见做法是控制模块在每个步长都运行比如50Hz规划模块每10个步长才重新规划一次比如5Hz。这样更接近真实系统架构因为轨迹规划的计算量远远大于控制计算量。我在这套Demo里也采用了类似设计决策模块甚至可以更慢约2Hz就够因为状态转移本身不是高频动作。让我把主循环的骨架贴出来你可以直接参考这个结构往里填函数dt 0.02; % 控制周期 50Hz plan_interval 10; % 每10步规划一次 sim_time 20; % 仿真总时长 state [0, 0, 0, 10]; % 初始位置原点航向0速度10m/s time 0; for step 1:sim_time/dt % 1. 更新虚拟环境信息例如前车位置随时间变化 env_info update_virtual_env(time); % 2. 决策层 if mod(step, 5) 1 % 约2Hz decision fsm_update(state, env_info, params); end % 3. 规划层 if mod(step, plan_interval) 1 ref_path plan_trajectory(state, decision, env_info, params); % 规划结果保存在ref_path中接下来若干步沿用 end % 4. 控制层 delta pure_pursuit_control(state, ref_path, Ld); a speed_pid_control(state(4), decision.target_speed); % 5. 更新车辆状态 state update_vehicle_state(state, delta, a, dt); % 6. 记录并实时绘制 record_log(time, state, ref_path); time time dt; end5.3 我踩过的三个典型问题振荡、切弯、状态闪烁第一版闭环跑起来的时候我遇到了三个非常典型的问题每个都是调参数就能解决但找原因的过程很折磨人。第一个问题是车辆在直线行驶时出现轻微蛇形振荡。一开始我以为是纯跟踪参数问题后来发现是规划层每次重新规划的起点状态位姿、航向与控制层实际跟踪状态之间存在微小偏差导致每次刷新参考轨迹时都引入了一个小的横向跳变。解决办法是规划层生成轨迹时起点状态不使用当前车辆状态的原始值而是使用期望位姿即横向误差和航向误差经过一个低通滤波后的值这样参考轨迹本身不会突变。第二个问题是变道时切弯过猛车辆冲过目标车道中心线然后大幅修正回来。根因是变道路径的五次多项式在端点处虽然连续但变道距离设置过短导致曲率峰值过大。把变道持续时间从2秒调整到3秒之后这个问题明显缓解。第三个问题就更有意思了——决策层状态闪烁。跟车状态下前方交通灯虚拟设置变红车辆切换到减速停车但车还没停稳虚拟信号就变绿了决策层又立即切回巡航。这样一红一绿之间车辆频繁启停体验极差。解决方法是增加一个时间维度的确认机制新状态必须持续0.5秒以上才生效这相当于在决策层加了状态持续时间约束。6. 从仿真Demo到工程思维我在这套实战里想明白的事当你把上面的闭环跑通看着那辆虚拟小车在坐标轴上稳稳完成跟车、减速、变道、超越这一整套动作时恭喜你你已经迈过了自动驾驶算法入门的第一道坎。但我想再延伸几个从Demo到工程化之间需要想明白的点这些不是代码层面的而是思维方式层面的。6.1 决策的离散性如何在连续世界里保持优雅有限状态机输出的指令是离散的但车辆运动是连续的。这个矛盾在简单场景下可以用平滑过渡掩盖但在复杂场景下会暴露出问题。比如高速公路出口场景车辆既需要保持速度又需要连续变道两次状态机需要依次切换“变道1 → 巡航 → 变道2”如果巡航状态持续时间不足就会触发前面提到的状态闪烁问题。我在这个Demo里使用的优化手段是“带累计距离的决策缓存”——不是简单的用时间确认状态而是要求车辆在当前状态下至少前进一定路程或者满足某个空间条件后才允许切换。这让决策的连续性好了很多。工程界还有更复杂的方案比如马尔可夫决策过程或者场景树它们解决的就是同一类问题。6.2 轨迹的评价函数不是所有可行轨迹都能用当你开始写更复杂的规划模块时你会发现可以生成多条可行轨迹但用哪条是最优的需要一个评价函数来打分。这个评价函数通常会看碰撞风险距离障碍物的最近间隙、平顺性曲率变化率、效率到达终点的时间、舒适性垂向加速度变化率等多个维度。我在Demo里的评价函数很简单优先选择碰撞风险系数低于阈值的轨迹然后选择平均加速度变化率最小的。工程化之后评价函数的权重调节会变成一个持续迭代的过程甚至结合用户测试反馈来做调整这一点越早意识到越好。规划算法从来不只是“生成一条路径”而是“生成一条被评价函数认可的最优路径”。6.3 学习路线的下一步建议如果你通过Matlab代码实战跑通了这个闭环系统我建议的下一步不是急着学更多花哨的算法而是先把仿真场景复杂度提升。给虚拟环境加入更多障碍物、弯道、汇入车道等场景观察当前代码哪些地方会崩溃哪些地方表现僵硬然后针对性地改进。等到你发现纯规则型的决策状态机已经难以覆盖所有场景时再去看更高级的决策算法、学习类规划方法就顺理成章了。如果你还想进一步贴近工程可以考虑把Matlab代码逐模块翻译成Python或C配合轻量级的仿真环境去做验证这时你会发现之前积累的接口设计和模块化思维比任何算法知识都更值钱。回头看我自己的学习过程在把这条决策规划控制的链路用Matlab完整跑通之前我看了不少理论和公式但总有一种“隔了一层”的模糊感。直到亲手调过那辆虚拟车的方向盘、看着它完成第一次变道超车那些公式和算法才真正长在了我的知识体系里。希望这篇文章和这套实战思路也能帮你更快地完成这个“从看懂到做出来”的跨越。