路径规划——图搜索算法（DFS、BFS、迪杰斯特拉、A*）

路径规划——图搜索算法

参考资料来源：A*算法原文、高飞老师的《移动机器人规划》课程、Steven M.LaValle的《planning algorithms》、csdn和知乎上的笔记摘要。

注：这里指的路径规划，指的是不包括时间信息的路径（并非轨迹）。

1 Configuration Space

• 机器人描述
  机器人的构型空间（Configuration Space）是指描述机器人所有可能构型的空间，它的维度等于机器人自由度的个数。例如 在一个平面上的移动机器人，它的构型可以由平面内的x坐标、y坐标和朝向角度来描述，因此构型空间就是一个三维空间。所以机器人的pose 都可以视作构型空间当中的一个点。

• 路径规划实质
  路径规划就是寻找一个从起点到终点的较优path在C-free空间上 (C-space = C-obstacle + C-free)

注：较优并非最优，是因为在高维空间中，找到一条可行路径都已经很困难，所以最优更加困难

2 Graph and Search Method

经典算法有DFS、PFS、Dijkstra、A*等算法，其都符合下述框架，各种算法的明显区别在于采用特定的优先级函数对优先级队列Q进行排序。

假设队列Q是一个FIFO(先进先出队列)。初始时Q包含整个初始状态XI。然后执行while循环，当Q为空时跳出循环(这种情况发生在已经探索整个图而没有发现任何目标状态时，导致失败FAILURE)。在每次while迭代中，Q.GetFirst(根据先进先出原则，Q中最靠前的元素x将被删除)。如果x在Xg中，输出SUCCESS,并停止搜索过程。否则会尝试各个可能的行动u.对下一个状态x’=f(x,u),必须确定是不是第一次访问x’。如果它还没有被访问过，那么将他插入队列Q中，否则就不需要考虑他(要么是不活动的，要么已经在队列Q中)。

注：这是一个通用性模板，大家在学习不同算法中，请关注优先级函数的区别

2.1 BFS(广度搜索，先进先出原则)

BFS 算法流程（先进先出原则）
1.初始化：将初始节点S加入到优先级队列Q中
2.while (Q~=0)
3.  删除最排队时间最长的节点（先进来的排队时间最长）
4.  IF 删除节点为目标节点
5.    return sucesss
6.  else
7.  寻找被删除节点的邻居，将未被访问的邻居加入到Q中
8.end
注：未发现目标节点返回FAILURE

对于一个拓扑图，我们可以根据枚举的方式找到类似上图中树的结构。不难发现BFS算法的搜索过程在树中是一层一层搜索。

推荐一个图搜索算法的可视化网站http://qiao.github.io/PathFinding.js/visual/,可以发现BFS搜索过程边界一致增长。

2.2 DFS（深度搜索，先进后出原则）

DFS 算法流程（先进后出原则）
1.初始化：将初始节点S加入到优先级队列Q中
2.while (Q~=0)
3.  删除最排队时间最短的节点（注意与BFS的区别）
4.  IF 删除节点为目标节点
5.    return sucesss
6.  else
7.  寻找被删除节点的邻居，将未被访问的邻居加入到Q中
8.end
注：未发现目标节点返回FAILURE

对于一个拓扑图，我们可以根据枚举的方式找到类似上图中树的结构。不难发现DFS算法的搜索过程在树中是深层搜索。

2.3 Dijkstra and A*算法

A*算法
输入：start,goal,map
输出：path
初始化：将起点加入到 open list 中
while true
    if open list == empty
        return flase;
        break;
    else 
        将 h(n)+g(n) 最小的节点弹出 // 将启发式函数置0，即退化到dijkstra算法中
        并将其加入到 closed list 中
    if 节点是终点
        return true;
        break;
    else
        遍历该节点的所有非 closed list 邻居
            if 邻居位于 open list 中
                更新 g(n) 值 // （g(n)）为当前时刻n距离起点的最小coset
            else
                计算节点 g(n) 值，并加入到 open list 中 
end

注：h(n)+g(n)为A*算法的优先级函数，另外将启发式函数h(n)置0，即退化到dijkstra算法

迪杰斯特拉图示：

A*算法图示：

相较于迪杰斯特拉算法，A* 再保证最优性的同时，拓展更少的节点。但是如果空间维数变多，A* 算法依旧需要花费很长的时间，可以考虑采用基于采样的路径搜索方法。

补充一下A*原文结论：

• The function $\hat{h}$ can be used to tailor A * for particular applications.(对于不同的应用场景，可以设计一个恰当的 $\hat{h}$ 来定制 A* 算法)
• 满足$\hat{h}<h$时（$\hat{h}$指的就是伪代码中的启发式函数，这里我和论文保持一致，大家不要搞混），A*算法求出的路径就是最短的。（同时$\hat{h}$越逼近$h$时，求解效率是越高的）

同时在这里给大家，推荐一个giuhub项目（包含各种全局和局部规划算法）学习，下图为A*算法在ros下的执行效果https://github.com/ai-winter/ros_motion_planning

对ros不熟悉的小伙伴，也可以参考下面这个github项目（包含各种全局和局部算法），MATLAB实现多种规划算法https://github.com/ai-winter/matlab_motion_planning