基于海图栅格化的路径规划——Dijkstra算法

在海图栅格化的基础上跑下路径规划算法

Dijkstra算法源代码启发于运动规划入门 | 1. 白话Dijkstra，从原理到Matlab实现 - 古月居 (guyuehome.com)

初始化：导入的地图是之前自制的栅格地图，设置起点、目标点、读取地图中障碍物位置

clear;
clc;

%% 导入数据、初始化参数

% 导入数据，该数据集中0显示是黑色，1显示是白色
map_struct = load('map30_60.mat'); % 地图是之前栅格化后，保存为一个矩阵.mat
map_names = fieldnames(map_struct);
map_data = getfield(map_struct, map_names{1}); % 读取load的数据并重命名为map_data
% 绘制颜色图
cmap = [1 1 1; ...              % 1 - white - clear cell 
        0 0 0; ...              % 2 - black - obstacle 
               1 0 0; ...       % 3 - red - visited 
               0 0 1; ...       % 4 - blue - on list 
               0 1 0; ...       % 5 - green - start 
               1 1 0;           % 6 - yellow - destination 
               0 1 1];          % 7

map_data(map_data==0) = 2; % 为0的点形成障碍物; map_data==1的点本身就是白色，不需要重新设置
% 起点、终点设置
map_data(1, 1) = 5;                    % 起点 
map_data(20, 50) = 7;                  % 终点
image(1.5, 1.5, map_data);
colormap(cmap);
axis image; 
% nrows为行、ncols为列 
nrows = 30;  
ncols = 60;
% 起点、终点索引设置
start_node = sub2ind(size(map_data), 1, 1); % 起点坐标
dest_node = sub2ind(size(map_data), 20, 50); % 终点坐标
% 初始化cost、起点设置为0
distanceFromStart = Inf(nrows,ncols); 
distanceFromStart(start_node) = 0; 
% 初始化父节点矩阵为0 
parent = zeros(nrows,ncols); 

主循环：寻路并绘图，Dijkstra的一个特点是把所有合法的相邻节点都遍历，就像石头扔进水里溅起涟漪，一圈圈全遍历直到找到目标点。

想深入了解Dijkstra算法原理的可以去本文提到的链接去学习

while true 
 % 绘制当前图窗 
 map_data(start_node) = 5; 
 map_data(dest_node) = 6; 
 image(1.5, 1.5, map_data);
 grid on; 
 axis image; 
 drawnow; 

 [min_dist, current] = min(distanceFromStart(:)); % 找到未完全遍历节点中cost最小的节点
  if ((current == dest_node) || isinf(min_dist))
       break; 
  end

 map_data(current) = 3;
 distanceFromStart(current) = Inf;
 % 寻找四个相邻节点，判断临近节点是否合法，更新合法节点的cost和partent
 [i, j] = ind2sub(size(distanceFromStart), current); 
neighbor = [i-1,j;... 
            i+1,j;... 
            i,j+1;... 
            i,j-1] ;  % 当前节点的临近四个节点 
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
                   (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) ;
locate = find(outRangetest>0);  % 返回大于0（不合法临节点）的索引
neighbor(locate,:)=[] ;
neighborIndex = sub2ind(size(map_data),neighbor(:,1),neighbor(:,2)) ;
for i=1:length(neighborIndex)  % 遍历合法节点的索引
    if (map_data(neighborIndex(i))~=2) && (map_data(neighborIndex(i))~=3 && map_data(neighborIndex(i))~= 5) 
        map_data(neighborIndex(i)) = 4;
        if distanceFromStart(neighborIndex(i))> min_dist + 1      
            distanceFromStart(neighborIndex(i)) = min_dist + 1;  % 相邻节点的cost为min_dist+1
            parent(neighborIndex(i)) = current;
        end 
    end 
end 
end
% 判断是否找到终点，找到了就提取路线
if (isinf(distanceFromStart(dest_node))) 
    route = [];
else 
    % 提取路线坐标
    route = dest_node; 
    while (parent(route(1)) ~= 0)  % 根据父节点顺藤摸瓜
        route = [parent(route(1)), route]; 
    end 
    % 动态显示出路线     
    for k = 2:length(route) - 1
        map_data(route(k)) = 7; 
        pause(0.1); 
        image(1.5, 1.5, map_data); 
        grid on; 
        axis image; 
    end 
end

附上结果图：

祝各位生活愉快