【自己练手的小项目】

自己练手的小项目——A*路径规划

提示：主要时数据分析、图像处理、图像识别相关技术案例，班门弄斧了

---

---

前言

功能：导入地图，设置启点、终点，自动计算路径。

---

一、A*是什么？

Astart算法在Dijkstra算法的基础上，增加了一个当前探测点到目标点的距离，而综合探测点到出发点的步长与探测点到目标点的距离合，f = distStart + distEnd，来选定当前已探测距离点中可以固定的最小距离点，也就是根据合的最小值来确定每一迭代步选入闭集的点。

二、使用步骤

1.A-star.m

%%下载地图并初始化参数
figure('Name','选择起点与终点')
mapOriginal=imbinarize(imread('Maps/a_map2.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(mapResized,1)
    for j=1:size(mapResized,2)
        if mapResized(i,j)==0
            if i-1>=1, map(i-1,j)=0; end
            if j-1>=1, map(i,j-1)=0; end
            if i+1<=size(map,1), map(i+1,j)=0; end
            if j+1<=size(map,2), map(i,j+1)=0; end
            if i-1>=1 && j-1>=1, map(i-1,j-1)=0; end
            if i-1>=1 && j+1<=size(map,2), map(i-1,j+1)=0; end
            if i+1<=size(map,1) && j-1>=1, map(i+1,j-1)=0; end
            if i+1<=size(map,1) && j+1<=size(map,2), map(i+1,j+1)=0; end
        end
    end
end

image((map==0).*0 + (map==1).*255 + (mapResized-map).*150);
colormap(gray(256))
%选出初始点和目标点
disp('请选择隔离区起点');
[x,y] = ginput(1);
source=[double(int8(y)) double(int8(x))];    % source position in Y, X format
disp('请选择病房门口');
[x,y] = 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(source,goal) 0+heuristic(source,goal) -1]; %A*算法的处理队列，开放列表
closed=ones(size(map)); % 黑色已标记存到闭列表
closedList=[]; % the closed list taken as a list
pathFound=false;
tic;
counter=0;

size(Q);

while size(Q,1)>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
     [rx,ry,rv]=find(conn==2); %连接矩阵中的机器人位置
     [mx,my,mv]=find(conn==1); %可能移动的数组
     for mxi=1:size(mx,1) %遍历所有的移动
         newPos=[n(1)+mx(mxi)-rx n(2)+my(mxi)-ry]; %可能的新节点
         if checkPath(n(1:2),newPos,map) %如果从n到newPos的路径是无碰撞的
              if closed(newPos(1),newPos(2))~=0 %还没有关闭
                  historicCost=n(3)+historic(n(1:2),newPos);
                  heuristicCost=heuristic(newPos,goal);
                  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(I,5)<totalCost, add=false;
                      else Q=[Q(1:I-1,:);Q(I+1:end,:);];add=true;
                      end
                  end
                  if add
                      Q=[Q;newPos historicCost heuristicCost totalCost size(closedList,1)+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(i0,i1) == 0) 
         temp_img = (map==0).*0 + ((closed==0).*(map==1)).*125 + ((closed==1).*(map==1)).*255 + (mapResized - map).*100 ;
         
         %画出目标和起始点
         temp_img(goal(1), goal(2) ) = 160;
            temp_img(source(1), source(2) ) = 160;
        
         image(temp_img);
       
        M(counter)=getframe;
     end
     
     size(Q);
end

if ~pathFound
    error('no path found')
end

%% Plot complete path绘制完整路线
figure('Name','最终路径展示')
path=[n(1:2)]; %从源信息检索路径
prev=n(6);
while prev>0
    path=[closedList(prev,1:2);path];
    prev=closedList(prev,6);
end
path=[(path(:,1)*size(mapOriginal,1))/resolutionX (path(:,2)*size(mapOriginal,2))/resolutionY];
pathLength=0;
for i=1:length(path)-1, pathLength=pathLength+historic(path(i,:),path(i+1,:)); end
fprintf('计算时间=%d \n路径长度=%d \n\n', toc,pathLength);

imshow(mapOriginal);
line(path(:,2),path(:,1));
text(path(1,2),path(1,1),'隔离区','Color','green');
s=size(path);
text(path(s(1),2),path(s(1),1),'病房','Color','red');

2.主要过程

（1）导入地图
（2）选择起点
（3）选择终点

3.总体效果

A-start路径规划

---

总结

资源链接：
https://download.csdn.net/download/qq_27663847/85465826