基于Matlab/Simulink 2021a与Carsim 2019.0的自动驾驶横纵向控制...

自动驾驶横纵向控制纵向采用pid控制横向采用mpc控制根据的是车辆二自由度车辆动力学模型得到各矩阵之后在S函数里面进行编写纵向参考百度Apollo纵向双环PID控制算法横向参考百度Apollo中mpc控制算法编写规划的轨迹为五次多项式函数形式。 控制效果车辆侧向位移和纵向位移跟踪效果很好纵向车速跟随也较好但有一定的误差。 Matlab/Simulink版本:2021a Carsim版本2019.0油门刹车交给PID方向盘交给MPC——这可能是自动驾驶控制最简单粗暴的组合拳了。最近在CarsimSimulink联合仿真里搞了套横纵向控制器实测在双移线工况下横向误差能压到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, -(CfCr)/(m*vx), (CfCr)/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时既能保证跟踪精度又不会让方向盘抽风。自动驾驶横纵向控制纵向采用pid控制横向采用mpc控制根据的是车辆二自由度车辆动力学模型得到各矩阵之后在S函数里面进行编写纵向参考百度Apollo纵向双环PID控制算法横向参考百度Apollo中mpc控制算法编写规划的轨迹为五次多项式函数形式。 控制效果车辆侧向位移和纵向位移跟踪效果很好纵向车速跟随也较好但有一定的误差。 Matlab/Simulink版本:2021a Carsim版本2019.0五次多项式轨迹生成器在实际跑起来时会出现曲率突变的问题。后来在规划层和控制器之间加了个低通滤波器把曲率变化率限制在0.1m⁻¹/s以内方向盘抖动明显改善。不过这也导致在急弯时会有约0.5m的滞后算是性能与稳定性的trade-off。联合仿真时最抓狂的是Carsim的接口配置特别是S-Function里那个carsim2simulink.dll的调用顺序。血的教训一定要在Initialize子函数里先调用VS_Init否则仿真跑起来车辆状态全是NaN。后来在模型里加了几个示波器监控关键信号总算把数据对齐的问题解决了。现在这套控制器吃的是细粮——依赖高精度定位和轨迹输入。试过在轨迹里加10cm的随机噪声结果MPC直接带着车辆画起正弦曲线。下一步打算把预瞄距离改成动态调整或许能提升抗干扰能力。控制这事儿永远都是在和现实世界的噪声较劲啊。