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

一、获取代码方式

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

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

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

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

二、简介

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

三、部分源代码

%% % 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

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

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