clc
clear
close all
%% 图定义
% 根据节点的邻近节点表及字母节点-数字节点对应表,构造节点元胞数组
nodes_dist = cell(0);
nodes_dist{1} = {1, [2, 6, 7], [12, 16, 14]};
nodes_dist{2} = {2, [1, 3, 6], [12, 10, 7]};
nodes_dist{3} = {3, [2, 4, 5, 6], [10, 3, 5, 6]};
nodes_dist{4} = {4, [3, 5], [3, 4]};
nodes_dist{5} = {5, [3, 4, 6, 7], [5, 4, 2, 8]};
nodes_dist{6} = {6, [1, 2, 3, 5, 7], [16, 7, 6, 2, 9]};
nodes_dist{7} = {7, [1, 5, 6], [14, 8, 9]};
%% 算法初始化
% S/U的第一列表示节点编号
% 对于S,第二列表示从源节点到本节点已求得的最小距离,不再变更;
% 对于U,第二列表示从源节点到本节点暂时求得的最小距离,可能会变更
S = [4, 0];
U = [1, 2, 3, 5, 6, 7; inf, inf, 3, 4, inf, inf]';
% 最优路径及暂时最优路径的初始化
path_opt = cell(7, 2);
path_opt{4, 1} = 4;
path_opt{4, 2} = {4};
path_temp = cell(7, 2);
path_temp{3, 1} = 3;
path_temp{3, 2} = {4, 3};
path_temp{4, 1} = 4;
path_temp{4, 2} = {4};
path_temp{5, 1} = 5;
path_temp{5, 2} = {4, 5};
%% 循环遍历所有节点
while ~isempty(U(:, 1))
% 在U集合找出当前最小距离值及对应节点,并移除该节点至S集合中
[dist_min, idx] = min(U(:, 2));
node_min = U(idx, 1);
S = [S; node_min, dist_min];
U(idx, :) = [];
% 将最小距离值的节点添加到最优路径集合
path_opt{node_min, 1} = node_min;
path_opt{node_min, 2} = path_temp{node_min, 2};
%% 依次遍历最小距离节点的邻节点,判断是否在U集合中更新邻节点的距离值
if length(nodes_dist{node_min}) >= 3
for i = 1:length(nodes_dist{node_min, 2})
% 需要判断的节点
node_temp = nodes_dist{node_min, 2}(i);
% 找出U集合中节点node_temp的索引值
idx_temp = find(node_temp == U(:, 1));
% 判断是否更新
if ~isempty(idx_temp)
new_dist = dist_min + nodes_dist{node_min, 3}(i);
if new_dist < U(idx_temp, 2)
U(idx_temp, 2) = new_dist;
% 更新暂时最优路径
current_path = path_opt{node_min, 2};
path_temp{node_temp, 2} = [current_path, node_temp];
end
end
end
else
error('nodes_dist for node %d is incomplete.', node_min);
end
end
% 输出最终结果
for i = 1:size(S, 1)
fprintf('节点 %d 到源节点的最短距离为:%.2f\n', S(i, 1), S(i, 2));
end
% 输出最优路径
for i = 1:size(S, 1)
fprintf('节点 %d 的最优路径为:', S(i, 1));
disp(path_opt{i, 2});
end
%% 小车路径规划初始化
% 定义节点坐标(假设每个节点的坐标)
node_coords = [
0, 0; % 节点1
10, 0; % 节点2
15, 5; % 节点3
20, 10; % 节点4(源节点)
25, 15; % 节点5
30, 5; % 节点6
35, 0; % 节点7
];
% 获取目标节点(例如节点7)
target_node = 7;
target_path = path_opt{target_node, 2};
% 将路径转换为坐标点
path_coords = [];
for i = 1:length(target_path)
path_coords = [path_coords; node_coords(target_path(i), :)];
end
% 小车参数
Pi = [20, 10, 1, 1]; % 小车起始位置和速度(假设起始位置为节点4的坐标)
len_step = 0.5; % 步长
Num_iter = 3000; % 最大循环次数
% 初始化路径点
Path = Pi(1:2);
%% 小车行驶循环
figure;
hold on;
plot(node_coords(:, 1), node_coords(:, 2), 'ko', 'MarkerSize', 8, 'DisplayName', '节点');
legend;
for i = 1:length(target_path) - 1
start_point = node_coords(target_path(i), :);
end_point = node_coords(target_path(i + 1), :);
% 绘制路径线段
plot([start_point(1), end_point(1)], [start_point(2), end_point(2)], 'b-', 'LineWidth', 1.5);
% 小车沿路径线段行驶
direction = end_point - start_point;
distance = norm(direction);
unit_direction = direction / distance;
current_pos = start_point;
while norm(current_pos - end_point) > 1e-3
% 更新小车位置
current_pos = current_pos + unit_direction * len_step;
Path = [Path; current_pos];
% 绘制小车
clf;
hold on;
plot(node_coords(:, 1), node_coords(:, 2), 'ko', 'MarkerSize', 8, 'DisplayName', '节点');
plot(Path(:, 1), Path(:, 2), 'r-', 'LineWidth', 1.5, 'DisplayName', '小车路径');
plot(current_pos(1), current_pos(2), 'rs', 'MarkerSize', 10, 'DisplayName', '小车');
legend;
axis equal;
xlim([0, 40]);
ylim([-5, 20]);
pause(0.1);
end
end
disp('小车已成功到达目标节点!');
最新发布