【路径规划】基于matlab 改进的人工势场法机器人动静态避障【含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

三、运行结果

四、备注

完整代码或者代写添加QQ1564658423。
往期回顾>>>>>>
【预测模型】基于matlab粒子群的lssvm预测【含Matlab源码 103期】
【lSSVM预测】基于matlab鲸鱼优化算法之lSSVM数据预测【含Matlab源码 104期】
【lstm预测】基于matlab鲸鱼优化算法之改进的lstm预测【含Matlab源码 105期】
【SVM预测】基于matlab蝙蝠算法改进的SVM预测（一）【含Matlab源码 106期】
【SVM预测】基于matlab灰狼算法优化svm支持向量机预测【含Matlab源码 107期】
【预测模型】基于matlab BP神经网络的预测【含Matlab源码 108期】
【lssvm预测模型】基于蝙蝠算法改进的最小二乘支持向量机lssvm预测【Matlab 109期】
【lssvm预测】基于飞蛾扑火算法改进的最小二乘支持向量机lssvm预测【Matlab 110期】
【SVM预测】基于matlab蝙蝠算法之改进的SVM预测（二）【含Matlab源码 141期】
【lssvm预测】基于matlab飞蛾扑火算法之改进的最小二乘支持向量机lssvm预测【含Matlab源码 142期】
【ANN预测模型】基于matlab差分算法改进ANN网络预测【含Matlab源码 151期】
【预测模型】基于matlab RBF神经网络预测模型【含Matlab源码 177期】
【预测模型】基于matlab SVM回归预测算法来预测股票趋势【含Matlab源码 180期】
【预测模型】基于matlab BP神经网络之模型优化预测【含Matlab源码 221期】
【预测模型】基于matlab RLS算法的数据预测【含Matlab源码 222期】
【预测模型】基于matlab碳排放约束下的煤炭消费量优化预测【含Matlab源码 223期】
【路径规划】基于matlab A星和改进A星的路径规划【含Matlab源码 225期】
【TSP】基于matlab 改进的禁忌搜索算法之求解旅行商问题【含Matlab源码 241期】
【TSP】基于matlab 改进的蚁群算法之求解旅行商问题【含Matlab源码 242期】
【路径规划】基于matlab模拟退火算法之求解火灾巡逻最短路径【含Matlab源码 252期】
【三维路径规划】基于matlab 无人机三维空间的航迹规划【含Matlab源码 270期】
【路径规划】基于matlab分布式目标检测和跟踪的多无人机【含Matlab源码 289期】
【路径规划】基于matlab粒子群算法求解无人机最短路径【含Matlab源码 300期】
【无人机】多无人协同任务分配程序平台【含Matlab源码 301期】
【路径规划】基于matlab任意架次植保无人机作业路径规划【含Matlab源码 322期】
【路径规划】基于matlab粒子群遗传求解多无人机三维路径规划【含Matlab源码 333期】
【VRP问题】基于matlab粒子群求解VRPTW模型【含Matlab源码 334期】
【路径规划】基于matlab改进蚁群算法的路径规划【含Matlab源码 335期】
【VRP】基于matlab改进的模拟退火和遗传算法求解VRP问题【含Matlab源码 343期】
【VRP问题】基于matlab灰狼算法求解VRPTW问题【含Matlab源码 361期】
【VRP问题】基于matlab遗传算法和模拟退火求解带时间窗的自行车调度问题【含Matlab源码 370期】