自动驾驶-寻径

自动驾驶-寻径问题

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

首先考虑一种较简单的场景，当道路没有其他车辆或障碍物时，则目标车辆只需要沿着道路形成的曲线匀速行驶即可，此时目标车辆只需得到一个能够拟合当前道路轴线所形成曲线的多项式即可。为了得到这一多项式，以当前车辆位置为起点，从下一个时间段车辆将要行驶过的道路轴线中抽取若干坐标点，将这些坐标点作为参考点得到能够拟合这些参考点的多项式。再以适当增量取若干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. 当栈为空或栈顶元素为终点位置时执行第六步，否则取栈顶元素并依次计算上下左右四个位置的坐标