Apollo中Lattice规划器结构梳理

Apollo中Lattice规划器结构梳理

参考源

刚接触Apollo，代码看的实在头疼，自己之前一直用C，一开始感觉烦的一批（自己水平太菜）…直到现在逐渐体会到其奥妙之处，果然体会到了逼近大佬的快乐～关于planning代码部分，参考了几个前辈的，如下
阿波罗3.5的Cyber planning任务调度：https://blog.csdn.net/davidhopper/article/details/89360385
个人感觉最清晰的Planning解释：https://www.cnblogs.com/liuzubing/p/11058612.html
以及很容易就能搜到的其他资源：https://zhuanlan.zhihu.com/p/61982682https://zhuanlan.zhihu.com/p/103317794?utm_source=wechat_session
关于ST、SL图的构建、五次多项式以及OSQP求解原理，后续搞清楚后再更新…

Planning的Proc执行结构

Apollo启动完成初始化后，执行其主要部分proc：
通过调用planning_base_->RunOnce()函数来开启决策规划之旅，目前含有两个大的规划器：navi_planning和on_lane_planning，至于选取哪个，在程序初始化的时候被配置了，详情可以参考参考源中的介绍～目前学习lattice，位于on_lane_planning中

Planning搞起：RunOnce 结构

在这里，主要做了下图中的事：

RunOnce中使用的planner

在RunOnce执行中，调用了planner_->Plan，whose内部包含了：lattice、navi、public_road、rtk_replay四种规划器，找到了我们的lattice

默默展开lattice

主要七个步骤：
1.将参考线转变为离散地图点
2.计算参考线上初始规划点的匹配点
3.根据匹配点计算Frenet帧的初始状态
4.解析决策，得到规划目标
5.分别生成纵向和横向一维轨迹束
6.评价：首先，根据动态约束条件对一维轨迹的可行性进行评价；其次，评估可行的纵向和横向轨迹对，并根据成本进行排序。
7.返回无碰撞的、符合条件的轨迹
如下图所示：

ST、SL图

在采用Frenet坐标系后，ST和SL图极为便捷，位于第4部分实现

横纵向轨迹束的生成

位于第5部分，分为纵向轨迹、横向轨迹两个实现

◆纵向轨迹的生成
主要使用了四次和五次多项式，五次多项式可保证一阶导v、二阶导a、三阶导jerk等连续性（这块理解有误么…）

◆横向轨迹的生成
横向轨迹生成用了五次多项式和二次规划两套方法，就像图中的一样…二次规划求解我没看懂，等闲下来搞定它，只知道大致原理不能算懂～额额

轨迹的评价

对于lattice第6部分，看到很多介绍的材料里都有解释，集火在cost函数上，cost函数形式在很多论文里可以直接看到了
这里主要两个大的环节：1.快速剔除无效的轨迹并对剩余的轨迹进行cost计算；2.对剩余的轨迹进行碰撞检测
◆为达到第1个计算cost的目的，我们得有基准才行～Apollo又分了两步走：

• 生成基准——完善参考线的速度信息
  整个过程在reference_s_dot_ = ComputeLongitudinalGuideVelocity(planning_target);中完成。
  速度规划的依据：是否有停车点，制动距离是否足够，制动减速度是否合适…等，原理和AEB类似，不过没后者复杂，算法实现主要靠PiecewiseAccelerationTrajectory1d这一部分(还有几个，统称分段加速度算法，参见大佬Apollo轨迹规划技术分享)，得到完成速度规划的referenceLine后，我们就有了评价的基准。
• 对比基准——拿着小皮鞭哦不…健全的参考线去鞭挞各个轨迹
  在对轨迹进行cost计算前，在纵向维度上再突突掉一批轨迹，
  如果有停车点—跨越停车点的轨迹：不要
  如果速度超范围、加速度超范围、加加速度超范围：不要
  突突完，幸存的纵向轨迹携手横向轨迹，开始生成cost。注：车辆的运动特性决定了，车辆的横向运动是伴随其纵向运动出现的（四轮独立转向的话…比如车辆可以横着走，这个应该就废了），因此横向轨迹的因变量和采样基准都用的纵向里程s。熟悉的cost配方，上：
  然后将配对的横纵向轨迹+cost值存好，为下一环节做好准备。每个cost的权重配比，一定程度反映了你的驾驶风格。
  ◆为达到第2个中的碰撞检测目的，需要每个时刻自车占据的空间+障碍物占据的空间，判断在同一时间二者的空间有没有发生重叠，这个过程Apollo通过实例化类collision_checker，在其构造函数中实现了：CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],*ptr_reference_line, reference_line_info, ptr_path_time_graph)，此时只是完成了碰撞检测的准备条件，大致干了这些事：
  真正的碰撞检测在下一环节进行，go on。

轨迹的挑选

在对轨迹进行筛选时，一开始没有看到对轨迹的剔除，后来才看到while循环时，不满足直接continue了…直接保证凡是执行到reference_line_info->SetTrajectory时，就意味着是成功符合的轨迹了，是在下村儿了
在这一环节，三件大事：
1.横纵向轨迹的合二为一

TrajectoryCombiner::Combine( *ptr_reference_line, *trajectory_pair.first, *trajectory_pair.second, planning_init_point.relative_time())
原理：基于采样时间t→得到轨迹对应时刻的s，s’，s"→根据s，计算对应的横向d，d’，d"→结合参考线信息由Frenet转换为笛卡尔坐标，统一后存入combined_trajectory返出
2.合并后的轨迹进行约束检查
ConstraintChecker::ValidTrajectory(combined_trajectory)
速度、加速度、曲率、加速度以及加加速度，包括横向加速度的约束检测，均在此中完成
3. 轨迹的碰撞检测
是的，没错…碰撞检测放在了最后进行了，不知是否是因为OBB(Oriented Bounding Box)在做碰撞检测时比较折腾，所以把不良轨迹尽可能剔除完后再进行。就是它：collision_checker.InCollision(combined_trajectory)，在第6部分实例化的类collision_checker，在此中大致干了两件事：构建自车box，用其去和障碍物box对对碰，一套下来没有重叠，说明这条轨迹可还行～碰撞检测方法可戳这里：Apollo中Lattice轨迹碰撞检测

下一阶段

OSQP实际应用求解学习
come on

---