无人驾驶的规划与控制（一）——路由寻径

1 无人车规划与控制的模块分类

无人车的规划与控制（Planning & Control）在广义上可以分为4个模块：

1. 路由寻径（Routing）
2. 行为决策（Behavior Decision）
3. 动作规划（Motion Planning）
4. 反馈控制（Feedback Control）
   
   
   

2 路由寻径

2.1 把车道划分成Lane序列

普通导航的最小元素是具体的某一条道路，或者某一条车道，无人车的路由寻径更加精细，要深入到无人车能使用的高精地图的车道级别。

无人车的车道划分！=自然道路划分，一条车道通常会被划分成若干个Lane，一个虚拟转向也会被分成若干个Lane，如图所示：

路由寻径模块需要解决的问题是什么？

从起点到终点的最佳道路行驶序列：

{ (lane, start_position, end_position) }

其中(lane, start_position, end_position)i称为一个Routing Segment（路由片段），其中，所在的道路由lane标识，start_position和end_position分别代表这条路由上的起始纵向距离。

2.2 建立有向带权图

无人车在规划控制的时候，考虑到安全问题， 会给换道路径赋予更高的代价（Cost）。

可以把无人车的Lane寻径问题，抽象成一个在有向带权图上的最短路径搜索问题。具体步骤为：

1. 在一定范围内所有可能的Lane上，进行分散“撒点（Lane Point）”，这些点之间由有向带权的边链接。Lane Point之间是沿着Lane前进方向单向可达的关系，边的权重就是行驶代价。

2. Lane Point采样点的采样频率需要保证：地图上最段的Lane，也能得到充分的采样点。

3. 设置代价，如下面的图表所示：
   

   当前Lane和下一个Lane的关系	代价Cost
   直行接右转	5
   直行接左转	8
   右转内部	2
   左转内部	3
   每条Lane内部	1
   相邻平行Lane换道	10

4. 在设置号的路网（Road Graph）中，寻找出最小代价的路径序列。
   

   例如在这张有向带权图上，最短路径就是上面那条路，代价为22，小于下面的那条路（Cost = 44）。

2.3 利用Dijkstra或A*算法搜索

参考：Dijkstra算法图文详解和C++代码
https://blog.youkuaiyun.com/cliukai/article/details/90769638

参考文章

《第一本无人驾驶技术书》刘少山