motion planing相关_dwa mpc

---

路径规划有DFS、BFS、Dijkstra、A*及各种变种、PRM、Lattice、DWA、MPC、PID等全局/局部规划算法

部分路径规划算法

路径规划算法包括基于搜索和基于采样的规划算法
基于搜索的路径规划算法已经较为成熟且得到了广泛应用，常常被用于游戏中人物和移动机器人的路径规划。
与基于搜索不同，基于采样的路径规划算法不需要显式构建整个配置空间和边界，并且在高维度的规划问题中得到广泛应用。

---

BFS/DFS

BFS(广度优先搜索) 和 DFS（深度优先搜索）
广度优先遍历图的方式为，一次性访问当前顶点的所有未访问状态相邻顶点，并依次对每个相邻顶点执行同样处理。因为要依次对每个相邻顶点执行同样的广度优先访问操作，所以需要借助队列结构来存储当前顶点的相邻顶点。（队结构，先入先出，类似树的层次遍历）
深度优先遍历图的方式，同样会访问一个顶点的所有相邻顶点，不过深度优先的方式为，首先访问一个相邻顶点，并继续访问该相邻顶点的一个相邻顶点，重复执行直到当前正在被访问的顶点出度为零，或者不存在未访问状态的相邻顶点，则回退到上一个顶点继续按照该深度优先方式访问。因为存在回溯行为，所以需要借助栈结构保存顶点，或者直接利用递归调用产生的方法栈帧来完成回溯。(栈结构，先入后出，类似树的后序遍历)
当前路径规划相关的图搜索方法大多是基于广度优先搜索的原理来进行拓展的

BFS实现
选择起点放入队列中，并标记为已访问
判断队列是否为空，当队列不为空时，从队列中取出顶点作为拓展点，随后将拓展点的所有相邻且未被访问过的节点放入队列，并标记为已访问；
重复步骤2直至队列为空或搜索到目标点

DFS实现
选择起点放入栈中，并标记为已访问
判断栈是否为空，当栈不为空时，将栈顶元素作为拓展点，随后搜索拓展点的相邻且未被访问过的节点，若存在，则将该节点入栈，否则则将拓展点出栈
重复步骤2 直至栈为空或者搜索到目标点

由于BFS以及DFS都是较为基础的搜索算法，因此是较难满足无人车或者机器人的路径规划需求的。因此在此思想基础上，衍生出了众多基于栅格地图的搜索算法，比如Dijkstra以及A*等

缺点：太简单太基础了，满足不了复杂的场景需求

Dijkstra

迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。

通过Dijkstra计算图G中的最短路径时，需要指定起点vs(即从顶点vs开始计算)。此外，引进两个集合S和U。S的作用是记录已求出最短路径的顶点(以及相应的最短路径长度)，而U则是记录还未求出最短路径的顶点(以及该顶点到起点vs的距离)。初始时，S中只有起点vs；U中是除vs之外的顶点，并且U中顶点的路径是"起点vs到该顶点的路径"。然后，从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。 然后，再从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。重复该操作，直到遍历完所有顶点。

先给出一个无向图

用Dijkstra算法找出以A为起点的单源最短路径步骤如下：

缺点：效率低 毫无方向的向四周探索、

# Dijkstra.狄杰斯特拉
import heapq
import math

def init_distance(graph, s):
    distance = {s: 0}
    for vertex in graph:
        if vertex != s:
            distance[vertex] = math.inf
    return distance


def dijkstra(graph, s):
    pqueue = []
    heapq.heappush(pqueue, (0, s))
    seen = set()
    parent = {s: None}
    distance = init_distance(graph, s)

    while len(pqueue) > 0:
        pair = heapq.heappop(pqueue)
        dist = pair[0]
        vertex = pair[1]
        seen.add(s)
        nodes = graph[vertex].keys()
        for w in nodes:
            if w not in seen:
                if dist + graph[vertex][w] < distance[w]:
                    heapq.heappush(pqueue, (dist + graph[vertex][w], w))
                    parent[w] = vertex
                    distance[w] = dist + graph[vertex][w]
    return parent, distance


if __name__ == '__main__':
    graph_dict = {
        "A": {"B": 5, "C": 1},
        "B": {"A": 5, "C": 2, "D": 1},
        "C": {"A": 1, "B": 2, "D": 4, "E": 8},
        "D": {"B": 1, "C": 4, "E": 3, "F": 6},
        "E": {"C": 8, "D": 3},
        "F": {"D": 6},
    }

    parent_dict, distance_dict = dijkstra(graph_dict, "A")
    print(parent_dict)
    print(distance_dict)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

A*

A算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。
A算法作为Dijkstra算法的扩展，因其高效性而被广泛应用于寻路及图的遍历，如星际争霸等游戏中就大量使用。

A*算法的几个概念：
搜索区域（The Search Area）：图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，当然我们也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。　　
开放列表(Open List)：我们将路径规划过程中待检测的节点存放于Open List中，而已检测过的格子则存放于Close List中。
父节点（parent）：在路径规划中用于回溯的节点，开发时可考虑为双向链表结构中的父结点指针。
路径排序（Path Sorting）：具体往哪个节点移动由以下公式确定：F(n) = G + H 。G代表的是从初始位置A沿着已生成的路径到指定待检测格子的移动开销。H指定待测格子到目标节点B的估计移动开销。
启发函数（Heuristics Function）：H为启发函数，也被认为是一种试探，由于在找到唯一路径前，我们不确定在前面会出现什么障碍物，因此用了一种计算H的算法，具体根据实际场景决定。在我们简化的模型中，H采用的是传统的曼哈顿距离（Manhattan Distance），也就是横纵向走的距离之和。

下图所示，绿色方块为机器人起始位置A，红色方块为目标位置B，蓝色为障碍物


对于几何路网来说，可以取两节点间曼哈顿距离做为距离估计，即f=g(n) + (abs(dx - nx) + abs(dy - ny))；这样估价函数f(n)在g(n)一定的情况下，会或多或少的受距离估计值h(n)的制约，节点距目标点近，h值小，f值相对就小，能保证最短路的搜索向终点的方向进行。明显优于Dijkstra算法的毫无方向的向四周搜索。

缺点：路径不平滑、当搜索图很大时计算开销也很大，不过它作为一个基础有很多变种来解决一些问题

hybridA*

来自于论文 Practical_Search_Techniques_in_Path_Planning_for_Autonomous_Driving
该算法解决的问题是：假设无人车有充分的感知和定位能力，则其能够在线重新规划生成障碍物地图，并且能够在未知环境中行驶。还有满足动力学约束（带转弯的规划）

hybridA其实是对传统的A 算法进行改进，与之不同的是，hybrid A* 是在连续坐标系下进行启发式搜索，并且能够保证生成的轨迹满足车辆非完整性约束，但算法运行过程中该路径并不一定是全局最优的，尽管如此，这条路径是在全局最优解的“附近”。下图可以很好地解释传统A* 与hyvbrid A* 的异同。两种算法都是基于网格世界的（grid world），A* 是赋予每个网格的中心点相应的损失并且算法只访问这些中心点，而hybrid A* 是先在这些网格中挑选满足车辆3D连续状态的点，并将损失赋值给这些点。


论文中称节点扩张的过程为Analytic Expansions。首先状态的表示为：x=(x,y,θ)，其中(x,y)是节点的位置，而θ是节点的朝向（heading），在前文所述的搜索过程中，使用到的是离散的控制动作（control actions, or steering），因此之前每个网格中的连续状态点是不可达的。为了进一步改进搜索速度和提高准确度，这就可以利用Reed-Shepp曲线。A* 的搜索过程都是用直线相连接，而hybrid A* 则是在与网格精度一致的前提下（对应某一小段时间）使用三种控制动作：最大左转，最大右转，不转向来生成路径，因此该路径是一些受车辆转弯半径约束的圆弧和直线。

以上过程会生成一些子节点，在此基础上，假设不考虑环境（对应第一个启发函数），算法会通过计算从起点到终点的最优Reed-Shepp曲线的方式，再生成一个额外的子节点；之后算法基于现有的障碍物地图对该路径进行碰撞检测，无碰撞路径对应的点会加到扩张树中。可以看出与直线相比，Reed-Shepp曲线的计算量是很大的。所以论文中作者使用简单的selection rule，在每N个节点中选取一个计算Reed-Shepp曲线（这里的N随启发函数递减而减少，即越发靠近终点时，N越小）。

hybrid A* 算法伪代码：
与A*算法类似，算法也是维护两个列表，一个open list, 一个是closed list。算法的结束条件是：open list为空或者已经搜索到终点。Line 21:算法不一定会准确搜索到终点，因此引入RoundState函数，在判断当前节点是否到达终点之前对此进行估算。如果没有达到终点，算法会通过执行动作空间中的所有动作对路径节点进行扩张。如果生成的节点不在C中（也就是没有被算法遍历过），则计算cost-so-far；如果生成的节点不在O中（已被遍历过）或者所得到的cost-so-far小于当前节点已有的cost，这时用当前得到的较小的cost更新cost-so-far


缺点：转弯还是不够自由、增加自由度和精确度时计算开销更大

人工势场法

人工势场法（Artifical Potential Field）路径规划是由Khatib提出的一种虚拟力法（Oussama Khatib，Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1985 IEEE.）。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。

人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动（这一点有点类似于A*算法中的启发函数h）。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。

我们利用势函数U来建立人工势场。势（场）函数是一种可微函数，空间中某点处势函数值的大小，代表了该点的势场强度。最简单的势函数是引力/斥力势函数。其作用思路很简单：让目标对机器人产生吸引力，障碍物对机器人产生排斥力。

缺点：单独做规划容易陷入局部最优或到不了目标，一般配合其他算法一起用

D*

D是动态A（Dynamic A Star），卡内及梅隆机器人中心的Stentz在1994和1995年两篇文章提出，主要用于机器人探路。是火星探测器采用的寻路算法。 D是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。

D算法在动态环境中寻路非常有效，向目标点移动中，只检查最短路径上下一节点或临近节点的变化情况，如机器人寻路等情况，属于局部路径规划
它适用于动态环境中的路径规划（motion planning)，与适用于静态环境的Dijkstra、A 形成对比

D* 的实现过程中有和Dijkstra、A* 相似的思想，在初始路径规划的过程中，是一个反向Dijkstra算法，在遇障碍重新规划的过程中，可看做是一个启发式算法A*，只是起点是一些不固定的点

同A算法类似，D通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D*不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以它是一个动态寻路算法）。

缺点：不适用于距离远的路径规划

DWA

DWA（dynamic window approach）算法是用于局部路径规划的算法，常常在ROS中实现，局部路径规划器使用的最广的就是DWA

DWA算法其原理主要是在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的（v,w）驱动机器人运动。

对于无法预测的动态障碍物，DWA算法可以较好地解决动态避障这个问题。DWA算法的优点是考虑到速度和加速度的限制，同时只有无碰撞的轨迹会被考虑，因此，采样的速度即形成了一个动态窗口。在这个动态窗口中又很多条可行轨迹，通过自行设计一个评价函数，挑选出一条最优轨迹，经过重复迭代，不断地搜索当前最优轨迹，最终到达目标点

动态窗口法 DWA 的实现包含两个步骤：
（1）对机器人速度进行约束限制，形成动态窗口进行速度采样；
（2）根据速度采样点求出待评价轨迹，最大化评价函数选取最优速度命令。
动态窗口是由一系列的约束构成，其中约束主要包括差动机器人的非完整约束、环境障碍物约束与受结构与电机影响的动力学约束。

此模型中大多计算是关于ROS动力学的限制和约束，目前我还没到深究的程度，所以省略

它计算复杂度低，考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小

缺点：不能很好的避障比如碰到碗状、每次都选择下一步的最佳路径，而非全局最优路径

RRT

快速探索随机树(Rapidly-exploring Random Tree，RRT)是最流行路径规划算法之一
它是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，用于有效地搜索非凸的(Non Convex)高维度的空间，特别适用于包含障碍物和非完整(Non-Holonomic)系统或反向动力学(Kino-dynamic)微分约束条件下的动作规划。

RRT 的特点是算法建构简单，并且可以快速遍历空间的未探索区域，而通过手动设定找到实现这种效果的函数却是很难的。同时RRT 可以在基本算法基础上，对空间的搜索加入带有输入参数的控制函数，使算法可以适用于不同约束条件下的路径搜索。
RRT是一种在完全已知的环境中通过采样扩展搜索的算法，相比较于基于图的搜索算法，最主要的优点就是快，因此在多自由度机器人的规划问题中发挥着较大的作用，比如机械臂的规划算法基本都是以RRT为基础的。但同时他也有比较明显的缺点，比如通常不最优、规划的路径非常不平滑等。但这些缺点的存在使得后面还有很多对RRT算法的改进

算法流程

1. 采样，从地图的无障碍区域采样一个随机位置。
2. 创建与随机位置相关联的节点。
3. 查找树中最接近随机位置的节点。
4. 计算一条从随机位置到节点位置的路径，这条路径在机器人上必须是可行的
   5.继续，如果路径与某物体碰撞，则继续进行下一次迭代。
   6.插入，将与随机位置关联的节点插入到树中，与其最接近的节点作为其父节点
   7.返回，一旦随机位置在目标位置的一定距离内，返回树。
   

缺点：可行路径而不是最优路径，RRT*只能渐进最优、全随机没有目标导向无效计算成本很大