路径规划 python DQN 路径规划最新算法

运动规划

运动规划(Motion Planning)包括路径规划(Path Planning)和轨迹规划(Trajectory Planning)，通常情况下先进行路径规划，再进行轨迹规划。路径规划和轨迹规划的定义如下：

• 路径规划只考虑静态障碍环境生成的路径，属于空间路径
• 轨迹规划考虑了移动机器人本身的运动能力和中途可能的动态障碍，生成一段时间内的动作序列，在路径规划的基础上加入了时间信息，属于时空路径。

路径规划包括基于图搜索的路径规划算法和基于采样的路径规划算法，下面主要介绍基于图搜索的路径规划算法

基于图搜索的路径规划算法

图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的路径，包括深度优先和广度优先两个方向。

1.Dijkasta算法

该算法使用了广度优先搜索，解决赋权有向图或无向图的单源最短路径问题，算法最终得到一个最短路径树。

算法思路 将地图抽象为Graph数据结构，实际应用场景中，地图各个路径代表的Graph的边的权重不同，例如将距离长的边权重低、拥堵的权重低。算法采用贪心策略，声明了一个数组保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合T。

• 算法开始时，原点s的路径权重设置为0(dis[s]=0)，对于顶点s可以直接到达的边(s,m)，权重设置为dis[m]=w,同时把其他s不能直接到达的顶点路径长度设置为无穷大。
• 初始时集合T只有顶点s，然后从dis数组选择最小值，该值为原点s到该值对应顶点的最短路径，并且将该点加入到T中，此时完成一个顶点。
• 接下来，查看新加入的顶点是否可以到达其他顶点并且查看通过该顶点到达其他点的路径长度是否比源点直接到达短，如果是，则替换这些顶点在dis中的值。
• 然后，从dis中找出最小值，重复上述动作，直到T中包含了图的所有顶点。

代码示意，求如下图的最短路径，起始点为1：

程序输入为邻接矩阵，上图的邻接矩阵为：

def startwith(start: int, mgraph: list) -> list:
    """
    start-为出发点的索引，从0开始
    mgraph-邻接矩阵
    return 列表，每个元素值为start到对应下标点的最短距离
    """
    #passed表示已经过的点，存放已经确定最短距离的点
    passed = [start]
    #nopass存放还不确定最短距离的点
    nopass = [x for x in range(len(mgraph)) if x != start]
    dis = mgraph[start]
    
    while len(nopass):
        idx = nopass[0]
        for i in nopass:
            if dis[i] < dis[idx]: idx = i

        nopass.remove(idx)
        passed.append(idx)

        for i in nopass:
            if dis[idx] + mgraph[idx][i] < dis[i]: 
                dis[i] = dis[idx] + mgraph[idx][i]
    return dis
 
 if __name__ == "__main__":
    inf = 10086
    mgraph = [[0, 1, 12, inf, inf, inf],
              [inf, 0, 9, 3, inf, inf],
              [inf, inf, 0, inf, 5, inf],
              [inf, inf, 4, 0, 13, 15],
              [inf, inf, inf ,inf, 0, 4],
              [inf, inf, inf, inf ,inf, 0]]

    dis = startwith(0, mgraph)

运行程序最终得到结果[0,1,8,4,13,17]，分别为起始点1到节点1,2,3,4,5,6的最短路径。

2. A*算法

A*算法核心为估价函数的设计，$f(n)=g(n)+h(n)$，其中$g(n)$为耗散函数，表示从起始点到节点n的实际代价；$h(n)$为启发函数，表示节点n到目标节点$n_{goal}$的估计代价，$f(n)$ 表示起始节点经由节点n到目标节点的估计代价。

$g(n)$可以定义为移动代价与代价因子的乘积；$h(n)$的一个计算方式是曼哈顿距离，以栅格地图为例，可以表示为当前方格到目标方格的水平距离+当前方格到起始点的垂直距离。正因为可以通过预估$h(n)$降低走弯路的可能性，因此A被称为启发式算法。当$h(n)=0$时，A算法即为Dijksatra算法。

• 算法定义一个开放列表，用于记录所有可考虑选择的格子；定义一个封闭列表，用于记录所有不再考虑的格子；
• a 当开始点记录为P
• b 将当前点P放入封闭列表
• c 搜寻点P所有临近点，加入某临近点既没有在开放列表或封闭列表里面，计算出该邻近点的F值，并设置父节点为P，然后将其放入开放列表；
• d 判断开放列表是否已经空了，如果没有说明在达到结束点前已经找完了所有可能的路径点，寻路失败，算法结束；否则继续
• e 从开放列表拿出的一个F值最小的点，作为驯鹿路径的下一步
• f 判断该点是否为结束点，如果是寻路成功，算法结束；否则继续；
• g 将该点设为当前点P，跳回步骤c

基于栅格实现了A*算法演示，算法参考地址：Astar

3. Hybrid A*

Hybrid A Star与A Star相比，需要考虑物体的实际运动约束，但继承了A Star的思想，比如open表和close表，还有$g(n)$和$h(n)$，但花费定义比AStar定义复杂。例如$g(n)$，AStar仅定义了走过的距离，Hybrid AStar中考虑了很多其他因素，比如转向角的改变、是否倒车、车辆行驶方向是否改变等。Hybrid主要特点如下：

• 考虑物体的实际运动方向约束，不像AStar假定所有相邻节点都可以顺利专一
• AStar物体总是出现栅格中心，Hybrid AStar不一定
• Hybrid AStar为连续路径，使用了Reeds-Shepp曲线进行了路线规划
• 启发函数$h(n)$不同于AStar中的欧式距离，选择由AStar所得的到达终点的花费作为Hybrid AStar的预期花费

4. JPS算法

JPS是对A*算法的改进。

A*算法在扩展节点时会把节点的所有邻居考虑进去，导致open表中含有大量节点，搜索效率变慢。 JPS满足以下需求：

• 无遮挡条件下存在很多等价路径，在起点到终点仅仅取一个路径，该路径外的其他节点不放入open表中
• 直线方向上中途的点不放入open表，仅放入每段直线子路径的起点和终点 如图所示，JPS搜索到的节点为跳跃性的，这些节点为需要改变行走方向的关键节点，成为Jump Point。 JPS算法中包含强迫邻居和跳点两个概念，解释如下：

4.1强迫邻居

对于栅格地图中的某个节点x，共有8个邻居节点，假设其中存在障碍，且x的父节点p经过x到达n比不经过x到达n的任意路径的代价小，成为n为x的强迫邻居。如图所示，黑色为障碍物，红圈为强迫邻居。

4.2跳点

当节点满足一下三个条件之一时，为跳点：

• 当前节点为起点或终点
• 当前节点至少有一个强迫邻居
• 父节点在斜方向，当前节点的水平或垂直方向上有节点满足前两个条件。 如图所示，黄色节点的父节点在斜方向，右方向发现一个蓝色跳点，因此黄色节点应为跳点。(对于水平或垂直方向，(1,1)对应的水平方向为(1,0)，垂直方向为(0,1)。

4.3 JPS寻路算法

JPS步骤如下：

• 在open表中取一个权值最低的节点，开始搜索(与A*相同)
• 搜索时，先进性直线搜索(此时为跳跃搜索，沿着直线方向一直搜下去，直到搜到跳点或者障碍)，然后再斜向搜索（只搜索一步），如果期间某个方向搜索到跳点或者碰到障碍物，当前方向完成搜索，搜到跳点则加入open表
• 若斜方向未完成搜索，斜方向前进一步，重复上述过程
• 若所有方向均已完成搜索，当前节点搜索完毕，将其从open表中移除，加入close表
• 重复取open表权值最低的节点进行搜索，直到open表为空或者找到终点JPS

5. JPS+

JPS+算法与JPS算法相比，增加了预处理过程，从而使寻路更加快速。

5.1预处理

首先对地图的每个节点进行跳点判断，找到所有主要跳点，然后对每个节点进行直线可达性判断并进行记录。当满足可达性时，记录好跳点的直线距离和斜向距离，剩余各个方向如果不可到达记为0或者负距离。此时，每个格子8个方向均记录。

(0距离表示对应方向移动一步就会碰到障碍，-n表示移动n+1步会碰到障碍)

5.2 JPS+过程

1. 得到预处理后的数据
2. 对于某个搜索方向，对于正数距离n，直接将n步远的节点作为跳点加入openlist
3. 对于对于0距离，不在该方向进行搜索
4. 对于负数距离-n，直接将n步远的节点进行一次跳点判断

6.D*算法

与$A^*$算法类似，$D^*$算法通过维护一个Open表对场景中的路径节点进行搜索，但$D^*$不是从起始点开始搜索，而是以目标点为起始，通过将目标点置于Open表中开始搜索，直到机器人当前的位置节点由队列中出队。

D*算法中每个节点被称为state，共分为new、open和closed三类，最开始state标记为new，加入open表中置为open，从open表中移走后被置为closed。

6.1符号描述

• 当前节点为X
• 终点为G
• $b(X)=Y$为后向指针，即Y为X的下一个节点
• c(X,Y)表示X到Y的花费
• h(G,X)为$D^*$的估计函数
• 与$A^*$不同的是$D^*$包含了一个key函数，表示最小的h值，当更新过当前节点后，如当前节点为new，令key=h；若当前节点为open，k为当前k和最新的h的最小值；若当前节点为closed，取当前h和最新的h的最小值，该值表示当前节点在全图环境中到达g点的最小代价。

6.2 算法思想

$D^*$包括两部分，process-state和modify-cost。$D^*$算法的特点是反向搜索，从目标点开始搜索过程，并可以在动态环境中进行寻路。

首次搜索时，使用Dijkstra进行搜索，直到搜索到起点，搜索结束后，搜索过的节点的state置为closed，每个节点的k=h，其父节点为邻域中k值最小的那个。此时，出现动态变化时我们可以利用计算的图尽快修改路径。

当规划路径的点遇到了障碍，$D^*$将会对其进行处理，否则仍按照规划的路径到达。遇到障碍时，即当前点的父节点为障碍，修改其h值并将其放入open表，但其k值仍然不变，此时$k=min(k, new_h)$，该点会被优先取出并扩散展开。

Process-State 扩散过程需要使用process-state(节点处理函数)，当扩散到某个节点后，计算后的h值不必其上次的k值小，该点结束对外扩散，伪代码如下：

1：X=Min-State（） #取所有节点中k值最小的
2：if X=Null then return -1 #如果open表为空，结束
3：K_old=Get-Kmin();Delete(X) #得该点k值，并将该点弹出Openlist队列

4：if K_old<h(x),  then- #比较该点h值与K值，（h值上升状态）
5: 		for each neighbor Y of X: #遍历X的邻域节点Y
6: 			if h(Y)<= K_old and h(x)>h(y)+c(x,y)  #Y的不处于上升状态，且用其更新x后，h的h值更小
7: 				b(x)= Y; h(x)=h(y)+c(x,y)  #更新x的父节点及其h值

8: if K_old=h(x) then  #比较该点h值与K值，（h值未变化状态）
9: 		for each neighbor Y of X: #遍历邻域节点Y
10: 		if t(Y)= New or  #Y的state为new添加到Openlist并处理
11: 		（b(Y)= X and h(Y)!=h(x)+c(x,y)）or #Y是X的子节点，并且其h值不需要改变
12: 		（b(Y)！= X and h(Y)>h(x)+c(x,y)）then #Y不是X的子节点，并且其h值可以变得更小
13: 		 b(Y)= X; insert（Y，h(x)+c(x,y)）#将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中

14：else #比较该点h值与K值，（h值下降状态），即k_old>h(x)
15: 	for each neighbor Y of X: #遍历邻域节点Y
16: 		if t(Y)= New or #Y是首次添加到Openlist并处理
17: 			（b(Y)= X and h(Y)!=h(x)+c(x,y)）then #Y是X的子节点，并且其h值不需要改变
18: 				b(Y)= X; insert（Y，h(x)+c(x,y)）#将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中
19: 		else
20: 			if((b(Y)！= X and h(Y)>h(x)+c(x,y)）then #Y不是X的子节点，并且其h值可以变得更小
21: 			insert（X，h(x)）#修改X的h值，并将其点添加到Openlist中
22: 			else
23: 				if((b(Y)！= X and h(Y)>h(x)+c(x,y) and #Y不是X的子节点，并且其h值可以变得更小
24: 				t(Y)= Closed and h(Y)>K_old then -#Y不在Openlist中，且h值处于上升状态
25: 				insert（Y，h(Y)）-#修改Y的h值，并将其点添加到Openlist中
25: return Get-Kmin() #返回该点k值

Modify-cost 该函数为两个节点之间的距离处理函数

L1：c(X,X)=cval #重置两节点间距离
L2：if t(X)=Closed then insert(X,h(x)) #X不在Openlist中，则修改其h值，并添加到Openlist
L3：return Get-Kmin() #返回X点k值

对于上述程序的理解 process-state用于计算到目标G的最优路径，首先从open表中选取k值最小的节点并将其移除，对该点进行分类，遍历其邻域，查看是否需要修改父节点、h值以及添加到open表中，分类大致如下：

a)当$k_{old}<h(x)$时，说明该点受到了障碍的影响，导致h值升高，遍历其邻域的点y，若y点的h值没有上升，并且x的h值能够通过该点变得更小，则修改x的父节点为y，重置其h值； b)当$k_{old}=h(x)$时，处于第一遍遍历的阶段，或者该节点并没有受到障碍的影响，遍历其邻域的点y。 若y是第一次被遍历到，或者y的父节点时x，但h(y)和h(x)+c(x,y)的值不想当，由于$k_{old}=h(x)$，表明h(y)被更改而h(x)还没有变；或者y的父节点不是X，但让X成为y的父节点将拥有更小的h(y)值。发生上述情况需要根据x的h值调整y的h值，并将x作为y的父节点，将y添加到open表中。 c) 当$k_{old}>h(x)$时，说明节点收到影响，遍历其领域的点y

• 若y为第一次遍历到的点，或者或者x是y的父节点，但是h(y)和h(x)+c(x,y)值却不等,这说明h(x)被更改了，但是h(y)还没有变，上述情况应该应该根据x的h值调整y的h值，并将x作为y的父节点，并将y添加到open表中；
• 如果y的父节点不是X，但是让X成为其父节点将拥有更小的h(y)值，上述情况应该应该调整x的h值，并将x添加到open表中；
• 如果y的父节点不是X，但是让Y成为X父节点，X将拥有更小的h(x)值，并且y被已经被open表移除，且h(y)值在上升(即y受到影响),上述情况应该应该调整y的h值，并将y添加到open表中。