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部分,分为纵向轨迹、横向轨迹两个实现
◆纵向轨迹的生成
主要使用了四次和五次多项式:
对于横向轨迹的多项式拟合,因为有有初始状态的零阶导,一阶导,二阶导,和末状态的零阶导,一阶导,二阶导,一共六个变量,所以拟合五次多项式;
对于纵向轨迹,在停车和跟车状态,也都是五次多项式,但对于巡航状态,由于我们不需要确定末状态的S值,所以只有五个变量,那么用四次多项式就可以了。
◆横向轨迹的生成
横向轨迹生成用了五次多项式和二次规划两套方法,就像图中的一样…二次规划求解我没看懂,等闲下来搞定它,只知道大致原理不能算懂~额额
轨迹的评价
对于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