入门轨迹规划（横向与纵向）
安全舒适两不误：探讨优化算法在规划控制中的应用
- 轨迹规划的概念安全舒适两不误：探讨优化算法在规划控制中的应用· 轨迹规划的概念
- 决策
- 横向规划
- 纵向规划
- 优化算法在规划（Planning）中的应用
轨迹规划的概念
已知交通环境下，轨迹规划的核心就是要解决车辆该怎么走的问题。
规划的输入输出
输入：包括拓扑地图，障碍物及障碍物的预测轨迹，交通信号灯的状态，还有定位导航（因为要知道目的地是哪才能规划路径）、车辆状态等其他信息。
输出：一个轨迹，即时间到位置的函数，就是在特定的时刻车辆在特定的位置上。
z为灰色，车不会飞
而规划输出的轨迹正好是控制（Control）模块的两项输入之一，另一项输入是车辆自身的状态，包括车的位置、heading（行驶朝向）、速度、加速度、角速度等信息。
优化问题的约束
轨迹规划本质上来说是一个优化问题。谈到优化问题，我们需要知道优化的约束和优化的目标。
约束：第一个约束就是车辆要遵守交规，这是强制性的约束；第二个约束是要避免碰撞；第三个约束是要使规划的轨迹在控制上可实现，不能出现规划了一个急弯的轨迹，车根本拐不过来的状况。
目标：安全、舒适（Jerk & Curvature rate）、高效
安全性约束问题可以具体表示为：车的物理特性、车辆刹车和油门的极限、车到左右boundary的距离以及不发生碰撞。
如下图所示：
舒适性约束意味着加速度变化率小，以及曲率的变化率小。那么对应的cost function（代价函数）就比较好设定了，即加速度+加速度变化率+曲率+曲率的变化率
优化问题是否为凸
我们关心一个优化问题首先看这个优化问题是不是凸的。
凸问题的定义：我们可以简单描述对于一个问题它的两个可行解，其线性组合也应该是可行的，而且不比这两个解都要差。（撞树问题）横向纵向都是非凸问题。
如何解决非凸优化问题？
决策
从数学上来讲，决策就是限定非凸问题的解空间，将问题转化为凸的。
我们可以看下图的例子，如果两个人在穿马路的话，我们大体上可以有四种不同的决策，既让两个人，超两个人，让一超一和超一让一。而一旦决策确定，那么其实就可转换为凸的问题，求解就会相对来说比较容易。
但需要注意决策的问题是一个 NP-hard 问题。有一些需要决策的场景，比如抢行还是让行，是否要冲黄灯，在哪两辆车之间变道、并线，是否要主动变道，是从左还是右绕行前方障碍物，只有做了决策才能使问题变为凸优化问题，便于求解。
对于凸问题，有很多比较快速的算法进行计算了。线性规划，二次规划，序列二次规划，都是数值求解优化问题的方式。我们可以看一下二次规划中，如果 Q 正定，二次规划就是凸的问题，有很快的方式求解它。
对于上面提到的两个行人问题：
首先是纵向规划的决策，决策的方法我们已经谈到了一些，比如动态规划。这是一个 NP-hard 问题所以没有一个简单的算法进行精确求解。看下图，如果有两个人穿马路，我们来研究一下如何走才是最优的，我们可以将 T-S 进行离散化，离散化之后，我们就可作出一个状态转移方程，而这个状态转移方程我们可以看作每一步的决策只与前边的两步有关系，所以有转移方程。虽然这个方程在实际过程中比较难写但确实是一种做法。虽然离散化 t 和 s 降低了精度，但降低精度也能降低了运行时间。注意，这种方法并不能保证最后的速度比较舒适，它只是大概的一个决策方法，既到底让或者不让。
解决让行问题
我们再来看一下决策方面的挑战。第一个挑战上面已经讲过了，决策问题是一个 NP-hard 问题，不易直接求解，有多种多样的近似算法。第二个挑战是很难用规则去拟合人的经验，包括上述的状态转移方程中的 cost 也很难去表示。
而一些解决的办法是我们可以根据各种不同的情况去建立一个数学模型。以及用机器学习的方法去做一些决策。比如说下图左边的例子，我们已经打算绕行，但前车突然起步了，我们该怎么办？我们是该变道还是跟在后边走还是继续绕行？这种情况确实很靠人的经验。再看右边的图，我们要左转，但是前边这辆车停了很久也没有走我们要不要变道呢？作为人类司机也很难判断。场景的多变而复杂使得决策面临着很多挑战。
横向规划
如何打方向盘的规划，只决定了轨迹的形状。分为无车道和有车道的情况。
无车道：freespace 中的规划，或者说是停车之类的问题，它是没有车道线信息或者说先验信息的，大多都用 OpenSpace 路径生成的方式去做，一般也是低速的。
有车道：输出 s ->(x,y)这个函数难度也是比较大的，常见的做法是是离线生成参考线，之后我们就可以将 s ->（x，y）这个问题变为一个求解 s ->L 的问题，L 是指车辆在这个参考线上的横向偏移量。
本来是沿车道往前走，但由于有车辆的遮挡，我们就必须绕行它。即横向偏移就可以了。
参考线生成
很类似于开卡丁车时别人教你的过弯的最优路线，它也是一个优化问题，当然也要保证安全性和舒适性，但是方便的是有了地图以后我们是可以离线去做的，所以可以用一些开销比较大的算法做到非常优。它的约束是在车道线内，且控制上要可实现。优化目标就要接近车道中心，曲率不能太大且曲率变化率不大。
有了参考线之后我们就可以把参考线离散化，采一些点出来，那么就变成了解一个离参考线偏移距离的一个问题。就变成了 s ->L 的问题，约束是不跨越边界，避免碰撞，而优化的目标是要离参考线近，要离障碍物远，曲率不大，曲率变化率不大等等。
我们看下这个例子，那么这个问题就可以看成一个二次规划 QP 的问题。0，-1，-2 是我们车过去的路径，0 号点是我们车当前的位置，我们需要解的就是 1，2，3，4，5，6 这种点的相对于参考线的横向偏移 x，已知就是 x[-2]，x[-1]，x[0]，求解 x[1],x[2]等，而约束就是他不能超过左右边界包括马路牙，实线，障碍物等。优化目标就是我们要离中心参考线近，方向盘不能打太多太快。下图中的公式的形式就是一个关于 x 的二次的形式，所以它们可以去用 QP 的方法来解。
横向规划的挑战
虽然大部分时候车都在一个有车道线的地方开，但比如像下图左下部分的路口，这边是三条直行的车道，而对面是四条条直行的车道，右边的白车就不会按车道线开，会横跨车道线挤你。右上的图，是有辅路的情况，他是没有车道线的，这种进辅路的问题更像是一种没有参考线的 freespace 的规划，这种都是比较大的挑战，如果说没有车道线或者说没有人按车道线开，那么这样的规划就会比较难做一点。
另外一些挑战就是环境的问题，因为周围的环境是瞬息万变的周围的环境预测也是困难的，我们看下图，本来车辆是要往左变道，而左下角黄色的块表示摩托车，这个摩托车正在飞速的过来，所以车辆又进行了取消变道的决策，也就是生成平滑的曲线回到原来的道路上去，这样的情况我如何能保证路径光滑且在控制上可实现，也是一件很难的事情。
纵向规划
纵向规划其实是对于我们设定好的路径上的速度上的规划，决定了在这整个轨迹上的运动过程。求解这种优化问题，有的约束第一就是遵守交规（信号灯、限速、停车让行等），第二就是避免碰撞。优化的目标，想都可以想的到就是舒适，舒适就是速度变化率不大，加速度变化率不大，行驶也要尽量快一点，等等。
避让行人
我们举一些例子，前边提到了行人横穿马路，行人就是这样一个矩形，我们对应了两种决策，决策完成后我们应该怎么办呢？如果决定要抢行，我们就可以将矩形的约束条件扩展到最下部，这样就是凸问题求最优解。如果是避让我们就可从下边过。
如果是行人斜着穿过马路呢？那么请大家想一下按 s ->t 图是如何表示的呢？答案就是它是斜向上的平行四边形。
斜=斜率速度不为0，斜向前进
黄灯决策
再看一些类似的例子,如下图。比如黄灯，黄灯是一个特殊的例子，如果决策要冲的话，需要尽快通过路口，否则的话很容易被逼停在路中间出不去，这种情况我们可以用一个这样的 s - t 图表示，左边界是表示黄灯亮的时刻，大家看这样的白色矩形有一个缺角，当黄灯亮起的时候，我们如果要尽快通过路口，t 在增大的过程中 s 也要尽快的增大，增大的速率要超过缺角的斜率。
前车匀减速
我们再看一些更高级的想法，我们在跟车的时候，假如前车在匀减速，在 s - t，如下图所示，如果能够精确描述前车的运动的状态，那么白色部分会有各种各样的形状，这样就可以在解优化问题是解出一个好的速度曲线。
纵向规划的挑战
决策中的博弈行为，需要在激进和保守之间掌握一个平衡点，很多时候要有不同的参数和不同的模式去处理这个事情。
感知与预测带来的困难：鬼探头问题，一些车辆的cut-in问题。
还有一些极端的情况，需要考虑到横纵向协调配合，比如下图一个极限的车辆插入，你在高速行驶，而旁边的车突然就要插进来，如果是人类司机他会打方向避让，如果左边没有车，甚至会变道，左边有车，他也会扭一点方向去避让。这种就需要横纵向的配合去一起解决这种极端的情况，从纵向规划来说，这种情况，你已经不可能保持安全车距了，你更多的只能说保证不撞，然后尽快拉开车距，而不是一脚刹到底。
优化算法在规划中的应用
规划问题其实是为了解出一个时间到位置的函数，而L4级自动驾驶车一般会行驶在结构化的道路上，这也意味着规划模块具备地图的信息，那么问题就可以进行转化——把（x，y）坐标系转化为下图的（l，s）坐标系。（frenet坐标系）
s沿着道路中线（reference line）的方向，l则是垂直于道路中线的方向。于是，问题就转化成了求解s（t）和l（s）的问题，我们希望得出，自动驾驶车在t时刻应该走到s上的哪个位置；以及走在这个位置时，该车距离s的左右偏离是多大。
最终，一个复杂问题就转化成三个子优化问题：一个横向优化、纵向优化以及中线问题（s方向是车辆的纵向规划，l方向为横向规划）。
横向优化
上图中右侧展示了一条路，中间的线是reference line（参考线），其中“0”点就是车当前所在的位置，“-1”，“-2”是车之前的坐标位置，接下来的“1，2，3，4”……就是规划模块的输出结果，也就是车未来的位置。
上图中右侧展示了一条路，中间的线是reference line（参考线），其中“0”点就是车当前所在的位置，“-1”，“-2”是车之前的坐标位置，接下来的“1，2，3，4”……就是规划模块的输出结果，也就是车未来的位置。
这条路的左右两边是boundary（道路边界/车道线），图中的半圆对应着路面上的障碍物。横向规划求解的是正是每一点的l坐标，也就是到中心线的偏离量。
对于横向优化问题，我们同样可以从安全性和舒适性两方面考虑。
安全性方面，取决于横向优化问题的constraints（约束条件），一般要求l不能太小，也不能太大，即不能压左边线，也不能压右边线，同时不能碰到左右两边的障碍物。
舒适性方面，我们则设计成cost function（代价函数），一般要求路线的曲率越小越好，曲率变化率越小越好。如果我们的参考线是一条完美的参考线，那么曲率就可以用一个相对曲率来表示。
对于一个优化问题来说，cost function（代价函数）变成了标准的二次函数，同时，所有的constraints（约束条件）都是一次的。
那么，这就成了一个标准的QP（Quadratic programming，二次规划）问题，不仅如此，这还是一个box constraint（闭箱约束）的QP问题，我们就能够非常快速求出优化的结果。
纵向优化
纵向优化求解的则是t到s的函数。
对于纵向规划的来说，安全性要求我们保证车辆不能发生前后方向上的碰撞。以上图右侧为例，此时有个行人正在过马路，规划模块会将行人的行为可以转换成（t，s）坐标，展示在坐标图里（上图）是一个平行四边形。
此时，面对这个行人，如果自动驾驶车选择让路，纵向规划解出来的曲线就应该在平行四边形的下方；如果选择超过这个行人，那么这条曲线应该从上面绕过去。
在纵向规划上，舒适性体现在加速度越小越好，以及加速度的导数越小越好。可见，纵向和横向规划涉及的constraints（约束条件）和 cost function（代价函数）都非常相似。
中线生成
中线设定的约束条件首先是不能压边界（车道线），同时要符合曲率的限制。对于cost function（代价函数）来说，希望达到的目标是中线的曲率越小越好，曲率的变化率越小越好。这样来看，中线问题也是求解k（t）和k（t）’的函数问题。
问题似乎又绕回去了，为了舒适性，我们还是需要求解非常复杂的方程吗？
并不是。由于中线是可以离线生成的，所以不需要在线计算，这些问题能放在服务器中进行并行的计算。
