基于Matlab算法五种算法优化机器人路径规划算法可任意更换，地图可修改。-优快云博客

本文链接：https://blog.youkuaiyun.com/2501_90824910/article/details/147163083

五种算法优化机器人路径规划
算法可任意更换！地图可修改！Matlab语言

分为简单路径规划和复杂路径规划两种情景，采用粒子群算法(PSO)
遗传算法(GA)
差分进化算法(DE)
灰狼优化算法(GWO)
麻雀优化算法(SSA)
五种经典算法对机器人二维路径规划问题进行求解
在这里插入图片描述

文章目录

以下是基于Matlab实现的五种算法优化机器人路径规划系统的代码，支持多种算法（如A*、Dijkstra、RRT、人工势场法、遗传算法）以及可修改的地图环境。

功能概述

地图建模：
- 用户可以自定义二维障碍物地图。
路径规划算法：
- 支持以下五种算法：
  - A* 算法
  - Dijkstra 算法
  - RRT（快速随机树）
  - 人工势场法
  - 遗传算法
结果显示：
- 显示起点、终点、障碍物和规划路径。
算法切换：
- 用户可以选择不同的算法进行路径规划。

代码实现

1. 主程序与GUI设计

clc; clear;

% 定义地图
map = [...
    0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 0 0 0 0;
    0 0 0 1 0 0 1 1 1 0;
    0 0 0 0 0 0 1 0 0 0;
    0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0];

start = [1, 1]; % 起点
goal = [5, 10]; % 终点

% 选择算法
algorithm = input('选择算法 (1: A*, 2: Dijkstra, 3: RRT, 4: 人工势场法, 5: 遗传算法): ');

switch algorithm
    case 1
        path = AStar(map, start, goal);
    case 2
        path = Dijkstra(map, start, goal);
    case 3
        path = RRT(map, start, goal);
    case 4
        path = ArtificialPotentialField(map, start, goal);
    case 5
        path = GeneticAlgorithm(map, start, goal);
    otherwise
        error('无效的算法选择');
end

% 显示结果
if ~isempty(path)
    disp('找到路径:');
    disp(path);
    
    % 可视化
    figure;
    imagesc(map);
    hold on;
    plot(path(:, 2), path(:, 1), 'r', 'LineWidth', 2); % 路径
    scatter(start(2), start(1), 'g', 'filled'); % 起点
    scatter(goal(2), goal(1), 'b', 'filled'); % 终点
    title('机器人路径规划');
    xlabel('X');
    ylabel('Y');
    legend('障碍物', '路径', '起点', '终点');
else
    disp('未找到路径');
end

2. A 算法*

function path = AStar(map, start, goal)
% ASTAR A* Path Planning Algorithm.
%
% Inputs:
%   map - Binary matrix representing the environment (0: free space, 1: obstacle).
%   start - Start point [x, y].
%   goal - Goal point [x, y].
% Outputs:
%   path - Planned path from start to goal.

[nRows, nCols] = size(map);

% 初始化
openList = struct('position', [], 'g', [], 'h', [], 'f', [], 'parent', []);
closedList = false(nRows, nCols);
openList(1).position = start;
openList(1).g = 0;
openList(1).h = norm(start - goal);
openList(1).f = openList(1).g + openList(1).h;
openList(1).parent = [];

while ~isempty(openList)
    % 找到f值最小的节点
    [~, idx] = min([openList.f]);
    current = openList(idx);
    openList(idx) = [];
    
    % 如果到达终点
    if isequal(current.position, goal)
        path = reconstructPath(current);
        return;
    end
    
    % 将当前节点加入关闭列表
    closedList(current.position(1), current.position(2)) = true;
    
    % 获取邻居节点
    neighbors = getNeighbors(current.position, map);
    for i = 1:size(neighbors, 1)
        neighbor = neighbors(i, :);
        if closedList(neighbor(1), neighbor(2))
            continue;
        end
        
        % 计算g值
        tentativeG = current.g + norm(neighbor - current.position);
        
        % 检查是否在开放列表中
        inOpenList = false;
        for j = 1:length(openList)
            if isequal(openList(j).position, neighbor)
                inOpenList = true;
                if tentativeG < openList(j).g
                    openList(j).g = tentativeG;
                    openList(j).f = openList(j).g + openList(j).h;
                    openList(j).parent = current;
                end
                break;
            end
        end
        
        % 如果不在开放列表中，添加新节点
        if ~inOpenList
            newNode.position = neighbor;
            newNode.g = tentativeG;
            newNode.h = norm(neighbor - goal);
            newNode.f = newNode.g + newNode.h;
            newNode.parent = current;
            openList(end+1) = newNode;
        end
    end
end

path = [];
end

function path = reconstructPath(node)
% RECONSTRUCTPATH Reconstruct the path from the goal node back to the start.
path = [];
while ~isempty(node.parent)
    path = [node.position; path];
    node = node.parent;
end
path = [node.position; path];
end