好多刚知道消息，最近是真忙，好想躺平算了
clear; clc; close all;
%% ===================== 1. 参数配置模块 =====================
% 车辆物理参数（标准化定义）
vehicle_L = 4.5; % 轴距/车长 (m)
vehicle_W = 2.0; % 车宽 (m)
vehicle_v_max = 30; % 最大速度 (m/s)
vehicle_a_max = 5; % 最大加速度 (m/s²)
vehicle_delta_max = deg2rad(30); % 最大转向角 (rad)
vehicle_delta_rate = deg2rad(100); % 转向角速度限制 (rad/s)
% 仿真参数
sim_dt = 0.1; % 时间步长 (s)
sim_t_total = 30; % 总仿真时间 (s)
sim_N = round(sim_t_total / sim_dt); % 总步数
% 初始状态 [x, y, θ, v, δ]
state_x = 0;
state_y = 0;
state_theta = 0;
state_v = 0;
state_delta = 0;
% 控制器PID参数（优化整定）
ctrl_Kp_lat = 1.0;
ctrl_Ki_lat = 0.08;
ctrl_Kd_lat = 0.15;
ctrl_Kp_heading = 1.8;
ctrl_Ki_heading = 0.12;
ctrl_Kd_heading = 0.25;
ctrl_look_ahead = 15; % 避障前瞻距离
%% ===================== 2. 地图与路径生成 =====================
% 平滑参考路径（优化路径平滑性）
path_len = 200;
path_res = 1000;
path_x = linspace(0, path_len, path_res);
path_y = sin(path_x/20)*10 + 0.5*exp(-((path_x-100)/50).^2)*50;
% 预计算路径切向角（避免循环重复计算）
path_dx = diff([path_x, path_x(end)]);
path_dy = diff([path_y, path_y(end)]);
path_theta = atan2(path_dy, path_dx);
% 生成安全区域内的随机障碍物（避免生成在路径上）- 内联实现
obs_num = 8;
obs_r_min = 3;
obs_r_max = 5;
obs_x = [];
obs_y = [];
obs_r = [];
while length(obs_x) < obs_num
cx = rand*max(path_x);
cy = (rand-0.5)*30;
cr = obs_r_min + rand*(obs_r_max-obs_r_min);
% 检查是否与路径冲突
d = min(sqrt((path_x-cx).^2 + (path_y-cy).^2));
if d > cr + 4
obs_x = [obs_x, cx];
obs_y = [obs_y, cy];
obs_r = [obs_r, cr];
end
end
%% ===================== 3. 数据存储初始化 =====================
hist_pos = zeros(sim_N, 2);
hist_state = zeros(sim_N, 4);
hist_lat_err = zeros(1, sim_N);
hist_head_err = zeros(1, sim_N);
% 积分误差初始化（PID积分项）
integral_lat = 0;
integral_head = 0;
%% ===================== 4. 主仿真循环 =====================
% 创建图形窗口
fig = figure('Position', [100, 100, 1300, 650], 'Color','w');
% 初始化图形对象
h_axes = axes('Parent', fig, 'Position', [0.1, 0.1, 0.8, 0.8]);
hold(h_axes, 'on');
grid(h_axes, 'on');
axis(h_axes, 'equal');
% 绘制静态元素
h_path = plot(h_axes, path_x, path_y, 'b--', 'LineWidth',2, 'DisplayName','参考路径');
h_trajectory = plot(h_axes, [], [], 'g-', 'LineWidth',2, 'DisplayName','实际轨迹');
h_vehicle_pos = plot(h_axes, state_x, state_y, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r', 'DisplayName','车辆位置');
h_vehicle_dir = plot(h_axes, [state_x, state_x+3*cos(state_theta)], [state_y, state_y+3*sin(state_theta)], 'r-', 'LineWidth',2);
% 绘制障碍物
h_obstacles = [];
for i = 1:length(obs_x)
th = 0:0.1:2*pi;
x_circle = obs_x(i) + obs_r(i) * cos(th);
y_circle = obs_y(i) + obs_r(i) * sin(th);
h_obstacles(end+1) = fill(h_axes, x_circle, y_circle, 'k', 'EdgeColor', 'none');
end
% 绘制前瞻点
h_lookahead = plot(h_axes, [], [], 'mo', 'MarkerSize',8, 'MarkerFaceColor','m', 'DisplayName','前瞻检测点');
% 设置坐标轴范围
xlim(h_axes, [path_x(1)-10, path_x(end)+10]);
ylim(h_axes, [min(path_y)-15, max(path_y)+15]);
xlabel(h_axes, 'X(m)');
ylabel(h_axes, 'Y(m)');
title_obj = title(h_axes, sprintf('自动驾驶仿真 | 时间:0.0s 速度:0.0m/s 横向误差:0.00m'));
legend(h_axes, 'Location','best');
% 开始仿真
finish_flag = false;
for t = 1:sim_N
time = t * sim_dt;
% --------------------- 寻找最近路径点 ---------------------
dist = sqrt((path_x - state_x).^2 + (path_y - state_y).^2);
[~, idx] = min(dist);
ref_x = path_x(idx);
ref_y = path_y(idx);
ref_theta = path_theta(idx);
% --------------------- 计算误差 ---------------------
% 横向误差
dx_path = path_dx(idx);
dy_path = path_dy(idx);
perp_x = -dy_path;
perp_y = dx_path;
mag = sqrt(perp_x^2 + perp_y^2);
if mag>0
perp_x = perp_x/mag;
perp_y = perp_y/mag;
end
lat_err = (state_x - ref_x)*perp_x + (state_y - ref_y)*perp_y;
% 航向误差（手动归一化到 [-π, π]）
head_err = state_theta - ref_theta;
while head_err > pi
head_err = head_err - 2*pi;
end
while head_err < -pi
head_err = head_err + 2*pi;
end
% 存储误差
hist_lat_err(t) = lat_err;
hist_head_err(t) = head_err;
% --------------------- PID轨迹跟踪控制器 ---------------------
% 积分分离（抗积分饱和）
if abs(lat_err) < 5
integral_lat = integral_lat + lat_err * sim_dt;
end
if abs(head_err) < pi/4
integral_head = integral_head + head_err * sim_dt;
end
% 微分项
if t>1
d_lat = (lat_err - hist_lat_err(t-1))/sim_dt;
d_head = (head_err - hist_head_err(t-1))/sim_dt;
else
d_lat = 0;
d_head = 0;
end
% 计算转向角
delta = -ctrl_Kp_lat*lat_err - ctrl_Ki_lat*integral_lat - ctrl_Kd_lat*d_lat...
-ctrl_Kp_heading*head_err - ctrl_Ki_heading*integral_head - ctrl_Kd_heading*d_head;
delta = max(min(delta, vehicle_delta_max), -vehicle_delta_max); % 限幅
% --------------------- 动态避障与速度控制 ---------------------
ahead_x = state_x + ctrl_look_ahead * cos(state_theta);
ahead_y = state_y + ctrl_look_ahead * sin(state_theta);
min_dist = inf;
for i = 1:obs_num
d = sqrt((ahead_x-obs_x(i))^2 + (ahead_y-obs_y(i))^2);
min_dist = min(min_dist, d - obs_r(i));
end
% 安全速度规划
if min_dist < 20
v_des = vehicle_v_max * max(0, (min_dist - 3)/17);
else
v_des = vehicle_v_max;
end
% 加速度控制
a_cmd = 0.9 * (v_des - state_v); % 提高响应速度
a_cmd = max(min(a_cmd, vehicle_a_max), -vehicle_a_max);
% --------------------- 车辆运动学更新（自行车模型） ---------------------
% 速度更新
state_v = state_v + a_cmd * sim_dt;
state_v = max(0, min(state_v, vehicle_v_max));
% 转向角动态响应
d_delta = max(min(delta - state_delta, vehicle_delta_rate*sim_dt), -vehicle_delta_rate*sim_dt);
state_delta = state_delta + d_delta;
% 位置与航向更新
beta = atan(vehicle_L/2 * tan(state_delta));
state_x = state_x + state_v * cos(state_theta + beta) * sim_dt;
state_y = state_y + state_v * sin(state_theta + beta) * sim_dt;
state_theta = state_theta + 2*state_v/vehicle_L * sin(beta) * sim_dt;
% 归一化角度到 [-π, π]
while state_theta > pi
state_theta = state_theta - 2*pi;
end
while state_theta < -pi
state_theta = state_theta + 2*pi;
end
% --------------------- 存储历史数据 ---------------------
hist_pos(t,:) = [state_x, state_y];
hist_state(t,:) = [state_x, state_y, state_theta, state_v];
% --------------------- 实时可视化（优化帧率） ---------------------
if mod(t, 2) == 0 % 每2步更新一次，提高动画流畅度
% 更新车辆位置
set(h_vehicle_pos, 'XData', state_x, 'YData', state_y);
set(h_vehicle_dir, 'XData', [state_x, state_x+3*cos(state_theta)], 'YData', [state_y, state_y+3*sin(state_theta)]);
% 更新轨迹
set(h_trajectory, 'XData', hist_pos(1:t,1), 'YData', hist_pos(1:t,2));
% 更新前瞻点
set(h_lookahead, 'XData', ahead_x, 'YData', ahead_y);
% 更新标题
set(title_obj, 'String', sprintf('自动驾驶仿真 | 时间:%.1fs 速度:%.1fm/s 横向误差:%.2fm', time, state_v, hist_lat_err(t)));
% 强制刷新显示
drawnow limitrate;
% 添加小延迟使动画更流畅
pause(0.01);
end
% --------------------- 终点判断 ---------------------
if sqrt((state_x-path_x(end))^2 + (state_y-path_y(end))^2) < 5 && state_v < 0.5
fprintf('✅ 车辆成功到达终点！用时：%.1f s\n', time);
finish_flag = true;
break;
end
end
if ~finish_flag
fprintf('⏱️ 仿真时间结束，未到达终点\n');
end
%% ===================== 5. 结果分析与绘图 =====================
fprintf('\n====== 仿真结果统计 ======\n');
fprintf('有效仿真时间：%.1f s\n', t*sim_dt);
fprintf('最终位置：(%.2f, %.2f) m\n', hist_state(t,1), hist_state(t,2));
fprintf('最终速度：%.2f m/s\n', hist_state(t,4));
fprintf('最大速度：%.2f m/s\n', max(hist_state(1:t,4)));
fprintf('平均速度：%.2f m/s\n', mean(hist_state(1:t,4)));
fprintf('平均横向误差：%.2f m\n', mean(abs(hist_lat_err(1:t))));
% 轨迹对比图
figure('Color','w');
subplot(2,1,1);
plot(1:t, hist_state(1:t,1),'b', 'LineWidth',1.5);
hold on;
path_x_interp = interp1(1:length(path_x), path_x, 1:t, 'linear', 'extrap');
plot(1:t, path_x_interp,'r--');
xlabel('时间步'); ylabel('X(m)'); title('X方向轨迹对比');
legend('实际','参考'); grid on;
subplot(2,1,2);
plot(1:t, hist_state(1:t,2),'b', 'LineWidth',1.5);
hold on;
path_y_interp = interp1(1:length(path_y), path_y, 1:t, 'linear', 'extrap');
plot(1:t, path_y_interp,'r--');
xlabel('时间步'); ylabel('Y(m)'); title('Y方向轨迹对比');
legend('实际','参考'); grid on;
% 速度曲线
figure('Color','w');
plot(1:t, hist_state(1:t,4), 'LineWidth',2);
xlabel('时间步'); ylabel('速度(m/s)'); title('速度变化曲线');
grid on;
% 横向误差曲线
figure('Color','w');
plot(1:t, hist_lat_err(1:t), 'LineWidth',2);
xlabel('时间步'); ylabel('横向误差(m)'); title('路径跟随横向误差');
grid on;
yline(0,'k--');
fprintf('\n仿真完成！\n');
