机器人路径规划算法（Dijkstra和A*两种）附带matlab代码

摘要：本文针对机器人路径规划提出了两种算法，分析了基于栅格地图的Dijkstra算法和A算法的不同之处，通过栅格法对机器人运动环境进行建模,最后在Matlab上将Dijkstra算法和A算法进行仿真比较,对比他们的搜索速度、效率以及规划结果，来验证所分析的正确性。
关键词：栅格地图、Dijkstra、A*
一. 引言
路径规划算法的研究是移动机器人研究领域中一个重要的组成部分，它的目的是使移动机器人能够在一个已知或者未知的环境中，找到一条从其实状态到目标状态的无碰撞路径。传统的路径规划算法大部分只考虑机器人的位姿空间，然而，实际上机器人不仅受到位姿空间的约束，还会受到各种外力的约束。路径规划技术作为移动机器人的导航技术甚至是整个移动机器人技术的重要核心技术，也更加受到广大研究学者的关注。
最短路径问题是图论中网络分析的经典问题，近年来，随着路径搜索技术的不断发展，已经涌现出很多成熟的路径规划算法，静态地图搜索算法通常分为盲目搜索算法和启发式搜索算法。最经典的盲目搜索算法是Dijkstra 算法，非常适合在带权有向图中求解最短路径问题，但是由于Dijkstra算法的搜索范围很大，算法效率比较低，因此在实际应用时受到了很大的限制。A算法是一种启发式搜索,作为人工智能领域的重要组成部分，其针对网格数据有着更高的运算效率，而且利用启发式信息大幅度提高搜索速度。
本文针对在已经建立的栅格地图下，分别应用Dijkstra算法和A算法来求解机器人路径的解。分析各自的优缺点和性能，最后在Matlab平台上运行仿真，得出在某一个地图下的路径，验证分析的正确性。
二. 问题描述
移动机器人路径规划是指在一个未知的环境中，机器人根据任务寻找一条最优的运动轨迹，该轨迹可以连接起点和目标点，同时避开环境中的障碍物，归纳起来分为下面两个步骤：

1. 地图模型的建立：根据机器人运动的环境然后抽象建立起栅格地图、
2. 路径搜索算法：机器人路径规划主要涉及3大问题：①明确起点位置以及终点；②规避障碍物；③尽可能做到路径上的优化。本文将从Dijkstra和A*算法实现路径规划的问题。
   三. 算法介绍
   3.1 Dijkstra算法原理：
   Dijkstra算法采用的是一种贪心的策略，声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点集合：T，初始时，原点s的路径权重被赋予0，对顶点s能够到达直接到达的边（s，m），则把dis[m]设为w（s，m），同时把所有其他不能到达的顶点路径长度设为无穷大；同时初始时集合T只有s。然后，从dis数组选择最小值，则该值就是源点s到该值对应的顶点最短路径，并且把改点加入到T中，然后需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径是否比源点直接到达短，如果是，那么就替换这些顶点在dis中的值。然后，又从dis中找出最小值，重复上述动作，直到T中包含了图的所有顶点。
   3.2 Dijkstra算法步骤
   

3.3 A*算法原理：
搜索区域被划分为了简单的二维数组,数组中每个元素对应一个小方格,我们将路径规划过程中待检测的格子存放于Open List中,而已检测过的格子存放于Close List中。由F=G+H确定往哪一个格子移动，其中的G代表从初始位置A沿着已生成的路径到待检测的格子的开销，H指待检测格子到目标点的估计移动开销（忽略障碍物）。
3.4算法步骤：

1．把起始格添加到开启列表。
2．重复如下的工作：
a) 寻找开启列表中F值最低的格子。我们称它为当前格。
b) 把它切换到关闭列表。
c) 对相邻的8格中的每一个？

• 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
• 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
• 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
  d) 停止，当你把目标格添加进了开启列表，这时候路径被找到，或者没有找到目标格,开启列表已经空了.这时候，路径不存在。

3. 保存路径: 从目标格开始，沿着每一格的父节点移动直到回到起始格算法区别:Dijkstra算法计算源点到其他所有点的最短路径长度，A关注点到点的最短路径(包括具体路径)。
   四. 仿真：
   Dijkstra伪代码
   1.For each node n in the graph
   n.distance=Infinity
   2.Create an empty list.
   3.Start.distance=0,add start to list.
   4.While list not empty
   (1)Le