当前位置：首页 > news >正文

自动驾驶横纵向控制仿真分享：从零开始的探索之旅

news 2026/3/26 19:52:53

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

最近在学习自动驾驶的控制算法，终于完成了横纵向控制的仿真，感觉整个过程挺有成就感的，想和大家分享一下我的学习心得和实现过程。

一、控制算法的选择与实现

纵向控制：PID控制

纵向控制我参考了百度Apollo的双环PID控制算法，这个算法在Apollo的开源代码中用得比较多，实现起来相对直观。PID控制的核心在于调节三个参数：比例系数（P）、积分系数（I）和微分系数（D）。通过调节这三个参数，可以实现对车速的有效控制。

% 纵向PID控制算法实现 function [throttle, brake] = longitudinalPID(current_speed, target_speed, dt) % PID参数 Kp = 0.5; Ki = 0.1; Kd = 0.2; % 计算误差 error = target_speed - current_speed; integral = integral + error * dt; derivative = (error - last_error) / dt; % 计算控制量 output = Kp * error + Ki * integral + Kd * derivative; % 分配油门和刹车 if output > 0 throttle = output; brake = 0; else throttle = 0; brake = -output; end % 更新last_error last_error = error; end

横向控制：MPC控制

横向控制我选择了模型预测控制（MPC），这个算法在Apollo中也有应用，主要通过预测未来的轨迹来优化当前的控制输入。MPC的核心在于建立车辆的动力学模型，并通过求解优化问题来得到最优控制量。

% MPC控制算法实现 function [steering] = mpcControl(state, reference, dt, T) % 状态变量：[x, y, yaw, v] % 参考轨迹：五次多项式 % 优化问题求解 % 这里省略优化问题的具体实现，具体可以参考Apollo的MPC实现 % 优化目标：最小化与参考轨迹的偏差 % 约束条件：方向盘转角限制、加速度限制等 % 优化求解 [steering] = optimizeControl(state, reference, dt, T); end

二、车辆动力学模型

我选择了二自由度车辆动力学模型，这个模型可以较好地描述车辆的运动特性，同时又不至于过于复杂。模型主要包括纵向运动学和侧向动力学两部分。

% 车辆动力学模型 function [next_state] = vehicleDynamics(state, control, dt) % 状态变量：[x, y, yaw, v] % 控制量：[throttle, brake, steering] % 计算加速度 acceleration = (throttle - brake) * engine_torque / vehicle_mass; % 更新纵向速度 next_state.v = state.v + acceleration * dt; % 计算侧向加速度 lateral_acceleration = next_state.v * tan(steering) / wheelbase; % 更新 yaw 角速度 yaw_rate = lateral_acceleration / state.v; % 更新位置 next_state.x = state.x + state.v * cos(state.yaw) * dt; next_state.y = state.y + state.v * sin(state.yaw) * dt; % 更新 yaw 角 next_state.yaw = state.yaw + yaw_rate * dt; end