【路径规划】全局路径规划算法——动态规划算法（含python实现）

参考资料

• 路径规划与轨迹跟踪系列算法

1. 算法简介

• 动态规划是运筹学的一个分支，是求解多阶段决策过程最优化问题的数学方法。
• 各个阶段决策的选取不是任意确定的，它依赖于当前面临的状态，又影响以后的发展。当各个阶段的决策确定后，就组成了一个决策序列，因而也就决定了整个过程的一条活动路线，这样的一个前后关联具有链状结构的多阶段过程就称为多阶段决策问题。
• 动态规划在车辆工程技术领域有着广泛的应用，如“两档变速器最优换挡规律”、“混合动力汽车最优能量管理策略”、“栅格地图最优路径搜索”等。

2. 算法思想

• 动态规划的思想就是将多阶段决策问题转化为一系列单阶段最优化问题。

• 对最佳路径（最佳决策过程）所经过的各个阶段，其中每个阶段始点到全过程终点的路径，必定是该阶段起始点到全
  过程终点的一切可能路径中的最佳路径（最优决策），这就是Bellman提出的著名的最优化原理。

• 简言之， 一个最优策略的子策略必然也是最优的。

DP算法逆向寻优，正向求解，本质由三层循环构成：

• 第一层遍历每一个阶段；
• 第二层遍历第$i$个阶段的每一个状态；
• 第三层循环遍历第$i$阶段的第$j$个状态到第$i-1$阶段的每一条路径， 更新当前状态的到上一个阶段的状态的最短距离

3. 算法示例

如图，设终点为$E$，逆向运用DP算法：

• 第Ⅳ阶段(D →E）： D 有两条路线到终点E ，权重分别为
  $$\begin{gathered} f_4(D_1)=5 \\ {f}_4(D_2)=2 \end{gathered}$$

• 第Ⅲ阶段（C →D）： C 到D 有 6 条路线。第3阶段的C有3个状态值，分别讨论经过该状态
  值的最优路线。

  • 经过C1
    $$\begin{array}{rl}{f}_3\left(C_1\right)=\min & \left\{\begin{array}{l}d\left(C_1,D_1\right)+f_4\left(D_1\right)\\ d\left(C_1,D_2\right)+f_4\left(D_2\right)\end{array}\right\}=\min \left\{\begin{array}{l}3+5\\ 9+2\end{array}\right\}=8\end{array}$$
    最短路线为 $C1\to D1\to E$

  • 经过C2
    $$\begin{array}{r}{f}_3\left(C_2\right)=\min \left\{\begin{array}{l}d\left(C_2,D_1\right)+f_4\left(D_1\right)\\ d\left(C_2,D_2\right)+f_4\left(D_2\right)\end{array}\right\}=\min \left\{\begin{array}{l}6+5\\ 5+2\end{array}\right\}=7\end{array}$$
    最短路线为$C2\to D2\to \mathrm{E}$

  • 经过C3
    $$f_3\left(C_3\right)=\min \left\{\begin{array}{l}d\left(C_3,D_1\right)+f_4\left(D_1\right)\\ d\left(C_3,D_2\right)+f_4\left(D_2\right)\end{array}\right\}=\min \left\{\begin{array}{l}8+5\\ 10+2\end{array}\right\}=12$$
    最短路线为 $C3\to D2\to E$

• 第Ⅱ阶段（B →C）： B 到C 有 9 条路线。第Ⅱ阶段的B有3个状态值，类似地，分别讨论经过该状态值的最优路线。

  • 经过B1
    $$f_2\left(B_1\right)=\min \left\{\begin{array}{l}d\left(B_1,C_1\right)+f_3\left(C_1\right)\\ d\left(B_1,C_2\right)+f_3\left(C_2\right)\\ d\left(B_1,C_3\right)+f_3\left(C_3\right)\end{array}\right\}=\min \left\{\begin{array}{l}12+8\\ 14+7\\ 10+12\end{array}\right\}=20$$
    最短路线为 $B1\to C1\to D1\to E$
  • 经过B2
    $$f_2\left(B_2\right)=\min \left\{\begin{array}{l}d\left(B_2,C_1\right)+f_3\left(C_1\right)\\ d\left(B_2,C_2\right)+f_3\left(C_2\right)\\ d\left(B_2,C_3\right)+f_3\left(C_3\right)\end{array}\right\}=\min \left\{\begin{array}{l}6+8\\ 10+7\\ 4+12\end{array}\right\}=14$$
    最短路线为 $B2\to C1\to D1\to E$
  • 经过B3
    $$f_2\left(B_3\right)=\min \left\{\begin{array}{l}d\left(B_3,C_1\right)+f_3\left(C_1\right)\\ d\left(B_3,C_2\right)+f_3\left(C_2\right)\\ d\left(B_3,C_3\right)+f_3\left(C_3\right)\end{array}\right\}=\min \left\{\begin{array}{l}13+8\\ 12+7\\ 11+12\end{array}\right\}=19$$
    最短路线为 $B3\to C2\to D2\to E$

• 第Ⅰ阶段（A →B）： A 到B 有 3 条路线。
  $$f_1(A)=\min \left\{\begin{array}{l}d\left(A,B_1\right)+f_2\left(B_1\right)\\ d\left(A,B_2\right)+f_2\left(B_2\right)\\ d\left(A,B_3\right)+f_2\left(B_3\right)\end{array}\right\}=\min \left\{\begin{array}{l}2+20\\ 5+14\\ 1+19\end{array}\right\}=19$$
  最短路线为 $A\to B2\to C1\to D1\to E$

  

4. python实现

根据第3节的分析，我们可以写出以下python程序（程序参考自知乎）：

INF = float('INF')
### 状态节点定义
graph = {
    '4': {'D1': {'E': 5}, 'D2': {'E': 2}},
    '3': {'C1': {'D1': 3, 'D2': 9}, 'C2': {'D1': 6, 'D2': 5}, 'C3': {'D1': 8, 'D2': 10}},
    '2': {'B1': {'C1': 12, 'C2': 14, 'C3': 10}, 'B2': {'C1': 6, 'C2': 10, 'C3': 4}, 'B3': {'C1': 13, 'C2': 12, 'C3': 11}},
    '1': {'A': {'B1': 2, 'B2': 5, 'B3': 1}}
    }

### 最优路径及其距离值定义
INF = float('INF')
# 初始时距离为无穷大
dists = {
    'A': INF,
    'B1': INF,
    'B2': INF,
    'B3': INF,
    'C1': INF,
    'C2': INF,
    'C3': INF,
    'D1': INF,
    'D2': INF,
    'E': 0
    }

path_opt = {
    'A': ['A'],
    'B1': ['B1'],
    'B2': ['B2'],
    'B3': ['B3'],
    'C1': ['C1'],
    'C2': ['C2'],
    'C3': ['C3'],
    'D1': ['D1'],
    'D2': ['D2'],
    'E': ['E']
}


# 每一个节点的父节点
parents = {
    'A': None,
    'B1': None,
    'B2': None,
    'B3': None,
    'C1': None,
    'C2': None,
    'C3': None,
    'D1': None,
    'D2': None,
    'E': None
    }

# 动态规划函数
def DP(graph, dists, parents):
    for period_key in graph.keys():  # 遍历每一个阶段
        for key_i in graph[period_key].keys():  # 遍历每个阶段的每一个状态节点
            min_key = None
            for key_i_dist in graph[period_key][key_i].keys(): # 遍历当前阶段的每个状态节点到下一阶段的每一条路径
                if graph[period_key][key_i][key_i_dist] + dists[key_i_dist] < dists[key_i]:
                    dists[key_i] = graph[period_key][key_i][key_i_dist] + dists[key_i_dist]
                    parents[key_i] = key_i_dist
                    min_key = key_i_dist  # 找出最小距离值的节点
            path_opt[key_i].extend(path_opt[min_key])  # 将最小距离值的节点添加到最优路径集合



DP(graph, dists, parents)
print("E到每个节点的最短距离：\n",dists)
print("====================")
print("最优时每个节点的父节点：\n",parents)
print("====================")
print("最优路径：\n",path_opt)

代码仓库见github