把烦恼痛了吞了认了LatticePlanner从学习到放弃（⼆）：⼆次规划的应⽤与调试前⾔
前情提要：
盲⽬激情的移植apollo的lattice，体会到了痛苦，当时为了先尽快让lattice算法在⾃⼰的框架⾥跑起来，在摘⽤源码的时候做了很多⽋考虑的改动，虽然快速实现了通过lattice planner规划轨迹，然后达到循迹的效果，但是在随后引⼊障碍物，产⽣了⼤量的坑，深深的被教育，也给带头⼤哥添了不少⿇烦。
看懂流程==>理解原理 ==>成功实现
最终通过planner中的⼆次规划实现了单车道内低速障碍物的避让规划。
简明扼要：添加静态障碍物→产⽣单车道内⽬标轨迹→绕⾏或停⽌
最近节奏⽐较撕裂，下午或晚上才能搞lattice的复现，但这个“重复造轮⼦”的过程真的获益良多，和⼤哥
们交流也多起来，知道好的轮⼦是什么样的，才能尝试去改进或完善，⼀瓶不响半瓶咣当，半瓶正是在下～保持积极与热情，也期待⼤佬们各种指导....
⼀.静态障碍物的引⼊
1.障碍物的格式
暂时不使⽤Apollo的那套，先⽤⾃有结构体接收障碍物信息，然后压⼊Obstacle类中即可。
对于静态或低速障碍物，⽆⾮就是id、长宽⾼、位置以及朝向，动态障碍物暂时没有进⾏。
2.⾃车sl的建⽴和障碍物的建⽴
⾃车相对于参考线起点⾥程和横向偏移量，以及障碍物的ST、SL信息都是要计算的，对于动态障碍物，SL已经不⾜以满⾜需求了吧？后续学习EM再拎出来分析。
⼆.单车道场景中⾯向静态障碍物的避让
1.轨迹规划原理
轨迹=纵向轨迹+横向轨迹，之前已经分析了纵向速度规划的过程，以及横纵向轨迹是如何combine成
亲亲小兔⼀条完整轨迹的。在lattice中，横向轨迹⽣成有两种⽅法：撒点采样法、⼆次规划法。
1.1基于采样点的轨迹规划
a女星撒点法前边也有记录，知道了起始点和采样点的，然后求解对应的五次多项式的系数，便可得到对应的横向轨迹，不重复了。
1.2基于⼆次规划的轨迹规划
Apollo源码中FLAGS_lateral_optimization默认是开启的，即横向规划默认使⽤⼆次规划进⾏，由于个⼈鲁莽认为撒点会更直观简洁，所以关了这个标志位，采⽤的是撒点采样，随后在实车调试时发现耗时很⼤，尤其是⾃车接近障碍物时，耗时飙升——轨迹数太多，障碍物碰撞检测遍历轨迹。额，配置⽐较低的⼯控机雪上加霜...然后试⽤⼀下⼆次规划呗，wtf，超nice。
学习了程⼗三的。
Apollo中⼆次规划求解使⽤的是OSQP求解器，官⽹：，⼆次规划求解有很多⽅法，关于为何选取OSQP，速度快～为何这么快，数学系的⼤佬们的战场。标准的⼆次规划形式：
下来要做的就很明确了：
搞清楚路径规划的约束；
搞清楚优化⽬标，即如何评价计算结果的优劣
转换成osqp求解器需要的形式，调⽤求解器
坐等结果～
考虑车宽以及和障碍物的距离buffer，车辆的中⼼的横向可移动范围⼤概就是如下图，Apollo中，前探60m，采样点间隔1m，所以有60个采样点，也可以根据实际情况调整。
1.2.1 车辆横向状态量设计
横向偏移量作为基本量，跑不掉，作为横向速度变化率，当然也跑不掉；作为横向加加速度，是控制乘坐舒适性的重要指标，也不能放掉；对于，把轨迹看作可拉伸的弹⼒绳，三阶导意味着其可拉伸的程度，可以⽤差分计算。so，需要的量齐全了。状态量可以得到：
1.2.2 车辆横向轨迹约束的建⽴
整个约束建⽴主要考虑到：
1.⾃车不能与障碍物碰撞或驶出边界，即
2.轨迹应该保持连续，即相邻两个采样点的
3.轨迹应该保持光滑，即导数连续相邻两个采样点的斜率
4.横向加加速度(相对于s的⼆阶偏导)应在⼀定范围内，即
公式2、3直接⽤的泰勒公式进⾏的2阶和3阶展开，将代⼊，可得到公式2和3的最终形式：
到这⼀步，基本上约束的内容已经很清晰了，整理后如下：
即对于每⼀个采样点，存在两个不等式和两个等式约束，⼀共60个采样点，那么约束⾄少应该为60x4=240个约束条件。
根据状态量的形式，约束矩阵也就定了（这⾥不⼀定准确，若有不对求指导额..）：
这⾥使⽤的话貌似应该是，⾏列貌似反了...其实转置以后应该长这样：
1.2.3 约束边界的建⽴
前边已经描述了约束的建⽴依据，根据障碍物对车道的侵占情况，更新横向位移的范围，最终约束的上下边界分别为：
其中-2,2作为缺省值不充值，凑够矩阵运算数量，最终⽤于：
1.2.4 ⽬标函数的建⽴
⼆次规划的⽬标为
其中，将权重作为P项对⾓阵传⼊，左右边界作为偏差项q⽤以控制轨迹和参考线的偏离程度，如下：
⾄此，整个⼆次规划建⽴所需要的材料，全部齐全。在⼆次规划中，对于矩阵P，以⽬前形式来看⼀定是正定的额，是否意味着此问题是凸优化问题，且⼀定有可⾏解？求⼤佬指点。。。
1.2.5 约束矩阵的压缩CSC
迟帅资料从上边已经可以看到，建⽴起来的矩阵基本都⾮常稀疏，在运算过程中是⾮常不⽅便的，常见的稀疏矩阵压缩⽅法主要有：
CSR—Compressed sparse row
那英歌曲CSC—Compressed sparse column
之前在学习apollo时，发现其采⽤的csc矩阵，当时并不了解，也是扩展后才知道，可见总结： ⾄于为何选择csc的原因可能是osqp官⽅使⽤的是csc？调⽤osqp建⽴workspace，需要以csc矩阵形式传⼊求解器。
闲话⼀⼤堆，下⾯上代码。
2.Lattice planning⼆次规划代码实现
you ll just never knowlattice planner中通过调⽤Trajectory1dGenerator实例化后的成员函数，⽣成横纵向轨迹，我们要看的常规撒点法和⼆次规划都在其中，
// 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
Trajectory1dGenerator trajectory1d_generator(
init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
trajectory1d_generator.GenerateTrajectoryBundles(
planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
进⼊函数后，很清晰没啥说的：纵向规划+横向规划。
void Trajectory1dGenerator::GenerateTrajectoryBundles(
const PlanningTarget& planning_target,
Trajectory1DBundle* ptr_lon_trajectory_bundle,
Trajectory1DBundle* ptr_lat_trajectory_bundle) {
//纵向速度规划
GenerateLongitudinalTrajectoryBundle(planning_target,
ptr_lon_trajectory_bundle);
//横向轨迹规划
GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
}
在横向规划内，通过宏定义FLAGS_lateral_optimization，决定是否采⽤⼆次规划，如下：
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
/
/是否使⽤优化轨迹，true，采⽤五次多项式规划
if (!FLAGS_lateral_optimization) {
auto end_conditions = end_condition_sampler_.SampleLatEndConditions();
// Use the common function to generate trajectory bundles.
GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
ptr_lat_trajectory_bundle);
} else {
double s_min = init_lon_state_[0];
double s_max = s_min + FLAGS_max_s_lateral_optimization;//FLAGS_max_s_lateral_optimization = 60
double delta_s = FLAGS_default_delta_s_lateral_optimization;//规划间隔为1m
//横向边界
auto lateral_bounds =
ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);
// LateralTrajectoryOptimizer lateral_optimizer;
std::unique_ptr<LateralQPOptimizer> lateral_optimizer(
new LateralOSQPOptimizer);
// 采⽤的是OSQP求解器
lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);
auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();
ptr_lat_trajectory_bundle->push_back(
std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));
}
}
流程很直⽩，通过GetLateralBounds函数获取包含障碍物信息的横向边界分布，然后传⼊lateral_optimizer->optimize()中开始osqp短暂愉快的⼀⽣，
完整源码可看apollo：
bool LateralOSQPOptimizer::optimize(
const std::array<double, 3>& d_state, const double delta_s,
const std::vector<std::pair<double, double>>& d_bounds) {
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
//建⽴⽬标函数中的P矩阵，主要包括权重分配
CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
delta_s_ = delta_s; //1m
const int num_var = static_cast<int>(d_bounds.size());
const int kNumParam = 3 * static_cast<int>(d_bounds.size());
const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3;
c_float lower_bounds[kNumConstraint];
c_float upper_bounds[kNumConstraint];
const int prime_offset = num_var;
const int pprime_offset = 2 * num_var;//=6？