【路径规划】基于matlab A_star算法机器人静态避障路径规划【含Matlab源码 495期】

举报
海神之光 发表于 2022/05/30 01:11:30 2022/05/30
【摘要】 一、获取代码方式 获取代码方式1: 完整代码已上传我的资源:【路径规划】基于matlab A_star算法机器人静态避障路径规划【含Matlab源码 495期】 获取代码方式2: 通过紫极神光博客主页...

一、获取代码方式

获取代码方式1:
完整代码已上传我的资源:【路径规划】基于matlab A_star算法机器人静态避障路径规划【含Matlab源码 495期】

获取代码方式2:
通过紫极神光博客主页开通CSDN会员,凭支付凭证,私信博主,可获得此代码。

获取代码方式3:
通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码。

备注:开通CSDN会员,仅只能免费获得1份代码(有效期为开通日起,三天内有效);
订阅紫极神光博客付费专栏,可免费获得2份代码(有效期为订阅日起,三天内有效);

二、简介

A算法
A算法是一种典型的启发式搜索算法,建立在Dijkstra算法的基础之上,广泛应用于游戏地图、现实世界中,用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数,如式(1)所示。
f(n)=g(n)+h(n)(1)
其中,f(n)是算法在搜索到每个节点时,其对应的启发函数。它由两部分组成,第一部分g(n)是起始节点到当前节点实际的通行代价,第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时,都选取f(n)值最小的那个节点作为最优路径上的下一个节点。
在实际应用中,若以最短路程为优化目标,h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0,表示没有利用任何当前节点与终点的信息,A
算法就退化为非启发的Dijkstra算法,算法搜索空间随之变大,搜索时间变长。
A*算法步骤如下,算法维护两个集合:P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点;Q表维护那些已加入最优路径树上的节点。
(1)P表、Q表置空,将起点S加入P表,其g值置0,父节点为空,路网中其他节点g值置为无穷大。
(2)若P表为空,则算法失败。否则选取P表中f值最小的那个节点,记为BT,将其加入Q表中。判断BT是否为终点T,若是,转到步骤(3);否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT,进行下列步骤:

①计算NT的启发值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代价。
②如果NT在P表中,且通过式(3)计算的g值比NT原先的g值小,则将NT的g值更新为式(3)结果,并将NT的父节点设为BT。
③如果NT在Q表中,且通过式(3)计算的g值比NT原先的g值小,则将NT的g值更新为式(3)结果,将NT的父节点设为BT,并将NT移出到P表中。
④若NT既不在P表,也不在Q表中,则将NT的父节点设为BT,并将NT移到P表中。
⑤转到步骤(2)继续执行。
(3)从终点T回溯,依次找到父节点,并加入优化路径中,直到起点S,即可得出优化路径。

三、部分源代码

close all; clear all;
%initial the map size
size_x = 10;
size_y = 10;
size_t = 100;
%1 - white - clear cell
%2 - black - obstacle
%3 - Grayish blue   - robot 1
%4 - Reddish blue - robot 2
%5 - purple - robot 3
%6 - vivid green - robot 4
%7 - Plum red - robot 5
cmap = [1 1 1;            
              0 0 0;
              0.69 0.87 0.90;
              0.25 0.41 0.88;
              0.63 0.13 0.94;
              0 1 0.5;
              0.87 0.63 0.87;
              ];
colormap(cmap);    %将颜色进行映射
%initial the space_time_map and reservation table
space_map = ones(size_x, size_y);
space_time_map = ones(size_x, size_y, size_t);
reservation_table = zeros(size_x, size_y, size_t);
%add the static obstacle
space_time_map(1:5, 5, :) = 2*ones(5,1, size_t);
reservation_table(1:5, 5, :) = 2*ones(5,1, size_t);
%the start point array and end point array
start_points= [2 4; 5 1];%[2 4; 5 1; 1 1; 9 6;7 3 ];
end_points = [8 7; 8 10];%[8 7; 10 10; 7 3; 3 2; 1 1];
len_route_max = 0;
route_all = [];
for index = 1:length(start_points(:,1))
%initial the start point and the end point
function [route] = Time_Astar_For_Cooperative_Path_Finding(start_coords, end_coords, space_time_map, reservation_table)
%1 - white - clear cell
%2 - black - obstacle
%3 - red - visited
%4 - blue - on list
%5 - green - start
%6 - yellow - destination
cmap = [1 1 1;
              0 0 0;
              1 0 0;
              0 0 1;
              0 1 0;
              1 1 0;
              0.5 0.5 0.5];
colormap(cmap);
[size_x, size_y, size_t] = size(space_time_map);
%add the start point and the end point 
space_time_map(start_coords(1), start_coords(2), :) = 5;
space_time_map(end_coords(1), end_coords(2), :) = 6;
%initial parent array and Heuristic array
parent = zeros(size_x, size_y, size_t);
[X, Y] = meshgrid(1:size_y, 1:size_x);
xd = end_coords(1);
yd = end_coords(2);
H =abs(X-xd) + abs(Y-yd);
H = H';
%initial cost arrays
g = inf(size_x, size_y, size_t);
f = inf(size_x, size_y, size_t);
g(start_coords(1), start_coords(2), 1) = 0;
f(start_coords(1), start_coords(2), 1) = H(start_coords(1), start_coords(2));
end_time = 1;
while true
    %find the node with the minimum f values
    [min_f, current] = min(f(:));
    [current_x, current_y, current_t] = ind2sub(size(space_time_map), current);
    if((current_x == end_coords(1) && current_y == end_coords(2))  || isinf(min_f))
        end_time = current_t;
        disp('hheheh');
        break;
   end
    %add the current node to close list
    space_time_map(current) = 3;
    f(current) = Inf;
    [i, j, k] = ind2sub(size(space_time_map), current);
    neighbours = [i-1, j, k+1;
                           i+1, j, k+1;
                           i, j-1, k+1;
                           i, j+1, k+1;
                           i, j, k+1];
    for index = 1:size(neighbours, 1)
            px = neighbours(index, 1);
            py = neighbours(index, 2);
            pt = neighbours(index, 3);
             % judge whether out of bound or not
            if (px >0 && px<=size_x && py >0 && py<= size_y )
                sub_neighbours = sub2ind(size(space_time_map), px, py, pt);
                %judge whether the node is obstacle, start point, end
                %point, or not
                if(space_time_map(sub_neighbours) ~= 2 && space_time_map(sub_neighbours) ~= 5 && space_time_map(sub_neighbours) ~= 3)
                    %judge whether the node has less f 
                    if(g(current) +1+ H(px, py) < f(sub_neighbours))
                        %judge whether the node is in reservation table or not
                        if (~reservation_table(sub_neighbours ))
                            %judge whether the special action
                             [special_action] = Special_action(current, sub_neighbours,reservation_table);
                            if (~special_action)
                                g(sub_neighbours) = g(current) + 1;
                                f(sub_neighbours) = g(sub_neighbours) +  H(px, py);
                                parent(sub_neighbours) = current;
                                space_time_map(sub_neighbours) = 4;
                            end
                       end
                    end
                end
            end
    end
    

  
 
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113

四、运行结果

在这里插入图片描述
在这里插入图片描述

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例(第2版)[M].电子工业出版社,2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社,2017.

文章来源: qq912100926.blog.csdn.net,作者:海神之光,版权归原作者所有,如需转载,请联系作者。

原文链接:qq912100926.blog.csdn.net/article/details/114669585

【版权声明】本文为华为云社区用户转载文章,如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱: cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论(0

0/1000
抱歉,系统识别当前为高风险访问,暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称,即可参与社区互动!

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。