自动驾驶决策规划控制Matlab代码实战，听起来又长又劝退，但我实际跑通之后发现，这其实是进入自动驾驶领域性价比最高的一条路。最近不少朋友来问我怎么入门，我的建议是别一上来就啃C++、Linux那一套，先用Matlab把决策、规划、控制这条主链路跑起来，你对整个自动驾驶系统会有非常直观的认知。这个项目解决的核心问题就是一套"感知结果进来之后，车该怎么想、怎么走、怎么控制方向"的完整逻辑，适合刚接触自动驾驶、对Simulink和Matlab脚本还不熟、想快速看到效果的人。
先说一下我自己的背景。我接触自动驾驶两年多，中间摸爬滚打踩了不少坑，后来用了这个Matlab代码实战项目，把决策、规划、控制三个模块从头到尾手写了一遍，才算真正理解了自动驾驶的"大脑"是怎么运作的。下面我会把整个项目的设计思路、代码实现、调试经验完整拆出来，照着做你也能跑出属于自己的第一个决策规划控制Demo。
1. 项目定位与整体设计思路
1.1 自动驾驶决策规划控制到底在解决什么问题
很多新手最容易犯的错，就是把自动驾驶系统想象成一个"感知模型"的附庸——好像只要目标检测做得够好，车就能自己开。实际上，感知只是给了你"周围有什么"，而决策规划控制才是回答"我接下来该怎么办"的部分。
具体拆开来说，决策模块负责在最宏观的层面做取舍：当前是跟着前车走，还是变道超车？前方路口应该直行还是转弯？碰到障碍物是停车等待还是绕行？这层逻辑在工程上常用有限状态机、行为树或者基于规则的打分函数来实现。
规划模块则是在决策的结果之上，进一步给出具体的行驶轨迹。路径规划关心的是"走哪条几何路径"，要避开障碍物；速度规划关心的是"每时每刻开多快"，要保证舒适性和安全性。最后控制模块把规划好的轨迹转换成方向盘转角、油门和刹车信号，让车实际执行下去。三者层层递进，缺一不可。
这个项目的定位就是用Matlab把这个闭环完整跑一遍——不追求拿到真正的实车上去，而是让你在仿真环境里亲眼看到从决策到控制的每个环节是怎么协作的。我个人觉得，先理解这条链路，比死磕某一个算法细节重要得多。
1.2 为什么选择Matlab作为入门工具
很多人一听到Matlab就皱眉头，觉得它不如Python或者C++那样"工业级"。但在这个项目里，选择Matlab的原因非常实际：调试直观、矩阵运算方便、可视化零成本。
举个例子，你在C++里写一个路径规划算法，想看一下生成轨迹是否正确，你得把轨迹数据导出，再用其他工具画图。而在Matlab里，plot、hold on、drawnow就是几行命令的事，而且可以一边跑一边在figure里刷新车的姿态和轨迹。这种即时反馈对建立直觉特别有帮助。
另外，Matlab的矩阵语法特别适合处理坐标变换和路径点集。自动驾驶里绕不开的坐标变换、曲线插值、矩阵旋转，在Matlab里写起来跟教科书公式几乎一一对应，不容易因为语言细节分心。加上Matlab自带的Vehicle Dynamics Blockset、Navigation Toolbox等工具箱，即使不依赖第三方库，也能实现一个结构完整的Demo。
当然，我不是让你一直停留在Matlab。我的建议是把Matlab当作"活地图"，把整个逻辑吃透之后，再往C++或Autoware之类的框架迁移。这也是我亲自验证过的路径。
1.3 项目整体架构与模块划分
我这边的Demo是纯脚本加函数实现的，没有用Simulink，因为脚本对新手来说更透明——每一步都能看到变量怎么变化，不会被Simulink里密密麻麻的连线搞晕。
整个项目分成五个部分：
- 场景定义：包括参考路径、障碍物位置、起点终点、目标速度。
- 车辆运动学模型：用自行车模型描述车辆位置和航向的变化。
- 决策模块：一个有限状态机，根据当前环境和驾驶目标输出驾驶行为。
- 规划模块：根据行为生成目标轨迹，包括路径点和每个点的速度。
- 控制模块：横向用纯跟踪控制，纵向用PID速度控制，输出前轮转角、加速度并更新车辆状态。
每个模块用一个独立的.m文件实现，主脚本负责把它们串联起来。这样的好处是，你想单独看哪个环节，直接跑那个函数就行，不用在几千行代码里翻来翻去。
2. 核心模块拆解：决策、规划、控制
2.1 决策模块：有限状态机与行为决策
决策模块是自动驾驶的"大脑"。在这个Demo里，我实现了一个非常经典的有限状态机，包含三个状态：巡航、跟车、停车等待。
- 巡航：前方没有障碍物，或者障碍物还很远，车辆以设定的目标速度行驶。
- 跟车：前方有障碍物且在安全距离以内，车辆减速并保持在障碍物后一段距离。
- 停车等待：前方出现无法绕行的障碍物，或接近目标点，车辆减速停车。
状态转移的规则放在一个函数里，输入是前方障碍物的距离、相对速度、当前速度以及预设的阈值，输出是下一个状态。这里有一个关键点：状态转移要有"滞后"处理。也就是说，从跟车切换到巡航，不能只要障碍物刚刚消失就立即切，而是要等它持续消失超过一定帧数。否则在障碍物边缘反复横跳时，车辆会一直风琴式加速减速。
代码实现上，我用了类似这样的结构：
function state = decideState(state, dist_obs, vel_obs, vel_ego, params) % state: 1-巡行 2-跟车 3-停车等待 switch state case 1 if dist_obs < params.cruise_gap && vel_ego > 0 state = 2; end case 2 if dist_obs < params.stop_gap state = 3; elseif dist_obs > params.cruise_gap_recover state = 1; end case 3 if dist_obs > params.stop_gap_recover state = 2; end end end注意我用的是不同的阈值来控制切入和切出的边界：cruise_gap是进入跟车的距离，cruise_gap_recover是退出跟车的距离，两者之间留了一个滞回区间。比如进入跟车设为30米，退出跟车设为50米，这样障碍物在30到50米之间波动时，状态不会乱变。这个思路在工程上叫滞回控制，比单纯一个阈值可靠得多。
2.2 规划模块：路径规划与速度规划的实现思路
决策层给出了"跟车"或"巡航"的行为，但车辆具体怎么走、速度怎么变化，还得靠规划层细化。在这个Demo里，因为我选的场景是相对简单的直道加一个弯道，所以没有用到特别复杂的RRT或A*算法，而是直接基于参考路径做偏移，再针对每个路径点分配速度。
路径规划的核心思路是：先定义一条全局参考路径，用一系列离散点表示。然后根据当前车辆位置找到最近的参考点，再向前取一段作为局部目标路径。如果遇到障碍物，则在参考路径的基础上做横向偏移，生成一条绕行曲线。
这里最实用的是用三次样条插值或参数多项式来平滑路径。如果不平滑，控制模块会产生很明显的抖动。我在项目中使用了interp1配合smoothdata做简单平滑，效果足够。
速度规划的逻辑相对简单但容易出错。我采用的是"分段速度曲线"法：每个路径点都有一个期望速度，在巡航段设为目标速度，在转弯段根据曲率限速，在跟车段设定为前方障碍物的速度。最后还要做加速度约束，防止速度跳变。
核心代码如下：
% 根据曲率计算弯道限速 curvature = computeCurvature(path_x, path_y); v_max_curve = sqrt(params.ay_max ./ max(curvature, 1e-6)); v_plan = min(params.cruise_speed, v_max_curve);注意这里ay_max是最大侧向加速度，通常取2到3 m/s^2，保证舒适性。如果曲率很大，允许的通速度就会很低。这个公式其实来源于向心加速度公式a = v^2 / r，属于规划里的基础操作。
2.3 控制模块：PID与纯跟踪控制算法
控制模块是把规划好的轨迹变成车辆实际动作的最后一环。横向控制我选用了纯跟踪算法，纵向控制用了带前馈的PID。
纯跟踪算法的原理非常简单：以车辆后轴中心为基准，在目标轨迹上找一个预瞄点，这个点与车辆当前位置有一定的前方距离，然后根据车辆航向和预瞄点的夹角计算前轮转角。
转弯半径 R 与航向角偏差的关系是：
R = Ld / (2 * sin(alpha)) delta = arctan(2 * L * sin(alpha) / Ld)其中 L 是轴距，alpha 是车辆航向与车辆到预瞄点方向的夹角，Ld 是预瞄距离。预瞄距离的设计很关键，它通常与车速成正比：速度越高，预瞄得越远，否则系统容易震荡。
我用的代码如下：
function delta = purePursuit(vehicle, path_xy, lookahead_dist) % 找到预瞄点 dist = hypot(path_xy(:,1) - vehicle.x, path_xy(:,2) - vehicle.y); idx = find(dist >= lookahead_dist, 1, 'first'); if isempty(idx) idx = size(path_xy, 1); end target_x = path_xy(idx, 1); target_y = path_xy(idx, 2); alpha = atan2(target_y - vehicle.y, target_x - vehicle.x) - vehicle.yaw; delta = atan2(2 * vehicle.L * sin(alpha), lookahead_dist); end纵向控制我用的PID加速：
error = v_target - v_ego; accel = Kp * error + Ki * integral_error + Kd * dv_error;在实际代码里，积分项通常要加一个限幅。不然车辆堵在路上时，积分误差会一直累积，造成起步时的猛蹿。我一般在积分项前加一个开关：与目标速度误差超过一定阈值时，不累加积分。这个小技巧在很多PID教程里不会写，但实战中非常有用。
3. Matlab代码实战：从零搭建一个可运行的决策规划控制Demo
3.1 环境搭建与车辆运动学模型
先把环境搭好。我用的Matlab版本并不重要，理论上R2019之后都能跑。你把主脚本、决策函数、规划函数、控制函数、车辆模型函数放在同一个目录，然后依次运行即可。
车辆模型我选的是自行车模型，也叫单车模型。它假设车辆可以简化为前后两个轮子，前轮控制转向，后轮沿车身方向滚动。状态量包括位置 (x, y)、航向角 yaw、纵向速度 v、前轮转角 delta。运动学方程如下：
x_dot = v * cos(yaw) y_dot = v * sin(yaw) yaw_dot = v * tan(delta) / L我用一个单独的函数vehicleModel.m来表示离散化后的更新过程：
function veh = updateVehicle(veh, delta, a, dt) % delta 前轮转角(rad), a 加速度(m/s^2), dt 仿真步长(s) veh.x = veh.x + veh.v * cos(veh.yaw) * dt; veh.y = veh.y + veh.v * sin(veh.yaw) * dt; veh.yaw = veh.yaw + veh.v * tan(delta) / veh.L * dt; veh.v = max(0, veh.v + a * dt); end注意速度不能为负，所以在更新后加了max(0, ...)。另外，实际车辆的液压转向系统有执行延迟，但这里作为入门Demo，先忽略延迟，踩过这个坑的同学可以去试试加入一阶惯性环节，你会发现控制失稳的概率明显增加，这就会让你理解为什么实车控制需要复杂的补偿了。
场景定义方面，我用一个简单的弯道参考路径。生成方式可以是若干个控制点，通过样条插值生成稠密路径点：
ref_pts = [0 0; 50 10; 100 30; 150 60; 200 80]; path_x = linspace(0, 200, 2000); path_y = interp1(ref_pts(:,1), ref_pts(:,2), path_x, 'spline');3.2 决策逻辑的代码实现
决策逻辑虽然短，但它是整个系统的状态中枢。我建议单独建一个decision.m文件，输入为前方障碍物信息、当前状态和参数结构体，输出为下一步的行为模式。
为了让代码更好读，我用枚举值代替1、2、3：
classdef Behavior enumeration CRUISE, FOLLOW, STOP end end在循环中调用：
switch behavior case Behavior.CRUISE v_target = params.cruise_speed; case Behavior.FOLLOW v_target = min(vel_obs, params.cruise_speed); dist_ref = params.follow_gap; case Behavior.STOP v_target = 0; end这里有一个特别容易忽略的地方，就是障碍物距离的获取。在真实系统中，障碍物坐标是感知给的，但在这个仿真Demo里，障碍物坐标是提前写死的。所以我在每个仿真步去计算车辆到障碍物的当前距离时，要特别注意：障碍物不一定在车辆的正前方。如果你直接取欧氏距离，车辆侧面的障碍物也会触发跟车，这是错的。
解决方法是用车辆航向方向的投影距离：
dx = obs_x - vehicle.x; dy = obs_y - vehicle.y; dist_front = dx * cos(vehicle.yaw) + dy * sin(vehicle.yaw);只有当dist_front大于0且绝对值小于阈值时，才认为障碍物在影响范围内。这个细节在后面的调试环节坑了我不少时间，新手往往想不到。
3.3 轨迹规划与轨迹跟踪控制代码
轨迹规划我采用了一个很直观的思路：把参考路径整条当作候选路径，如果前方有障碍物，就从当前点到障碍物旁边生成一个局部绕行路径，再把平滑后的轨迹作为目标路径。具体代码不展开太多，核心是：
function [path_x_local, path_y_local, v_local] = planTrajectory(vehicle, ref_path, obstacle, behavior, params) % 先在参考路径上截取一段 [path_x_local, path_y_local] = extractLocalPath(vehicle, ref_path, params.lookahead_distance); % 如果有障碍且需要绕行，生成一个偏移点 if behavior == Behavior.FOLLOW || behavior == Behavior.STOP % 这里简单处理：保持车道内跟随，不做变道绕行 v_local = min(params.cruise_speed, obstacle.vel); else v_local = params.cruise_speed * ones(size(path_x_local)); end end注意，我最初的版本打算做变道绕行，但后来发现变道逻辑会让整个脚本瞬间复杂很多，尤其是状态机的冲突处理。为了保持Demo的可读性，最后就只做车道内跟车和停车。如果你想让自己的Demo更有挑战性，可以在决策层加上"左道空闲"的判断，然后规划一条平滑的偏移轨迹。这部分后续扩展性很强，但第一版不建议上。
控制模块的调用就更直接了。每个仿真步，先根据当前车辆位置在目标轨迹上找预瞄点，再调用纯跟踪函数得到前轮转角，同时用速度误差计算加速度：
delta = purePursuit(vehicle, [path_x_local path_y_local], params.lookahead_speed * vehicle.v + params.base_lookahead); a = pidSpeedControl(v_target, vehicle.v, pid_params, dt);这里预瞄距离跟车速挂钩，params.lookahead_speed取2到3，base_lookahead取3到5米比较合适。预瞄太大，车辆拐弯时会切内线；太短，则会蛇形摆动。后面调试部分我会详细讲怎么整定这个参数。
3.4 主循环与可视化
主脚本是整个Demo的骨架。我设置仿真步长dt = 0.05s（20Hz），总时长60秒，使用for循环执行。每步按顺序做四件事：更新决策、规划轨迹、计算控制、更新车辆状态。同步用动态figure画车辆和轨迹。
这里有个性能小技巧：不要每次都重绘所有图形对象，而是只更新句柄的位置数据。像这样：
set(h_veh, 'XData', vehicle.x, 'YData', vehicle.y); set(h_traj, 'XData', path_x_local, 'YData', path_y_local); drawnow;如果直接plot全量重绘，仿真速度会慢很多，尤其是路径点上万时，每步刷新几千条线会很卡。用句柄更新后速度会快一个量级。
主循环的伪代码如下：
for t = 0:dt:T % 1. 决策 behavior = decideState(behavior, dist_front, vel_obs, vehicle.v, params); % 2. 规划 [path_x, path_y, v_plan] = planTrajectory(...); % 3. 控制 delta = purePursuit(...); a = pidSpeedControl(...); % 4. 车辆模型更新 vehicle = updateVehicle(vehicle, delta, a, dt); % 5. 记录数据，用于画图和后续分析 ... end运行结束后，可以单独画一张速度-时间图、转向角-时间图，对比期望速度和实际速度的跟踪情况。这一步看似简单，但能直观地暴露出决策状态切换的合理性、PID参数的好坏、预瞄距离是否合适。
4. 调试经验：常见问题与避坑指南
4.1 仿真步长与控制频率的匹配
我一开始用了dt=0.1s，也就是10Hz控制频率，结果发现车辆轨迹有明显的锯齿，特别是转弯时。后来改成0.05s，效果立刻平滑了很多。原因很简单：纯跟踪控制依赖车辆位置和航向的反馈，如果状态更新过于离散，算法算出的转角就会一步大一步小，像人开车时反应很慢一样。
但步长也不是越短越好。步长太短（比如0.001s）会让仿真循环次数太多，脚本跑起来太慢，而且车辆模型的高频噪声会被PID放大。经验值是：横向控制用20Hz到50Hz，纵向控制甚至可以10Hz，但最好统一步长，方便调试。
我建议在代码里加一个时间步长参数，放在params.dt里统一引用。这样后面换步长只要改一行。不要在各个函数里硬编码dt。
4.2 坐标系不统一导致的奇葩问题
这个坑在自动驾驶里特别常见。你规划出的路径可能是局部坐标系下的，而车辆状态是全局坐标系下的，如果不做变换，控制模块算出来的转角会完全错误。你会发现车辆绕着一个莫名其妙的中心旋转，或者朝错误的方向开。
我踩坑的情况是这样的：我从参考路径截取局部路径时，直接取了全局坐标下的点，然后传给纯跟踪函数。纯跟踪函数的alpha计算是基于全局坐标的，这其实没问题。问题出现在有一次我想加入车辆坐标系下的路径，但忘了把路径点减去车辆位置并旋转到车体坐标系，结果转角计算全乱了。
解决方法是明确约定：整个项目统一使用全局坐标系。规划模块输出全局坐标，控制模块也基于全局坐标计算。只有在算投影距离时，才临时把障碍物坐标变换到车辆坐标系，但计算完立马转回。这个约定让我后来的代码清爽了很多。
如果你想把局部坐标系加入，那就要牢记坐标变换公式：
x_local = (x_global - x_vehicle) * cos(yaw) + (y_global - y_vehicle) * sin(yaw) y_local = -(x_global - x_vehicle) * sin(yaw) + (y_global - y_vehicle) * cos(yaw)实际工程里，各种坐标系变换最容易出错，所以好的代码风格是在文件开头注释写明"本函数使用全局坐标系"。
4.3 参数整定的经验技巧
参数整定是这个项目里最花费时间的地方。我整理几个规律，能让你少走弯路。
第一，PID系数先调比例项，再加积分和微分。纯纵向速度控制，可以先用P控制，慢慢增大Kp，直到稳态误差可接受；再加入较小的Ki消除稳态误差；最后加一点Kd抑制超调。我用的起始参数是Kp=1.0，Ki=0.1，Kd=0.05，然后根据速度阶跃响应微调。
第二，预瞄距离的整定有个手感公式：lookahead = 2.5 * v + 3.0。也就是说，车速10m/s时预瞄28米，车速5m/s时预瞄15.5米。太快的时候预瞄远一点，避免震荡；低速时预瞄近一点，保证过弯精度。但这个参数跟路径曲率还有关系，弯道急的地方可以适当减小预瞄系数，让车早点转向。
第三，状态机的阈值要有滞回，这个前面讲过。我再推荐一个经验值：进入跟车的距离设为30米，退出跟车设为45米；进入停车设为15米，退出停车设为20米。这个值是针对乘用车60km/h以下时速的，你可以按比例缩放。
第四，积分项的"防饱和"处理。我常年在PID代码里加一行：
integral_error = min(max(integral_error, -0.5), 0.5);把积分控制在正负0.5以内，防止长时间停车时积分累积过大。不加这行的后果就是，前车起步后，你的车会猛地窜出去，非常吓人。
4.4 状态机震荡问题的排查
在调试过程中，最常见、也最难排查的问题是车辆反复加速减速。比如障碍物距离一直在30到40米之间波动，状态就在巡行和跟车之间反复切换，速度曲线像锯齿一样。
排查思路如下：
- 先画出障碍物距离随时间的变化曲线，看波动频率。
- 再看状态切换的每一帧，确认是梯度阈值不够大还是噪声影响。
- 如果是距离计算的噪声，就做低通滤波，比如一阶滞后滤波：
dist_smooth = alpha * dist_raw + (1-alpha) * dist_smooth，alpha取0.2左右。 - 如果是状态机迟滞不足，就调大两个阈值之间的间隔。
我最后是同时做了低通滤波和加大迟滞，才把锯齿完全消掉。对于入门项目，我建议先直接加低通滤波，不一定要把阈值间隔调得很大，因为间隔太大会让车辆对真实障碍物的反应变迟钝。
4.5 发散、侧滑等现象的本质原因
有时你运行Demo，车辆会画圈或者直接冲出路面，这种"发散"现象也很吓人。本质上大多数是横向控制失稳。
我的经验按优先级排查：
- 预瞄距离过短：车辆对轨迹误差反应过度，导致转角震荡，逐步发散。先增大预瞄系数试试。
- 控制频率过低：dt太大，状态滞后严重，尤其在车速较快时，系统接近不稳定。把频率提到20Hz以上。
- 路径不平滑：如果没有经过样条平滑，路径点会出现尖角，纯跟踪算法会试图猛打方向，引发蛇形。先用
smoothdata平滑路径。 - 车辆模型轴距符号错误：如果L写成了负数，转弯方向就会反，车会迅速掉头。
把这些因素一个个排除后，系统基本能稳定运行。
5. 从Demo到真实系统的进阶思考
跑通这个Matlab Demo只是第一步。如果你真的想在自动驾驶这条路走下去，我建议你在现有代码上做几次"升级"。
5.1 升级决策算法
项目中的有限状态机只适用于固定场景。真实道路上的驾驶情况要复杂得多，比如旁边车道车辆突然并入、红灯变成绿灯、前车急刹等。你可以把有限状态机替换成基于优先级的行为树，或者用蒙特卡洛树搜索做更高级的决策。但我不建议一开始就上强化学习，因为你会很难判断算法失败是因为决策不对，还是规划控制没跟到位。先把可解释的规则逻辑吃透，再去碰学习类算法。
5.2 升级规划算法
目前规划是固定参考路径加简单避障，你可以尝试实现经典的A算法做全局路径规划，然后用时间弹性带或者模型预测控制做局部轨迹优化。你不需要一下子全做出来，可以先在Matlab里实现A，然后把生成的路径接入现有的控制模块。这样能让你在同一个框架下对比不同规划算法的优劣。
5.3 升级控制算法
纯跟踪虽然简单，但它在高速和动态弯道中的表现有限。你可以试试LQR（线性二次型调节器）或者MPC（模型预测控制）。Matlab对这两者都有很好的支持，LQR的lqr函数可以直接调用，MPC工具箱也有例程。升级后你会发现，在同样的场景下，车辆的跟踪精度和舒适性明显不同。这也能让你直观体会到，为什么现代自动驾驶系统主流都在用MPC做轨迹跟踪。
不过我想强调：不要因为算法高级就觉得它一定更好。在Demo里，纯跟踪调好了，直线巡航和缓弯道完全够用。它简单、鲁棒、可解释，很多时候是工程落地里性价比最高的选择。
我个人在实际操作中的体会是：这个项目最大的价值不在于"跑通"，而在于它把你从看客变成了理解者。当你能亲手调出平滑的速度曲线、稳定的过弯轨迹，你才真正建立起对自动驾驶系统的工程直觉。后面你再去看Autoware、Apollo这些开源框架的代码，很多概念都能对号入座，不会再觉得那是一个黑盒。
最后再分享一个小技巧：代码注释写得详细一点，尤其是每个模块的输入输出和坐标约定。因为在调试中你会反复怀疑是自己逻辑错了，还是坐标系错了，透明的注释能帮你快速定位问题。保存好你的第一版Demo，过几个月再回头看，你会明显感到自己在进步。
