路径规划算法学习

function path=DJS(Map,origin,destination)cmap = [1 1 1; ...white        0 0 0; ...black        0 1 0; ...green        1 1 0; ...yellow        1 0 0; ...red        0 0 1; ...blue        0 1 1];    colormap(cmap);%map visualization 
[rows, cols]=size(Map);logical_map = logical(Map);map=zeros(rows, cols);map(~logical_map)=1;%freemap(logical_map)=2;%obstacle%定义一个变量node_cost_list来保存邻居以及它们到起始格的路程%node_cost_list来保存这些信息，初始化为 Inf，表示从没有访问过。一旦有值，就说明是邻居，赋值的大小就表示该点跟起始点的路程。一旦变成红色，就把它的值再改回 Inf。node_cost_list = Inf(rows, cols);node_cost_list(origin(1),origin(2))=0;% set the node_cost of the origin node zero%定义变量parent_list来保存路径parent_list=zeros(rows, cols);% create parent_listdestination_index=sub2ind(size(Map),destination(1),destination(2));origin_index=sub2ind(size(Map),origin(1),origin(2));
plan_success=false;while true    map(origin(1),origin(2))=3;            map(destination(1),destination(2))=4;
            image(0.5,0.5,map);            grid on;            set(gca,'xtick',1:1:rows);            set(gca,'ytick',1:1:cols);            axis image;            drawnow;            %找出距离最小的节点                      %搜索中心与起始点的路程min_node,搜索中心的索引坐标：current_node,           [min_node,current_node]=min(node_cost_list(:));           if(min_node == inf || current_node == destination_index)               plan_success=true;               break;           end           node_cost_list(current_node) = inf;%当前搜索中心这个位置赋值为 Inf，表示它已经当过搜索中心了。min函数就不会再找这个位置           map(current_node) = 5;           [i,j]=ind2sub(size(Map),current_node);           for k = 0:3  % four direction                if(k == 0)                    adjacent_node = [i-1,j];                     elseif (k == 1)                    adjacent_node = [i+1,j];                    elseif (k == 2)                        adjacent_node = [i,j-1];                    elseif(k == 3)                    adjacent_node = [i,j+1];                end                if((adjacent_node(1)>0 && adjacent_node(1)<=rows) && (adjacent_node(2)>0 &&adjacent_node(2)<=cols))                    if(map(adjacent_node(1),adjacent_node(2)) ~= 2 && map(adjacent_node(1),adjacent_node(2)) ~= 5)                       if(node_cost_list(adjacent_node(1),adjacent_node(2)) > min_node + 1)                          node_cost_list(adjacent_node(1),adjacent_node(2)) = min_node + 1;                          if(map(adjacent_node(1),adjacent_node(2)) == 3)                             parent_list(adjacent_node(1),adjacent_node(2)) = 0;%如果相邻节点是原点，则将父节点设置为0。                          else                             parent_list(adjacent_node(1),adjacent_node(2))=current_node;%否则设置当前节点为父节点                          end                          map(adjacent_node(1),adjacent_node(2)) = 6;                       end                    end                end           endend
if(plan_success)  path=[];  node=destination_index;  while parent_list(node)~=0    path=[parent_list(node),path];    node=parent_list(node);  end  for k = 2:size(path,2)    map(path(k)) = 7;    image(0.5,0.5,map);    grid on;    set(gca,'xtick',1:1:rows);    set(gca,'ytick',1:1:cols);    axis image;    drawnow;  endelse   path=[];endend