A*(AStar或A星)算法栅格地图路径规划

        今天给大家讲解一下A*算法的一些流程。A*算法相对于广度优先算法做了一些优化，广度优先搜索算法可以在获取路径的情况下保证路径是最优解，但是这种算法相对来说比较慢，有时候会遍历整个地图，这对于一些大型地图来说是很吃算力的。然鹅A*算法可以说在这方面做了一些改进。A*算法在遍历openlist时，并不会将全部的点都作为当前点进行扩散，而是将代价最少的点作为当前进行遍历，这就意味着A*算法是一种启发式算法，相对于广度优先搜索算法这种无启发式算法在收敛速度上快了很多。但是具体的算法步骤以文字的形式很难说懂，我以视频的形式向大家讲解。视频通俗易懂，而且包含matlab手把手教学。视频连接在 这里。

        给大家看一下实现的效果

函数部分：

下面是部分代码（不用复制了，指定是运行不成功，我就是单纯凑字数，混个播放量）。

算法的main函数

%% 清空变量
clear
clc

%% 导入数据
mode = 1;                           %距离选择，当为1时表示曼哈顿距离，当为2时表示欧式距离
objectGridNumber = -1;              %障碍物坐标为-1
isGif = 0;                          %是否播放动画，1播放，0不播放
load('mapData.mat');                %加载地图信息
startPoint = [2 3];                 %起始点
goalPoint = [38 37];                %目标点

%% A*算法寻找路径
[pathGrid,mapData] = AStarFunction(startPoint,goalPoint,mapData,mode,objectGridNumber);

%% 将找到的路径回溯
indexPoint = goalPoint;
pathPoint = [];
while any(indexPoint~=startPoint)
    pathPoint(end+1,:) = indexPoint;
    indexPoint = [pathGrid(indexPoint(1),indexPoint(2),1) pathGrid(indexPoint(1),indexPoint(2),2)];
end
pathPoint(end+1,:) = startPoint;

%% 画图
DrawPicture(mapData,goalPoint,startPoint,pathPoint,objectGridNumber,isGif)

%% 输出
totalLen = 0;
for k1 = 1:size(pathPoint,1)-1
    totalLen = totalLen+norm([pathPoint(k1+1,1)-pathPoint(k1,1),pathPoint(k1+1,2)-pathPoint(k1,2)],mode);
end
disp(['路径长度为:',num2str(totalLen)])

核心算法部分：

function [pathGrid,mapData] = AStarFunction(startPoint,goalPoint,mapData,mode,objectGridNumber)

[lin,col] = size(mapData);%计算mapdata的行和列
pathGrid = zeros(lin,col,4); %声明一个变量，第一页是本行本列的父节点行，第二页是本行本列的父节点列

for k1 = 1:lin
    for k2 = 1:col
        pathGrid(k1,k2,1) = k1;%节点 [k1,k2]的父节点行为k1（只是为了初始化，后面会修改）
        pathGrid(k1,k2,2) = k2;%节点 [k1,k2]的父节点行为k2
        pathGrid(k1,k2,3) = norm([goalPoint(1)-k1 goalPoint(2)-k2],mode);  %预估代价g
        pathGrid(k1,k2,4) = inf;%当前代价f
    end
end
open = startPoint;
pathGrid(startPoint(1),startPoint(2),4) = 0;
close = [-1 -1];
while ~isempty(open)
    %% 找到open中花费最小的点作为当前点minNode 
    [open,minNode] = FindMinPointInOpen(open,pathGrid);
    
    %% 遍历完该点需要把它放入close中
    close(end+1,:) = minNode;%#ok<AGROW>
    
    %% 将当前点在4个方向上进行遍历（8个也可以，只需要把）ExpendNewPoint的9 10行注释掉
    [pathGrid,open,close,mapData,flag] = ExpendNewPoint(minNode,goalPoint,mapData,pathGrid,open,close,mode,objectGridNumber);
    
    if flag
        return;
    end
    
    
end