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

原创于 2025-12-12 18:30:00 发布 · 242 阅读

CC 4.0 BY-SA版权

文章标签：

自动驾驶横纵向控制，纵向采用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时，既能保证跟踪精度又不会让方向盘抽风。