自动驾驶汽车运动规划问题的算法抽象与Python学习（一）—A_star算法

最近在看自动驾驶汽车运动规划相关的基础内容，想着从一些最基础的算法入手，逐步的了解一些关于决策规划算法相关的东西，方便更好的进行工作。因为之前对这方面基本上是个小白，所以决定写博客记录一下自己的学习过程，写的不会很深，但是希望能够对这方面感兴趣的想入门的小伙伴一些帮助，如有错误，还希望各位不吝指教。

1. 自动驾驶运动规划问题

1.1 从人开车的角度思考自动驾驶运动规划问题

首先想一下汽车的运动规划是一个什么样的问题的，如下图1所示，你坐在汽车的驾驶位上，想要从A点开车前往B点，那么你会怎么样进行开车呢？

首先，我们在一条长长的大马路上以一定的速度进行行驶（“定位传感器”，知道了自己所在的位置为A点，速度为Xm/s）；然后眼睛（类比摄像头、激光雷达等）看到前方有一个静态车辆；我们还要继续往前开到B点（“规划目的地”）；这个时候大脑做出了需要向左换道的避障逻辑（“行为决策”）；然后手开始打方向盘，脚开始控制油门，规划出来一条路线（“运动规划”），然后根据实时距离障碍车辆的距离调整方向盘转角和油门开度（“动态规划和反馈控制”），然后平稳地绕过障碍车辆，开往B点。

1.2 自动驾驶运动规划问题抽象

对于人来说，这么一个场景再简单不过，一般的新手司机对于完成这样的任务都是毫无压力，但是大家有没有想过对于机器来说，怎么样从A点开始，绕过静态障碍车辆，行驶到B点呢？如何把这样一个问题进行抽象，抽象成计算机语言能够识别的问题呢？其实这个问题可以抽象成为一个Path Finding Problem，是一个搜索问题，搜索从A点到B点的最优路径问题（如图2，从A点（红色）到B点（绿色）的路径寻找问题，将空间离散化成一个个的小方格,就像小时候玩的贪吃蛇游戏一样，想办法怎么样绕过障碍。从A点到B点吃到奖励）

1.3 基本的搜索算法入门

好了，目前的自动驾驶汽车的运动规划问题已经变成了从A点到B点的路径搜索问题，既然是从A点到B点的搜索问题，那我们怎么样进行搜索呢？首先我们能够想到的就是最简单的图形搜索算法就是广度优先搜索算法BFS和深度优先搜索算法DFS
在过去的博客中对广度优先搜索算法BFS和深度优先搜索算法DFS的介绍。
https://mp.csdn.net/mdeditor/97247647#

2. A_star算法用于自动驾驶运动规划

2.1 为什么是A_star算法

但是无论时深度优先搜索算法和广度优先搜索算法都属于no_information search，这两个算法在进行搜索时，都是没有先验信息的，在搜索时各个方向和节点的权重是相同的，所以搜索起来会向各个方向进行试探，所以总体下来耗时非常多。特别是离散节点较多的情况下，对于无人车这种对耗时很敏感的应用领域，没有先验信息的搜索是远远不能满足的。
那么我们就要考虑如何利用已知的道路信息进行搜索呢，我们首先能够想到的就是A_star算法。

2.2 A_star算法的基本介绍

A_star算法相对于BFS和DFS的最大的优势就是就在起点和终点已知的情况下，可以根据终点的方向和路径的长短制定cost, 从而制定每个节点的F值，使搜索能够更快速的趋近与终点。
F=H+G
H是从网格上当前方格移动到终点的预估移动耗费，G值是从当前格的父亲格移动到当前格的预估移动耗费。H就是根据终点的方向制定的预估移动耗费，有很多种方法定义H值，比如定义H值为 终点所在行减去当前格所在行的绝对值与终点所在列减去当前格所在列的绝对值之和。 这样通过F值作为一个先验信息，越接近终点给的奖励值越高，那么在相同情况下程序自然会趋近于更快速的接近终点，也就是更快速的寻找可行路径。

2.3 场景抽象成数学问题

好，下面我们就对这个避障问题抽象成数学问题。

根据道路的可通行情况，车辆需要在道路上进行行驶，不能够碰撞路侧的路沿。那么把两侧的路沿设置为1，数字1表示为不可通行。静态车辆不能够进行碰撞，那么把静态车辆所在的位置也全都设置为1，需要绕过车辆进行通行。0表示可以进行通行的区域。

2.4 A_star算法的代码逻辑解释

1、在程序中把这个道路场景抽象成一个矩阵
道路场景抽象成的矩阵在程序中表示为grid，表示了道路各处的可通行情况。
2、指定启发矩阵
启发矩阵定义为距离终点的“距离”越近，那么启发矩阵对应的值就越小，具体的方案为终点所在行减去当前格所在行的绝对值与终点所在列减去当前格所在列的绝对值之和。另外，在不可通行区域，启发矩阵对应位置的值设置成一个很大的值，在这里设置为99
3、 指定起点终点和移动cost
指定起点为[4,1]，指定终点为[4,8]，指定每移动一格的常规cost为1
4、确定从当前格子移动到其他格子所能执行的动作，定义为delta。
为了简便起见，在每一个格子都可以进行上下左右四个方向的移动。向上移动为[-1,0 ]，也就是x-1 往上边移动一行；向右移动为[0,1]，也就是y+1向右移动一列；依次类推进行定义。
5、定义搜索的主函数 search函数。
主函数的输入参数有 grid道路可通行情况矩阵、init起点、goal目标终点、cost每移动一步的耗费、heuristic启发矩阵。
定义一个closed矩阵，用来存放已经搜索过的点。没有搜索过记为0，搜索过记为1。然后第一步就是把起点设置为搜索过的点。
定义一个action矩阵，用来存放各个节点的动作，这个程序action矩阵每一个xy坐标点表示的是上一个坐标点到当前点的动作action（上下左右，根据之前delta定义，0代表上，1代表左，2代表下，3代表右）
然后定义xy为当前点的位置， g 为从起点A沿着已生成的路径到一个给定方格的移动开销，f为总开销，f=从起点A沿着已生成的路径到一个给定方格的移动开销+从给定方格到目的方格的估计移动开销。
定义结束条件，找到终点结束，找不到可用的路径也结束。

开始进行路径搜索：
首先搜索当前点所有可能采取的动作，根据采取的动作记录采取这个动作的总耗费f，移动耗费g，下一个点的xy坐标，存放在cell里，然后对cell进行从小到大排序然后翻转，比较得出总耗费最小的cell进行pop弹出，作为下一步的行动动作next；对于已经搜索过的点，存放到closed矩阵中，不进行重复搜索。一直循环搜索直到找到指定终点或者没有可通行路线就结束。

输出搜索得出的结果路径 path
结果路径是从终点开始，根据到达终点上一步所采取的action情况，进行反推，得到上一个路径点，然后依次类推，直到找到起点。然后把路径进行反转，得到正向的路径。

2.5 A_star算法的python代码实现

# encoding=utf8 
from __future__ import print_function
# 在开头加上这句之后，即使在python2.X，使用print就得像python3.X那样加括号使用。
# python2.X中print不需要括号，而在python3.X中则需要

grid = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
        [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],    #0 are free path whereas 1's are obstacles
        [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 1, 1, 0, 0, 0],
        [0, 0, 0, 1, 1, 1, 1, 0, 0, 0],
        [0, 0, 0, 1, 1, 1, 1, 0, 0, 0],
        [1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]

# heuristic 不考虑不可通行区域的启发矩阵

init = [4, 1]    # 指定起点[4,1],x是行，y是列
goal = [4, 8]    # 指定终点[4,8]
cost = 1         # 初始化cost函数,每一步的行进cost

#the cost map which pushes the path closer to the goal
heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):    
    for j in range(len(grid[0])):            
        heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
        if grid[i][j] == 1:
            heuristic[i][j] = 99 #added extra penalty in the heuristic map # 如果不通就设置一个很大的cost

print("Heuristic")
for i in range(len(heuristic)):
        print(heuristic[i])

#the actions we can take   # 坐标系向上为x-1，向右为y+1
delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right


#function to search the path
def search(grid,init,goal,cost,heuristic):

    closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]# 行rows,列cols
    closed[init[0]][init[1]] = 1    # 初始化一个7行10列的矩阵,closed[0,0] = 1,存储已经搜索过的点
    
    # 初始化一个7行10列的矩阵,action
    action = [[0 for col in range(len(grid[0]))] for row in range(len(grid))] #the action grid

    x = init[0]
    y = init[1]
    g = 0    # 从起点A沿着已生成的路径到一个给定方格的移动开销
    f = g + heuristic[init[0]][init[1]]   # 从起点A沿着已生成的路径到一个给定方格的移动开销+从给定方格到目的方格的估计移动开销
    cell = [[f, g, x, y]]   # 总开销,移动开销,x位置,Y位置

    found = False  # flag that is set when search is complete   # 判断是否找到
    resign = False # flag set if we can't find expand  # 判断是否有可通行路径

    while not found and not resign:
        if len(cell) == 0:   # 搜索完所有的路点，再也找不到action还没找到，就fail
            resign = True
            return "FAIL"
        else:

            # 对所有的开销矩阵做排序,大小按cell的第一个元素排序，第一个元素大小相同就按照第二个元素排序
            cell.sort()     # to choose the least costliest action so as to move closer to the goal      
            # 把排序后的开销矩阵进行翻转
            cell.reverse()  # 从大到小进行排列
            next = cell.pop()   # next决定下一步动作,next就是选择开销最小的cell矩阵,把最右侧的也就是最小的cell弹出

            x = next[2]
            y = next[3]
            g = next[1]
            f = next[0]
            
            if x == goal[0] and y == goal[1]:  # 判断是否到达终点
                found = True
            else:
                for i in range(len(delta)):#to try out different valid actions  尝试不同的action
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]
                    # 判断条件1：行列数都在"地图"矩阵范围内
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
                        # 判断条件2：没有经过的路点、无障碍的路点
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:   
                            g2 = g + cost    # 每一步的行进cost
                            f2 = g2 + heuristic[x2][y2]
                            cell.append([f2, g2, x2, y2])   #相应的action对应的cell矩阵
                            closed[x2][y2] = 1    # 把已经经过的点放到closed矩阵中去
                            action[x2][y2] = i    # 把每一步对应所有可能的action输出到Action矩阵中去
    # 为什么要反向从终点开始输出路径？ 
    # 因为Action map上对应的点上的值就是上一个点到这一个点的action,只要按照action回退回去就能回到起始点
    # 而如果从起点开始搜的话，那Action在每个当前点是不知道下一步动作的，下一步四个方向都有可能,没办法输出路径

    invpath = []
    x = goal[0]
    y = goal[1]
    invpath.append([x, y])#we get the reverse path from here
    while x != init[0] or y != init[1]:   # 判断是否为搜索起点,如果不是搜索起点，根据action回退到上一个路点
        x2 = x - delta[action[x][y]][0]
        y2 = y - delta[action[x][y]][1]
        x = x2
        y = y2
        invpath.append([x, y])

    path = []
    for i in range(len(invpath)):
    	path.append(invpath[len(invpath) - 1 - i])

    print("ACTION MAP")
    for i in range(len(action)):
        print(action[i])
              
    return path

    
a = search(grid,init,goal,cost,heuristic)
print("===========path===========")
for i in range(len(a)):
	print(a[i]) 

程序的执行结果如下：

python a_star.py 
Heuristic
[99, 99, 99, 99, 99, 99, 99, 99, 99, 99]
[11, 10, 9, 8, 7, 6, 5, 4, 3, 4]
[10, 9, 8, 7, 6, 5, 4, 3, 2, 3]
[9, 8, 7, 99, 99, 99, 99, 2, 1, 2]
[8, 7, 6, 99, 99, 99, 99, 1, 0, 1]
[9, 8, 7, 99, 99, 99, 99, 2, 1, 2]
[99, 99, 99, 99, 99, 99, 99, 99, 99, 99]
ACTION MAP
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[1, 0, 0, 3, 3, 3, 3, 3, 3, 3]
[1, 0, 0, 0, 0, 0, 0, 2, 2, 3]
[1, 0, 3, 0, 0, 0, 0, 2, 2, 0]
[2, 2, 2, 0, 0, 0, 0, 2, 0, 0]
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
===========path===========
[4, 1]
[4, 2]
[3, 2]
[2, 2]
[2, 3]
[2, 4]
[2, 5]
[2, 6]
[2, 7]
[2, 8]
[3, 8]
[4, 8]

可以看出来，成功搜索出了“车辆”绕行通过"障碍物"的路径。

3 待续

好，那么我们现在使用A_star算法进行了无人车运动规划问题的抽象，那么A_star算法能不能直接用于无人车的运动规划呢？？？ 答案显然是不行的，首先A_star算法解决的是环境静态情况下的最短路径规划问题；实际车辆行驶过程中环境是一直进行变化的。另外A_star算法还要求已经知道起点和终点了，然后进行启发式搜索，是一个fully observed search problem；实际车辆行驶过程中对环境并不是全局了解的，是一个partially observed problem；那么如何解决无人车在已知局部环境下的动态规划问题呢？

欲知后事如何，且听下回分解。

欢迎转载，但转载请注明原文作者及出处。