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

地图是海图栅格化后的栅格地图，A*算法是在Dijkstra算法基础上加入启发函数

启发函数可以采用曼哈顿距离或者欧几里得距离

百度百科上的解释：

 

绿色代表欧氏距离，也就是直线距离，而蓝色和黄色代表等价的曼哈顿距离。

采用不同的距离作为启发函数得到的结果也不一样，曼哈顿距离得到的路径只有水平和竖直方向，欧几里得距离可以有45°斜线。

g(n)为后退代价，该节点距起点的成本；h(n)是预估代价，该节点距目标的成本。

代码如下：

初始化地图、起点终点、f、父节点矩阵

clear all;
clc;


%% 导入数据、初始化参数
% 导入数据，该数据集中0显示是黑色，1显示是白色
map_struct = load('map30_60.mat');
map_names = fieldnames(map_struct);
map_data = getfield(map_struct, map_names{1});
% 绘制颜色图
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
% colormap(cmap); 
% map = zeros(23,23); 
% 添加障碍物
map_data (find(map_data==0)) = 2; 
% 起点、终点设置
map_data(1, 1) = 5;                  % 起点
map_data(20, 50) = 7;                % 终点 
image(1.5, 1.5, map_data);
colormap(cmap);
grid on; 
axis image; 
% 设置行数列数
nrows = 30; 
ncols = 60; 
% 设置起点、终点索引
start_node = sub2ind(size(map_data), 1, 1); 
dest_node = sub2ind(size(map_data), 20, 50); 
% 初始化给g(n)为inf，起点设置为0
distanceFromStart = Inf(nrows,ncols); 
distanceFromStart(start_node) = 0; 

% =====================
% 评价函数
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - 20) + abs(X - 50);  % 曼哈顿距离作为启发函数
f = Inf(nrows,ncols);  % f(n)为inf
f(start_node) = H(start_node);  % f(1,1)=g(1,1)+h(1,1)=0+h(1,1)
% =====================
% 初始化各点的父节点为0
parent = zeros(nrows,ncols); 

主循环：

while true 
 % 绘制当前地图
 map_data(start_node) = 5; 
 map_data(dest_node) = 6; 
 image(1.5, 1.5, map_data); 
 grid on; 
 axis image; 
 drawnow; 
 % ===================
 % 寻找最短距离的节点 
 [~, current] = min(f(:));  % f(n)的最小值用于寻找当前节点
 % ===================
 if ((current == dest_node) || isinf(distanceFromStart(current))) 
    break; 
 end 

 map_data(current) = 3; 
 f(current) =Inf;
 % 选取相邻的节点，考虑相邻节点是否合法，遍历相邻节点
 [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); 
 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))> distanceFromStart(current) + 1      
            distanceFromStart(neighborIndex(i)) = distanceFromStart(current)+1; 
            parent(neighborIndex(i)) = current; 
            f(neighborIndex(i)) = H(neighborIndex(i))+distanceFromStart(neighborIndex(i)); 
        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

结果如下：

 

 

其实，该算法并没有经典A*里维护OPEN和CLOSE表，但是原理一样，另一种看情况要不要上传。

祝大家生活愉快，身体健康，学业事业顺利