机器人路径规划算法：A*与RRT在栅格地图中的实现

1. 机器人路径规划算法概述

路径规划是机器人导航中的核心问题，其本质是在给定环境中找到从起点到终点的最优或可行路径。根据环境信息的完整程度，路径规划可分为全局规划（已知完整地图）和局部规划（动态感知环境）。本文将重点探讨五种经典的全局路径规划算法在栅格地图中的应用实现。

栅格地图（Grid Map）是最常用的环境建模方式之一，它将工作空间离散化为均匀的网格单元，每个网格代表一个可通行或障碍状态。这种表示方法简单直观，便于算法实现和可视化。在Matlab中，我们可以用二维矩阵表示栅格地图，其中0表示自由空间，1表示障碍物。

提示：栅格大小直接影响规划精度和计算效率。通常2-5cm的栅格分辨率适合大多数移动机器人应用。

2. 栅格地图生成与可视化

2.1 基础地图生成

在Matlab中创建自定义栅格地图的核心代码如下：

matlab复制% 初始化10x10的空地图（全为自由空间）
mapSize = [10,10];  
map = zeros(mapSize);  

% 设置随机障碍物（约20%密度）
obstacleCount = round(prod(mapSize)*0.2);  
obstacleIndices = randperm(prod(mapSize), obstacleCount);  
map(obstacleIndices) = 1;  

% 定义起点和终点
startPos = [2,3];  % [行,列]坐标
goalPos = [9,8];   

% 可视化设置
figure;
imagesc(map);
colormap([1 1 1; 0 0 0]); % 白色-自由，黑色-障碍
hold on;
plot(startPos(2), startPos(1), 'ro', 'MarkerSize',10, 'LineWidth',2); % 起点-红色圆圈
plot(goalPos(2), goalPos(1), 'g*', 'MarkerSize',10, 'LineWidth',2);  % 终点-绿色星号
axis equal; grid on;

这段代码实现了：

1. 创建指定尺寸的空白栅格地图
2. 按比例随机生成障碍物
3. 设置起点和终点位置
4. 可视化地图及路径起止点

2.2 地图定制化扩展

实际应用中，我们常需要更灵活的地图控制：

matlab复制% 手动添加特定障碍物
map(3:5, 7) = 1;  % 垂直障碍
map(8, 2:6) = 1;  % 水平障碍

% 添加不规则障碍物
[x,y] = meshgrid(1:mapSize(2),1:mapSize(1));
circleMask = (x-5).^2 + (y-5).^2 <= 4;
map(circleMask) = 1;

% 保存/加载地图配置
save('customMap.mat','map','startPos','goalPos');
% load('customMap.mat');  % 读取已有配置

注意事项：Matlab矩阵索引是(行,列)顺序，而plot函数使用(x,y)即(列,行)坐标，可视化时需注意坐标转换。

3. A*算法实现与优化

3.1 基础A*算法

A*算法结合了Dijkstra的广度优先搜索和启发式估计，其核心数据结构为：

matlab复制% 节点数据结构
node = struct(...
    'pos', [0,0],    % 节点位置[row,col]
    'gCost', 0,      % 从起点到当前节点的实际代价
    'hCost', 0,      % 当前节点到终点的启发估计
    'fCost', 0,      % 总代价f = g + h
    'parent', []     % 父节点索引
);

% 优先队列实现（简化版）
openList = containers.Map('KeyType','char','ValueType','any');

完整A*算法实现步骤：

1. 初始化：

matlab复制startNode = createNode(startPos, 0, heuristic(startPos,goalPos), []);
openList(mat2str(startPos)) = startNode;
closedList = zeros(mapSize);

2. 主循环：