Matlab路径规划：10分钟从零创建栅格地图（附完整代码）

1. 项目概述为什么从栅格地图开始如果你刚接触路径规划无论是做机器人导航、游戏AI寻路还是自动驾驶的仿真测试第一个拦路虎往往不是复杂的A*或Dijkstra算法而是“地图”。没有一张清晰、可计算的地图再精妙的算法也无用武之地。很多教程一上来就讲算法却默认你已经有了一个现成的、格式完美的地图数据这对新手来说很不友好。我自己在带学生和做项目时发现快速构建一个可用的地图环境是让整个路径规划流程跑通的第一步也是最关键的一步。Matlab在这方面有着得天独厚的优势。它不只是一个数学计算软件其强大的矩阵操作和图形可视化能力让它成为快速原型开发的利器。用Matlab绘制栅格地图本质上就是把真实或虚拟的环境抽象成一个由“0”和“1”或者更多状态组成的二维矩阵。其中“0”代表自由可通行区域“1”代表障碍物。这种抽象简洁明了后续几乎所有经典的搜索算法如BFS、DFS、A*都能直接在这个矩阵模型上运行。所以这个内容的目标非常直接抛开复杂的理论手把手带你用Matlab在10分钟内从零创建一张可用于路径规划算法的栅格地图。无论你是自动化、计算机相关专业的学生还是刚进入机器人领域的工程师掌握这套方法就能立刻为你自己的算法提供一个“练兵场”。接下来我会从设计思路、代码逐行解析到常见坑点完整地走一遍这个过程。2. 核心思路与栅格地图设计在动手写代码之前我们先要搞清楚栅格地图的几个核心设计参数。这就像盖房子先画图纸参数定好了代码写起来就顺畅了。2.1 地图的数学模型矩阵就是地图栅格地图在程序里最直接的体现就是一个二维矩阵。假设我们创建一个20行、30列的地图在Matlab里就是一个20x30的矩阵。map zeros(20, 30)生成的就是一张全是0空地的地图。如果我们想把第5行、第10列设置为障碍物只需要map(5, 10) 1。你看地图的修改本质上就是矩阵的索引和赋值这就是Matlab的方便之处。这里有一个非常重要的注意事项矩阵的行列索引与通常的平面直角坐标系X-Y容易混淆。在Matlab的矩阵map(i, j)中i代表行号垂直方向可类比Y轴j代表列号水平方向可类比X轴。而我们在纸上画地图习惯说“坐标(x, y)”。为了避免混乱我强烈建议在头脑中建立一个约定矩阵的行索引i对应坐标y列索引j对应坐标x。即map(y, x) value。这个约定贯穿始终能省去很多调试的麻烦。2.2 障碍物设置灵活性与可读性兼顾一张空白地图没有意义我们需要添加障碍物。障碍物的形状可以是矩形、圆形或者任意多边形。从简单开始矩形障碍物最常用。我们需要定义矩形的左上角起点start_pos和它的宽度width与高度height。注意这里的高度和宽度指的是占据的栅格数。例如要在矩阵地图中从坐标(x3, y2)开始放置一个宽4格、高3格的矩形障碍物。根据上面的约定对应到矩阵中就是行从2到423-1列从3到634-1的区域全部赋值为1。代码就是map(2:4, 3:6) 1。为了代码更清晰我习惯把地图参数、障碍物参数都单独定义成变量而不是把数字硬编码在绘图语句里。这样想修改地图大小或障碍物位置时只需改动开头的几个变量后面的绘图代码完全不用动维护起来非常方便。2.3 可视化方案让地图一目了然地图矩阵存在于内存里我们需要把它画出来。Matlab的imagesc函数是绝佳选择。它可以将矩阵的值映射为颜色直接显示出来。通常我们用彩色映射colormap([0 0 0; 1 1 1])来定义一个简单的黑白映射0空地映射为白色1障碍物映射为黑色对比鲜明。但光有颜色还不够我们需要网格线来清晰界定每一个栅格。grid on可以开启网格但默认的网格线太细。为了更醒目我通常会手动绘制网格线使用hold on配合plot函数画出一系列横线和竖线。同时关闭坐标轴axis off可以让图面更干净或者使用axis equal保证每个栅格是正方形不变形。一个实操心得是在正式运行路径规划算法前一定要先确保你的地图可视化结果和你的设计意图完全一致。我见过太多错误是因为坐标索引弄反导致算法在“看起来”是空地的地方撞墙。所以画完图后花点时间人工检查一下障碍物的位置和形状是否正确。3. Matlab代码逐行解析与实现下面我将分模块详细拆解代码。你可以直接复制到一个新的.m脚本文件中运行。3.1 初始化与地图参数设置%% 1. 清空与初始化 clear all; close all; clc;这三行代码是Matlab脚本的好习惯清空工作区变量、关闭所有图形窗口、清空命令窗口。避免之前运行的残留数据或图形干扰本次结果。%% 2. 定义地图参数 map_size_row 20; % 地图行数 (Y轴方向) map_size_col 30; % 地图列数 (X轴方向) % 初始化地图矩阵0代表自由空间1代表障碍物 map zeros(map_size_row, map_size_col);这里明确定义了地图尺寸。将地图初始化为全零矩阵即一片空旷区域。变量名使用map_size_row/col比简单的m/n更清晰便于后续阅读。3.2 构建矩形障碍物障碍物我们用结构体数组来组织这样管理多个障碍物会很方便。%% 3. 定义障碍物矩形 % 每个障碍物用其左上角坐标和宽高定义 obstacles [ struct(x, 5, y, 3, width, 8, height, 2); % 障碍物1一个横条 struct(x, 15, y, 10, width, 3, height, 8); % 障碍物2一个竖条 struct(x, 22, y, 5, width, 5, height, 5); % 障碍物3一个方块 ];这里定义了三个矩形障碍物。struct创建结构体包含了障碍物的位置和尺寸信息。注意坐标(x,y)是障碍物矩形左上角在栅格地图中的坐标。接下来通过一个循环将这些障碍物“绘制”到地图矩阵中%% 4. 将障碍物设置到地图矩阵中 for i 1:length(obstacles) obs obstacles(i); % 注意矩阵索引行对应y列对应x % 计算障碍物覆盖的行范围和列范围 row_range obs.y : (obs.y obs.height - 1); col_range obs.x : (obs.x obs.width - 1); % 边界检查重要避免索引超出矩阵范围 row_range row_range(row_range 1 row_range map_size_row); col_range col_range(col_range 1 col_range map_size_col); % 将对应区域标记为障碍物值为1 map(row_range, col_range) 1; end这是核心代码段有几个关键点索引计算obs.y : (obs.y obs.height - 1)生成了从起始行到结束行的所有行索引。因为Matlab的索引是从1开始的所以减1是正确的。例如从第3行开始高为3那么覆盖的行就是3, 4, 5。边界检查这是一个非常重要的避坑技巧。如果障碍物的定义不小心超出了地图边界比如x定义为28width为5地图宽度只有30那么列索引会到32超出范围Matlab会直接报错“索引超出矩阵维度”。通过row_range row_range(row_range 1 ...)这行代码我们过滤掉了超出边界的索引只保留有效的部分。这样即使参数设置有些许溢出程序也能继续运行只是障碍物会被“裁剪”在地图边缘增强了代码的健壮性。批量赋值map(row_range, col_range) 1是Matlab的矩阵索引赋值优势一次性将矩形区域内的所有元素置1效率远高于逐元素的双重循环。3.3 高级功能创建复杂障碍与随机地图除了手动定义规则障碍物有时我们需要更复杂或随机的地图。创建环形障碍物环形障碍物在机器人导航中模拟柱子或树木。思路是计算每个栅格到圆心的距离。%% 可选添加一个圆形障碍物 center_x 10; center_y 15; radius 4; % 为地图中每个点生成坐标网格 [col_grid, row_grid] meshgrid(1:map_size_col, 1:map_size_row); % 计算每个点到圆心的距离 dist_from_center sqrt((col_grid - center_x).^2 (row_grid - center_y).^2); % 将距离小于半径的点标记为障碍物 map(dist_from_center radius) 1;这里用到了meshgrid函数来生成代表每个栅格坐标的矩阵然后通过矩阵运算一次性计算所有距离再通过逻辑索引dist_from_center radius一次性标记所有障碍点。这是Matlab向量化编程的典型应用比用循环快得多。生成随机障碍地图对于算法压力测试随机地图很有用。%% 可选生成随机障碍地图覆盖指定百分比 obstacle_ratio 0.25; % 障碍物覆盖率25% % 生成与地图同尺寸的随机矩阵值在0~1之间 random_matrix rand(map_size_row, map_size_col); % 将随机值小于阈值的点设为障碍物 map(random_matrix obstacle_ratio) 1; % 注意通常要保证起点和终点是空的这里需要额外处理rand函数生成均匀分布的随机数。通过设置obstacle_ratio阈值可以控制地图的拥堵程度。但务必注意随机地图可能把起点和终点也变成障碍物导致规划问题无解。因此在实际使用中生成随机地图后需要手动或通过代码确保起点和终点坐标处的值为0。3.4 地图可视化与美化地图矩阵map已经准备好了现在把它画出来。%% 5. 可视化栅格地图 figure(1); imagesc(1:map_size_col, 1:map_size_row, map); % 显示地图 colormap([1 1 1; 0 0 0]); % 设置颜色映射[白色; 黑色] - [0; 1] hold on; % 绘制栅格线让每个格子更清晰 for i 0.5:1:map_size_row0.5 plot([0.5, map_size_col0.5], [i, i], k-, LineWidth, 0.5); end for j 0.5:1:map_size_col0.5 plot([j, j], [0.5, map_size_row0.5], k-, LineWidth, 0.5); end % 设置坐标轴 axis([0.5, map_size_col0.5, 0.5, map_size_row0.5]); % 固定坐标轴范围与栅格线匹配 axis equal; % XY轴等比例保证格子是正方形 axis off; % 关闭坐标轴显示图面更干净 title(栅格地图 (黑色为障碍物));imagesc(1:map_size_col, 1:map_size_row, map)这里的前两个参数指定了X轴和Y轴的坐标范围。这样设置后图像上的一个像素点就精确对应一个栅格的中心。colormap([1 1 1; 0 0 0])定义了一个只有两行的颜色映射表。第一行[1 1 1]是白色RGB对应矩阵中的最小值0第二行[0 0 0]是黑色对应最大值1。如果你的地图有更多状态比如2代表未知区域可以定义更多颜色。绘制栅格线这是让地图看起来像“栅格”的关键。我们在每个整数行和列的交界处画线。所以循环从0.5开始到size0.5结束步长为1。plot画的是横线和竖线‘k-’代表黑色实线‘LineWidth’, 0.5让线细一些。axis equal至关重要它保证了在屏幕上显示时每个栅格在X和Y方向上的物理长度相等不会因为图形窗口拉伸而变形。否则正方形格子可能被拉成长方形影响视觉判断和后续坐标计算。3.5 标注起点与终点一张完整的地图还需要起点和终点我们用醒目的符号标出来。%% 6. 标注起点和终点 start_node [3, 4]; % [x, y] 格式 goal_node [28, 18]; % [x, y] 格式 % 注意plot的坐标参数是 (x, y)即 (列行) plot(start_node(1), start_node(2), o, MarkerSize, 10, MarkerFaceColor, g, MarkerEdgeColor, k); plot(goal_node(1), goal_node(2), p, MarkerSize, 12, MarkerFaceColor, r, MarkerEdgeColor, k); % 添加图例 legend(, 障碍物, 起点, 终点); % imagesc默认会占一个图例句柄用空字符串跳过 hold off;这里start_node和goal_node我们依然按照[x, y]即[列 行]的格式存储。plot时直接使用即可。我用绿色实心圆表示起点红色五角星表示终点MarkerFaceColor设置填充色MarkerEdgeColor设置边框色这样非常醒目。一个常见问题是图例显示。因为imagesc会创建一个图像对象默认会出现在图例里。为了图例整洁我在legend函数中第一个参数用了空字符串来对应这个图像对象不显示它的标签只为后面的起点、终点添加标签。4. 完整代码整合与运行将上述所有代码段按顺序整合到一个.m文件中点击运行你就能得到一张类似下图的栅格地图 此处为文字描述一张20x30的黑白网格图图中有三个黑色矩形障碍物一个黑色圆形障碍物左下角有一个绿色圆点起点右上角有一个红色五角星终点。为了让你能直接使用这里提供一份整合后的、包含基础障碍和随机地图选项的完整代码%% 路径规划栅格地图生成器 clear all; close all; clc; %% 参数设置 map_size_row 20; % 地图行数 (Y) map_size_col 30; % 地图列数 (X) use_random_map false; % true:使用随机地图 false:使用预设障碍物 random_obstacle_ratio 0.2; % 随机地图障碍物覆盖率 %% 初始化地图 map zeros(map_size_row, map_size_col); if ~use_random_map % 方案一预设规则障碍物 obstacles [ struct(x, 5, y, 3, width, 8, height, 2); struct(x, 15, y, 10, width, 3, height, 8); struct(x, 22, y, 5, width, 5, height, 5); ]; for i 1:length(obstacles) obs obstacles(i); row_range obs.y : (obs.y obs.height - 1); col_range obs.x : (obs.x obs.width - 1); % 边界检查 row_range row_range(row_range 1 row_range map_size_row); col_range col_range(col_range 1 col_range map_size_col); map(row_range, col_range) 1; end % 添加一个圆形障碍物 center_x 10; center_y 15; radius 4; [col_grid, row_grid] meshgrid(1:map_size_col, 1:map_size_row); dist_from_center sqrt((col_grid - center_x).^2 (row_grid - center_y).^2); map(dist_from_center radius) 1; else % 方案二生成随机障碍地图 map rand(map_size_row, map_size_col) random_obstacle_ratio; % 将地图四边设为障碍物模拟有边界的空间 map(1, :) 1; map(end, :) 1; map(:, 1) 1; map(:, end) 1; end %% 定义起点和终点并确保它们不在障碍物上 start_node [3, 4]; % [x, y] goal_node [28, 18]; % [x, y] % 如果起点或终点在障碍物上则将其清除 if map(start_node(2), start_node(1)) 1 map(start_node(2), start_node(1)) 0; fprintf(注意起点位于障碍物上已自动清除。\n); end if map(goal_node(2), goal_node(1)) 1 map(goal_node(2), goal_node(1)) 0; fprintf(注意终点位于障碍物上已自动清除。\n); end %% 可视化 figure(Name, 栅格地图, NumberTitle, off); imagesc(1:map_size_col, 1:map_size_row, map); colormap([1 1 1; 0.5 0.5 0.5]); % [白色; 灰色] 灰色障碍物更柔和 hold on; % 绘制精细栅格线 for i 0.5:1:map_size_row0.5 plot([0.5, map_size_col0.5], [i, i], k-, LineWidth, 0.3); end for j 0.5:1:map_size_col0.5 plot([j, j], [0.5, map_size_row0.5], k-, LineWidth, 0.3); end % 标记起点终点 plot(start_node(1), start_node(2), o, MarkerSize, 10, MarkerFaceColor, g, MarkerEdgeColor, k, LineWidth, 1.5); plot(goal_node(1), goal_node(2), p, MarkerSize, 12, MarkerFaceColor, r, MarkerEdgeColor, k, LineWidth, 1.5); % 坐标轴与标签 axis([0.5, map_size_col0.5, 0.5, map_size_row0.5]); axis equal; % axis off; % 可根据需要选择是否显示坐标轴 xlabel(X (列)); ylabel(Y (行)); title(路径规划栅格地图); % 添加文字标注 text(start_node(1)0.5, start_node(2), 起点, FontSize, 9, Color, b); text(goal_node(1)0.5, goal_node(2), 终点, FontSize, 9, Color, b); hold off; %% 输出地图信息 fprintf(地图尺寸: %d 行 x %d 列\n, map_size_row, map_size_col); fprintf(障碍物栅格数量: %d\n, sum(map(:))); fprintf(起点坐标: (%d, %d)\n, start_node(1), start_node(2)); fprintf(终点坐标: (%d, %d)\n, goal_node(1), goal_node(2));这份代码更加健壮和实用。它提供了两种地图生成模式增加了起点终点的安全性检查自动清除障碍使用了更柔和的灰色表示障碍物并添加了坐标轴和信息输出。5. 常见问题、调试技巧与扩展思路即使代码写完了你可能还会遇到一些意想不到的问题。下面是我在实际教学和项目中总结的几个典型坑点和解决思路。5.1 索引越界错误与调试问题描述运行时报错“索引超出矩阵维度”。这是Matlab新手最常见错误之一。原因分析几乎可以肯定是在给map矩阵赋值时行索引或列索引的值超过了矩阵的尺寸比如地图是20x30但你的索引计算出了21或31。排查步骤检查障碍物定义仔细核对每个障碍物的x,y,width,height。确保xwidth-1 map_size_col且yheight-1 map_size_row。使用调试器在出错的行设置断点运行程序。将鼠标悬停在row_range和col_range变量上查看它们计算出的最大值是否超出了地图边界。采用防御性编程就像我在代码中做的那样在赋值前加入“边界检查”代码段。这能有效防止因参数设置疏忽导致的程序崩溃。5.2 可视化时图形错位或变形问题描述障碍物显示的位置和预期不符或者栅格不是正方形。原因与解决坐标轴范围不对imagesc显示时默认的坐标轴范围是[1, size(map,2)]和[1, size(map,1)]每个栅格的中心在整数坐标上。而我们画的栅格线是在0.5, 1.5, 2.5...这些位置。如果axis范围设置不正确会导致线和图像对不齐。务必使用axis([0.5, map_size_col0.5, 0.5, map_size_row0.5])来严格匹配。未使用axis equal如果省略这行代码当你调整图形窗口大小时X和Y轴的比例会变化导致正方形栅格被拉伸成矩形。axis equal命令强制两个坐标轴的单位长度相等。hold on的顺序务必在imagesc之后、plot画线之前执行hold on。如果顺序反了可能会清空之前画的图像。5.3 地图矩阵与算法输入的衔接问题描述地图画好了但不知道如何提供给路径搜索算法使用。核心要点对于大多数搜索算法你只需要这个map矩阵。算法会将其作为全局变量或函数参数读入。示例A*算法伪代码思路function path AStarSearch(map, start, goal) % map: 就是我们生成的二维矩阵0可通行1障碍 % start/goal: 是[x, y]坐标注意可能需要转换为行列索引 start_index sub2ind(size(map), start(2), start(1)); % 将坐标转为线性索引 goal_index sub2ind(size(map), goal(2), goal(1)); % ... 后续搜索逻辑基于map矩阵判断每个格子是否可通行 ... end关键转换算法内部经常使用节点的“线性索引”一个标号或“行列索引”来操作。sub2ind和ind2sub是两个非常实用的函数用于在坐标(x,y)和矩阵的线性索引之间转换。记住我们的约定行索引 i y,列索引 j x。5.4 扩展思路让地图更实用基础的黑白栅格地图已经能满足很多算法的需求但你可以在此基础上进行丰富多值地图用不同的值表示不同地形。例如0自由空间1障碍物2草地通行成本高3沼泽通行成本极高。在可视化时定义对应的颜色映射colormap([1 1 1; 0 0 0; 0 1 0; 0 0.5 0])。在搜索算法中将节点的移动代价g(n)与地图值关联。导入真实地图你可以将一张建筑平面图或室内地图的图片如PNG用imread读入Matlab转换为灰度图再通过设定阈值将其二值化为0-1矩阵就得到了一张真实的栅格地图。这能极大提升仿真的真实感。动态障碍物在仿真循环中你可以动态地修改map矩阵中某些区域的值然后刷新imagesc显示就能模拟移动的障碍物。这需要结合定时器timer或循环来实现动画效果。地图保存与加载使用save(myMap.mat, map, start_node, goal_node)可以将生成好的地图变量保存到文件。下次使用时用load(myMap.mat)即可载入避免重复生成。画出一张正确的栅格地图就像是为你后续的路径规划算法搭建好了舞台。这个过程中对矩阵操作的熟悉、对坐标索引的谨慎、以及对可视化细节的把握都是宝贵的编程实践。当你熟练之后这套方法可以在5分钟内完成从而让你把主要精力集中在算法逻辑本身。在接下来的内容里我们就可以在这张地图上让搜索算法跑起来了。