matlab编程经验记录（Dijkstar和A*

前言

以实战中的程序来直接学习matlab编程语言
把Dikstar变成A*只要再加个到目标的代价就可以了，相关提示在代码中给了

笔记

1、如何批量地行或列进行以增量形式的初始化

U(:,1)=1:row*col;%第一列表示行列号索引，第二列表示从源节点到该节点的距离代价；
 **变一维的作用是好找邻居节点，前后加减4即是所有邻居节点**
U(:,2)=inf;

2、在搜索算法中存在opening 集和 closed 集，如何快速将点存入和取出。来实现opening集作为一个优先级队列去维护的效果。
3、从U集合中删除掉StartPoint一行，剩下的顺序上移

U(StartPoint,:)=[];  

4、邻节点的代价更新，是找到路径的操作对象
5、 ind2sub函数——是将线性索引转换为下标。
https://blog.csdn.net/jk_101/article/details/110948383
6、sub2ind函数——sub2ind该函数用于把数组或矩阵中元素的全下标转化为单下标
https://blog.csdn.net/jk_101/article/details/110948646
7、地图生成代码解析
①障碍物的生成思路——依据地图大小和障碍物在其中的占比随机生成；缺点是不保证有解

obsRate=0.4;                           %障碍物生成概率
obsNum=floor(row*col*obsRate); %转化成障碍物数量

obsIndex=randi([1,row*col],obsNum,1); %确定了障碍物在地图中的位置索引
% r = randi([-10 10],100,1);%1是一列数字(可以理解成一组这样的随机数)，数量为100个，范围是[-10,10]；

②障碍物生成,用的这个colormap不是很理解，

cmap=[1 1 1;...%白色 空地
    0 0 0;...  %黑色 障碍
    1 0 0;...  %红色 动态障碍
    1 1 0;...  %黄色 起点
    1 0 1;...  %品红 终点
    0.1 0.7 0.1;...  %绿色 规划路径
    0 1 1;];   %青色 重规划路径
colormap(cmap);

8、在邻居节点寻找完成后(代价值确定)，如何在全部节点集合中进行替换？——用一维的index进行匹配

index=find(U(:,1) == ChildNode);%==左右两边都是一维下的索引值， 因为会U中会删除节点，行号 和对应的一维化后的原index会不一样，必须find一次确定行号

9、最优路径的存储用的是cell

path=cell(row*col,2); %path为最优路径集合
%cell是matlab中的一种数据类型，用大括号定义，括号里可以是任意类型的数据或矩阵。

10、ismember函数主要用于判断某个元素是不是在集合数组中，返回值为1和0

if ~isinf(ChildNode) && ~ismember(ChildNode,S)     %该子节点存在 且 未被选为最优的点
index_ChildNode_In_U=find(ChildNode==U(:,1));  %找到该子节点的索引值

11、其实也是动态规划的思想

           if cost_min + cost < U(index_ChildNode_In_U,2) %如果该子节点在当前父节点的前提下，代价值=cost_min【父节点代价值】+cost【父节点到子节点的代价值】 < U中的记录值，则说明该路径比当前的更好，进行替换
               U(index_ChildNode_In_U,2)=cost_min + cost;

以下是完整代码（来自我的师兄——杭哥

1、Dijkstar.m

close all
clear
clc
row=10;
col=20;
StartPoint=2;
GoalPoint=row*col-3;
field=field_Generate(row,col,StartPoint,GoalPoint);
S=[];%原始节点
U=[];%未遍历节点
% image(1.5,1.5,field)
% grid on 
% set(gca,'gridline','-','gridcolor','k','LineWidth',2,'GridAlpha',0.5);
% set(gca,'xtick',1:col+1,'ytick',1:row+1)
% axis image
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   初始化   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  S和U的第一列表示行列号索引，第二列表示从源节点到该节点的距离代价
%  其中S是已经得到的最小代价，不会再变
%  U是暂时的得到的最小代价，可能会变更
U(:,1)=1:row*col;
U(:,2)=inf;
S=[StartPoint,0];       %将起始点加入S集合
U(StartPoint,:)=[];     %从U集合中删除掉StartPoint一行，剩下的顺序上移

NeighborNodes=Get_Neighborhood(field,StartPoint,row,col);
%%%%%%%%%%%%% NeighborNode意义 %%%%%%%%%%%%%  
%左上(1,:)    上(4,:)   右上(6,:)     
% 左 (2,:)    检测点     右 (7,:)
%左下(3,:)    下(5,:)   右下(8,:)
% 可以自己定义每行代表哪个周围的点
%%%%%%%%%%%%% NeighborNode意义 %%%%%%%%%%%%%
%%%%%%%%%%%%% 更新起始点的相邻节点代价值 %%%%%%%%%%%%%
for i=1:8
    ChildNode=NeighborNodes(i,1);
    if  ~isinf(ChildNode) %是否有相应位置的子节点 不是inf则存在
        index=find(U(:,1) == ChildNode);  % 因为会删除节点，行号和对应的一维化后的位置会不一样，必须find一次确定行号
        U(index,2)=NeighborNodes(i,2);    
    end
end  %现在U在排除了起始点后，更新了和起始点相邻的代价值
%%%%%%%%%%%%% 更新起始点的相邻节点代价值 %%%%%%%%%%%%%

%%%%%%%%%%%%% 更新S集合的最优路径集合 %%%%%%%%%%%%%
path=cell(row*col,2); %path为最优路径集合
for i=1:row*col
    path{i,1}=i;
end

for i=1:8
    ChildNode=NeighborNodes(i,1);
    if ~isinf(NeighborNodes(i,2)) %不存在的点 (i,2)肯定是inf 存在的点 (i,2)可能是inf 所以再剔除一遍
        path{ChildNode,2}=[StartPoint ChildNode];
    end
end
%%%%%%%%%%%%% 更新S集合的最优路径集合 %%%%%%%%%%%%%
%% 循环遍历
while ~isempty(U)
   [cost_min,index]=min(U(:,2));      %返回当前的最小值 和 对应的行数索引
   ParentNode=U(index,1);             %作为下次搜索的父节点
   S(end+1,:)=[ParentNode, cost_min]; %将该最小值的节点放入S集合 
   U(index,:)=[];                     %然后在U中删除该最小值节点
%    if ParentNode==89
%        disp('nn')
%        pause
%    end
   % 获得当前父节点的所有邻居节点
   NeighborNodes=Get_Neighborhood(field,ParentNode,row,col);
   
   %遍历该父节点的所有邻居子节点，判断是否在U集合(未遍历的点）中，并更新对应点的代价值
   for i=1:8
       ChildNode= NeighborNodes(i,1);
       cost=NeighborNodes(i,2);
       if ~isinf(ChildNode) && ~ismember(ChildNode,S)     %该子节点存在 且 未被选为最优的点
           index_ChildNode_In_U=find(ChildNode==U(:,1));  %找到该子节点的索引值
           if cost_min + cost < U(index_ChildNode_In_U,2) %如果该子节点在当前父节点的前提下，代价值=cost_min【父节点代价值】+cost【父节点到子节点的代价值】 < U中的记录值，则说明该路径比当前的更好，进行替换
               U(index_ChildNode_In_U,2)=cost_min + cost;
               path{ChildNode,2}=[path{ParentNode,2},ChildNode]; %更新到该子节点的最优路径
           end
       end
   end
end
%% %画出规划的路径
path_opt=path{GoalPoint,2};
field(path_opt(2:end-1))=6; %选择涂的颜色，在field_Generate.m函数中定义的

image(1.5,1.5,field)
grid on 
set(gca,'gridline','-','gridcolor','k','LineWidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:col+1,'ytick',1:row+1)
axis image

2、 Get_Neighborhood.m

function NeighborNodes=Get_Neighborhood(field,Index,rows,cols)
MartixSize=[rows,cols];
[row,col]=ind2sub(MartixSize,Index);
SourceNode=[row,col];
%rows cols 告诉函数矩阵的大小，Index是二维变一维后的行列索引 变一维的作用是好找邻居节点，前后加减4即是所有邻居节点

NeighborNodes=inf(8,2);%先初始化一个NeighborNodes 假设都是障碍物或者不存在，这样在下面就不用再重复写障碍物或不存在判断后的cost的else部分了

%如果只有四个移动方向只要注释掉其他四个方向即可

%左上节点
if row-1>0 && col-1>0  %存在左上节点，否则不存在
    child_node_sub=[row-1, col-1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(1,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);  %如果要改成A*只要在此处加上“norm(child_node_sub-index_goal”,当然需要自己定义目标的位置index_goal并把它加入该函数的变量之中
        NeighborNodes(1,2)=cost;
    end
end

%左节点
if col-1>0   %存在左节点，否则不存在
    child_node_sub=[row, col-1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(2,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(2,2)=cost;
    end
end

%左下节点
if row+1<=rows && col-1>0  %存在左下节点，否则不存在
    child_node_sub=[row+1, col-1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(3,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(3,2)=cost;
    end
end

%上节点
if row-1>0   %存在上节点，否则不存在
    child_node_sub=[row-1, col];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(4,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(4,2)=cost;
    end
end

%下节点
if row+1<=rows   %存在下节点，否则不存在
    child_node_sub=[row+1, col];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(5,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(5,2)=cost;
    end
end

%右上节点
if row-1>0 && col+1<=cols  %存在右上节点，否则不存在
    child_node_sub=[row-1, col+1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(6,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(6,2)=cost;
    end
end

%右节点
if col+1<=cols  %存在右节点，否则不存在
    child_node_sub=[row, col+1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(7,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(7,2)=cost;
    end
end

%右下节点
if row+1<=rows && col+1<=cols  %存在右下节点，否则不存在
    child_node_sub=[row+1, col+1];
    child_node_ind=sub2ind(MartixSize,child_node_sub(1),child_node_sub(2));%转换成1维的索引
    NeighborNodes(8,1)=child_node_ind;
    if field(child_node_sub(1),child_node_sub(2))~=2
        cost=norm(child_node_sub-SourceNode);
        NeighborNodes(8,2)=cost;
    end
end

3、field_Generated.m

function field=field_Generate(row,col,StartPoint,GoalPoint)
%地图生成程序
%设置地图色块颜色
cmap=[1 1 1;...%白色 空地
    0 0 0;...  %黑色 障碍
    1 0 0;...  %红色 动态障碍
    1 1 0;...  %黄色 起点
    1 0 1;...  %品红 终点
    0 1 0;...  %绿色 规划路径
    0 1 1;];   %青色 重规划路径
colormap(cmap);

field=ones(row,col);
%生成障碍物
obsRate=0.5;                           %障碍物生成概率
obsNum=floor(row*col*obsRate);
obsIndex=randi([1,row*col],obsNum,1);
field(obsIndex)=2;                     %matlab会自己转换,从上往下数

%生成起始点，终点

field(StartPoint)=4;%和上面颜色对应
field(GoalPoint)=5; %和上面颜色对应