%% 船只队伍最优路径规划MATLAB代码
clear;
clc;
%% 1. 读取Excel数据
filename = '概率计算2.xlsx';
prob_sheet = '发现概率分布表';
depth_sheet = '水深分布表';
% 读取概率表 (sheet2)
[~, ~, prob_raw] = xlsread(filename, prob_sheet);
% 概率表结构: 第2行是y=25, 第3行是y=24, ..., 第26행是y=1; 列B是x=1 , C是x=2, ..., AW是x=48 (共49列数据列)
P_detect = nan(25, 48); % 初始化概率矩阵: 行y=1:25, 列x=1:48; y=1为南侧
for y = 1:25
data_row = 27 - y; % y=1对应Excel第26행, y=25对应Excel第2행
for x = 1:48
excel_col = 1 + x; % x=1对应Excel列B (索引2), x=48对应列49
val = prob_raw{data_row, excel_col};
if isnumeric(val) % 数值概率
P_detect(y, x) = val;
else % '--' 无效点
P_detect(y, x) = NaN;
end
end
end
% 读取水深表 (sheet3)
[~, ~, depth_raw] = xlsread(filename, depth_sheet);
depth = nan(25, 48); % 初始化水深矩阵: 行y=1:25, 列x=1:48
for y = 1:25
data_row = 27 - y; % 同概率表
for x = 1:48
excel_col = 1 + x;
val = depth_raw{data_row, excel_col};
if isnumeric(val)
depth(y, x) = val;
else % '--' 处理为NaN
depth(y, x) = NaN;
end
end
end
% 标记可航行网格: 水深≥2且概率有效
valid_mask = depth >= 2 & ~isnan(P_detect) & ~isnan(depth);
P_detect(~valid_mask) = NaN; % 无效点设为NaN
%% 2. 定义核心区和起始区
target = [24, 1]; % 目标点 (x=24, y=1)
core_radius = 2; % 核心区网格距离
start_radius_min = 23.5; % 起始区最小网格距离
start_radius_max = 24.5; % 起始区最大网格距离
% 计算每个网格到目标点的距离
[xx, yy] = meshgrid(1:48, 1:25);
dist_to_target = sqrt((xx - target(1)).^2 + (yy - target(2)).^2);
% 核心区掩膜 (距离≤2)
core_mask = dist_to_target <= core_radius;
core_mask(~valid_mask) = false; % 只含可航行点
core_mask(3,25) = 1;
core_mask(2,26) = 1;
core_mask(3,26) = 1;
% 起始区掩膜 (23.5 ≤ 距离 ≤ 24.5)
start_mask = dist_to_target >= start_radius_min & dist_to_target <= start_radius_max;
start_mask(~valid_mask) = false; % 只含可航行点
% 显示起始区点数用于调试
fprintf('起始区内可航行的网格数: %d\n', sum(start_mask(:)));
% 定义函数：获取编队三艘船的位置
getFormationPositions = @(current, previous, move_type) get_formation_positions(current, previous, move_type);
%% 3. 动态规划值迭代
V = nan(25, 48); % 值函数矩阵, 初始化为NaN
policy = cell(25, 48); % 存储最优动作 (邻居索引)
direction_record = cell(25, 48); % 记录移动方向
% 初始化核心区点
V(core_mask) = 1.0; % 成功概率初始化为1
% 迭代设置
max_iter = 1000; % 最大迭代次数
tolerance = 1e-6; % 收敛容差
changed = true;
iter = 0;
% 迭代更新
while changed && iter < max_iter
changed = false;
V_old = V;
for x = 1:48
for y = 1:25
% 跳过不可航行点或核心区点
if ~valid_mask(y, x) || core_mask(y, x)
continue;
end
% 获取可航行邻居
neighbors = [];
move_types = cell(0); % 存储移动类型
% 水平移动（左右）
for dx = [-1, 1]
nx = x + dx;
if nx >= 1 && nx <= 48 && valid_mask(y, nx) && ~isnan(V_old(y, nx))
neighbors = [neighbors; nx, y];
move_types{end+1} = 'H'; % 水平移动
end
end
% 垂直移动（上下）
for dy = [-1, 1]
ny = y + dy;
if ny >= 1 && ny <= 25 && valid_mask(ny, x) && ~isnan(V_old(ny, x))
neighbors = [neighbors; x, ny];
move_types{end+1} = 'V'; % 垂直移动
end
end
% 无有效邻居则跳过
if isempty(neighbors)
V(y, x) = 0;
continue;
end
% 计算邻居最大值
neighbor_vals = zeros(size(neighbors, 1), 1);
for n = 1:size(neighbors, 1)
next_pos = neighbors(n, :);
nx = next_pos(1);
ny = next_pos(2);
move_type = move_types{n};
% 根据移动方向确定编队位置
positions = getFormationPositions([x, y], [nx, ny], move_type);
% 计算不被发现概率
prob_undetected = 1;
for j = 1:size(positions, 1)
pos = positions(j, :);
px = pos(1);
py = pos(2);
% 检查位置是否在网格内
if px >= 1 && px <= 48 && py >= 1 && py <= 25
if valid_mask(py, px) && ~isnan(P_detect(py, px))
prob_undetected = prob_undetected * (1 - P_detect(py, px));
else
prob_undetected = 0; % 位置无效，概率为0
break;
end
else
prob_undetected = 0; % 位置无效，概率为0
break;
end
end
% 更新邻居值
neighbor_vals(n) = prob_undetected * V_old(ny, nx);
end
[max_val, max_idx] = max(neighbor_vals);
% 更新值函数、策略和方向记录
if isnan(V(y, x)) || abs(max_val - V_old(y, x)) > tolerance
V(y, x) = max_val;
policy{y, x} = neighbors(max_idx, :);
direction_record{y, x} = move_types{max_idx};
changed = true;
end
end
end
iter = iter + 1;
fprintf('迭代次数: %d, 最大变化: %.8f\n', iter, max(abs(V(:)-V_old(:))));
end
%% 4. 选择最优起始点和路径
% 计算所有起始点的V值
start_vals = V;
start_vals(~start_mask) = -Inf; % 只考虑起始区
[best_val, idx] = max(start_vals(:));
if isinf(best_val) || best_val <= 0
error('未找到有效起始点!');
end
[start_y, start_x] = ind2sub(size(start_vals), idx);
start_point = [start_x, start_y]; % 最优起始点
% 回溯最优路径
path = start_point;
current = start_point;
previous_pos = []; % 记录上一步位置
fprintf('找到起始点: (%d,%d)\n', start_x, start_y);
while true
% 检查是否到达核心区
if core_mask(current(2), current(1))
fprintf('到达核心区!\n');
break;
end
% 获取下一个点
if isempty(policy{current(2), current(1)})
fprintf('路径中断!\n');
break;
end
next_point = policy{current(2), current(1)};
if size(next_point,1) ~= 1
next_point = next_point(1,:);
end
if ismember(next_point, path, 'rows')
fprintf('检测到循环路径!\n');
break;
end
path = [path; next_point];
previous_pos = current;
current = next_point;
end
% 计算路径成功概率
if size(path, 1) < 2
error('路径太短!');
end
path_probs = ones(size(path, 1), 1);
for i = 1:size(path, 1)
if i == 1
prev = []; % 第一步没有前一步
move_type = direction_record{path(1,2), path(1,1)};
else
prev = path(i-1, :);
move_type = direction_record{prev(2), prev(1)};
end
% 获取编队位置
positions = getFormationPositions(prev, path(i, :), move_type);
% 计算不被发现概率
prob_step = 1;
for j = 1:size(positions, 1)
pos = positions(j, :);
if pos(1) >= 1 && pos(1) <= 48 && pos(2) >= 1 && pos(2) <= 25
if valid_mask(pos(2), pos(1)) && ~isnan(P_detect(pos(2), pos(1)))
% prob_step = prob_step * (1 - P_detect(pos(2), pos(1)));
prob_step = (1 - P_detect(pos(2), pos(1)));%概率只计算一次
else
prob_step = 0; % 位置无效
break;
end
else
prob_step = 0; % 位置无效
break;
end
end
path_probs(i) = prob_step;
end
path_probs(end)=1;
path_probs(1)=1;
success_prob = prod(path_probs);
%% 5. 绘制网格路径图
figure;
hold on;
axis([0.5 48.5 0.5 25.5]);
grid on;
title(sprintf('船只队伍最优路径 (成功概率: %.6f%%)', success_prob * 100), 'FontSize', 12);
xlabel('横向网格位置 x');
ylabel('纵向网格位置 y');
% 绘制核心区
[core_y, core_x] = find(core_mask);
plot(core_x, core_y, 'rs', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
% 绘制可航行网格
[valid_y, valid_x] = find(valid_mask);
plot(valid_x, valid_y, 'ks', 'MarkerSize', 4, 'MarkerFaceColor', 'k');
% 绘制起始区
[start_y, start_x] = find(start_mask);
plot(start_x, start_y, 'bo', 'MarkerSize', 6);
% 绘制最优起始点
plot(start_point(1), start_point(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
% 绘制路径
plot(path(:,1), path(:,2), 'm-', 'LineWidth', 2);
plot(path(:,1), path(:,2), 'mo', 'MarkerSize', 8);
% 目标点
plot(target(1), target(2), 'rp', 'MarkerSize', 15, 'MarkerFaceColor', 'r');
% 图例
legend('核心区', '可航行网格', '起始区', '最优起始点', '路径', 'Location', 'best');
hold off;
% 添加编队位置计算函数
function positions = get_formation_positions(previous, current, move_type)
% 参数处理：确保所有输入正确
if isempty(previous) % 初始状态
% 初始状态三艘船在垂直方向排列
positions = [current; % 中心船
current(1), current(2)+1; % 上船
current(1), current(2)-1]; % 下船
else
% 处理可能的输入错误
if nargin < 3 || isempty(move_type)
move_type = 'V';
end
% 根据移动类型确定编队位置
if strcmpi(move_type, 'H') % 水平移动
positions = [current; % 当前船
previous; % 前船
mean([current; previous])]; % 中间的船
else % 垂直移动
positions = [current; % 当前船
previous; % 前船
mean([current; previous])]; % 中间的船
end
end
% 确保所有位置是整数（网格坐标）
positions = round(positions);
end
帮我修改这个程序的绘图部分，要求可航行区域的网格显示从点变成颜色网格，颜色蓝色表示被发现概率低，红色表示被发现概率高，最优路径显示从点变成线。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
1128
