全局路径规划算法—A*算法—学习笔记

A*算法结合了贪心算法（深度优先）和Dijkstra算法（广 度优先），是一种启发式搜索算法

贪心算法存在可能搜索不到目标结点的情况，非最优路径

Dijkstra算法是最优路径

深度优先：以当前搜索到的最小路径作为目标路径继续往下搜索

广度优先：对搜索空间内的每一个点进行搜索

路径优劣评价公式为： f(n)=g(n)+h(n)。 

f(n) 是从初始状态经由状态n到目标状态的代价估计，

g(n) 是在状态空间中从初始状态到状态n的实际代价，

h(n) 是从状态n到目标状态的最佳路径的估计代价。

使用了两个状态表，分别称为openList表和closeList表。（类似于Dijkstra算法中的U和S）

openList表由待考察的节点组成，

closeList表由已经考察 过的节点组成。

预处理

1.地图栅格化

2.定义两个列表集合

3.更新集合直至找到目标节点

从openList中选择 f值最小的 ( 方格 ) 节点I，从 openList 里取出，放到 closeList 中，并把它作为新的父节点 。检查所有与它相邻的子节点，忽略障碍物不可走节点、 忽略已经存在于closeList的节点；如果方格不在openList 中，则把它们加入到 openList中。 

4.完成路径搜索，结束算法

%% 进入循环
flag = 1;
while flag   
    
    % 找出父节点的忽略closeList的子节点
    child_nodes = child_nodes_cal(parent_node,  m, n, obs, closeList); 
    
    % 判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
    for i = 1:size(child_nodes,1)
        child_node = child_nodes(i,:);
        [in_flag,openList_idx] = ismember(child_node, openList, 'rows');
        g = openList_cost(min_idx, 1) + norm(parent_node - child_node);
        h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
        f = g+h;
        
        if in_flag   % 若在，比较更新g和f        
            if g < openList_cost(openList_idx,1)
                openList_cost(openList_idx, 1) = g;
                openList_cost(openList_idx, 3) = f;
                openList_path{openList_idx,2} = [openList_path{min_idx,2}; child_node];
            end
        else         % 若不在，追加到openList
            openList(end+1,:) = child_node;
            openList_cost(end+1, :) = [g, h, f];
            openList_path{end+1, 1} = child_node;
            openList_path{end, 2} = [openList_path{min_idx,2}; child_node];
        end
    end