1. 机器人路径规划系统概述
在机器人导航领域,路径规划是最基础也是最重要的功能之一。我最近用MATLAB开发了一套完整的栅格地图路径规划系统,包含地图生成、路径规划和可视化三大模块。这套系统特别适合用于教学演示、算法验证和科研实验。
这个系统的核心价值在于:
- 提供了4种不同的障碍物地图生成方式(随机、聚类、迷宫、自定义)
- 实现了3种经典路径规划算法(A*、Dijkstra、RRT)
- 具备完整的可视化功能,可以直观展示算法搜索过程
- 支持算法性能比较和动态障碍物模拟
提示:在实际应用中,50×50的栅格地图已经能够满足大多数仿真需求,每个栅格可以对应实际环境中的0.1-1米,具体取决于机器人尺寸和运动精度要求。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系统架构与核心模块
2.1 整体架构设计
系统采用模块化设计,主要分为以下几个部分:
- 地图生成模块:负责创建各种类型的障碍物地图
- 路径规划模块:实现不同搜索算法
- 可视化模块:展示地图和路径规划结果
- 性能分析模块:比较算法效率
- 动态模拟模块:演示移动障碍物场景
2.2 关键技术选型
选择MATLAB作为开发平台主要基于以下考虑:
- 强大的矩阵运算能力,适合处理栅格地图
- 丰富的可视化工具,便于结果展示
- 快速的算法原型开发能力
- 广泛的科研社区支持
3. 栅格地图生成详解
3.1 地图参数设置
地图的基本参数包括:
matlab复制map_params.grid_size = [50, 50]; % 地图尺寸[行,列]
map_params.cell_size = 1.0; % 栅格物理尺寸(米)
map_params.resolution = 1.0; % 分辨率(栅格/米)
这些参数决定了地图的精细程度。根据我的经验:
- 对于室内服务机器人,建议使用0.2-0.5米的栅格尺寸
- 对于仓库AGV,0.5-1米的栅格更为合适
- 无人机等高速应用可能需要更大的栅格
3.2 障碍物生成算法
3.2.1 随机障碍物生成
matlab复制function grid_map = generate_random_obstacle_map(params, obstacle_params)
grid_map = zeros(params.grid_size);
total_cells = prod(params.grid_size);
num_obstacles = round(total_cells * obstacle_params.obstacle_density);
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
这个函数的关键点:
- 根据密度参数计算需要生成的障碍物数量
- 为每个障碍物随机选择大小和位置
- 在地图上标记障碍物区域
注意:障碍物不能覆盖起点和终点,这是路径规划的基本要求。在实际应用中,还需要考虑机器人本身的尺寸,适当扩大障碍物区域。
3.2.2 聚类障碍物生成
聚类障碍物更接近真实场景,如仓库中的货架区域:
matlab复制function grid_map = generate_clustered_obstacle_map(params, obstacle_params)
grid_map = zeros(params.grid_size);
num_clusters = round(5 * obstacle_params.obstacle_density);
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
3.2.3 迷宫地图生成
迷宫地图适合测试算法的复杂路径搜索能力:
matlab复制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; % 出口
end
4. 路径规划算法实现
4.1 A*算法详解
A*算法是最常用的路径规划算法之一,结合了Dijkstra的最优性保证和启发式搜索的高效性。
4.1.1 算法核心逻辑
matlab复制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
4.1.2 启发式函数
matlab复制function h = heuristic(node, goal)
% 欧几里得距离启发式
h = norm(node - goal);
end
在实际应用中,可以根据需要选择不同的启发式函数:
- 曼哈顿距离:适合网格移动(无对角线)
- 对角线距离:结合了曼哈顿和欧几里得距离
- 切比雪夫距离:适合任意方向移动
4.1.3 路径重建
matlab复制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
4.2 Dijkstra算法实现
Dijkstra算法是A*算法的特例(启发式函数h=0),保证找到最短路径但效率较低:
matlab复制function [path, visited, cost] = dijkstra_algorithm(grid_map, start, goal)
[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(:))
[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
4.3 RRT算法实现
RRT(快速探索随机树)算法适合高维空间和复杂环境:
matlab复制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
5. 可视化与性能分析
5.1 地图可视化
matlab复制figure('Position', [100, 100, 1400, 600]);
% 原始栅格地图
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.2 路径规划结果可视化
matlab复制% 显示搜索过程
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;
% 显示最终路径
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;
sgtitle(sprintf('机器人路径规划 - %s障碍物地图', obstacle_params.obstacle_type), ...
'FontSize', 16, 'FontWeight', 'bold');
5.3 算法性能比较
matlab复制algorithms_to_test = {'A*', 'Dijkstra'};
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;
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
6. 动态障碍物模拟
动态障碍物模拟是测试算法鲁棒性的重要手段:
matlab复制% 创建动态障碍物地图
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');
7. 实际应用与扩展
7.1 仓库AGV路径规划
matlab复制% 模拟仓库环境
map_params.grid_size = [100, 150]; % 仓库尺寸
obstacle_params.obstacle_type = 'custom'; % 自定义货架布局
start_point = [5, 5]; % 充电站位置
goal_point = [95, 145]; % 拣货点位置
7.2 无人机避障规划
matlab复制% 考虑三维空间
map_params.grid_size = [50, 50, 30]; % 三维栅格地图
obstacle_params.obstacle_density = 0.15; % 建筑物密度
algorithm = 'RRT*'; % 使用RRT*算法
7.3 服务机器人室内导航
matlab复制% 室内环境建模
load('floor_plan.mat'); % 加载建筑平面图
obstacle_params.obstacle_type = 'custom'; % 基于实际布局
addpath('social_forces'); % 添加人群避让模型
8. 开发经验与优化建议
在实际开发过程中,我总结了以下几点经验:
-
地图尺寸选择:不是越大越好,要根据实际应用场景和计算资源平衡
-
障碍物密度:0.2-0.3的密度最能模拟真实环境,过高会导致路径规划困难
-
算法选择:
- 结构化环境:A*或Dijkstra
- 复杂动态环境:RRT或其变种
- 实时性要求高:可以考虑D* Lite算法
-
性能优化技巧:
- 使用MATLAB的矩阵运算替代循环
- 预分配数组内存
- 对于大型地图,可以考虑分块处理
-
常见问题排查:
- 如果算法找不到路径,首先检查起点和终点是否被障碍物包围
- 检查障碍物密度是否过高
- 验证启发式函数是否合理
-
扩展方向:
- 添加更多路径规划算法(如PRM、D*等)
- 集成传感器噪声模型
- 开发实时重规划功能
- 添加多机器人协同规划
