自动驾驶

自动驾驶

自动驾驶-寻径问题

在自动驾驶项目中，路径规划是指目标车辆利用地图和传感器得到的数据，规划出一条能够到达目的地的安全可行的路线，并交给车辆控制器去执行的过程。

首先考虑一种较简单的场景，当道路没有其他车辆或障碍物时，则目标车辆只需要沿着道路形成的曲线匀速行驶即可，此时目标车辆只需得到一个能够拟合当前道路轴线所形成曲线的多项式即可。为了得到这一多项式，以当前车辆位置为起点，从下一个时间段车辆将要行驶过的道路轴线中抽取若干坐标点，将这些坐标点作为参考点得到能够拟合这些参考点的多项式。再以适当增量取若干X轴节点，作为多项式的输入参数得到对应的y的坐标位置。这些（x,y)坐标对就是目标车辆下一步将要走过的坐标点。

当车辆能够沿着道路行驶以后，下一步要完成的任务是找到一条有效的路径将车辆驶向目的地，即寻径问题，本章将介绍三种寻径方法： 深搜方法，A*方法和动态规划方法。

1 深搜方法

map = [[s, 99, 0, 0, 0, 0],
[0, 99, 0, 0, 0, 0],
[0, 99, 0, 0, 0, 0],
[0, 99, 0, 0, 0, 0],
[0, 0, 0, 0, 99, d]]
如map矩阵所示，把地图按块划分并抽象表示为一个二维矩阵其中0表示可以行驶到达的位置即道路，99表示不能到达的位置，即障碍；s为目标车辆的起点，d为目的地。寻径即在这个二维矩阵找到一条从s到d的路线。一般的寻径问题可以用深度优先搜索的方法：
1. 定义一个保存坐标对的堆栈
2. 将起点坐标入栈
3. 当栈为空或栈顶元素为终点位置时执行第六步，否则取栈顶元素并依次计算上下左右四个位置的坐标
4. 每得到一个新的坐标位置，检查其在二维矩阵中的值，如果为0则入栈，并将其值+1，重复第三步，如果不为0，跳过
5. 如果栈顶元素周围的四个位置的值都不为0，弹栈，重复第三步
6. 检查堆栈，如果堆栈不为空时，从栈底到栈顶就是从起点到终点的路径，如果堆栈为空，则说明目的地不可到达。

虽然深搜可以解决寻径问题，但由于固定的搜索顺序，可能走过很多无用的位置，以二维矩阵为例，当车辆处在【4，2】位置时，车辆会优先搜索上方的位置是否何用，则最终会到达【0，2】的位置才右转，但实际情况是从【2，2】的位置右转更加高效。所以，下边介绍第二种寻径方法A*方法

heristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]

2 A*方法

A*方法依然以深搜为基础，区别是加入一个新的heristic矩阵，用来改变栈顶元素新的搜索顺序，以便寻找更短的路径到达目标位置。heristic矩阵的规模和地图二维矩阵相同，其中每个坐标点的值为该点到目标位置的距离。
1. 定义一个保存二维矩阵坐标位置的堆栈
2. 初始化heristic矩阵，
2. 将起点坐标入栈
3. 当栈为空或栈顶元素和目标位置重合执行第六步
4. 得到栈顶元素，计算上下左右四个坐标位置，当新的坐标矩阵的map为0时，选择对应heristic值最小的值放入堆栈，并将map值+1，重复第三步，如果不为0，跳过
5. 如果栈顶元素周围的四个位置的值都不为0，弹栈，重复第三步
6. 检查堆栈，如果堆栈不为空时，从栈底到栈顶就是从起点到终点的路径，如果堆栈为空，则说明目的地不可到达。

使用heristi矩阵，当目标车辆再次到达【2，2】坐标位置时会由于【2，3】处的值更小从而优先选择右转，从而可以更快达到目标位置。

3 动态规划

动态规划是从目的地为起始点，根据地图矩阵得到每一坐标位置到目的地的最短距离，然后目标车辆从起始点起沿着距离更小的方向行驶，最终到达目的地。
如下图所示为通过动态规划得到的矩阵。
[11, 99, 7, 6, 5, 4]
[10, 99, 6, 5, 4, 3]
[9, 99, 5, 4, 3, 2]
[8, 99, 4, 3, 2, 1]
[7, 6, 5, 4, 99, 0]