近期实现新能源整车的巡航功能,初期采用PID控制方式实现,初步用matlab做算法初步校验,在此直接是上代码:
% 初始化参数
target_speed = 80; % 目标速度(km/h)
max_acceleration = 2; % 最大加速度(m/s^2)
max_deceleration = 2; % 最大减速度(m/s^2)
dt = 0.1; % 时间步长
% 初始化车辆状态
x = 0; % 车辆位置
v = 0; % 车辆速度
a = 0; % 车辆加速度
% 时间向量
t = 1:dt:50; % 总时间10秒
v_arry = t;
x_arry = t;
t_targetarry = t;
% 主循环
for i = 1:length(t)
% 计算目标速度和误差 randn 2*randn
target_speed = target_speed + 4*randn ; % 添加一些随机噪声以模拟真实情况
if i == 1
t_error(i) = target_speed - 0;
else
t_error(i) = target_speed - v_arry(i-1);
end
% 计算PID控制器的输出
Kp = 0.5; % 比例增益
Ki = 6; % 积分增益
Kd = 0.08; % 微分增益
if i == 1
u = Kp* t_error(i) + Ki*sum(t_error);
else
u = Kp* t_error(i) + Ki*sum(t_error) + Kd*(t_error(i) - t_error(i-1))/dt;
% u = Kp* t_error(i) + Ki*sum(t_error) + Kd*(t_error(i) - t_error(i-1))/dt;
% u = Kp*error + Ki*sum(error.*t(1:i).')/dt + Kd*(error(i) - error(i-1))/dt;
end
% 限制加速度的取值范围
% u = max(min(u, max_acceleration), -max_deceleration);
% 时间积分车辆状态
v_arry(i) = v_arry(i) + u*dt;
x_arry(i) = x_arry(i) + v_arry(i)*dt;
t_targetarry(i) = target_speed;
end
% 绘制车辆轨迹和目标速度曲线
figure;
cruise1 = plot(t, v_arry, 'g');
set(cruise1,'DisplayName','RealSpeed'); hold on;
cruise2 = plot(t, x_arry, 'b');
set(cruise2,'DisplayName','Distance');hold on;
cruise3 = plot(t, t_targetarry, 'r');
set(cruise3,'DisplayName','TargetSpeed');hold on;
xlabel('Time (s)');
ylabel('Position (m)');
title('Cruise Control Simulation');
grid on;
clear;
在处理循环中添加噪声,模拟实际系统中目标的实时更新。
在稳态没有噪声的场景下,系统响应:
绿线为实际的响应速度,橙色线为目标速度,蓝色线为实际行驶距离
整个响应过程基本没能稳定在目标转速下,距离也能表现较好的线性度。
当在目标速度中增加随机噪声,以模拟行车的动态场景:
绿线为实际的响应速度,橙色线为目标速度,蓝色线为实际行驶距离
可以看到这个算法在动态冲击下也能较好的拟合到目标速度。