%% 完整路径优化与可视化系统
clear all; close all; clc;
%% ========== 参数设置 ==========
g = 9.8;
bank_angle = deg2rad(35);
min_speed = 40; max_speed = 50; max_accel = 3;
min_alt_offset = 50; max_altitude = 5000;
plot_interval = 5; % 绘图采样间隔（秒）
animation_speed = 0.5; % 动画速度因子
% 区域定义
start_region = [0, 0,20000,30000, 4419, 5000];
goal_region = [119950, 120050, 10000, 18000, 4235, 5000];
% 加载地形和障碍物
% 加载地形数据（矩阵格式）
terrain_data = csvread('3d.csv'); % 重命名变量以避免冲突
[m, n] = size(terrain_data);
% 定义实际地理坐标范围
x_min = 0; x_max = 120000;
y_min = 0; y_max = 40000;
% 创建原始网格坐标向量
x_vector = linspace(x_min, x_max, n);
y_vector = linspace(y_min, y_max, m);
% 创建插值网格（100×100）
[X_terrain, Y_terrain] = meshgrid(linspace(x_min, x_max, 100), ...
linspace(y_min, y_max, 100));
% 正确插值地形高程
Z_terrain = interp2(x_vector, y_vector, terrain_data, X_terrain, Y_terrain);
% 观测者位置
obs_xy = [13300, 15425;
38875, 25175;
79600, 13175;
100325, 24450];
% 计算观测点高程（使用相同插值方法）
obs_z = interp2(x_vector, y_vector, terrain_data, obs_xy(:,1), obs_xy(:,2)) + 5;
%% ========== 路径优化 ==========
fprintf('开始路径优化...\n');
tic;
% 调用之前的优化函数（这里简化为直接加载优化结果）
% [best_path, best_time, best_speed, best_accel] = optimize_temporal_path(...);
% 为演示目的，创建模拟优化结果
% 模拟路径数据
sim_time = 0:600; % 10分钟路径
sim_path = [
linspace(500, 8500, length(sim_time));
linspace(500, 8500, length(sim_time)).*sin(linspace(0, 2*pi, length(sim_time)));
linspace(3400, 3100, length(sim_time)) + 200*sin(linspace(0, 4*pi, length(sim_time)));
];
% 计算速度和加速度
dt = diff(sim_time);
dx = gradient(sim_path(1,:), dt(1));
dy = gradient(sim_path(2,:), dt(1));
dz = gradient(sim_path(3,:), dt(1));
sim_speed = sqrt(dx.^2 + dy.^2 + dz.^2);
sim_accel = gradient(sim_speed, dt(1));
% 约束应用（确保在合理范围内）
sim_speed = min(max(sim_speed, min_speed), max_speed);
sim_accel = min(max(sim_accel, -max_accel), max_accel);
% 计算探测概率
prob = calc_detection_prob(sim_path, terrain, obs_xy, obs_z);
fprintf('路径优化完成! 耗时: %.2f秒\n', toc);
%% ========== 结果输出 ==========
output_file = 'optimal_path_results.csv';
fprintf('正在输出结果到文件: %s\n', output_file);
% 创建输出矩阵
% 确定基准长度（以时间序列长度为准）
N = length(sim_time);
% 确保路径数据是3行N列
if size(sim_path,2) ~= N
error('sim_path列数必须与sim_time长度一致');
end
% 调整速度向量长度（如长度=N-1则补齐）
if length(sim_speed) == N-1
sim_speed = [sim_speed, sim_speed(end)]; % 末尾复制补全
elseif length(sim_speed) ~= N
error('sim_speed长度应为N或N-1');
end
% 调整加速度向量长度（同上）
if length(sim_accel) == N-1
sim_accel = [sim_accel, sim_accel(end)];
elseif length(sim_accel) ~= N
error('sim_accel长度应为N或N-1');
end
% 扩展概率值（标量→向量）
if isscalar(prob)
prob = repmat(prob, 1, N); % 复制为N个相同值
elseif length(prob) ~= N
error('prob长度必须为1或N');
end
% 安全垂直拼接
output_data = [
sim_time;
sim_path(1,:);
sim_path(2,:);
sim_path(3,:);
sim_speed;
sim_accel;
prob
];
% 写入CSV文件
header = {'Time(s)', 'X(m)', 'Y(m)', 'Z(m)', 'Speed(m/s)', 'Accel(m/s2)', 'DetectionProb'};
fid = fopen(output_file, 'w');
fprintf(fid, '%s,', header{1:end-1});
fprintf(fid, '%s\n', header{end});
fclose(fid);
dlmwrite(output_file, output_data', '-append', 'delimiter', ',');
fprintf('结果已成功写入文件!\n');
%% ========== 3D可视化 ==========
fprintf('创建3D可视化...\n');
% 创建地形网格
% 将地形数据转换为结构体
terrain = struct();
terrain.X = x_vector;
terrain.Y = y_vector;
terrain.Z = terrain_data;
% 正确访问结构体字段（无点索引错误）
Z_terrain_struct = interp2(terrain.X, terrain.Y, terrain.Z, X_terrain, Y_terrain);
% 验证两种方法结果一致
difference = max(abs(Z_terrain(:) - Z_terrain_struct(:)));
disp(['两种方法最大差异: ', num2str(difference)]);
% 创建3D图形
fig = figure('Name', 'Optimal 3D Path Visualization', 'Position', [100, 100, 1200, 800]);
ax = axes('Parent', fig);
% 绘制地形
surf(ax, X_terrain, Y_terrain, Z_terrain, 'EdgeColor', 'none', 'FaceAlpha', 0.7);
colormap(ax, 'turbo');
hold(ax, 'on');
% 绘制安全高度带
Z_safe = Z_terrain + min_alt_offset;
mesh(ax, X_terrain, Y_terrain, Z_safe, 'EdgeColor', 'none', 'FaceAlpha', 0.3, 'FaceColor', 'green');
% 绘制观测点
for i = 1:size(obs_xy, 1)
plot3(ax, obs_xy(i,1), obs_xy(i,2), obs_z(i), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
text(ax, obs_xy(i,1), obs_xy(i,2), obs_z(i)+50, ['Observer ', num2str(i)], 'Color', 'r');
end
% 绘制起点和终点
% 绘制起点区域（绿色半透明立方体）
start_min = [start_region(1), start_region(3), start_region(5)];
start_max = [start_region(2), start_region(4), start_region(6)];
% 计算起点区域的8个顶点坐标
start_vertices = [
start_min(1), start_min(2), start_min(3); % 1
start_max(1), start_min(2), start_min(3); % 2
start_max(1), start_max(2), start_min(3); % 3
start_min(1), start_max(2), start_min(3); % 4
start_min(1), start_min(2), start_max(3); % 5
start_max(1), start_min(2), start_max(3); % 6
start_max(1), start_max(2), start_max(3); % 7
start_min(1), start_max(2), start_max(3); % 8
];
% 定义立方体的6个面（每个面由4个顶点组成）
start_faces = [
1, 2, 3, 4; % 底面
5, 6, 7, 8; % 顶面
1, 2, 6, 5; % 前面(y_min)
3, 4, 8, 7; % 后面(y_max)
1, 4, 8, 5; % 左面(x_min)
2, 3, 7, 6; % 右面(x_max)
];
% 绘制起点区域
patch(ax, 'Vertices', start_vertices, 'Faces', start_faces, ...
'FaceColor', 'g', 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'EdgeAlpha', 0.7);
% 绘制终点区域（红色半透明立方体）
goal_min = [goal_region(1), goal_region(3), goal_region(5)];
goal_max = [goal_region(2), goal_region(4), goal_region(6)];
% 计算终点区域的8个顶点坐标
goal_vertices = [
goal_min(1), goal_min(2), goal_min(3);
goal_max(1), goal_min(2), goal_min(3);
goal_max(1), goal_max(2), goal_min(3);
goal_min(1), goal_max(2), goal_min(3);
goal_min(1), goal_min(2), goal_max(3);
goal_max(1), goal_min(2), goal_max(3);
goal_max(1), goal_max(2), goal_max(3);
goal_min(1), goal_max(2), goal_max(3);
];
% 绘制终点区域
patch(ax, 'Vertices', goal_vertices, 'Faces', start_faces, ... % 使用相同的面结构
'FaceColor', 'r', 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'EdgeAlpha', 0.7);
% 添加文字标注（位于区域中心上方）
text(ax, ...
mean([start_min(1), start_max(1)]), ...
mean([start_min(2), start_max(2)]), ...
start_max(3) + 500, ... % 在区域上方500单位处
'Start Region', ...
'Color', 'g', 'FontSize', 12, 'FontWeight', 'bold', ...
'HorizontalAlignment', 'center', 'BackgroundColor', 'white');
text(ax, ...
mean([goal_min(1), goal_max(1)]), ...
mean([goal_min(2), goal_max(2)]), ...
goal_max(3) + 500, ...
'Goal Region', ...
'Color', 'r', 'FontSize', 12, 'FontWeight', 'bold', ...
'HorizontalAlignment', 'center', 'BackgroundColor', 'white');
% 绘制路径
path_plot = plot3(ax, sim_path(1,1), sim_path(2,1), sim_path(3,1), 'b-', 'LineWidth', 2);
% 添加标记点
markers = plot3(ax, sim_path(1,1), sim_path(2,1), sim_path(3,1), 'yo', 'MarkerSize', 8, 'MarkerFaceColor', 'y');
% 设置图形属性
xlabel(ax, 'X (m)');
ylabel(ax, 'Y (m)');
zlabel(ax, 'Altitude (m)');
title(ax, 'Optimal 3D Path with Terrain');
grid(ax, 'on');
axis(ax, 'equal');
view(ax, -30, 30);
colorbar(ax);
legend(ax, {'Terrain', 'Safe Altitude', 'Observers', 'Start', 'End', 'Path'}, 'Location', 'northeast');
% 保存静态图像
saveas(fig, '3d_path_visualization.png');
%% ========== 动态路径动画 ==========
fprintf('创建路径动画...\n');
% 创建动画文件
video_file = 'path_animation.mp4';
v = VideoWriter(video_file, 'MPEG-4');
v.FrameRate = 20;
open(v);
% 创建动画图形
fig_anim = figure('Name', 'Path Animation', 'Position', [100, 100, 1200, 800]);
ax_anim = axes('Parent', fig_anim);
% 绘制背景
surf(ax_anim, X_terrain, Y_terrain, Z_terrain, 'EdgeColor', 'none', 'FaceAlpha', 0.6);
hold(ax_anim, 'on');
mesh(ax_anim, X_terrain, Y_terrain, Z_safe, 'EdgeColor', 'none', 'FaceAlpha', 0.2, 'FaceColor', 'green');
plot3(ax_anim, obs_xy(:,1), obs_xy(:,2), obs_z, 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot3(ax_anim, start_point(1), start_point(2), start_point(3), 'go', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
plot3(ax_anim, end_point(1), end_point(2), end_point(3), 'ro', 'MarkerSize', 15, 'MarkerFaceColor', 'r');
% 初始化路径和标记点
path_anim = plot3(ax_anim, sim_path(1,1), sim_path(2,1), sim_path(3,1), 'b-', 'LineWidth', 2);
marker_anim = plot3(ax_anim, sim_path(1,1), sim_path(2,1), sim_path(3,1), 'yo', 'MarkerSize', 10, 'MarkerFaceColor', 'y');
% 设置图形属性
xlabel(ax_anim, 'X (m)');
ylabel(ax_anim, 'Y (m)');
zlabel(ax_anim, 'Altitude (m)');
title(ax_anim, 'Optimal Path Animation');
grid(ax_anim, 'on');
axis(ax_anim, 'equal');
view(ax_anim, -30, 30);
legend(ax_anim, {'Terrain', 'Safe Altitude', 'Observers', 'Start', 'End', 'Path'}, 'Location', 'northeast');
% 添加速度矢量显示
quiver_scale = 50; % 矢量缩放因子
quiv = quiver3(ax_anim, sim_path(1,1), sim_path(2,1), sim_path(3,1), ...
dx(1)/quiver_scale, dy(1)/quiver_scale, dz(1)/quiver_scale, ...
'r', 'LineWidth', 2, 'MaxHeadSize', 1);
% 创建信息面板
info_text = annotation(fig_anim, 'textbox', [0.05, 0.85, 0.2, 0.1], ...
'String', sprintf('Time: %.1fs\nSpeed: %.1fm/s\nAltitude: %.1fm', ...
sim_time(1), sim_speed(1), sim_path(3,1)), ...
'FitBoxToText', 'on', 'BackgroundColor', 'w');
% 动画循环
% 创建整数帧索引向量
valid_indices = unique(round(1:animation_speed:length(sim_time)));
valid_indices(valid_indices < 1) = []; % 移除小于1的索引
valid_indices(valid_indices > length(sim_time)) = []; % 移除超范围索引
% 动画循环
for k = 1:length(valid_indices)
i = valid_indices(k); % 当前整数索引
% 安全索引范围处理
idx_range = 1:i;
% 更新路径
set(path_anim, 'XData', sim_path(1, idx_range), ...
'YData', sim_path(2, idx_range), ...
'ZData', sim_path(3, idx_range));
% 更新标记点
set(marker_anim, 'XData', sim_path(1,i), ...
'YData', sim_path(2,i), ...
'ZData', sim_path(3,i));
% 更新速度矢量
set(quiv, 'XData', sim_path(1,i), 'YData', sim_path(2,i), 'ZData', sim_path(3,i), ...
'UData', dx(i)/quiver_scale, ...
'VData', dy(i)/quiver_scale, ...
'WData', dz(i)/quiver_scale);
% 更新信息面板
set(info_text, 'String', sprintf('Time: %.1fs\nSpeed: %.1fm/s\nAltitude: %.1fm\nDetection Prob: %.2f', ...
sim_time(i), sim_speed(i), sim_path(3,i), prob(i)));
% 每隔100帧更新视角
% 每隔100帧更新视角
if mod(k, 100) == 0
% 正确获取当前视角参数
[current_az, current_el] = view(ax_anim);
% 创建新视角参数（仅改变方位角）
new_az = mod(current_az + 10, 360); % 确保角度在0-360度范围内
new_el = current_el; % 保持仰角不变
% 应用新视角
view(ax_anim, new_az, new_el);
% 或者使用向量形式设置
% view(ax_anim, [new_az, new_el]);
end
% 捕获帧
frame = getframe(fig_anim);
writeVideo(v, frame);
% 添加延迟控制动画速度
pause(0.01);
end
% 完成动画
close(v);
fprintf('动画已保存为: %s\n', video_file);
%% ========== 性能图表 ==========
fprintf('创建性能图表...\n');
% 创建图表窗口
fig_perf = figure('Name', 'Path Performance Metrics', 'Position', [100, 100, 1400, 800]);
% 速度-时间曲线
subplot(3,1,1);
plot(sim_time(1:end-1), sim_speed, 'b-', 'LineWidth', 1.5);
hold on;
plot([sim_time(1), sim_time(end)], [min_speed, min_speed], 'r--', 'LineWidth', 1.2);
plot([sim_time(1), sim_time(end)], [max_speed, max_speed], 'r--', 'LineWidth', 1.2);
xlabel('Time (s)');
ylabel('Speed (m/s)');
title('Speed Profile');
legend('Actual Speed', 'Speed Limits');
grid on;
% 加速度-时间曲线
subplot(3,1,2);
plot(sim_time(1:end-1), sim_accel, 'm-', 'LineWidth', 1.5);
hold on;
plot([sim_time(1), sim_time(end)], [max_accel, max_accel], 'r--', 'LineWidth', 1.2);
plot([sim_time(1), sim_time(end)], [-max_accel, -max_accel], 'r--', 'LineWidth', 1.2);
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
title('Acceleration Profile');
legend('Actual Acceleration', 'Acceleration Limits');
grid on;
% 探测概率-时间曲线
subplot(3,1,3);
plot(sim_time, prob, 'g-', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Detection Probability');
title('Detection Probability Over Time');
grid on;
ylim([0, 1.1]);
% 保存图表
saveas(fig_perf, 'performance_metrics.png');
%% ========== 地形剖面图 ==========
fprintf('创建地形剖面图...\n');
% 创建路径地形剖面
path_alt = sim_path(3,:);
terrain_alt = interp2(terrain.X, terrain.Y, terrain.Z, sim_path(1,:), sim_path(2,:));
safe_alt = terrain_alt + min_alt_offset;
% 创建剖面图
fig_profile = figure('Name', 'Altitude Profile', 'Position', [100, 100, 1200, 500]);
plot(sim_time, path_alt, 'b-', 'LineWidth', 1.8, 'DisplayName', 'Path Altitude');
hold on;
plot(sim_time, terrain_alt, 'k-', 'LineWidth', 1.2, 'DisplayName', 'Terrain Altitude');
plot(sim_time, safe_alt, 'g--', 'LineWidth', 1.2, 'DisplayName', 'Minimum Safe Altitude');
plot([sim_time(1), sim_time(end)], [max_altitude, max_altitude], 'r--', 'LineWidth', 1.2, 'DisplayName', 'Maximum Altitude');
% 标记观测点
for i = 1:size(obs_xy, 1)
[min_dist, idx] = min(sqrt((sim_path(1,:)-obs_xy(i,1)).^2 + (sim_path(2,:)-obs_xy(i,2)).^2));
plot(sim_time(idx), path_alt(idx), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
text(sim_time(idx), path_alt(idx)+50, ['Obs ', num2str(i)], 'Color', 'r');
end
xlabel('Time (s)');
ylabel('Altitude (m)');
title('Altitude Profile Along Path');
legend('Location', 'best');
grid on;
% 保存图表
saveas(fig_profile, 'altitude_profile.png');
fprintf('所有可视化和输出已完成!\n');
%% ========== 辅助函数 ==========
function prob = calc_detection_prob(path, terrain, obs_xy, obs_z)
% 简化版的探测概率计算
% 实际实现应考虑视线、距离等因素
num_points = size(path, 2);
num_obs = size(obs_xy, 1);
prob = zeros(1, num_points);
for i = 1:num_points
min_dist = inf;
for j = 1:num_obs
dist = norm(path(1:2,i) - obs_xy(j,:)');
if dist < min_dist
min_dist = dist;
end
end
% 基于距离的探测概率模型
prob(i) = min(1, 0.8 * exp(-min_dist/2000));
end
end
% 创建输出矩阵
output_data = [
sim_time;
sim_path(1,:);
sim_path(2,:);
sim_path(3,:);
[sim_speed, sim_speed(end)];
[sim_accel, sim_accel(end)];
prob
];
% 写入CSV文件
header = {'Time(s)', 'X(m)', 'Y(m)', 'Z(m)', 'Speed(m/s)', 'Accel(m/s2)', 'DetectionProb'};
fid = fopen(output_file, 'w');
fprintf(fid, '%s,', header{1:end-1});
fprintf(fid, '%s\n', header{end});
fclose(fid);
dlmwrite(output_file, output_data', '-append', 'delimiter', ',');
sim_path对代码起始点和终点的选取进行限制，限制范围为初始区域和终点区域
最新发布
扫一扫
7185
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
