《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文件固定障碍物同上一篇

14    20
3    4
15    6
3    8
3    2
13    14
7    9
14    20
15    9
12    13
15    4
5    8
15    4
20    16
18    18
2    8
8    14
8    6
14    11
12    17
16    12
8    7
5    6
2    10
16    9
5    8
8    12
12    15
5    9
13    9
10    3
4    1
16    6
3    7
6    14
5    20
11    19
2    10
9    5
3    16
3    16
16    15
6    15
13    3
20    14
9    10
14    5
16    2
9    17
14    4
3    4
19    14
4    18
6    11
16    15
10    4
16    20
8    11
6    14
1    1
14    17
9    15
10    3
13    11
2    7
7    11
16    8
14    9
3    4
3    6
2    1
1    19
9    14
14    19
15    4
11    19
3    16
13    12
3    9
3    6
2    16
3    5
4    2
4    16
7    14
7    15
5    13
6    9
18    8
15    17
12    7
4    17
5    16
2    18
19    11
15    13
12    20
7    9
4    2
13    18

4.文件结构

运行在A_star.m

5.运行结果


OVER

  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
D*Lite是一种用于路径规划的增量搜索算法,它可以被应用于ROS(机器人操作系统)的动作规划模块ros-motion-planning中。 D*Lite算法的核心思想是将环境建模成一个图形,每个图形的节点代表机器人在环境中的一个离散位置,边代表机器人从一个位置移动到另一个位置的成本。该算法使用两个主要的数据结构,即一个状态图(State Graph)和一个搜索树(Search Tree),来描述机器人在环境中的当前位置和已知的目标位置之间的最佳路径。 D*Lite算法的工作流程如下: 1. 初始化状态图和搜索树,将机器人当前位置作为起始节点。 2. 根据当前的起始节点和目标位置,通过边的成本计算启发值(Heuristic Value),并估计机器人到目标位置的最佳路径。 3. 根据启发值更新搜索树,并选择一个代价最小的路径作为当前的最佳路径。 4. 根据最佳路径,移动机器人到下一个节点,并更新状态图和搜索树。 5. 重复步骤3和步骤4,直到机器人到达目标位置。 在ROS中,ros-motion-planning模块提供了D*Lite算法实现和接口,以帮助机器人实现自动路径规划。通过使用该模块,机器人可以根据当前环境状态,通过D*Lite算法快速生成最佳路径,并实时更新路径以应对环境的变化。同时,该模块还提供了可视化工具,使用户可以直观地了解机器人的路径规划过程和结果。 总体而言,ros-motion-planning的D*Lite算法是一种强大的工具,可以帮助机器人在复杂环境中快速生成最佳路径,提高机器人的实时性和自主性。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

HenceDang

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值