路径规划算法实现原理

前言

算法原理：参考路径规划算法学习Day1

此方法会结合网络占用法-栅格法来进行实现

提示：

本文会用matlab实现Dijkstra算法，并且会分享一些函数用法的链接，也是本人学习得来，供大家参考，批评指正。

1、Dijkstra算法

1.1、地图创建

总所周知:栅格法生成地图常规是的自己一个一个打，这样既麻烦还浪费时间。

这里介绍几种方法：

way1:在命令框中码：map=rand(k)>0.7 %k代表多少维地图

way2:在matlab中安装Robotics Toolbox工具箱 里有专门的函数makemap可以帮助我们生成一张地图

1.2、matlab实现

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;%free
map(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_list
destination_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
      end
end


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;
 end
else
  path=[];
end
end

1.3、20*20地图

1.4、50*50地图

gif太大无法上传，后面我会完善。

主要就是想对比一下，可以让大家看到迪杰斯特拉算法的缺点

2、A*（Astar）算法

2.1、原理

A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。

算法中的距离估算值与实际值越接近，最终搜索速度越快。

公式表示为：f(n)=g(n)+h(n)。

其中：

f(n) ：是从初始状态经由状态n到目标状态的代价估计，

g(n)：是在状态空间中从初始状态到状态n的实际代价，

h(n)：是从状态n到目标状态的最佳路径的估计代价。

对于路径搜索问题，状态就是图中的节点，代价就是距离

h(n)的选取保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取（或者说h(n)的选取）。

我们以d(n)表达状态n到目标状态的距离，那么h(n)的选取大致有如下三种情况：

如果h(n)< d(n)到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。

如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的。

如果 h(n)>d(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

A* 算法是在迪杰斯特拉算法的基础上进行改进的一种算法。

与之不同的是，A算法是一种启发式搜索，不会像dijkstra算法一样对整个地图都进行遍历，A算法是有方向的遍历。

2.2、启发式搜索

启发式搜索(Heuristically Search)又称为有信息搜索(Informed Search)。

它是利用问题拥有的启发信息来引导搜索，达到减少搜索范围、降低问题复杂度的目的。

这种利用启发信息的搜索过程称为启发式搜索。

这种搜索方式优点是搜索快，提高了效率，缺点就是得到的解有可能是次优解也有可能什么都得不到。

一句话就是牺牲了精度得到了效率。

3、总结

Dijkstra与A* 对比

相同点：

两者都是以寻找最短路径为目的的算法。

不同点：

Dijkstra算法遍历的时候是对4周平等对待，没有区分的盲目进行遍历。

A* 算法是在迪杰斯特拉算法的基础上进行改进的一种算法。

与之不同的是，A* 算法是一种启发式搜索，不会像dijkstra算法一样对整个地图都进行遍历，A* 算法是有方向的遍历。

它会对周围各点进行评估，然后再进行搜索。

后续程序依旧是基于栅格进行，用matlab实现

审核编辑：汤梓红