基于A*算法的机器人路径规划的MATLAB实现

基于A*算法的机器人路径规划的MATLAB实现，可自由选择地图和起始终止点。下载后有问题可私信或者留言与我联系。

%%下载地图并初始化参数
figure（1）
mapOriginal=imbinarize（imread（‘Maps/a_map11.bmp‘））; %从bmp文件读取地图

resolutionX=100;
resolutionY=100;

mapResized=imresize（mapOriginal[resolutionX resolutionY]）;
map=mapResized; 

%连接矩阵-定义机器人可允许的动作
conn=[1 1 1;
      1 2 1;
      1 1 1];

display_process=true; %显示节点的处理

% 增加一个单位像素的边界——考虑到机器人的大小
for i=1:size（mapResized1）
    for j=1:size（mapResized2）
        if mapResized（ij）==0
            if i-1>=1 map（i-1j）=0; end
            if j-1>=1 map（ij-1）=0; end
            if i+1<=size（map1） map（i+1j）=0; end
            if j+1<=size（map2） map（ij+1）=0; end
            if i-1>=1 && j-1>=1 map（i-1j-1）=0; end
            if i-1>=1 && j+1<=size（map2） map（i-1j+1）=0; end
            if i+1<=size（map1） && j-1>=1 map（i+1j-1）=0; end
            if i+1<=size（map1） && j+1<=size（map2） map（i+1j+1）=0; end
        end
    end
end

image（（map==0）.*0 + （map==1）.*255 + （mapResized-map）.*150）;
colormap（gray（256））
%选出初始点和目标点
disp（‘select source in the image‘）;
[xy] = ginput（1）;
source=[double（int8（y）） double（int8（x））];    % source position in Y X format
disp（‘select goal in the image‘）;
[xy] = ginput（1）;
goal = [double（int8（y）） double（int8（x））];    % goal position in Y X format

if length（find（conn==2））~=1 error（‘no robot specified in connection matrix‘）; end

%% Compute path

%节点的结构被认为是位置、位置、历史成本、启发式成本、总成本、封闭列表中的父索引
Q=[source 0 heuristic（sourcegoal） 0+heuristic（sourcegoal） -1]; %A*算法的处理队列，开放列表
closed=ones（size（map））; % 黑色已标记存到闭列表
closedList=[]; % the closed list taken as a list
pathFound=false;
tic;
counter=0;

size（Q）

while size（Q1）>0
     [A I]=min（Q[]1）;
     n=Q（I（5）:）; % 过程最小成本元素
     Q=[Q（1:I（5）-1:）;Q（I（5）+1:end:）]; % 删除正在处理中的元素
     if n（1）==goal（1） && n（2）==goal（2） %目标测试
         pathFound=true;break;
     end
     [rxryrv]=find（conn==2）; %连接矩阵中的机器人位置
     [mxmymv]=find（conn==1）; %可能移动的数组
     for mxi=1:size（mx1） %遍历所有的移动
         newPos=[n（1）+mx（mxi）-rx n（2）+my（mxi）-ry]; %可能的新节点
         if checkPath（n（1:2）newPosmap） %如果从n到newPos的路径是无碰撞的
              if closed（newPos（1）newPos（2））~=0 %还没有关闭
                  historicCost=n（3）+historic（n（1:2）newPos）;
                  heuristicCost=heuristic（newPosgoal）;
                  totalCost=historicCost+heuristicCost;
                  add=true; %还没有在更好的代价下排好队
                  if length（find（（Q（:1）==newPos（1）） .* （Q（:2）==newPos（2））））>=1
                      I=find（（Q（:1）==newPos（1）） .* （Q（:2）==newPos（2）））;
                      if Q（I5）                      else Q=[Q（1:I-1:）;Q（I+1:end:）;];add=true;
                      end
                  end
                  if add
                      Q=[Q;newPos historicCost heuristicCost totalCost size（closedList1）+1]; %在队列中添加新节点
                  end
              end
         end           
     end
     closed（n（1）n（2））=0;closedList=[closedList;n]; %更新闭列表
     i0 = counter;
     i1 = 40;
     counter=counter+1;
     
     if display_process == true && （rem（i0i1） == 0） 
     

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4112  2018-10-21 18:57  基于A-Star算法的机器人路径规划的MATLAB实现\a_star.m

     文件        492  2018-10-13 13:32  基于A-Star算法的机器人路径规划的MATLAB实现\checkPath.m

     文件        241  2018-10-13 13:31  基于A-Star算法的机器人路径规划的MATLAB实现\feasiblePoint.m

     文件         56  2018-10-13 13:32  基于A-Star算法的机器人路径规划的MATLAB实现\heuristic.m

     文件         49  2018-10-13 13:32  基于A-Star算法的机器人路径规划的MATLAB实现\historic.m

     文件     251078  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map1.bmp

     文件    1049654  2018-10-18 09:49  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map11.bmp

     文件     251078  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map2.bmp

     文件     251078  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map3.bmp

     文件     251078  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map4.bmp

     文件     251078  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map5.bmp

     文件    1049654  2018-10-13 14:14  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map6.bmp

     文件    1049654  2018-10-13 15:07  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map7.bmp

     文件     533878  2018-10-13 18:34  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map8.bmp

     文件     533878  2018-10-15 14:28  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\a_map9.bmp

     文件     308346  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\Maps\map1.bmp

     文件        362  2016-11-28 22:46  基于A-Star算法的机器人路径规划的MATLAB实现\README.md

     目录          0  2018-10-18 09:47  基于A-Star算法的机器人路径规划的MATLAB实现\Maps

     目录          0  2018-10-21 19:44  基于A-Star算法的机器人路径规划的MATLAB实现

----------- ---------  ---------- -----  ----

              5785766                    19