栅格障碍物地图生成与机器人路径规划MATLAB程序，包含多种地图生成方法、路径规划算法和可视化功能。
matlab
%% 机器人路径规划栅格地图系统 - MATLAB实现
clear; close all; clc;
warning('off', 'all');
fprintf('=== 机器人路径规划栅格地图系统 ===\n');
%% 1. 栅格地图参数设置
fprintf('1. 设置栅格地图参数...\n');
% 1.1 地图基本参数
map_params.grid_size = [50, 50]; % 栅格地图尺寸 [行, 列]
map_params.cell_size = 1.0; % 每个栅格的物理尺寸 (m)
map_params.resolution = 1.0; % 地图分辨率 (cells/m)
% 1.2 障碍物生成参数
obstacle_params.obstacle_density = 0.25; % 障碍物密度 (0-1)
obstacle_params.min_obstacle_size = 1; % 最小障碍物尺寸 (栅格)
obstacle_params.max_obstacle_size = 5; % 最大障碍物尺寸 (栅格)
obstacle_params.obstacle_type = 'random'; % 'random', 'clustered', 'maze', 'custom'
% 1.3 起点和终点
start_point = [3, 3]; % 起点坐标 [行, 列]
goal_point = [45, 45]; % 终点坐标 [行, 列]
% 1.4 路径规划算法选择
algorithm = 'A*'; % 'A*', 'Dijkstra', 'RRT', 'PRM', 'Theta*'
% 1.5 可视化选项
show_animation = true; % 是否显示路径搜索动画
save_map = true; % 是否保存地图数据
%% 2. 核心函数定义
% 2.1 生成随机障碍物地图
function grid_map = generate_random_obstacle_map(params, obstacle_params)
% 初始化空白地图 (0=自由, 1=障碍物)
grid_map = zeros(params.grid_size);
% 计算总栅格数
total_cells = prod(params.grid_size);
num_obstacles = round(total_cells * obstacle_params.obstacle_density);
fprintf(' 生成随机障碍物: %d 个障碍栅格\n', num_obstacles);
% 随机放置障碍物
for i = 1:num_obstacles
% 随机选择障碍物大小
obs_size = randi([obstacle_params.min_obstacle_size, ...
obstacle_params.max_obstacle_size]);
% 随机选择中心位置
center_row = randi([1+obs_size, params.grid_size(1)-obs_size]);
center_col = randi([1+obs_size, params.grid_size(2)-obs_size]);
% 生成方形障碍物
row_start = max(1, center_row - floor(obs_size/2));
row_end = min(params.grid_size(1), center_row + floor(obs_size/2));
col_start = max(1, center_col - floor(obs_size/2));
col_end = min(params.grid_size(2), center_col + floor(obs_size/2));
grid_map(row_start:row_end, col_start:col_end) = 1;
end
end
% 2.2 生成聚类障碍物地图
function grid_map = generate_clustered_obstacle_map(params, obstacle_params)
grid_map = zeros(params.grid_size);
% 确定聚类数量
num_clusters = round(5 * obstacle_params.obstacle_density);
fprintf(' 生成聚类障碍物: %d 个聚类\n', num_clusters);
for cluster = 1:num_clusters
% 随机选择聚类中心
center_row = randi([10, params.grid_size(1)-10]);
center_col = randi([10, params.grid_size(2)-10]);
% 聚类中的障碍物数量
cluster_size = randi([5, 15]);
for i = 1:cluster_size
% 在中心周围随机偏移
offset_row = randi([-8, 8]);
offset_col = randi([-8, 8]);
row = center_row + offset_row;
col = center_col + offset_col;
% 确保在边界内
if row >= 1 && row <= params.grid_size(1) && ...
col >= 1 && col <= params.grid_size(2)
% 生成随机大小的障碍物
obs_size = randi([obstacle_params.min_obstacle_size, ...
obstacle_params.max_obstacle_size]);
row_start = max(1, row - floor(obs_size/2));
row_end = min(params.grid_size(1), row + floor(obs_size/2));
col_start = max(1, col - floor(obs_size/2));
col_end = min(params.grid_size(2), col + floor(obs_size/2));
grid_map(row_start:row_end, col_start:col_end) = 1;
end
end
end
end
% 2.3 生成迷宫地图
function grid_map = generate_maze_map(params)
grid_map = zeros(params.grid_size);
% 创建边界
grid_map(1,:) = 1;
grid_map(end,:) = 1;
grid_map(:,1) = 1;
grid_map(:,end) = 1;
% 创建内部迷宫结构
cell_size = 3; % 迷宫单元大小
for row = 2:cell_size:params.grid_size(1)-1
for col = 2:cell_size:params.grid_size(2)-1
% 随机决定墙的方向
if rand() > 0.5
% 水平墙
wall_length = min(cell_size, params.grid_size(2)-col);
grid_map(row, col:col+wall_length-1) = 1;
else
% 垂直墙
wall_length = min(cell_size, params.grid_size(1)-row);
grid_map(row:row+wall_length-1, col) = 1;
end
end
end
% 创建入口和出口
grid_map(2,1) = 0; % 入口
grid_map(end-1,end) = 0; % 出口
fprintf(' 生成迷宫地图\n');
end
% 2.4 生成自定义形状障碍物地图
function grid_map = generate_custom_obstacle_map(params)
grid_map = zeros(params.grid_size);
% 示例：创建几个特定形状的障碍物
% 1. 矩形障碍物
rect1 = [10, 10, 8, 15]; % [起始行, 起始列, 高度, 宽度]
grid_map(rect1(1):rect1(1)+rect1(3)-1, ...
rect1(2):rect1(2)+rect1(4)-1) = 1;
% 2. L形障碍物
L_shape_row = 25;
L_shape_col = 30;
grid_map(L_shape_row:L_shape_row+10, L_shape_col:L_shape_col+3) = 1;
grid_map(L_shape_row:L_shape_row+3, L_shape_col:L_shape_col+15) = 1;
% 3. 圆形障碍物
center_row = 35;
center_col = 15;
radius = 5;
for i = 1:params.grid_size(1)
for j = 1:params.grid_size(2)
if sqrt((i-center_row)^2 + (j-center_col)^2) <= radius
grid_map(i,j) = 1;
end
end
end
% 4. 对角线障碍物
for i = 1:20
row = 5 + i;
col = 40 + i;
if row <= params.grid_size(1) && col <= params.grid_size(2)
grid_map(row, col) = 1;
end
end
fprintf(' 生成自定义形状障碍物地图\n');
end
% 2.5 A*路径规划算法
function [path, visited, cost] = a_star_algorithm(grid_map, start, goal)
% 初始化
[rows, cols] = size(grid_map);
% 定义移动成本（8方向）
moves = [-1, 0, 1; % 行偏移
0, 1, 1; % 列偏移
1, 1, sqrt(2)]; % 成本
% 初始化数据结构
g_score = inf(rows, cols); % 从起点到当前节点的实际成本
f_score = inf(rows, cols); % 估计总成本: f = g + h
parent = zeros(rows, cols, 2); % 父节点位置
closed_set = false(rows, cols); % 已访问节点
open_set = false(rows, cols); % 待访问节点
% 设置起点
g_score(start(1), start(2)) = 0;
f_score(start(1), start(2)) = heuristic(start, goal);
open_set(start(1), start(2)) = true;
% 主循环
while any(open_set(:))
% 找到f值最小的节点
[min_f, idx] = min(f_score(open_set));
[current_row, current_col] = ind2sub([rows, cols], find(open_set & (f_score == min_f), 1));
current = [current_row, current_col];
% 如果到达目标
if isequal(current, goal)
path = reconstruct_path(parent, start, goal);
visited = closed_set;
cost = g_score(goal(1), goal(2));
return;
end
% 从开放集中移除当前节点
open_set(current_row, current_col) = false;
closed_set(current_row, current_col) = true;
% 检查所有可能的移动
for move_idx = 1:size(moves, 2)
new_row = current_row + moves(1, move_idx);
new_col = current_col + moves(2, move_idx);
move_cost = moves(3, move_idx);
% 检查是否在地图范围内且不是障碍物
if new_row >= 1 && new_row <= rows && ...
new_col >= 1 && new_col <= cols && ...
grid_map(new_row, new_col) == 0
% 如果邻居节点已经在关闭集中，跳过
if closed_set(new_row, new_col)
continue;
end
% 计算新的g值
tentative_g = g_score(current_row, current_col) + move_cost;
% 如果找到更好的路径
if tentative_g < g_score(new_row, new_col)
% 更新父节点
parent(new_row, new_col, :) = [current_row, current_col];
% 更新g值和f值
g_score(new_row, new_col) = tentative_g;
f_score(new_row, new_col) = tentative_g + heuristic([new_row, new_col], goal);
% 添加到开放集
open_set(new_row, new_col) = true;
end
end
end
end
% 如果无法找到路径
path = [];
visited = closed_set;
cost = inf;
end
% 2.6 Dijkstra算法
function [path, visited, cost] = dijkstra_algorithm(grid_map, start, goal)
% 与A*类似，但没有启发式函数
[rows, cols] = size(grid_map);
moves = [-1, 0, 1; 0, 1, 1; 1, 1, sqrt(2)];
g_score = inf(rows, cols);
parent = zeros(rows, cols, 2);
closed_set = false(rows, cols);
open_set = false(rows, cols);
g_score(start(1), start(2)) = 0;
open_set(start(1), start(2)) = true;
while any(open_set(:))
% 找到g值最小的节点
[min_g, idx] = min(g_score(open_set));
[current_row, current_col] = ind2sub([rows, cols], find(open_set & (g_score == min_g), 1));
current = [current_row, current_col];
if isequal(current, goal)
path = reconstruct_path(parent, start, goal);
visited = closed_set;
cost = g_score(goal(1), goal(2));
return;
end
open_set(current_row, current_col) = false;
closed_set(current_row, current_col) = true;
for move_idx = 1:size(moves, 2)
new_row = current_row + moves(1, move_idx);
new_col = current_col + moves(2, move_idx);
move_cost = moves(3, move_idx);
if new_row >= 1 && new_row <= rows && ...
new_col >= 1 && new_col <= cols && ...
grid_map(new_row, new_col) == 0
if closed_set(new_row, new_col)
continue;
end
tentative_g = g_score(current_row, current_col) + move_cost;
if tentative_g < g_score(new_row, new_col)
parent(new_row, new_col, :) = [current_row, current_col];
g_score(new_row, new_col) = tentative_g;
open_set(new_row, new_col) = true;
end
end
end
end
path = [];
visited = closed_set;
cost = inf;
end
% 2.7 快速探索随机树(RRT)算法
function [path, tree] = rrt_algorithm(grid_map, start, goal, max_iter)
[rows, cols] = size(grid_map);
% 初始化树
tree.nodes = start;
tree.parents = 0;
tree.costs = 0;
% RRT参数
step_size = 5;
goal_bias = 0.1; % 偏向目标的概率
for iter = 1:max_iter
% 随机采样（有一定概率采样目标点）
if rand() < goal_bias
random_point = goal;
else
random_point = [randi(rows), randi(cols)];
end
% 找到树中最近的节点
nearest_idx = find_nearest_node(tree.nodes, random_point);
nearest_node = tree.nodes(nearest_idx, :);
% 向随机点方向移动一步
direction = random_point - nearest_node;
dist = norm(direction);
if dist > step_size
direction = direction / dist * step_size;
end
new_node = nearest_node + direction;
new_node = round(new_node);
% 确保在地图范围内
new_node(1) = max(1, min(rows, new_node(1)));
new_node(2) = max(1, min(cols, new_node(2)));
% 检查路径是否与障碍物碰撞
if ~check_collision(grid_map, nearest_node, new_node)
% 添加到树中
tree.nodes = [tree.nodes; new_node];
tree.parents = [tree.parents; nearest_idx];
tree.costs = [tree.costs; tree.costs(nearest_idx) + norm(new_node - nearest_node)];
% 如果接近目标，尝试连接
if norm(new_node - goal) < step_size && ~check_collision(grid_map, new_node, goal)
% 找到路径
path = reconstruct_rrt_path(tree, size(tree.nodes, 1), goal);
return;
end
end
end
% 未找到路径
path = [];
end
% 2.8 辅助函数
function h = heuristic(node, goal)
% 欧几里得距离启发式
h = norm(node - goal);
end
function path = reconstruct_path(parent, start, goal)
% 从目标回溯到起点重建路径
path = goal;
current = goal;
while ~isequal(current, start)
parent_pos = parent(current(1), current(2), :);
current = [parent_pos(1), parent_pos(2)];
path = [current; path];
end
end
function idx = find_nearest_node(nodes, point)
% 找到离点最近的节点索引
distances = sum((nodes - point).^2, 2);
[~, idx] = min(distances);
end
function collision = check_collision(grid_map, p1, p2)
% 检查两点之间的线段是否与障碍物碰撞
collision = false;
% 使用Bresenham算法采样线段上的点
points = bresenham_line(p1, p2);
for i = 1:size(points, 1)
if grid_map(points(i,1), points(i,2)) == 1
collision = true;
return;
end
end
end
function points = bresenham_line(p1, p2)
% Bresenham画线算法
x1 = p1(2); y1 = p1(1);
x2 = p2(2); y2 = p2(1);
dx = abs(x2 - x1);
dy = abs(y2 - y1);
if x1 < x2
sx = 1;
else
sx = -1;
end
if y1 < y2
sy = 1;
else
sy = -1;
end
err = dx - dy;
points = [];
while true
points = [points; y1, x1];
if x1 == x2 && y1 == y2
break;
end
e2 = 2 * err;
if e2 > -dy
err = err - dy;
x1 = x1 + sx;
end
if e2 < dx
err = err + dx;
y1 = y1 + sy;
end
end
end
function path = reconstruct_rrt_path(tree, node_idx, goal)
% 重建RRT路径
path = goal;
while node_idx > 0
path = [tree.nodes(node_idx, :); path];
node_idx = tree.parents(node_idx);
end
end
%% 3. 生成栅格地图
fprintf('\n2. 生成栅格障碍物地图...\n');
% 根据类型生成地图
switch obstacle_params.obstacle_type
case 'random'
grid_map = generate_random_obstacle_map(map_params, obstacle_params);
case 'clustered'
grid_map = generate_clustered_obstacle_map(map_params, obstacle_params);
case 'maze'
grid_map = generate_maze_map(map_params);
case 'custom'
grid_map = generate_custom_obstacle_map(map_params);
otherwise
grid_map = generate_random_obstacle_map(map_params, obstacle_params);
end
% 确保起点和终点不是障碍物
grid_map(start_point(1), start_point(2)) = 0;
grid_map(goal_point(1), goal_point(2)) = 0;
% 计算障碍物统计数据
obstacle_count = sum(grid_map(:));
free_count = numel(grid_map) - obstacle_count;
obstacle_percentage = obstacle_count / numel(grid_map) * 100;
fprintf(' 地图统计: %dx%d 栅格\n', map_params.grid_size(1), map_params.grid_size(2));
fprintf(' 障碍物: %d 个 (%.1f%%)\n', obstacle_count, obstacle_percentage);
fprintf(' 自由空间: %d 个\n', free_count);
%% 4. 可视化栅格地图
fprintf('\n3. 可视化栅格地图...\n');
figure('Position', [100, 100, 1400, 600]);
% 4.1 原始栅格地图
subplot(1, 3, 1);
imagesc(grid_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]); % 白色=自由, 灰色=障碍物
hold on;
% 标记起点和终点
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
text(start_point(2), start_point(1), '起点', ...
'Color', 'g', 'FontSize', 12, 'FontWeight', 'bold', ...
'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'right');
text(goal_point(2), goal_point(1), '终点', ...
'Color', 'r', 'FontSize', 12, 'FontWeight', 'bold', ...
'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'left');
axis equal tight;
xlabel('列索引');
ylabel('行索引');
title(sprintf('栅格障碍物地图 (%s类型)', obstacle_params.obstacle_type));
grid on;
set(gca, 'XTick', 1:5:map_params.grid_size(2), 'YTick', 1:5:map_params.grid_size(1));
%% 5. 路径规划
fprintf('\n4. 运行路径规划算法: %s...\n', algorithm);
% 根据选择的算法进行路径规划
switch algorithm
case 'A*'
[path, visited, cost] = a_star_algorithm(grid_map, start_point, goal_point);
case 'Dijkstra'
[path, visited, cost] = dijkstra_algorithm(grid_map, start_point, goal_point);
case 'RRT'
max_iter = 2000;
[path, tree] = rrt_algorithm(grid_map, start_point, goal_point, max_iter);
visited = [];
if ~isempty(path)
cost = size(path, 1);
else
cost = inf;
end
otherwise
% 默认使用A*算法
[path, visited, cost] = a_star_algorithm(grid_map, start_point, goal_point);
end
% 显示结果
if ~isempty(path)
fprintf(' 找到路径! 路径长度: %d 步, 成本: %.2f\n', size(path, 1), cost);
else
fprintf(' 未找到可行路径!\n');
end
%% 6. 可视化路径规划结果
% 6.1 显示搜索过程
subplot(1, 3, 2);
imagesc(grid_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
if ~isempty(visited)
% 显示已访问的节点
[visited_rows, visited_cols] = find(visited);
plot(visited_cols, visited_rows, 'y.', 'MarkerSize', 8);
end
% 标记起点和终点
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
axis equal tight;
xlabel('列索引');
ylabel('行索引');
title(sprintf('%s算法搜索过程', algorithm));
grid on;
set(gca, 'XTick', 1:5:map_params.grid_size(2), 'YTick', 1:5:map_params.grid_size(1));
% 6.2 显示最终路径
subplot(1, 3, 3);
imagesc(grid_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
% 绘制路径
if ~isempty(path)
plot(path(:,2), path(:,1), 'b-', 'LineWidth', 3);
plot(path(:,2), path(:,1), 'bo', 'MarkerSize', 6, 'LineWidth', 2);
end
% 标记起点和终点
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
axis equal tight;
xlabel('列索引');
ylabel('行索引');
title(sprintf('%s算法规划路径', algorithm));
grid on;
set(gca, 'XTick', 1:5:map_params.grid_size(2), 'YTick', 1:5:map_params.grid_size(1));
sgtitle(sprintf('机器人路径规划 - %s障碍物地图', obstacle_params.obstacle_type), ...
'FontSize', 16, 'FontWeight', 'bold');
%% 7. 动画显示路径搜索过程（如果启用）
if show_animation && ~isempty(path)
fprintf('\n5. 生成路径搜索动画...\n');
figure('Position', [100, 100, 800, 800]);
% 创建动画帧
frames = struct('cdata', [], 'colormap', []);
frame_count = 1;
% 初始化显示
imagesc(grid_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 15, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 15, 'LineWidth', 3);
axis equal tight;
title(sprintf('%s算法路径搜索动画', algorithm), 'FontSize', 14);
xlabel('列索引');
ylabel('行索引');
grid on;
% 逐步显示搜索过程
if ~isempty(visited)
[visited_rows, visited_cols] = find(visited);
for i = 1:100:length(visited_rows)
% 显示已访问节点
plot(visited_cols(1:i), visited_rows(1:i), 'y.', 'MarkerSize', 8);
% 捕获帧
drawnow;
frames(frame_count) = getframe(gcf);
frame_count = frame_count + 1;
end
end
% 显示最终路径
if ~isempty(path)
for i = 1:size(path, 1)-1
% 绘制路径段
plot([path(i,2), path(i+1,2)], [path(i,1), path(i+1,1)], ...
'b-', 'LineWidth', 3);
plot(path(i,2), path(i,1), 'bo', 'MarkerSize', 8, 'LineWidth', 2);
% 捕获帧
drawnow;
frames(frame_count) = getframe(gcf);
frame_count = frame_count + 1;
end
% 绘制最后一个点
plot(path(end,2), path(end,1), 'bo', 'MarkerSize', 8, 'LineWidth', 2);
drawnow;
frames(frame_count) = getframe(gcf);
end
% 保存为GIF动画
if save_map
filename = sprintf('path_planning_%s_%s.gif', ...
algorithm, obstacle_params.obstacle_type);
for idx = 1:length(frames)
% 转换为索引图像
[im, map] = rgb2ind(frames(idx).cdata, 256);
% 写入GIF文件
if idx == 1
imwrite(im, map, filename, 'gif', ...
'LoopCount', Inf, 'DelayTime', 0.05);
else
imwrite(im, map, filename, 'gif', ...
'WriteMode', 'append', 'DelayTime', 0.05);
end
end
fprintf(' 动画已保存为: %s\n', filename);
end
end
%% 8. 算法性能比较
fprintf('\n6. 算法性能比较...\n');
% 测试不同算法的性能
algorithms_to_test = {'A*', 'Dijkstra'}; % RRT需要更多配置
performance_data = struct();
for i = 1:length(algorithms_to_test)
alg = algorithms_to_test{i};
fprintf(' 测试 %s 算法...', alg);
% 计时
tic;
switch alg
case 'A*'
[test_path, ~, test_cost] = a_star_algorithm(grid_map, start_point, goal_point);
case 'Dijkstra'
[test_path, ~, test_cost] = dijkstra_algorithm(grid_map, start_point, goal_point);
end
elapsed_time = toc;
% 记录性能数据
performance_data(i).algorithm = alg;
performance_data(i).found_path = ~isempty(test_path);
performance_data(i).path_length = size(test_path, 1);
performance_data(i).cost = test_cost;
performance_data(i).time = elapsed_time;
fprintf(' 完成 (%.3f秒)\n', elapsed_time);
end
% 显示性能比较
fprintf('\n 性能比较结果:\n');
fprintf(' %-12s %-10s %-12s %-10s %-10s\n', ...
'算法', '是否找到', '路径长度', '成本', '时间(s)');
fprintf(' %s\n', repmat('-', 1, 60));
for i = 1:length(performance_data)
if performance_data(i).found_path
found_str = '是';
else
found_str = '否';
end
fprintf(' %-12s %-10s %-12d %-10.2f %-10.3f\n', ...
performance_data(i).algorithm, ...
found_str, ...
performance_data(i).path_length, ...
performance_data(i).cost, ...
performance_data(i).time);
end
%% 9. 保存地图和路径数据
if save_map
fprintf('\n7. 保存地图和路径数据...\n');
% 创建结果结构
results = struct();
results.map = grid_map;
results.map_params = map_params;
results.obstacle_params = obstacle_params;
results.start_point = start_point;
results.goal_point = goal_point;
results.algorithm = algorithm;
results.path = path;
results.cost = cost;
results.performance = performance_data;
% 保存为MAT文件
filename = sprintf('robot_path_planning_%s.mat', ...
datestr(now, 'yyyymmdd_HHMMSS'));
save(filename, 'results');
% 保存为图像
figure('Position', [100, 100, 800, 800], 'Visible', 'off');
imagesc(grid_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
if ~isempty(path)
plot(path(:,2), path(:,1), 'b-', 'LineWidth', 3);
plot(path(:,2), path(:,1), 'bo', 'MarkerSize', 6, 'LineWidth', 2);
end
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
axis equal tight;
title(sprintf('路径规划结果 - %s算法', algorithm), 'FontSize', 14);
xlabel('列索引');
ylabel('行索引');
grid on;
img_filename = sprintf('path_planning_result_%s.png', ...
datestr(now, 'yyyymmdd_HHMMSS'));
saveas(gcf, img_filename);
fprintf(' 地图数据已保存为: %s\n', filename);
fprintf(' 结果图像已保存为: %s\n', img_filename);
end
%% 10. 高级功能：动态障碍物模拟
fprintf('\n8. 动态障碍物模拟演示...\n');
% 创建动态障碍物地图
dynamic_map = grid_map;
original_map = grid_map; % 保存原始地图
% 定义几个移动的障碍物
moving_obstacles = struct();
num_moving = 3;
for i = 1:num_moving
% 随机生成移动障碍物
moving_obstacles(i).position = [randi([10,40]), randi([10,40])];
moving_obstacles(i).size = randi([2,4]);
moving_obstacles(i).velocity = [randi([-1,1]), randi([-1,1])];
moving_obstacles(i).velocity = moving_obstacles(i).velocity / ...
norm(moving_obstacles(i).velocity + eps);
% 在地图上标记
pos = moving_obstacles(i).position;
sz = moving_obstacles(i).size;
row_start = max(1, pos(1) - floor(sz/2));
row_end = min(map_params.grid_size(1), pos(1) + floor(sz/2));
col_start = max(1, pos(2) - floor(sz/2));
col_end = min(map_params.grid_size(2), pos(2) + floor(sz/2));
dynamic_map(row_start:row_end, col_start:col_end) = 1;
end
% 显示动态地图
figure('Position', [100, 100, 1200, 500]);
subplot(1,2,1);
imagesc(original_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
title('原始静态地图');
axis equal tight;
grid on;
subplot(1,2,2);
imagesc(dynamic_map);
colormap([1, 1, 1; 0.3, 0.3, 0.3]);
hold on;
% 标记移动障碍物
for i = 1:num_moving
pos = moving_obstacles(i).position;
rectangle('Position', [pos(2)-1.5, pos(1)-1.5, 3, 3], ...
'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'r', 'LineWidth', 2);
text(pos(2), pos(1), sprintf('%d', i), ...
'Color', 'w', 'FontSize', 10, 'FontWeight', 'bold', ...
'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle');
end
plot(start_point(2), start_point(1), 'go', 'MarkerSize', 12, 'LineWidth', 3);
plot(goal_point(2), goal_point(1), 'ro', 'MarkerSize', 12, 'LineWidth', 3);
title('带动态障碍物的地图');
axis equal tight;
grid on;
sgtitle('静态 vs 动态障碍物地图', 'FontSize', 14, 'FontWeight', 'bold');
%% 11. 使用说明
fprintf('\n=== 使用说明 ===\n');
fprintf('1. 主要功能:\n');
fprintf(' - 生成多种类型的栅格障碍物地图\n');
fprintf(' - 实现多种路径规划算法 (A*, Dijkstra, RRT)\n');
fprintf(' - 可视化路径搜索过程和结果\n');
fprintf(' - 算法性能比较\n');
fprintf(' - 动态障碍物模拟\n');
fprintf('2. 参数调整:\n');
fprintf(' - 修改 map_params 调整地图大小和分辨率\n');
fprintf(' - 修改 obstacle_params 调整障碍物类型和密度\n');
fprintf(' - 修改 algorithm 选择不同的路径规划算法\n');
fprintf('3. 扩展功能:\n');
fprintf(' - 添加新的障碍物类型\n');
fprintf(' - 实现更多路径规划算法 (如D*, PRM等)\n');
fprintf(' - 添加机器人运动学约束\n');
fprintf(' - 集成传感器模型进行实时避障\n');
fprintf('\n=== 程序执行完成 ===\n');
fprintf('地图尺寸: %d x %d 栅格\n', map_params.grid_size(1), map_params.grid_size(2));
fprintf('障碍物密度: %.1f%%\n', obstacle_percentage);
if ~isempty(path)
fprintf('规划路径长度: %d 步\n', size(path, 1));
else
fprintf('警告: 未找到可行路径!\n');
end说明
1. 核心功能模块
| 模块 | 功能描述 | 关键技术 |
|---|---|---|
| 地图生成 | 生成多种类型的栅格障碍物地图 | 随机生成、聚类算法、迷宫生成 |
| 路径规划 | 实现多种路径规划算法 | A*, Dijkstra, RRT算法 |
| 可视化 | 地图显示、路径动画、性能分析 | 图像处理、动画生成 |
| 性能评估 | 比较不同算法的性能 | 时间测量、路径质量评估 |
| 动态模拟 | 模拟移动障碍物环境 | 动态障碍物建模 |
2. 栅格地图类型
程序支持生成四种类型的障碍物地图：
- 随机障碍物地图：随机分布的方形障碍物
- 聚类障碍物地图：障碍物以聚类形式分布
- 迷宫地图：生成迷宫结构的地图
- 自定义形状地图：包含特定几何形状的障碍物
3. 路径规划算法实现
A*算法：
- 特点：结合启发式搜索的最优路径算法
- 启发函数：欧几里得距离
- 移动方式：8方向移动（包含对角线）
Dijkstra算法：
- 特点：经典的最短路径算法，保证找到最优解
- 无启发式：完全基于实际成本
RRT算法：
- 特点：适用于高维空间的快速探索算法
- 适用场景：复杂环境、动态环境
4. 关键参数说明
地图参数：
matlab
% 调整地图大小和分辨率
map_params.grid_size = [60, 60]; % 增加地图尺寸
map_params.cell_size = 0.5; % 更精细的分辨率障碍物参数：
matlab
% 调整障碍物特性
obstacle_params.obstacle_density = 0.3; % 增加障碍物密度
obstacle_params.max_obstacle_size = 8; % 增大障碍物尺寸
obstacle_params.obstacle_type = 'maze'; % 切换地图类型算法选择：
matlab
% 选择不同的路径规划算法
algorithm = 'RRT'; % 切换为RRT算法
show_animation = true; % 启用动画显示5. 输出结果
程序生成以下输出：
-
可视化图表：
- 原始栅格地图
- 路径搜索过程
- 最终规划路径
- 动态障碍物演示
-
性能数据：
- 算法执行时间
- 路径长度和成本
- 搜索节点数量
-
保存文件：
- MAT格式的地图数据
- PNG格式的结果图像
- GIF格式的搜索动画
参考代码 用matlab做出栅格障碍物图，用于机器人路径规划 www.youwenfan.com/contentcsp/96841.html
6. 应用扩展建议
机器人平台集成：
matlab
% 添加机器人运动学约束
robot_params.max_velocity = 1.0; % 最大速度
robot_params.max_acceleration = 0.5; % 最大加速度
robot_params.turning_radius = 0.3; % 转弯半径实时避障：
matlab
% 添加传感器模拟
sensor_params.range = 5.0; % 传感器范围
sensor_params.fov = 180; % 视场角
sensor_params.update_rate = 10; % 更新频率 (Hz)多机器人协同：
matlab
% 多机器人路径规划
num_robots = 3;
robot_positions = rand(num_robots, 2) * map_size;
robot_goals = rand(num_robots, 2) * map_size;7. 实际应用示例
仓库AGV路径规划：
matlab
% 模拟仓库环境
map_params.grid_size = [100, 150]; % 仓库尺寸
obstacle_params.obstacle_type = 'custom'; % 自定义货架布局
start_point = [5, 5]; % 充电站位置
goal_point = [95, 145]; % 拣货点位置无人机避障规划：
matlab
% 考虑三维空间
map_params.grid_size = [50, 50, 30]; % 三维栅格地图
obstacle_params.obstacle_density = 0.15; % 建筑物密度
algorithm = 'RRT*'; % 使用RRT*算法服务机器人室内导航：
matlab
% 室内环境建模
load('floor_plan.mat'); % 加载建筑平面图
obstacle_params.obstacle_type = 'custom'; % 基于实际布局
addpath('social_forces'); % 添加人群避让模型