自动驾驶横纵向控制，纵向采用pid控制，横向采用mpc控制，根据的是车辆二自由度车辆动力学模型

自动驾驶横纵向控制，纵向采用pid控制，横向采用mpc控制，根据的是车辆二自由度车辆动力学模型，得到各矩阵之后在S函数里面进行编写，纵向参考百度Apollo纵向双环PID控制算法，横向参考百度Apollo中mpc控制算法编写，规划的轨迹为五次多项式函数形式。 控制效果：车辆侧向位移和纵向位移跟踪效果很好，纵向车速跟随也较好，但有一定的误差。 Matlab/Simulink版本:2021a Carsim版本：2019.0

油门刹车交给PID，方向盘交给MPC——这可能是自动驾驶控制最简单粗暴的组合拳了。最近在Carsim+Simulink联合仿真里搞了套横纵向控制器，实测在双移线工况下横向误差能压到0.2米以内，纵向速度误差控制在±3km/h，虽然离完美还有距离，但已经能看出这两个经典算法的实战能力。

纵向控制直接复刻了Apollo的双环套路，外层速度环出加速度，内层加速度环算油门/刹车。这里有个反直觉的设计：油门和刹车居然用同一个PID控制器。代码里用Simulink的Switch模块做了个平滑切换：

function [throttle, brake] = fcn(target_accel, current_speed) persistent pid; if isempty(pid) pid = pidController('Kp',0.8,'Ki',0.05,'Kd',0.02); end accel_error = target_accel - getActualAccel(); control = step(pid, accel_error); if control > 0 throttle = saturate(control, 0, 1); brake = 0; else throttle = 0; brake = saturate(-control, 0, 1); end end

你可能注意到了积分项没做抗饱和处理，这是故意留的破绽——当车辆处于稳态时，积分项会自己找到平衡点。实测在匀速30km/h时，油门开度能稳定在18%左右，和实车数据基本吻合。

横向控制才是重头戏。MPC的预测模型用二自由度模型线性化得来，状态矩阵A长这样：

// MPC初始化代码片段 A << 0, 1, 0, 0, 0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, -(a*Cf - b*Cr)/(m*vx), 0, 0, 0, 1, 0, -(a*Cf - b*Cr)/(Iz*vx), (a*Cf - b*Cr)/Iz, -(a²*Cf + b²*Cr)/(Iz*vx);

这里有个坑：当车速vx趋近于零时矩阵元素会爆炸。解决办法是在低速时冻结横向控制器，等纵向速度起来后再激活。MPC的权重矩阵调参最费时间，最终发现前轮转角权重设为10，横向误差权重设为500时，既能保证跟踪精度又不会让方向盘抽风。

五次多项式轨迹生成器在实际跑起来时，会出现曲率突变的问题。后来在规划层和控制器之间加了个低通滤波器，把曲率变化率限制在0.1m⁻¹/s以内，方向盘抖动明显改善。不过这也导致在急弯时会有约0.5m的滞后，算是性能与稳定性的trade-off。

联合仿真时最抓狂的是Carsim的接口配置，特别是S-Function里那个carsim2simulink.dll的调用顺序。血的教训：一定要在Initialize子函数里先调用VS_Init，否则仿真跑起来车辆状态全是NaN。后来在模型里加了几个示波器监控关键信号，总算把数据对齐的问题解决了。

现在这套控制器吃的是"细粮"——依赖高精度定位和轨迹输入。试过在轨迹里加10cm的随机噪声，结果MPC直接带着车辆画起正弦曲线。下一步打算把预瞄距离改成动态调整，或许能提升抗干扰能力。控制这事儿，永远都是在和现实世界的噪声较劲啊。