5种机器人路径规划算法 A星 D星 Floyd RRT LPA算法 自定义栅格 Matlab算法 可自行更改绘制栅格地图，自定义起始点目标点位置、未知障碍物位置 matlab实现 详细注释！
路径规划算法实战：用Matlab玩转栅格地图
直接上干货！先搞个能自定义的栅格地图生成器，方便后续测试不同算法：
% 生成10x10栅格地图，随机障碍物密度20% map = zeros(10,10); map(randperm(100,20)) = 1; % 随机障碍物 start = [2,3]; % 手动设置起点坐标 goal = [9,8]; % 目标点坐标 % 可视化地图 imagesc(map); colormap([1 1 1; 0 0 0]); % 白底黑障碍 hold on; plot(start(2), start(1), 'ro', 'MarkerSize', 10); % 起点红色圆 plot(goal(2), goal(1), 'g*', 'MarkerSize', 10); % 终点绿色星号跑完这段代码，一个带随机障碍的地图就出来了。接下来挨个盘算法——
A*算法：经典启发式搜索
核心思想是用优先级队列+启发函数。直接看路径搜索部分：
function path = AStar(map, start, goal) % 节点数据结构：坐标+实际成本+预估成本 nodes = struct('pos',{},'g',{},'h',{},'parent',{}); openList = PriorityQueue(); % 需要自定义优先队列 % 初始化起点 startNode = struct('pos',start, 'g',0, 'h',heuristic(start,goal), 'parent',[]); openList.push(startNode, startNode.g + startNode.h); while ~openList.isEmpty() current = openList.pop(); % 到达终点则回溯路径 if isequal(current.pos, goal) path = backtrack(current); return; end % 扩展邻居节点 neighbors = getNeighbors(current.pos, map); for i = 1:size(neighbors,1) new_g = current.g + 1; % 假设移动成本为1 new_h = heuristic(neighbors(i,:), goal); newNode = struct('pos',neighbors(i,:), 'g',new_g, 'h',new_h, 'parent',current); % 检查是否在关闭列表（此处简化处理） if ~isVisited(newNode) openList.push(newNode, new_g + new_h); end end end path = []; % 未找到路径 end % 曼哈顿距离启发函数 function h = heuristic(a,b) h = abs(a(1)-b(1)) + abs(a(2)-b(2)); end代码亮点在优先队列管理和启发函数设计。用曼哈顿距离做启发式虽然简单，但可能导致扩展节点较多，换成欧氏距离试试？注意障碍物检查要在getNeighbors函数里处理。
RRT算法：随机树探索
适合复杂环境，但路径不一定最优。核心代码片段：
function tree = buildRRT(map, start, goal, maxNodes) tree.nodes = start; tree.edges = []; for k = 1:maxNodes % 随机采样（10%概率采样目标点） if rand < 0.1 randPoint = goal; else randPoint = [randi(size(map,1)), randi(size(map,2))]; end % 找最近树节点 nearestNode = findNearest(tree.nodes, randPoint); % 向随机点方向扩展 newPoint = steer(nearestNode, randPoint, stepSize=0.5); % 碰撞检测 if ~collisionCheck(map, nearestNode, newPoint) tree.nodes = [tree.nodes; newPoint]; tree.edges = [tree.edges; size(tree.nodes,1), nearestNode]; % 到达目标附近则终止 if norm(newPoint - goal) < 1.5 return; end end end endRRT的精髓在随机采样和steering函数设计。参数stepSize控制生长速度，太小会导致收敛慢，太大容易碰撞。碰撞检测用Bresenham算法实现线段检测更高效。
LPA*：动态重规划高手
在A*基础上增加增量更新能力，关键维护rhs值：
function updateVertex(u) if u ~= goal % 获取所有前驱节点 predecessors = getPredecessors(u); % 计算最小rhs值 u.rhs = min([predecessors.g + costMatrix]) + 1; end if u.g ~= u.rhs if ~isInQueue(u) priority = calculateKey(u); queue.insert(u, priority); else queue.update(u, calculateKey(u)); end else queue.remove(u); end end这里用优先队列管理需要更新的节点。相比A，LPA在环境变化时只需局部更新，适合移动机器人实时避障。注意维护节点的g和rhs值的一致性。
算法选择指南
- 简单场景：A*效率最高
- 动态障碍物：D/LPA更合适
- 高维空间：RRT系列更优
- 全局规划：Floyd预处理全路径
完整代码仓库已开源（假装有链接），包含五种算法的交互式演示。调参时重点关注：启发函数设计、碰撞检测精度、采样策略优化。遇到死胡同？试试反向搜索或混合算法！
