《Motion Planning》【2】A*算法实现

个人学习笔记使用。

1.A_star.m文件

%%初始化起点、终点、静态障碍物、地图的大小等
clc; clear; close all;
rows = 20;
cols = 20;
sz = [rows cols];
dy_search_area = [];
start_sub = [20, 1];
goal_sub = [1, 20];
load obs_sub.mat -ascii
search_head = 2;
search_end = search_head;

%%定义格栅地图
field = ones(sz);

%设置起点、终点、障碍物索引
obs_r = obs_sub(:, 1);
obs_c = obs_sub(:, 2);
obs_ind = sub2ind(sz, obs_r, obs_c);
start_ind = sub2ind(sz, start_sub(1), start_sub(2));
goal_ind = sub2ind(sz, goal_sub(1), goal_sub(2));

%设置起点、终点、障碍物颜色
field(obs_ind) = 2;
field(start_ind) = 4;
field(goal_ind) = 5;

%绘制地图
cmap = [1 1 1; ...           % 1-白色-空地
        0 0 0; ...           % 2-黑色-静态障碍
        1 0 0; ...           % 3-红色-动态障碍
        1 1 0; ...           % 4-黄色-起始点 
        1 0 1; ...           % 5-品红-目标点
        0 1 0; ...           % 6-绿色-到目标点的规划路径   
        0.2 0.8 0.6];        % 7-翠绿-动态规划的路径
colormap(cmap);
image(1.5,1.5,field);

%设置栅格属性
grid on;
hold on;
set(gca, 'gridline', '-', 'gridcolor', 'k', 'linewidth', 0.5, 'GridAlpha', 0.5);
set(gca,'xtick', 1:cols+1, 'ytick', 1:rows+1);
set(gca, 'XAxisLocation', 'top');
axis image;

%%A*实现流程
%*****************************第一步：建立openlist和closelist两个表
%openList为n×4的矩阵用于存放周围记录点得ind, G, H, F 
openList = [start_ind 0 0 0];

%closeList为n×2的矩阵用于存放已经使用过的父节点的ind和F值
closeList = [];

%*****************************第二步：建立path,起点到任意地图中的点的路径
for i = 1:rows*cols
    path{i, 1} = i;
    path{i, 2} = [];
end
path{start_ind, 2} = start_ind; % 初始化起点的路径就是自身到自身

%*****************************第三步：正片开始
while true
    %3.1 从openList中找到F值即第四列最小的点此时就是起点
    [~, parent_node_index] = min(openList(:, 4)); % F最小值的索引默认是从上到下1....n
    parent_node = openList(parent_node_index, 1); % 此时的才是真正的地图上的ind
    
    %3.2 判断是否到终点
    if parent_node == goal_ind
        break;
    end

    %3.3 在openList中选出F最小的作为父节点
    next_nodes = Astar_NextNode(closeList, parent_node, rows, cols, field);
    for i = 1:length(next_nodes)
        nextNode = next_nodes(i); % 把周围节点进行判断

        %计算F G H需要将openList中的最小的F终点 当前nextNode转换为sub
        [parent_node_R, parent_node_C] = ind2sub(sz, parent_node);
        [nextNode_R, nextNode_C] = ind2sub(sz, nextNode);
        [goal_sub_R, goal_sub_C] = ind2sub(sz, goal_ind);

        %开始计算
        g = openList(parent_node_index, 2) + norm([parent_node_R, parent_node_C] - [nextNode_R, nextNode_C]);
        h = abs(goal_sub_R - nextNode_R) + abs(goal_sub_C - nextNode_C);
        f = g + h;

        %判断该子节点是否在openList中
        [in_openList, nextNode_index] = ismember(nextNode, openList(:, 1)); % 返回的第一个值是逻辑值，第二个值就是当前在openList中的索引1.....n

        %在的话比较F值，小的话更新F G H
        if in_openList && f < openList(nextNode_index, 4)
            openList(nextNode_index, 2) = g;
            openList(nextNode_index, 3) = h;
            openList(nextNode_index, 4) = f;
            %更新路径
            path{nextNode, 2} = [path{parent_node, 2}, nextNode];
        end

        %如果不在的话放进openList中并更新路径
        if ~in_openList
            openList(end+1, :) = [nextNode, g, h, f];
            path{nextNode, 2} = [path{parent_node, 2}, nextNode];
        end
    end

    %将父节点移出openList添加到closeList中
    closeList(end+1, :) = [openList(parent_node_index, 1), openList(parent_node_index, 4)];
    openList(parent_node_index, :) = [];

    %动态绘制区域包括openList和closeList
    dy_search_area = [openList(:, 1)', closeList(:, 1)']; % 将两个表中的ind放到一起是一行
    field(dy_search_area) = 7;
    field(start_ind) = 4; % 起始点和终点保持不变
    field(goal_ind) = 5;
    if mod(search_head, search_end) == 0
        image(1.5, 1.5, field);
        drawnow;
    end
    search_head = search_head + 1000;
end

%%绘制折线图
image(1.5, 1.5, field);
optimal_path = path{goal_ind, 2};
[plot_r, plot_c] = ind2sub(sz, optimal_path);
plot(plot_c + 0.5, plot_r + 0.5, '-b', 'LineWidth', 2);
scatter(plot_c + 0.5, plot_r + 0.5, 30, 'filled', MarkerEdgeColor='b', MarkerFaceColor='red'); % 将路径经过的点标出来

2.Astar_NextNode.m文件

function next_nodes = Astar_NextNode(closeList, parent_node, rows, cols, field)
    %初始化next_nodes用于存放地图内非障碍物非在closeList中的节点
    next_nodes = [];
    sz = [rows cols];
    %将此时的父节点转换为sub方便8方向移动
    [parent_node_r, parent_node_c] = ind2sub(sz, parent_node);
    %移动方向矩阵
    move_pos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1];
    
    closenode = []; %用来判断当前的nextnode是否在closeList中
    if ~isempty(closeList)
        closenode = closeList(:, 1); % 将closeList中的索引值即做过父节点的值赋值给closenode
    end

    %遍历周围节点
    for i = 1:8
        %不能超出地图边界
        if 0 < parent_node_r + move_pos(i, 1) && parent_node_r + move_pos(i, 1) <= rows &&...
           0 < parent_node_c + move_pos(i, 2) && parent_node_c + move_pos(i, 2) <= cols
             next_node_sub = [parent_node_r + move_pos(i, 1) parent_node_c + move_pos(i, 2)];
             next_node_ind = sub2ind(sz, next_node_sub(1), next_node_sub(2));
             if field(next_node_ind) ~= 2
                if ~ismember(next_node_ind, closenode) % 这一步的closenode本来可以写成是closeList(:, 1)但是初始化的时候closeList为空不能这么用
                    next_nodes(end+1) = next_node_ind;
                end
             end
        end
    end
end

3.obs_sub.mat文件固定障碍物同上一篇

4.文件结构

运行在A_star.m

5.运行结果

OVER