算法流程省流版合集（Lattice planner、EMplanner、混合A*泊车、LQR、PID）

lattice planner

先对t采样，得到s，根据s采样，得到L，然后组合得到完整的S、L、t，再从Frenet坐标系转为世界坐标系，这样轨迹就出现了。
0、轨迹拼接；*在上一帧轨迹中的匹配点。两个维度出发：时间t和历程s。拼接轨迹规划起点；
1、离散参考线上的点；
2、寻找参考线上匹配的点；*匹配点，最近点；
3、frenet坐标系转换；
4、ST、SL图构建；200m，8s
5、横向、纵向规划；*降维解耦，轨迹=纵向+横向+时间；*T和s得到s和速度，根据s和l得到坐标，
纵向规划，得到s和v，巡航（四次多项式）、跟车（五次多项式）、超车（五次多项式），难点在于终点的位置和速度选择，以及分辨率的选择。
横向规划根据s得到l，分为采样点规划（五次多项式）、二次规划（调用osqp求解器的库），Apollo默认进入二次规划，因为速度快。
以ds=1m进行采样，遍历每个采样点的横向可达范围，即每个点的横向约束，通过这么多约束构建对应的二次型，最后通过调用OSQP进行二次规划求解…

6、轨迹评价；*可行性判断；*cost计算
cost的内容
a、Cost of missing the objective, e.g., cruise, stop, etc.决策项
b、Cost of longitudinal jerk
c、Cost of longitudinal collision
d、向心加速度
e、Cost of lateral offsets
f、Cost of lateral comfort

7、合并轨迹；我们可以计算出在T*时刻的纵向偏移量和横向偏移量，再通过参考线，即可还原成一个二维平面中的轨迹点。通过一系列的时间点T0，T1，…，Tn，可以获得一系列的轨迹点P0，P1，…，Pn，最终形成一条完整的轨迹；

EMplannner

1、上周期规划轨迹和目标预测，将障碍物投影至SL图上；SL的自然坐标系是参考线/ST的自然坐标系是在规划的轨迹上
采样、DP求解出最优路径；（8s/200m，cost包括平滑度、避撞安全度，靠近参考线，决策）
2、根据最优路径得到凸空间，构建目标函数和约束进行二次优化求解，得到最优路径；（靠近粗解、平滑、舒适）
3、根据最优路径和目标预测，投影至ST图中；
4、稠密采样得到点，进行DP求解；（不同疏密度撒点，）
根据DP结果得到凸空间，构建目标函数和约束进行二次优化求解，得到最优速度曲线；（约束主要包括可行驶空间，动力学约束以及交通法规限制）
5、合并成为轨迹，传给控制和输入给下周期规划。
路径DP过程：
*1、先采样，撒点得到path_waypoints；
*2、然后构造graph，即forward过程；
*3、backward过程，顺藤摸瓜得到cost最小路径；
*4、目标函数构建；路径（path cost（平滑性、舒适性、紧凑性）、静态障碍物cost、动态障碍物cost）；
路径QP过程：
1、cost函数：平滑性，横向位置速度加速度，距离参考线近，l-lref；
2、结合等式约束和不等式约束；
3、ospp求解；
速度DP过程：
*1、先采样，撒点得到ST区域；
*2、然后构造graph，即forward过程；
*3、backward过程，顺藤摸瓜得到最优速度曲线；
*4、目标函数构建；速度（edge cost（速度加速度加加速度）、静态障碍物cost、动态障碍物cost），前一个点的cost；
速度QP过程：
4、cost函数：平滑性，纵向位置速度加速度，距离参考线近，s-sref，终点；
5、结合等式约束和不等式约束；
6、ospp求解；

lattice优点：参数少且统一化，计算消耗低，流程简单，简单场景解空间大，适合简单场景。同时生成了符合车辆动力学的路径与速度曲线，不用去进一步平滑处理；
缺点：对于城区复杂道路，求解较慢或计算量增大；
EM优点：适合复杂场景；
缺点：参数较多，流程负责；

混合A*泊车算法：

首先进入泊车的场景后进行两个task的判断
****如果在泊车位范围内，可以开始泊车的task；
****否则先生成一个行驶至泊车位的一个路径；
开始泊车后：
****进行ROI区域的获取；
********坐标系转换；
********车位等信息转换；
********boundary获取；
********障碍物过滤，超平面构建；
***使用混合A进行粗解的求取；
********周围roi区域外的障碍物被过滤；
********DP，反向计算并存储目标点到某点的启发代价；
***********使用A算法构建，8向拓展节点，累积cost；
***********超出边界和碰撞检测筛选；
********在混合A开始
************初始校验；
************节点拓展的过程中，会进行碰撞检测和运动学约束；
************碰撞检测是判断bounding box和障碍物的边界相交；
************得到最优路径后进行速度规划；
****************差值速度计算；
****************曲率速度计算；
****************得到最优速度曲线；
****得到粗解后进行路径和速度的分别优化；
********路径和速度的平滑；
************是否平行优化；
************是：
****************DL_IAPS算法进行碰撞检测；
****************路径平滑采用fem算法进行平滑；
********************平滑、紧凑、偏离参考线的程度；xy坐标系
****************速度平滑是使用piecewise jerk算法来进行平滑；frenet坐标系
********************速度平滑过程中没有碰撞检测；
************否：
****************距离优化；
************得到最优曲线后合并输出轨迹；
************坐标系转换；
************存储并发布至control；

LQR算法：

lqr算法是横向控制的重要算法：
算法输入：
1、整车参数（a、b、侧偏刚度、车辆质量、转动惯量）；
2、车辆位置与状态；

3、规划轨迹点；
算法输出：

算法过程：
1、整车参数 + 位置状态 —> A、B矩阵
2、LQR求解—>Q、R矩阵—>K矩阵；
（需要注意V=0的情况）
（LQR求解K模块：迭代求黎卡迪方程，可以对应不同的速度建表，进行查表法查找，提升计算效率，空间换时间。）
3、位置状态 + 规划轨迹点 —> 实际位置在投影点的曲率k，横向误差；
（求投影点，最近的匹配点，计算误差和投影点的曲率）
4、前两步的结果 K + k + 输入 —> 前轮转角；
5、K矩阵 + 横向误差 + 第三步的前轮转角 —> 算法输出；
6、预瞄控制；
（预瞄时间内计算偏差）
调参技巧：Q、R初始值小；数值小，对应的系统简单，实时性高；
Q矩阵是状态变量的权重矩阵；
Q矩阵的元素值越大，对应的状态变量在控制过程中所占的权重就越大，系统会更加重视减小该状态变量的误差。
哪项误差大，对应Q值增大，对应的信号响应越快；
Q越小，响应越快，；
Q越大，精度越高，系统减小对应误差项。
Q过大导致系统容易振荡，系统不稳定。

R矩阵用于衡量控制输入的代价。
误差越大，降低R矩阵，使其快速响应。
R越小，控制量变化更大，快速响应。
R越大，控制量变化越小，平滑无突变。
R过大，系统输入不稳定，变化大，代价高。

PID算法：

P：比例环节；比例放大：快速响应，加快收敛速度；过大的KP系数会引起超调；
I：积分环节；滞后调节，消除稳态误差，使系统收敛至目标位置；但是会引起超调，且该超调不可被微分环节抑制；
D：微分环节；超前控制，抑制比例环节引起的超调（参考傅里叶级数，相位移动），不可调节比例环节引起的超调；
误差范围较大的系统调节P即可，消除超调使用PD。