五种机器人路径规划算法详解：A星、D星、Floyd、RRT与LPA算法，Matlab实现自定义...

5种机器人路径规划算法 A星 D星 Floyd RRT LPA算法自定义栅格 Matlab算法可自行更改绘制栅格地图，自定义起始点目标点位置、未知障碍物位置matlab实现详细注释！

qq487739278

315人浏览 · 2025-12-22 10:47:51

qq487739278 · 2025-12-22 10:47:51 发布

路径规划算法实战：用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
end

RRT的精髓在随机采样和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值的一致性。