规划系列的常见术语：龙格现象、控制点、型值点和插值点、规划控制的开环、闭环、前馈、反馈与重规划

参考b站大佬Ally的规划控制系列

1 龙格现象

1.1 初探龙格现象

龙格现象由德国数学家Carl Runge（卡尔·龙格）于1901年发现，龙格函数定义为：
$$f(x)=\frac{1}{25x^2+1}$$
我们在$[-1,1]$区间按照0.1间隔将龙格函数的散点绘制出来，并利用MATLAB的polyfit函数分别用5次多项式、9次多项式、14次多项式及16次多项式进行拟合。

• 当多项式的次数较低时，与散点的全局拟合效果较差；随着多项式次数增大，散点的局部拟合效果非常好，但是在靠近-1和1这两个端点时有较大振荡；
• 上述现象表明使用高次多项式拟合并不总是能提高准确性。随着多项式阶次的增加，以这种方式产生的多项式实际上可能偏离，通常发生在靠近插值点的端点。

1.2 龙格现象引发的多项式路径规划算法的思考

① 为了避免出现高阶多项式拟合时出现的龙格现象，通常使用多段样条曲线对散点进行插值。
② 实际上，在B样条曲线中，我们已经接触过多段样条曲线进行插值的思想，B样条曲线通过基函数和节点向量的作用机制，一段完整的B样条曲线就是由若干段阶数更低的样条曲线首位相连而成。
③ 样条函数属于分段光滑插值，其基本思想是在由两个相邻的型值点所构成的每一个小区间内用低次多项式来逼近，并且在各型值点的连接处又保证是光滑的（即导数连续）。
④ 基于ST图进行分段五次多项式曲线速度规划思想，本质上也是为了避免龙格现象

2 规划/控制算法系列术语

2.1 控制点

在样条曲线中，用于约束曲线走势、控制曲线形状、且一般不位于曲线上的点称为控制点。

2.2 型值点

在求解参数曲线时，要求参数曲线必须经过的点统称为型值点。

2.3 插值点

若求解的参数曲线连续，在定义域内选取曲线上的若干散点可称为插值点。

3 规划控制的开环/闭环/前馈/反馈与重规划

3.1 案例

驾驶员高速公路控车过程

• 当前路段道路限速100km/h，驾驶员控制车辆起步。刚开始由于车速较低，所以驾驶员深踩油门踏板。
• 随着车速逐步接近目标车速，驾驶员根据当前车速与目标车速的差值，逐步缓抬油门踏板；
• 当到达目标车速后，驾驶员轻踩油门踏板，并不时观察仪表车速，采取相应动作：如果车速超过目标车速，则松开踏板，依靠车辆惯性逐步减速；如果车速低于目标车速，则稍微增大油门，使其更加接近目标车速。
• 按照上述过程持续一段时间后，前方遇到隧道，限速为80km/h。驾驶员当前目标车速随即降低，首先松开油门，驾驶员会优先判断依靠惯性减速是否可以在进入隧道前降低至80km/h，否则会直接踩制动踏板使车速快速降低。

开环、闭环及重规划

• 当前路段的限速值是一个理想达到的速度值，它不在乎当前车速是否是否达到、何时达到100km/h，只是作为车辆期望达到的一个目标速度，这就是规划器的开环性。
• 驾驶员观察当前实际车速就好比控制器接收系统输出的反馈信号；深踩、轻踩油门踏板和制动踏板的过程就是控制器输出控制指令的过程；这就实现了驾驶过程的闭环控制。
• 当车辆快进入隧道时，限速发生变化，这表明规划器的目标车速也应当随即发生变化。由于规划的起点并不是上周期规划的终点，可理解为重规划。

3.2 规划器的开环性与控制器的闭环性

如图所示：在起点处，我们希望规划一条光滑的速度曲线，达到目标速度Vr，现有两种方案。
① 方案一是开环规划，如图中绿色曲线所示。在T0时刻，先规划T0 ~ T1的绿色速度曲线段，随后车辆立即响应指令。但由于执行误差，到达T1时刻后车辆速度为V1，未达到理想速度Vr1。开环速度规划不会局限于此周期实际速度到底有无达到规划速度，而是继续沿着上个周期末的规划结果，以Vr1为起点，继续规划T1~T2时间段的光滑绿色速度曲线。以此类推，从T0到T3的整个规划区间就是整条速率曲线，而实际速度响应为蓝色曲线。此方案优点是曲线更加光滑，满足最优性，缺点是未考虑系统的实际响应能力。
② 方案二是闭环规划，如图中紫色曲线所示。既然是闭环，就需要引入实际速度。以T1时刻为例，由于此时V1≠Vr1，故此时以实际速度V1作为起点，重新规划一条从V1到Vr3的紫色速度曲线。同理，到达T2时刻后， V2≠Vr2’，故再次以V2作为起点，重新规划一条从V2到Vr3的紫色速度曲线。优缺点与开环相反。

哪种方案更优呢？实际上，两种方案在规控算法种都应该采用，只不过将他们进行了分工：
（1）在规划层，除了起点以及需要重规划的特殊场景外，都应采用开环，保证规划层在综合考虑众多约束条件下，求解最优速度曲线，即绿色曲线；
（2）在控制层，引入实际速度，并与规划速度共同构造加速度的反馈分量，使速度能更及时响应规划的绿色曲线。

3.3 前馈+反馈的闭环控制器模型

规划层加速度的理解：
规划层是开环的，不仅规划了速度，也规划了加速度，这个加速度从两方面进行理解。

• 规划加速度也需要适当参考，因为它和规划速度共同反映了当前车辆需要达到的一种理想状态，控制模块以此作为理想目标，逐步趋近达成。
• 另一方面，规划加速度不能直接用于指令输出给执行器。理由如下：如目标速度为60kph，此时速度为58kph，根据规划层的开环性可知，规划速度将首先
  达到60kph，此时对应的规划加速度为0。若直接将0加速度指令下发给执行器，当执行器完美响应时，则速度不再继续上升，因此最终实际车速永远停留在58kph。

加速度前馈与反馈

• 将规划层输出的参考加速度做一个比例增益Gain1，其结果作为目标加速度指令的一部分。由于参考加速度直接由速度规划器输出，故称之为前馈加速度；
• 为了解决系统稳态误差影响，将系统实际响应的速度与当前实际速度作差，并再增加一个比例增益Gain2，其结果作为目标加速度指令的另一部分。由于利用了系统实际响应反馈的速度，故称之为反馈加速度。

由加速度前馈和反馈共同构成了纵向控制模型

3.4 闭环控制的各种环

位置环：

（1）一般来说，我们要求跟停前方静止车时，本车车头与前车车尾的距离控制在3~4m较为合适。因此，在所有驾驶场景中，动静态跟停场景对位置的控制要求相对最高。
（2）我们设主车在A0位置开始减速跟停，目标跟停位置距离前方B车的相对距离为Ls。
（3）将上述条件输入到速度规划器，可以输出满足安全性和舒适性要求的参考速度序列，车辆执行器接受相应指令后向前运动。
（4）经过单位周期后，车辆到达A1位置，距离目标位置还有L1的距离，根据上述条件，继续规划速度序列，不断循环往复，也就构成了最外层的位置环。

速度环：
（1）在定速巡航等场景中，对位置控制要求较低，而对速度控制要求更高，故此时会产生速度环。
（2）将位置环中与目标位置相关的变量替换为与目标速度的变量，即可将该图整体替换为速度环的示意图。

加速度环：
在起步加速至目标车速的过程中，尽可能以最大加速度（如$1.5m/s^2$）不断靠近目标车速，这个过程常需要加速度进行精确控制，以期能够及时、足量产生加速度，此时则构成了加速度环。

3.5 规划器的重规划

绿色曲线表示规划的速度曲线，蓝色表示车辆实际响应速度曲线

• 假设在T0~T1时刻，车辆正常巡航加速，实际响应速度曲线也在逐步加速。当到达T1时刻后，车辆突然遇到紧急情况，例如前车急减速、邻车道突然cut in，此时需要紧急规划减速。这种情况，我们期望车辆速度能立即按照紫色曲线那样响应减速。
• T1~T2时间段，虽然规划速度在减小，但仍大于实际车速，导致控制层的反馈加速度为正，此时前馈加速度为负，但前馈与反馈加速度两者之和可能仍为正，这就导致最终车辆还会继续加速。
• 因此，即使抛开系统时延和迟滞的影响，实际速度曲线仍会继续上升至V2，V3，这样就错过了减速的最佳时机，可能导致制动不足而与前车相撞
• 因此在规划层，为了避免上述情况的发生，当识别到某种危险场景后，规划速度不再沿着上一周期的规划结果继续规划，而是直接从当前车速进行规划。如图2所示：绿色虚线表示识别到某种危险场景后规划的曲线，T1时刻规划速度发生跳变，从Vr1跳变到V1开始规划。如此一来，最终实际车速响应可能就类似按照T1之后的蓝色曲线。
  

重规划就是针对一些特殊场景，需要尽可能降低系统时延和迟滞带来的加速或制动响应不足的问题