【路径规划】 A_star算法机器人动静态避障路径规划【含Matlab源码 371期】

最新推荐文章于 2024-09-12 22:27:28 发布

Matlab领域

最新推荐文章于 2024-09-12 22:27:28 发布

阅读量980

点赞数 26

分类专栏： Matlab路径规划（初级版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/135500659

版权

Matlab路径规划（初级版）专栏收录该内容

111 篇文章 8 订阅

订阅专栏

在这里插入图片描述

⛄一、获取代码方式

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

获取代码方式2：
付费专栏Matlab路径规划（初级版）

备注：
点击上面蓝色字体付费专栏Matlab路径规划（初级版），扫描上面二维码，付费29.9元订阅海神之光博客付费专栏Matlab路径规划（初级版），凭支付凭证，私信博主，可免费获得1份本博客上传CSDN资源代码（有效期为订阅日起，三天内有效）；
点击CSDN资源下载链接：1份本博客上传CSDN资源代码

⛄二、简介

机器人由当前点向目标点运动的过程中，所处环境经常为动态变化且未知的,这使得传统的路径规划算法对于移动机器人避障过程很难建立精确的数学模型。为此，针对环境信息完全未知的情况，为移动机器人设计一种基于模糊控制思想的多行为局部路径规划方法。该方法通过对各种行为之间进行适时合理的切换，以保证机器人安全迅速地躲避静态和动态障碍物，并利用改进的人工势场法实现对变速目标。

⛄三、部分源代码

%% % set up color map for display
cmap = [1 1 1; …% 1 - white - clear cell
0 0 0; …% 2 - black - obstacle
0 1 0; …% 3 - green - start
0 0 1; …% 4 - blue - on list
1 1 0; …% 5 - yellow - destination
1 0 0];% 6 - red = visited
colormap(cmap);
map = zeros(20); %地图尺寸
start1=10;%起点坐标
start2=2;
goal1=10;%终点坐标
goal2=18;
% Add an obstacle
map (6:16, 7) = 2;
map (6, 5:6) = 2;
map (16, 5:6) = 2;
map (8:12, 15) = 2;
map(start1,start2) = 5; % start_coords
map(goal1, goal2) = 6; % goal_coords
image(1.5,1.5,map);
grid on;
axis image;
%%
nrows = 20;
ncols = 20;
start_node = sub2ind(size(map), start1,start2); %sub2ind把数组中元素下标转换为该元素在数组中对应的索引值
goal_node = sub2ind(size(map), goal1, goal2);
% Initialize distance array
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0;

%====================
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - goal1) + abs(X - goal2);
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
%=======================
% For each grid cell this array holds the index of its parent 对于每个网格单元，这个数组保存其父节点的索引。
parent = zeros(nrows,ncols);
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(goal_node) = 6;
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
%====================
% Find the node with the minimum distance
[~, current] = min(f(😃);
[min_dist, ~] =min(distanceFromStart(😃);
%===================
if ((current == goal_node) || isinf(min_dist))
break;
end;

map(current) = 3;
%============
f(current) = Inf; %无穷大
%============
[i, j] = ind2sub(size(distanceFromStart), current);

neighbor = [i-1,j;… %邻域
i+1,j;…
i,j+1;…
i,j-1] ;
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +(neighbor(:,2)<1) + (neighbor(:,2)>ncols ) ;
locate = find(outRangetest>0);
neighbor(locate,:)=[] ;
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) ;
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4;
if distanceFromStart(neighborIndex(i))> min_dist + 1
distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current;
f(neighborIndex(i)) =H(neighborIndex(i));
end
end
end
end