上一节的参考线模块提供了光滑的frenet坐标轴
规划参考线：正义之拳：老王讲的自动驾驶（规划参考线模块）
1、确定规划起点
避障还是得在自然坐标系下做，host向参考线投影为坐标原点建立自然坐标系，
所有障碍物都投影到自然坐标系下面，生成SL图
自车的位置为（0,l0），因为其在原点上
接下来就开始规划，第一步是确定规划的起点
定位得到的host_x，host_y，投影referenceline，得到SL坐标(0,l0)，以此点为路径规划的起点，这么做是错误的。
因为考虑控制是不完美的
规划以100ms周期执行，在上个周期已经规划出轨迹
从控制的角度来看，规划的轨迹是割裂的，一段一段不连续的
正确的做法是
判断current location与pre-period规划结果是否误差太大
如果误差太大，就以current location来投影referenceline，以（0,l0）为规划起点
若差别不大，要么当前点投影上个周期轨迹得到P点，要么干脆直接拿当前点当P点
以（s0,l0）作为规划的起点而不是定位的坐标作为起点，效果怎么样
从控制的角度来看，就是一小截一小截的多段首尾连续的轨迹拼接而成
如果用定位作为轨迹的起点
SL图
搜索出一条最优路径
最优一般有以下三个标准
（1）平滑
（2）和障碍物保持适当的距离
（3）贴近参考线
可以用二次规划解决
cost function：平滑代价+参考线距离代价+障碍物距离代价
很遗憾：避障问题的二次规划是非凸的，cost function有多个极小值点
比如，下面这个车绕树的问题，
代价函数如下
对于非凸问题，没有一个完美的解决方案
方法：离散化，启发式搜索出一个粗解，以粗解为基础优化出最终解
撒点离散化，然后在粗解附近迭代求解，看往左绕还是往右绕最小，如果找到一个最小值往右绕的凸空间，我们就以往右绕的凸空间作为二次规划的凸空间。
这种方法并不完美，会搜到次优解，但是对于自动驾驶也够用了
在离散空间下的最优路径是什么，此最优路径将开辟一个凸空间，在此凸空间上做二次规划
离散空间（图1）的最优路径称为粗解（图2），粗解开辟凸空间（图3），在此凸空间上优化出最终解
如何在离散空间上找到粗解？ 答：动态规划
下面先说动态规划
连接每两个点的曲线是什么？
答：五次多项式五次多项式6个系数，求
例：对于起点和下个点
路径规划的起点一般有heading约束，边界条件为：
如果对于中间点，怎么确定边界条件
更简单，认为导数为0即可
因为这个解是粗解，所以长得什么样无所谓，是用来判断的，开辟凸空间用。
这样就获得了一个离散点组成的路径
如何评价路径的好坏？cost function
cost function可以设置为：一二三阶导数求和的平滑代价，再加上障碍物距离代价，参考线距离代价
下面说一下为什么这么设置
（1）平滑：越像直线越平滑，直线最短
所以期望
最小，等价于
最小，但是积分处理起来麻烦，所以离散化成求和
最小
二阶和三阶求和主要考虑到曲率和曲率的变化率，不希望扭来扭曲，曲率变化太快
（2）障碍物距离代价
g(x)这么设置：在d1范围外为0，在（d2，d1）范围内线性增长，在（0，d1）内为无穷大
或者
其实就是惩罚函数
2、动态规划
下面来说如何找最短路径
每个路径都有对应的cost，问题变为找图的最短路径问题
计算机：dijstra算法，自动驾驶没必要
使用动态规划的方法解决这个问题
记cost（P0,Pij）表示P0到Pij点花费的最小代价
可以得到第二层的最小cost
第三层
可以得到第三层的最小cost
前面已经算过
所以cost(P0,P13)可由子问题推出
第j层
状态转移方程
得到P0到任一点的最小距离
如果要算第五层
动态规划计算出
，选最小的作为动态规划的输出
3、控制接口，轨迹拼接
现在的控制接口（控制输入）为
规划为100ms，控制周期为10ms，也就是说控制有9个周期都在跟踪同一个点，这样的效果不好，所以在规划周期和控制周期不同步的时候，不能直接用这个输入
想想
是怎么来的
规划：规划出了trajectory，包含了一系列的
规划周期为100ms，在trajectory找到t=100ms所对应的
（找不到就找100ms相邻的2个时间，做插值），作为规划输出发出去
在规控周期同步时，无问题。不同步时，有问题
因为规划已经规划出了10ms,20ms,……,100ms的轨迹，但只把100ms后的点发了出去，必然造成控制效果变差
所以：原代码中的控制接口不可取，需要改进
改造：规划直接发一条带时间的轨迹给控制
规划输出
控制每10ms搜索trajectory
控制根据当前控制周期的时间，搜索轨迹，找到当前时间对应的
，输出给控制算法去跟踪
方法很简单，细节是魔鬼
问题一：轨迹的时间是相对时间还是绝对时间？
例：16:00:00开始规划，16:00:10规划完成，规划出了4秒轨迹
有三种发给控制的选择
（1）trajectory-t = （0,……,4）相对时间
（2）trajectory-t = （16:00:00,……,16:04:00）绝对时间
（3）trajectory-t = （16:00:00,……,16:04:10）绝对时间
23区别在于规划的起点
到底是相对时间还是绝对时间？
推荐发绝对时间，因为做拼接，做控制，绝对时间更直观方便
例：16:00 规划了4秒本周期16:00:10 首先获得本周期的时间（16:00:10），直接查找上一时刻规划的16:00:10对应的点
对于控制：16:00:13，查trajectory对应的16:00:13的点，输出给算法进行跟踪
问题二：规划起点
设在本周期规划开始的时间为T
规划起点是T，还是T+100ms?
答：T+100ms
为什么？
控制频率比规划快，在[T, T+100ms]内，此时规划未计算完成，那么控制跟踪的是哪个轨迹
答：上个周期的轨迹
从控制看来
如果以T+100ms为规划起点
这里引用一篇文章：
百度 Apollo 轨迹规划技术分享笔记 – 知乎 (zhihu.com)
如果误差不大的话，就以T+100ms作为规划起点，如果误差较大的话，就用车辆动力学外推100ms后的状态作为规划起点，这就是轨迹拼接算法
解释为什么不能用定位的点作为规划起点
控制不完美，用定位做就会导致曲线不连续
解决办法：轨迹拼接
发表回复