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

一、简介

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

二、源代码

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

三、运行结果

四、备注

版本：2014a