从入门到放弃 | 面向感知的决策规划介绍

最新推荐文章于 2025-04-09 13:58:59 发布

自动驾驶之心

最新推荐文章于 2025-04-09 13:58:59 发布

阅读量736

点赞数 1

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247629682&idx=2&sn=f1f8aa26ff5de15caee33fb548e81d0b&chksm=cf11eb7f61126b27c9b0d30f1f7a4c37110d599d58d9f1da8bcf07455f15a69f35ff9879a983&scene=126&sessionid=0

版权

作者 | Michael Zhou 编辑 | 自动驾驶之心

原文链接：https://zhuanlan.zhihu.com/p/716725077

点击下方卡片，关注“自动驾驶之心”公众号

戳我-> 领取自动驾驶近15个方向学习路线

>>点击进入→自动驾驶之心『规划控制』技术交流群

本文只做学术分享，如有侵权，联系删文

我觉的来看这篇文章的人应该是相关从业人员，所以决策规划是什么以及为什么要做决策规划就先不做赘述。

虽然搞了目录，但内容还是有些多，可以搜索关键字跳着看。

1. 组织架构

一般会有一层环境模型（车道模型、导航推荐、红绿灯决策）并行与预测（AI组），接着是决策层（变道、绕行等），然后是运动规划motion planning（横向规划，纵向规划），最后是控制。

2. 从入门到放弃

视频学习顺序：

https://space.bilibili.com/631671239/channel/collectiondetail?sid=992923
https://space.bilibili.com/287989852?spm_id_from=333.337.search-card.all.click
https://space.bilibili.com/325034144?spm_id_from=333.337.0.0

2.1 坐标系

FreeNet与笛卡尔

FreeNet优劣势

优势：

在弗莱纳坐标系下做运动规划的好处在于借助于指引线做运动分解，将高维度的规划问题，转换成了多个低维度的规划问题，极大的降低了规划的难度。
另外一个好处在于方便理解场景，无论道路在地图坐标系下的形状与否，我们都能够很好的理解道路上物体的运动关系。相较直接使用地图坐标系下的坐标做分析，使用弗莱纳坐标，能够直观的体现道路上不同物体的运动关系。

劣势：

XY坐标系下的规划器能力：为什么是xy，freenet的投影问题：

投影有误差
变量没有实际物理意义，算出来的cost有问题，
nudge场景：sl投影nudge膨胀，导致求解失败，xy最原始，贴合实际场景可以求解，
大曲率弯道，SL曲率有误差会影响车辆居中甚至会碰撞RB

对于轨迹生成，我认为在XY坐标系下，基于车辆运动学或再加动力学得到轨迹的方法更优。通过减少求解信息，达到减少耗时，提高限定时间内成功求解的概率；减少坐标系转换带来的损失。

Frenet高精度坐标转化需要使用优化算法，否则就可能存在厘米级误差，这会被城市场景中的决策判断放大。在Frenet下的障碍物筛选也没有XY下精度高，为了安全，只能提高冗余量，牺牲决策判断。

kd tree查点

笛卡尔：车道宽度，横向都可以解决

横向规划和纵向规划

速度规划的 s沿着轨迹的方向，路径规划的 s沿着参考线的方向。

左手坐标系和右手坐标系

左右顺时针为正，右手逆时针为正，注意0轴的定义

https://blog.csdn.net/zhouqiping/article/details/124522367

3. 预测

3.1 方案

DenseTNT需要四五十ms，改进后8ms。vectornet前半部分+denseTNT的轨迹输出部分

当前位置到目标位置：通过BFS搜索路径

3.2 现有问题

点的概率会有问题
argoverse数据集偏高速，导致实际场景的点的位置比较远
原有数据集没有的场景轨迹预测结果会较差，比如T型路口等

4. 环境模型

4.1 routing

比较稳定，A*
车道级导航算法开发（非高精地图）：采用AStar算法搜索2km路径，用于少变多选道、导航变道和路口选道。目前保定100km，北京泛化超过2000Km，选道成功率和导航变道触发率均在95%以上。

不同供应商的数据格式定义可能不同：

4.2 pnc map (lane graph)

4.2.1 Perception lane

无高精地图路口建模功能：

最大的问题还是强依赖与车道线平行假设。

基于SDPro地图进行路口建模，目前直行路口和少变多场景采用三次曲线、左右转采用五段式曲线（直线段、螺旋线、圆弧、螺旋线、直线段）

a. SD PRO vs HDMap：1m内，道路中心点dot_point，车道属性，有几个车道，宽度，得到拓扑map

b. routing：道路级导航，车道级导航（高德）1.映射到SDPro；2.车道级，起点是当前车道，终点无须指定，最早变道点，最晚变道点（点+buffer），右边有车卡会减速变道 road->segment->；

c. 左右转采用五段式曲线，如何保证分检点连续：转弯半径不变

d. 成功率：根据kappa是否满足最小转弯半径,R500m是个坎

输入：感知车道线，sd pro
输出：1.变道，2.laneid， 2变3，routing选用cost：车道中心线heading
红绿灯控车和红绿灯绑路：

红绿灯也是bbox,通过距离去匹配，匈牙利匹配。难点在于2匹1或者1匹2等存在感知或者地图丢失的情况。

主要根据灯箱的航向区分看哪组灯箱，以及根据车道箭头信息进行子灯颜色分类。根据灯的颜色进行决策，在上游稳定的情况下，控车成功率达到95%以上。

普通道路跟踪策略：

利用车道线平行假设，利用消失点计算pitch。为什么不用高精度定位的pitch？
对观测到的散点进行关键点BA优化，
对c0\1\2\3,width宽度进行滤波跟踪

多变少\少变多：

一般是车道线偏右，此时关键是要识别出远处（10-40m）的新增车道线，以进行点到点的连接。看不见新增的线（感知对线段起点有近距离要求？），会先往右打再往左打。

虚拟车道生成（无SDPro地图）应对场景：直行路口、道路分流合流场景（少变多、多变少）以及拥堵场景。主要分为跟车和跟线。

输入：感知车道线和障碍物
输出：local车道线

a. 跟车模式和跟线切换：看和线的距离

视觉感知车道线与EHR的融合：增减车道；高精地图的类型滞后；heading补偿（通常是0.3）；

4.2.2 HDMap

1 问题比较多，建议与视觉感知做融合

弯道点不够密
匝道处和曲率大弯道定位不准
匝道的时候，定位不准，单点rtk，依赖视觉线
heading偏差一般是两个原因：1.标定，定位装置/天线和车辆的标定 2.坐标系转换，wgs84转UTM坐标系存在畸变误差
定位的heading问题排查方法：画出车辆历史轨迹，然后和heading的线对比
怎么消除wgs84转utm时的误差 heading问题通过set_origin解决
匝道合入主路时地图要给全，防止左后方的车辆
定位偏还是控制偏：看时间戳，谁先震荡的，定位可以通过planning确认
怎么识别汇入口、分叉口：pre succ 所有车道的拓扑关系，难点在于哪些可换道,哪些不可以换道：通过图商给的host/subhost、 merge_left/right 、split_left/right
merge点不准；导航变道不准；参考线偏；
DR要退出系统，但是不要轻易进入DR

2.1.1 限速问题

2.1.2 虚实线问题

2.1.3 施工问题

2.1.4 匝道问题

2.1.5 隧道桥下树下等遮挡场景

xx只能DR5s(递推)

2.1.6 heading偏

标定误差：走直线标定
坐标转换误差：set_oring

2.1.7 方向乱晃yaw不准

检查惯导、RTK等硬件

2.2 地图质检模块的

专门需要修复层fix layer：十个图层，拓扑关系，lane、road；中心线起点终点的连续性；交通灯的判断生成停止墙

前后继点对不上的问题是由于可以允许1mm的误差：点连续性是因为utm 转换后有畸变造成，把阈值从0改为1mm就可以了；

offset个别会出现超过10（之前设置的是10）；还有一个是脚本错误码14写错了

新的检测脚本做了如下改动：1. offset 10->20, 2. 允许前后继节存在1mm误差，做出以上调整按理说错误信息多应该消失了

策略点：指引线平滑程度以及指引线的偏移策略点，比如最右的车道往左偏，避免频繁减速

黑名单：奇怪的位置，尽量不去偏远的地方，不去某些lane_id;

哪里的指引线，以及怎么偏移:最右侧，容易减速

2.3特殊点处理

贝塞尔曲线的链接，

消亡车道的处理

merge/split处理

2.4用到的数据结构：

center line，左右boundary，拓扑类型，后继车道，form/to_path_id，

3.SDmap

十字路口的重建：利用dot点，五次多项式螺旋曲线拟合

3.1 打断的规则

分界有几个原因：1.车道数目发生变化 2.车道拓扑关系发生变化 3.车道边界属性发生变化 4.限速 5.类型和颜色

就是保证了每个打断内的每根线属性不会变化

4.3 参考线生成

一些参考：

https://blog.csdn.net/sinat_52032317/article/details/128386386

https://blog.csdn.net/weixin_65089713/article/details/128855681

https://blog.csdn.net/ChenGuiGan/article/details/124633387

https://github.com/YannZyl/Apollo-Note/blob/master/docs/planning/reference_line_provider.md

4.3.1 多参考线生成

该项目对于车道行驶过程中除了当前车道参考线外，给出了(可能的)左右两条车道参考线。并且，每条参考线给出了换道类型、换道距离等信息，并将地图元素(汇入口、分叉口、公交车道等)绑定到参考线中。其目的在于帮助下游决策模块实现扩大行驶范围，扩大借道范围，避免急刹和碰撞风险等功能。在实际运营过程中，参考线数量增多至原先的1.8倍。

绑定hdmap元素，overlap属性位置等, sl坐标，l怎么确认：地图给

4.3.2 参考线平滑：

参考线平滑是有必要的，如果使用优化算法来光滑，个人感觉大材小用（不排除后续path求解强耦合很平滑的参考线，此处没有量化分析过）。个人感觉参考线平滑使用拟合就够了。

https://zhuanlan.zhihu.com/p/371585754

https://zhuanlan.zhihu.com/p/565854845

knots分段与二次规划进行参考线平滑:osqp不等式约束条件，cost，在已知box内优化，特殊点的平滑

1.1 预处理

通过第一阶段路径点采样与轨迹点矫正，可以得到这条路径的anchor_point集合,将anchor_point分成n组(n+1个knot节点)，每组用一个多项式函数去拟合，可以得到n个多项式函数

默认5m一个anchor，每段qp_spline(knot)是25m,

1.2 设置约束条件

需要拟合的多项式函数数量为2*num_spline个，每两个knots之间会拟合x和y两个多项式
多项式最高阶数为5(qp_spline.spline_order: 5)，所以每个多项式共6个参数，参数总和：(spline_order+1)2num_spline
使用每个段内的anchor point去拟合多项式函数，自变量范围[0,1]，应变量相对于第一个anchor point的相对坐标。所以最后拟合出来的函数f和g的结果是相对于第一个anchor point的相对坐标。

那么在拟合过程中还需要满足一些约束，包括等式约束和不等式约束，例如：

预测的x'和y'需要保证在真实x和y的L轴lateral_bound、F轴longitudinal_bound领域内
第一个anchor point的heading和函数的一阶导方向需要一致，大小可以不一致，但是方向必需一致！
x和y的n段函数之间，两两接壤部分应该是平滑的，两个函数值(位置)、一阶导(速度)、二阶导(加速度)必须一致。

1.3 cost函数计算

平滑度，长度，相对原始点偏离度

平滑度：FemPosSmooth相对不精准，但是只需用二次规划能快速求解

CosThetaSmooth相对精准，但是需要非线性规划，计算量大

Qk·Rk是点乘

1.4优化求解系数

求解完这个QP问题就可以得到系数矩阵，也变相得到了x和y的这n段平滑曲线，最后的工作就是包装秤一条ReferenceLine，，一共500个点

1.5平滑参考线校验

对平滑参考线SmoothReferenceLine上的点(可以采样，比如累积距离每10m采样)，投影到原始参考线RawReferenceLine的距离为l，l不能太大（阈值是6m），否则两条参考线有较大的偏移。

2,平滑参考线的拼接

复用、拼接、收缩

https://github.com/YannZyl/Apollo-Note/blob/master/docs/planning/reference_line_provider.md

5. 决策

5.1 EUDM：

用dcp-tree语义动作减少减少动作搜索空间，并通过串联动作长时间决策能力弥补MPDP不足

cfb确定周围车辆的语义意图，选出具有风险的周车意图。

belief update体现在哪？

所以POMDP方法相当于没有用现有的求解器？但感觉思路差不多呀，相当于进行了行动空间和状态空间的改进。

初始信念和更新信念在EUDM和EPSILON中如何体现的？信念更新说的一个是基于规则一个基于学习，那初始信念怎么确定？下图啥意思？

参考IGP2我们考虑短期机动意图和长期预测意图
我们用现有存在的POMDP求解器去求解POMDP问题（EUDM中相当于用自己的方法去求解，和POMCP的区别是信念更新和MCTS？）
优化动作机动和长期意图序列，可以参考之前做博弈时的一些资料？EUDM中考虑的意图只是那三个？

5.2 POMDP

Michael Zhou：behavior planning 行为规划（一POMDP）

Michael Zhou：马尔可夫决策过程

5.3 Bayesian+MDP+Probabilistic Risk(Constrained)

（这段引用自卓荦锋芒：对自动驾驶中决策算法的思考）自动驾驶的决策算法的最优解是**Bayesian+MDP+Probabilistic Risk(Constrained)**。简要说明下理由，

Bayesian：基于数据得到先验（Prior）概率，然后使用后验（Posterior）概率，保证整个算法的拟人性和可解释性。实际上PGM的模型包括三层：因果层、意图层和观测层。

MDP：非常适合做交互决策，又能保证局部最优性。

**Probabilistic Risk(Constrained)**：就功能安全而言，风险的定义包括了风险的发生概率和风险的危害程度。在城市场景下，若自动驾驶做到全路段的话，需要在有约束的概率风险下的做决策。

导航变道，施工变道，超车变道（最适合cost）：变道类型不同，

大框架是使用MDP做交互，核心是MDP要素的处理，

状态S：采用语义化行为。致使该算法是基于行为的，而不是场景的，泛化能力更强。

自车的状态转移T：采用数据学习(非DNN)+车辆运动学模型的方法，模拟学习自车的规划性能。保证最终的结果符合规划的性能，又避免采用决策+规划的耦合方式的低泛化能力。

它车的状态转移T：采用Bayesian的方法学习和推断它车行为。根据它车的历史信息，使用先验和后验计算状态的转移及对应的概率。根据先验计算状态的转移及概率，避免深度学习、强化学习的不可解释性、低泛化性。

**可违背风险的行为(R,A)**：基于风险概率选择最优的动作序列。保证拥堵场景的通勤效率。

整体设计的思想遵循：交互性、准确性、合理性。

缺点：耗时，他车意图不确定，

根据最近的研究，再结合自身的积累储备，发现一条可行的算法方向：MDP+LinearProgram+HomotopyClass+Interpolation(2D）。从理论上来说，该算法能够保证最优性和连续性，设计来源于十字路口的无保护转向场景；从可行性来讲，因为有过基于网格插值的MDP开发和凸优化的基础，所以不存在算法开发的堵塞点。

6. 横向规划

1 LaneChangeDecider产生是否换道的决策，更新换道状态：产生换道的状态，之后将结果保存在injector中

首先判断是否产生多条参考线，若只有一条参考线，则保持直行。若有多条参考线，则根据一些条件（主车的前方和后方一定距离内是否有障碍物，旁边车道在一定距离内是否有障碍物）进行判断是否换道，当所有条件都满足时，则进行换道决策。

// 纵向安全距离  2*adc_l + 1.2 * adc_v
const double common_safety_distance = config_.safety_distance_l_weight() * adc_l + config_.safety_distance_v_weight() * adc_v;
// input
Process(Frame* frame)
std::list<ReferenceLineInfo>* reference_line_info =
    frame->mutable_reference_line_info();
std::map<planning::ReferenceLineType, ReferenceLineInfo*>* reference_line_info_map =
    frame->mutable_reference_line_info_map();
// output
auto* prev_status = injector_->planning_context()
                        ->mutable_planning_status()
                        ->mutable_change_lane();
prev_status->set_is_clear_to_change_lane(is_clear_to_change_lane);
prev_status->set_left_lane_boundary_dist(left_lane_boundary_dist);
prev_status->set_right_lane_boundary_dist(right_lane_boundary_dist);

换道决策决定ADC是否进行换道。目前Apollo的体系是当有多条参考线时即进行换道。

如果不换道，在PathBoundsDecider中会将l ll的边界限制在本车道内(如果不借道)；
如果换道，在PathBoundsDecider中会将l ll的边界向目标车道一侧进行拓展；

1.process()
LaneChangeDecider 的主要决策逻辑在Process() 方法中
根据reference line的数量判断是否处于变道场景中，size() > 1则处于变道过程中，需要判断变道的状态

只有一条reference line，没有进行变道
有多条reference line，说明处在变道中
如果上一时刻处在变道中，根据上一时刻自车所处道路ID与当前时刻所处道路ID对比，来确认变道状态
// 1. 换道参考线选择决策，输出选择的ReferenceLineType
// 2. 安全空间判断以及对应换道参考线上是否有危险的障碍物
    // 此处判断传进来的referenceLineinfo是否是变道参考线，如果是则通过
    // IsClearToChangeLane()检查该参考线是否满足变道条件，
    // IsClearToChangeLane只考虑传入的参考线上的动态障碍物，不考虑虚的和静态的障碍物。疑点：为什么只/考虑动态障碍物？
// 3. 删除其它参考线
// 4. 换道状态机
    // 头次进入task，车道换道状态应该为空，默认设置为换道结束状态

2.PrioritizeChangeLane()
在调整参考线的顺序时，使用了PrioritizeChangeLane() 函数，它的调整参考线顺序的功能，需要配置enable_prioritize_change_lane为True，这个函数的完整代码及注释如下：

void LaneChangeDecider::PrioritizeChangeLane(
const bool is_prioritize_change_lane,
std::list<ReferenceLineInfo>* reference_line_info) const {
if (reference_line_info->empty()) {
AERROR << "Reference line info empty";return;
}
const auto& lane_change_decider_config = config_.lane_change_decider_config();
// TODO(SHU): disable the reference line order change for now
  // 如果没有配置变道优先，则退出该函数
  if (!lane_change_decider_config.enable_prioritize_change_lane()) {return;}auto iter = reference_line_info->begin();while (iter != reference_line_info->end()) {ADEBUG << "iter->IsChangeLanePath(): " << iter->IsChangeLanePath();/* is_prioritize_change_lane == true: prioritize change_lane_reference_line
       is_prioritize_change_lane == false: prioritize
       non_change_lane_reference_line */// 0、is_prioritize_change_lane 根据参考线数量置位True 或 False
    // 1、如果is_prioritize_change_lane为True
    // 首先获取第一条参考线的迭代器，然后遍历所有的参考线，
    // 如果当前的参考线为允许变道参考线，则将第一条参考线更换为当前迭代器所指向的参考线,
    // 注意，可变车道为按迭代器的顺序求取，一旦发现可变车道，即推出循环。
    //
    // 2、如果is_prioritize_change_lane 为False，
    // 找到第一条不可变道的参考线，将第一条参考线更新为当前不可变道的参考线
    if ((is_prioritize_change_lane && iter->IsChangeLanePath())
    ||(!is_prioritize_change_lane && !iter->IsChangeLanePath())) {
    ADEBUG << "is_prioritize_change_lane: " << is_prioritize_change_lane;
    ADEBUG << "iter->IsChangeLanePath(): " << iter->IsChangeLanePath();
    break;
    }
    ++iter;
    }
    reference_line_info->splice(reference_line_info->begin(),*reference_line_info, iter);ADEBUG << "reference_line_info->IsChangeLanePath(): "<< reference_line_info->begin()->IsChangeLanePath();
}

3.IsClearToChangeLane() 判断当前的参考线是否变道安全
sClearToChangeLane() 遍历了当前参考线上所有目标，并根据目标的行驶方向设置安全距离，通过安全距离判断是否变道安全，代码及注释如下：

bool LaneChangeDecider::IsClearToChangeLane(ReferenceLineInfo* reference_line_info) {// 或得当前参考线的s坐标的最大最小值，以及自车速度
  double ego_start_s = reference_line_info->AdcSlBoundary().start_s();double ego_end_s = reference_line_info->AdcSlBoundary().end_s();double ego_v =std::abs(reference_line_info->vehicle_state().linear_velocity());// 遍历每个目标
  for (const auto* obstacle :reference_line_info->path_decision()->obstacles().Items()) {// 跳过静止与虚拟目标
    if (obstacle->IsVirtual() || obstacle->IsStatic()) {ADEBUG << "skip one virtual or static obstacle";continue;}// 初始化
    double start_s = std::numeric_limits<double>::max();double end_s = -std::numeric_limits<double>::max();double start_l = std::numeric_limits<double>::max();double end_l = -std::numeric_limits<double>::max();// 遍历当前目标的预测轨迹点集，或得预测轨迹的边界点
    for (const auto& p : obstacle->PerceptionPolygon().points()) {SLPoint sl_point;reference_line_info->reference_line().XYToSL(p, &sl_point);start_s = std::fmin(start_s, sl_point.s());end_s = std::fmax(end_s, sl_point.s());start_l = std::fmin(start_l, sl_point.l());end_l = std::fmax(end_l, sl_point.l());}// 如果目标距离当前参考线距离太远， 则跳过该目标
    if (reference_line_info->IsChangeLanePath()) {static constexpr double kLateralShift = 2.5;if (end_l < -kLateralShift || start_l > kLateralShift) {continue;}}// Raw estimation on whether same direction with ADC or not based on
    // prediction trajectory
    // 判断车与障碍物是否同一方向行驶，同时设置安全距离。之后判断距离障碍物距离是否安全
    // 根据航向角判断是否为相同方向
    bool same_direction = true;if (obstacle->HasTrajectory()) {double obstacle_moving_direction =obstacle->Trajectory().trajectory_point(0).path_point().theta();const auto& vehicle_state = reference_line_info->vehicle_state();double vehicle_moving_direction = vehicle_state.heading();if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {vehicle_moving_direction =common::math::NormalizeAngle(vehicle_moving_direction + M_PI);}double heading_difference = std::abs(common::math::NormalizeAngle(obstacle_moving_direction - vehicle_moving_direction));same_direction = heading_difference < (M_PI / 2.0);}// TODO(All) move to confs
    static constexpr double kSafeTimeOnSameDirection = 3.0;static constexpr double kSafeTimeOnOppositeDirection = 5.0;static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;static constexpr double kDistanceBuffer = 0.5;double kForwardSafeDistance = 0.0;double kBackwardSafeDistance = 0.0;// 根据方向设置安全距离
    if (same_direction) {kForwardSafeDistance =std::fmax(kForwardMinSafeDistanceOnSameDirection,(ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);kBackwardSafeDistance =std::fmax(kBackwardMinSafeDistanceOnSameDirection,(obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);} else {kForwardSafeDistance =std::fmax(kForwardMinSafeDistanceOnOppositeDirection,(ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;}// 判断障碍物是否满足安全距离
    if (HysteresisFilter(ego_start_s - end_s, kBackwardSafeDistance,kDistanceBuffer, obstacle->IsLaneChangeBlocking()) &&HysteresisFilter(start_s - ego_end_s, kForwardSafeDistance,kDistanceBuffer, obstacle->IsLaneChangeBlocking())) {reference_line_info->path_decision()->Find(obstacle->Id())->SetLaneChangeBlocking(true);ADEBUG << "Lane Change is blocked by obstacle" << obstacle->Id();return false;} else {reference_line_info->path_decision()->Find(obstacle->Id())->SetLaneChangeBlocking(false);}}return true;
}

2 PathReuseDecider路径是否可重用,提高帧间平顺性

主要判断是否可以重用上一帧规划的路径。若上一帧的路径未与障碍物发生碰撞，则可以重用，提高稳定性，节省计算量。若上一帧的规划出的路径发生碰撞，则重新规划路径。

1.PathReuseDecider 是第二个task,作用是
根据横纵向跟踪偏差，来决策是否需要重新规划轨迹
如果横纵向跟踪偏差，则根据上一时刻的轨迹生成当前周期的轨迹，以尽量保持轨迹的一致性

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER
}

PublicRoadPlanner 的 LaneFollowStage 配置了以下几个task 来实现具体的规划逻辑，判断上一帧轨迹是否与障碍物碰撞

2.使用path_reuse的好处：
黑色轨迹为采用path_reuse，可以看到整个换道过程，轨迹完全一致; 红色、黄色轨迹为重新规划轨迹并采用trajectory_stitcher 拼接。速度和位置都会有问题。https://zhuanlan.zhihu.com/p/524300384 可以看到在轨迹拼接处以及整体轨迹可能出现不连贯不平滑的情况，可能会影响控制的平顺性：

时为了保证轨迹的连续性，当前拍的规划起点应该选在last_traj离pos_cur最近的投影点上（一般情况下需要增加dt的向前预测量）。得到该投影点的信息后，即可规划出蓝色cur_traj曲线。即使当前位置不在轨迹上，但我们规划出来的轨迹与上一拍规划轨迹完全重合。从而保证了轨迹的连续性，使得控制连贯，不会产生跳变的情况。
值得注意的是，这里需要设定pos_cur和投影点的偏差阈值，若两者距离太大，说明控制无法跟上规划的轨迹。此时，应该考虑实际位置做进一步的规划（比如设定阈值30cm，自车位置离轨迹线35cm，此时可以设定起点为离投影点20cm的地方作为规划的起点，而不是继续拿投影点作为起点继续规划。主要是因为此时偏差较大，继续使用投影点规划会导致控制的超调，从而引起更大的画龙。）。

3.PathReuseDecider 只对外暴露了Process() 一个接口，它的主要逻辑如下：
（1）当前处于非LaneFollow_Scenario场景，置位false
（2）当前未处于IN_CHANGE_LANE状态，置位false;
（3）如果存在可变车道，且已经完成换道轨迹生成，则置位false;
（4）前一时刻采用path_reuse, 若轨迹重规划、轨迹与静态障碍物发生碰撞、轨迹长度过短以及纵向求解失败，则置位false;
（5）只有前方静止障碍物走开(或大于阈值)、纵向求解成功、未与静态障碍物发生碰撞且轨迹长度大于阈值，才可置位true;
4.当path_reusable置位后，后续的task会跳过处理的过程

// skip path_lane_borrow_decider if reused path
  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {// for debug
    AINFO << "skip due to reusing path";return Status::OK();}

5.一旦path_reusable置位，则使用上一周期轨迹生成当前周期的规划轨迹
6.在判断是否 path_reusable时，会调用IsCollisionFree 判断静态目标是否安全
3 PathLaneBorrowDecider
产生是否借道的决策：当产生阶导决策时，会将相应标志位置true。同时，在进行借道决策时，会对左右借道进行判断。借道的状态保存在injetor里。

该决策有以下的判断条件：• 是否只有一条车道• 是否存在阻塞道路的障碍物• 阻塞障碍物是否远离路口• 阻塞障碍物长期存在• 旁边车道是实线还是虚线当所有判断条件都满足时，会产生借道决策。
ADC在借道工况中：判断本车道可通过性，如果在连续n nn(参数配置)帧规划中本车道可以通行，则取消借道。

Process(
    Frame* const frame, ReferenceLineInfo* const reference_line_info)
// input
*frame
// output
reference_line_info->set_is_path_lane_borrow(

ADC不在借道工况中：ADC需要同时满足以下条件才可以进入借道工况：

必须只有一条参考线；（是否只有一个障碍物）
规划起点的速度不能过高(参数配置)；
不能在SIGNAL、STOP_SIGN 和Junction附近；
不能在终点附近；
Block Obstacle在ADC一定范围内，并且堵塞原因不是Traffic Flow；
地图车道线线型（虚线）允许借道；（实线还是虚线）
如果借道，在PathBoundsDecider中会将l ll的边界借道方向一侧进行拓展。
是否长期存在

作用主要是换道时：

已处于借道场景下判断是否退出避让；
未处于借道场景下判断是否具备借道能力。

1.PathLaneBorrowDecider只是判断是否满足借道条件
具体的轨迹是否借道，是由后面的task决定
2.Process()
借道决策器的主要功能为判断当前车辆是否具备借道能力，其实现在类PathLaneBorrowDecider的成员函数process()中。process()函数的功能一共分为三部分：检查输入、如果路径复用则跳过借道决策、判断当前街道状态。
2.1判断是否借道IsNecessaryToBorrowLane（）
借道判断主要通过核心函数IsNecessaryToBorrowLane()判断是否借道，主要涉及一些rules，包括距离信号交叉口的距离，与静态障碍物的距离，是否是单行道，是否所在车道左右车道线是虚线等规则。主要有两个功能：
（1）已处于借道场景下判断是否退出避让；
（2）未处于借道场景下判断是否具备借道能力。
需要满足下面条件才能判断是否可以借道：

只有一条参考线，才能借道
起点速度小于最大借道允许速度
阻塞障碍物必须远离路口
阻塞障碍物会一直存在
阻塞障碍物与终点位置满足要求
为可侧面通过的障碍物

2.2CheckLaneBorrow()
主要根据前方道路的线型判断是否可以借道；在此函数中2m间隔一个点遍历车前100m参考线或全部参考线，如果车道线类型为黄实线、白实线则不借道。
2.3IsNonmovableObstacle()
IsNonmovableObstacle() 中主要对前方障碍物进行判断，利用预测以及参考线的信息来进行判断：

目标太远不借道
目标停止借道
目标

3.结果
PathLaneBorrowDecider 的输出结果存在mutable_path_decider_status当中，

4 PathBoundsDecider

产生路径边界：根据现有决策在参考线上进行采样，获得每个点在l ll的边界。有四种边界决策：GenerateRegularPathBound（自车道行驶）、GenerateFallbackPathBound（失败回退）、GenerateLaneChangePathBound、GeneratePullOverPathBound。最后将边界保存在SetCandidatePathBoundaries中，供下一步使用。

利用前几个决策器，根据相应条件，产生相应的SL边界。这里说明以下Nudge障碍物的概念——主车旁边的障碍物未完全阻挡主车，主车可以通过绕行避过障碍物（注意图中的边界）。它的作用主要是：

根据借道信息、道路宽度生成FallbackPathBound
根据借道信息、道路宽度生成、障碍物边界生成PullOverPathBound、LaneChangePathBound、RegularPathBound中的一种

PathBoundsDecider会根据换道决策和借道决策生成相应的l ll的边界。
FallbackBound+PullOverBound；
FallbackBound+LaneChangeBound；
FallbackBound+NoBorrow/LeftBorrow/RightBorrow；
不管在何种决策下，PathBoundsDecider都会生成一条FallbackBound，其与NoBorrow的区别是，不会删除Block Obstacle后道路边界。

// intput
    // Initialization.
    InitPathBoundsDecider(*frame, *reference_line_info);
// output
PathBound fallback_path_bound;
PathBound lanechange_path_bound;
PathBound regular_path_bound;
PathBound -> PathBoundary -> candidate_path_boundaries.emplace_back(regular_path_boundary);
reference_line_info->SetCandidatePathBoundaries(
    std::move(candidate_path_boundaries));

1.计算path 的boundary
PathBoundsDecider根据lane borrow决策器的输出、本车道以及相邻车道的宽度、障碍物的左右边界，来计算path 的boundary，从而将path 搜索的边界缩小，将复杂问题转化为凸空间的搜索问题，方便后续使用QP算法求解
2.输出是对纵向s等间距采样，横向s对应的左右边界
PathBoundsDecider计算path上可行使区域边界，其输出是对纵向s等间距采样，横向s对应的左右边界，以此来作为下一步路径搜索的边界约束；与其他task一样，PathBoundsDecider的主要逻辑在Process() 方法中。
在Process() 方法中分四种情况对pathBound进行计算，按照处理顺序分别是fallback、pullover、lanechange、regular，不同的boundary对应不同的应用场景，其中fallback对应的path bound一定会生成，其余3个只有一个被激活，即按照顺序一旦有有效的boundary生成，就结束该task。
3.关于ADC_bound 与lane_bound的计算

（1）根据车道生成左右的lane_bound，从地图中获得；根据自车状态生成adc_bound， adc_bound = adc_l_to_lane_center_ + ADC_speed_buffer + adc_half_width + ADC_buffer
上式中的各项：
adc_l_to_lane_center_ - 自车
adc_half_width - 车宽
adc_buffer - 0.5
ADCSpeedBuffer表示横向的瞬时位移，公式如下：

其中kMaxLatAcc = 1.5
（2）根据ADC_bound 与lane_bound 生成基本的bound
左侧当前s对应的bound取MAX(left_lane_bound,left_adc_bound), 即取最左边的
右侧当前s对应的bound取MIN(right_lane_bound,right_adc_bound)，即取最右边的
4.InitPathBoundsDecider()
在Process()过程中，首先会初始化bounds，用规划起始点自车道的lane width去初始化 path boundary，如果无法获取规划起始点的道路宽度，则用默认值目前是5m，来初始化。
path_bounds由一系列采样点组成，数据结构如下，这组数据里共有0～199个200个采样点，每两个点的间隔是0.5m；每个点由3个变量组成，分别是根据参考线建立的s-l坐标系的s坐标，右边界的l取值，左边界的s取值：
5.GenerateFallbackPathBound()
无论当前处于何种场景，都会调用GenerateFallbackPathBound() 来生成备用的path bound，在计算FallbackPathBound时不考虑障碍物边界，仅使用道路边界，并考虑借道信息来进行计算。
// Generate the fallback path boundary.
// 生成fallback_path_bound，无论何种场景都会生成fallback_path_bound
// 生成fallback_path_bound时，会考虑借道场景，向哪个方向借道，对应方向的path_bound会叠加这个方向相邻车道宽度
PathBound fallback_path_bound;Status ret =// 生成fallback_path_bound，
// 首先计算当前位置到本车道两侧车道线的距离；
// 然后判断是否借道，并根据借道方向叠加相邻车道的车道宽度
// 带有adc的变量表示与自车相关的信息
GenerateFallbackPathBound(*reference_line_info, &fallback_path_bound);if (!ret.ok()) {ADEBUG << "Cannot generate a fallback path bound.";return
Status(ErrorCode::PLANNING_ERROR, ret.error_message());}if
(fallback_path_bound.empty()) {const std::string msg = "Failed to get a valid fallback path boundary";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}if (!fallback_path_bound.empty()) {CHECK_LE(adc_frenet_l_, std::get<2>
(fallback_path_bound[0]));CHECK_GE(adc_frenet_l_, std::get<1>
(fallback_path_bound[0]));}// Update the fallback path boundary into the reference_line_info.
// 将fallback_path_bound存入到candidate_path_boundaries
std::vector<std::pair<double, double>> fallback_path_bound_pair;for (size_t i = 0; i < fallback_path_bound.size(); ++i) {fallback_path_bound_pair.emplace_back(std::get<1>
(fallback_path_bound[i]),std::get<2>
(fallback_path_bound[i]));}candidate_path_boundaries.emplace_back(std::get<0>
(fallback_path_bound[0]),kPathBoundsDeciderResolution,fallback_path_bound_pair);candidate_path_boundaries.back().set_label("fallback");
6.障碍物边界生成
（1）首先筛选障碍物，障碍物筛选规则如下，需要符合以下所有的条件，才加到obs_list中：

不是虚拟障碍物
不是可忽略的障碍物（其他decider中添加的ignore decision）
静态障碍物或速度小于FLAGS_static_obstacle_speed_threshold（0.5m/s）
在自车的前方

（2）将每个障碍物分解成两个ObstacleEdge，起点一个终点一个，记录下s，start_l,end_l,最后按s排序

7.根据场景生成另外一条path_bound依次判断是否处于pull_over、lane_change、regular；当处于其中一个场景，计算对应的path_bound并返回结果；即以上3种场景，只会选一种生成对应的根据场景生成另外一条path_bound。
这3种path bound均考虑了障碍物的边界，用障碍物的边界来修正path bound：

首先根据借道与否，用道路宽度来生成基本的path bound
然后遍历path上的每个点，并且判断这个点上的障碍物，利用障碍物的边界来修正path bound
如果目标在左边将left_bound 设置为目标右边界
如果目标在右边，将right_bound设置为目标的左边界

Status PathBoundsDecider::GenerateLaneChangePathBound(const ReferenceLineInfo& reference_line_info,std::vector<std::tuple<double, double, double>>* const path_bound) {// 1. Initialize the path boundaries to be an indefinitely large area.
  // 1、用numeric 的最大最小值初始化path bound
  if (!InitPathBoundary(reference_line_info, path_bound)) {const std::string msg = "Failed to initialize path boundaries.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// PathBoundsDebugString(*path_bound);


  // 2、用当前车道宽度计算path bound，同时考虑lane borrow，如果借道将目标车道的宽度叠加
  // 2. Decide a rough boundary based on lane info and ADC's position
  std::string dummy_borrow_lane_type;if (!GetBoundaryFromLanesAndADC(reference_line_info,LaneBorrowInfo::NO_BORROW, 0.1, path_bound,&dummy_borrow_lane_type, true)) {const std::string msg ="Failed to decide a rough boundary based on ""road information.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// PathBoundsDebugString(*path_bound);


  // 3、在换道结束前，用ADC坐标到目标道路边界的距离，修正path bound
  // 3. Remove the S-length of target lane out of the path-bound.
  GetBoundaryFromLaneChangeForbiddenZone(reference_line_info, path_bound);PathBound temp_path_bound = *path_bound;std::string blocking_obstacle_id;// 根据path上的障碍物修正path_bound，遍历path上每个点，并在每个点上遍历障碍物；
  // 如果目标在左边将left_bound 设置为目标右边界
  // 如果目标在右边，将right_bound设置为目标的左边界
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),path_bound, &blocking_obstacle_id)) {const std::string msg ="Failed to decide fine tune the boundaries after ""taking into consideration all static obstacles.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// Append some extra path bound points to avoid zero-length path data.
  int counter = 0;while (!blocking_obstacle_id.empty() &&path_bound->size() < temp_path_bound.size() &&counter < kNumExtraTailBoundPoint) {path_bound->push_back(temp_path_bound[path_bound->size()]);counter++;}ADEBUG << "Completed generating path boundaries.";return Status::OK();
}

5 PiecewiseJerkPathOptimizer

PiecewiseJerkPathOptimizer 是lanefollow 场景下，所调用的第 5 个 task，属于task中的optimizer类别
基于二次规划算法，对每个边界规划出最优路径.调用piecewise_jerk_problem类进行求解，会设置一些权重以及一些约束，利用Optimize函数进行求解。
reference_line_info_->GetCandidatePathBoundaries();保存候选路径。

// input
auto &reference_line = reference_line_info->reference_line();
auto &init_point = frame->PlanningStartPoint();
const auto& path_boundaries = reference_line_info->GetCandidatePathBoundaries(); 
PathData path_data = *reference_line_info->mutable_path_data();
const double lat_acc_bound =
    std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) /
    veh_param.wheel_base();
 
// output

问题点：

约束设计时，曲率模型做了近似，假设太理想，
特点(1) 平滑程度高，cost_fuc中加入了目标轨迹与QP输出轨迹的贴切程度
(2) 复杂度问题，为了保证安全性，QP中的约束条件Piecewise也几乎都需要使用，一般通过第三方osqp开源库进行解算，解算耗时有较大波动
(3) 舒适度，差分考虑加加速度，对舒适性指标进行平滑
(4) 拓展性，cost_fuc可自定义
1.它的作用主要是：

1、根据之前decider决策的reference line和 path bound，以及横向约束，将最优路径求解问题转化为二次型规划问题；
2、调用osqp库求解最优路径；

2.数学模型
https://www.chuxin911.com/osqp-introduction_20210715/
2.1二次规划标准型
https://blog.csdn.net/sinat_52032317/article/details/128406586

2.2定义优化变量

路径规划一般是在Frenet坐标系中进行的。s为沿着参考线的方向，l为垂直于坐标系的方向。

根据导引线和障碍物生成路径边界
将导引线在s方向等间隔采样
对每个s方向的离散点迭代的优化 , ', '' 。

一些设置
delta s : --path_bounds_decider_resolution=0.5
最大迭代次数：（15会掉帧）6
dl error 最大是0.02满足
2.3 设计目标函数

l的三阶导数，dddl可通过二阶导差分的方式获取。

piecewise-jerk method

横向位置的l三阶导数如下图所示，

图：dddl的表达式
另外，为考虑path的连续性下方的l,l',l''等式约束需要考虑到约束条件中，如下图，

图：l,dl,ddl的等式约束
整个过程的路径规划的目标函数为：

路径优化的目标函数

可变的道路边界对车辆运行学约束，道路曲率的等式约束如下，kr，kr_dot为当前点的曲率参数，△θ为车辆的航向角与当前点曲率的差值。

道路曲率与车辆的关系

QP问题的表达形式:

将bound转为qp格式
bound主要由三部分组成：

在path bound decider中获取的上下边界
l' l'' l'''的约束
起点约束
数学约束，上述的l,l',l'',l'''的数学关系

所以需要将目标函数J 以及边界bound的约束条件改为QP 问题的形式，这里Python代码在考虑约束时忽略了车辆运动学的约束，在Apollo 的算法中还考虑了车辆运动学的约束。
将路径优化问题转化为QP问题的方法如下：
（1）用path上每个采样点的横向坐标，横向一阶导、二阶导构建X 矩阵：
（2）通过展开多项式推导后，按下列公式构建P、q矩阵：

（3）根据上下边界的约束，构建A 矩阵：
（4）根据path bound的约束，构建l、u矩阵：
整个formulate problem的过程，通过以上步骤完成了P,q,A,LB,UB五个矩阵，接下来使用osqp求解器进行求解需要满足半正定(https://www.cnblogs.com/icathianrain/p/14407626.html)
2.4构建约束
曲率约束：根据坐标系转换，kappa(xy) = kappa(frent) + kappa(refenrence) 。代码里优化的是kappa(frenet)。但是在曲率比较大的地方，前面的等式不成立，导致这个约束并不准确。所以业界才考虑在xy坐标下做规划。

2.4.1约束总结

3.流程
(1) 初始化
获取障碍物信息(SL图)、Frenet坐标信息、车辆信息(位姿、车辆自身几何约束)、QP后的轨迹
(2) 纵向离散化
对于每一个离散点，计算其纵向通行区域(lmin,lmax)

(3) 选择优化变量
对于第2步中的横向偏移量，求解一阶导和二阶导，表征车辆靠近与偏离指引线的趋势同时，对相邻离散点，通过差分计算三阶导数
量化表示: 离散点i，横向偏移li，一二三阶导数分别为li'，li''，li'''

(4) 优化目标设定
(1) 车辆贴近参考线
(2) 一二三阶导数尽可能小，控制横向加速度，保证舒适性

备注: 可以根据需求，选择性的加入与边界距离的优化目标，使车辆与障碍物保持适当距离
4.求解
求解器：考虑曲率才用的是非线性的osqp，如果不加曲率约束的话就用QP，不用迭代。

6 PathAssessmentDecider

路径评价，选出最优路径。调用ComparePathData函数，对路径进行两两比较。 依据以下规则，进行评价。路径是否和障碍物碰撞路径长度路径是否会停在对向车道路径离自车远近哪个路径更早回自车道…

路径两两进行对比，选出最优的路径。
PathAssessmentDecider 是lanefollow 场景下，所调用的第 6 个 task，属于task中的decider 类别它的作用主要是：

选出之前规划的备选路径中排序最靠前的路径；
添加一些必要信息到路径中

PathAssessmentDecider会依据设计好的规则筛选处最终的path，并在规划路径上的采样点添加标签（IN_LANE、OUT_ON_FORWARD_LANE、OUT_ON_REVERSE_LANE等），作为路径筛选的依据，并为速度规划提供限制。
路径筛选的规则是：

不能偏离参考线和Road太远；
不能和Static Obstacle相撞；
不能停止在对向车道上；
选择优先级最高的Path，排序规则：

Regular path优先于fallback path；
如果两条路径至少有一条是self_lane，并且两条路径长度的差大于15m，选择路径长的；
如果两条路径至少有一条是self_lane，并且两条路径长度的差小于5m，是self_lane的；
如果两条路径都不是self_lane，并且两条路径长度的差大于25m，选择路径长的；
选择占据反向车道更少的路径；
是否停到对向车道
如果有block obstacle，选择占据空间少的方向的路径；
如果没有block obstacle，选择ADC靠近方向的路径，使车辆少打方向盘；
选择返回本车道更早的路径；
在上述情况无法区分的情况下选择左侧的路径；

7. PathDecider

根据选出的路径给出对障碍物的决策

若是绕行的路径，则产生绕行的决策；若前方有障碍物阻塞，则产生停止的决策。
PathDecider 是lanefollow 场景下，所调用的第 7 个 task，属于task中的decider 类别它的作用主要是：
在上一个任务中获得了最优的路径，PathDecider的功能是根据静态障碍物做出自车的决策，对于前方的静态障碍物是忽略、stop还是nudge

遍历每个障碍物, 根据规则判断前面优化并筛选出来的path生成对应的decisions(GNORE, STOP, LEFT NUDGE, RIGHT NUDGE等)。

对以有IGNORE/STOP/KEEP_CLEAR决策的obstacle不做处理；
如果是block obstacle，并且不是借道工况，设为STOP决策；
不在path纵向范围内的障碍物设为IGNORE决策；
对于碰撞的obstacle，设为STOP决策；
根据位置关系设置LEFT NUDGE或者RIGHT NUDGE的决策；

8 RuleBasedStopDecider

RuleBasedStopDecider 是lanefollow 场景下，所调用的第 8 个 task，属于task中的decider 类别它的作用主要是：

根据一些规则来设置停止标志。

横向输出

message PathPoint {
  // coordinates
  optional double x = 1;
  optional double y = 2;
  optional double z = 3;


  // direction on the x-y plane
  optional double theta = 4;
  // curvature on the x-y planning
  optional double kappa = 5;
  // accumulated distance from beginning of the path
  optional double s = 6;


  // derivative of kappa w.r.t s.
  optional double dkappa = 7;
  // derivative of derivative of kappa w.r.t s.
  optional double ddkappa = 8;
  // The lane ID where the path point is on
  optional string lane_id = 9;


  // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
  optional double x_derivative = 10;
  optional double y_derivative = 11;
  // Ali-XLab-Begin motai.yy 2019-10-11
  optional double slope_angle = 12 [default = 0.0];  // [rad]
  // Ali-XLab-End
}

纵向输出

message SpeedPoint {
  optional double s = 1;
  optional double t = 2;
  // speed (m/s)
  optional double v = 3;
  // acceleration (m/s^2)
  optional double a = 4;
  // jerk (m/s^3)
  optional double da = 5;
}

纵向：
https://blog.csdn.net/weixin_56492465/article/details/128870540?spm=1001.2101.3001.6650.1&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-128870540-blog-122009754.pc_relevant_multi_platform_whitelistv3&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-128870540-blog-122009754.pc_relevant_multi_platform_whitelistv3&utm_relevant_index=2
https://blog.csdn.net/mpt0816/article/details/122009754
优化模型离散方式
在处理最优化问题时，一般会转化成离散形式，将轨迹s ( t ) s(t)s(t)按照某参数离散，并计算离散点处的约束和c o s t costcost。不同的离散方式会构建不同的优化模型。对于速度规划问题，一般可以按照等间距离散(Spatial Parameter Discretization)和等时间离散(Temporal Parameter Discretization)。
1.1.1 Spatial Parameter Discretization
使用纵向距离s ss作为离散采样参数，假设第一个采样点s = 0 s=0s=0，连续采样离散点之间等间隔Δ s \Delta sΔs离散，那么优化问题的决策变量为每个离散点的s ˙ , s ¨ 和相邻采样点之间的时间间隔Δ t \Delta tΔt。

优点：
参考线或者规划路径的曲率κ \kappaκ是纵向距离s ss的函数，因此向心加速度约束是容易处理的；
地图限速等速度约束是和纵向距离s ss相关的，那么优化问题的s ˙ 约束容易计算；
缺点：
优化问题的目标函数难以建模。Apollo规划算法用来优化纵向冲击度最小，冲击度使用相邻两个离散点的差分计算，如果采用Δ s \Delta sΔs离散会使决策变量之间有除法运算，形成非凸优化问题；
难以施加安全约束。障碍物一般投影在ST Graph中，会得到每个时刻下的安全行驶范围( s m i n , s m a x ) )，即可行驶区域s f r e e ( t ) (t)是时间t tt的函数。
1.1.2 Temporal Parameter Discretization
使用时间t tt作为离散采样参数，假设第一个采样点t = 0 t=0t=0，连续采样离散点之间等间隔Δ t \Delta tΔt离散，那么优化问题的决策变量为每个离散点的s , s ˙ , s ¨ cost funtion

使用等时间间隔离散的方式，和使用等距离离散方式的优缺点恰好相反。
优点：
优化模型可以保持凸包性质，冲击度的计算仍然是线性形式；
障碍物的安全约束容易施加在优化模型中；
缺点：
由于曲率是s ss的函数，因此向心加速度约束难以处理；
速度约束难以处理；
最小任务完成时间的优化目标难以处理；
处理方法：
使用规划路径的最大曲率计算向心加速度限制；
将离散点处的位置s ss与DP得到纵向位置s ss的差作为目标函数的一部分，使用DP结果在采样时刻t处的s处对应的速度限值；
将离散采样点处的速度s ˙ 与目标速度(reference speed)的差作为目标函数的一部分。以保证完成任务花费的时间最少；
此外，Apollo还设计了非线性速度规划来处理以上问题。
const funtion

约束
等式约束

不等式约束

7.纵向规划

7.1 STBoundsDecider

百度已经关掉此task，区间过于宽泛。
作用

生成DrivableSTBoundary 供dp搜索的时候用
对不影响纵向规划的障碍物设置IGNORE属性;

// 输入：
const PathData &path_data = reference_line_info->path_data();
PathDecision *path_decision = reference_line_info->path_decision();
// 输出：
regular_st_bound, regular_vt_bound
RecordSTGraphDebug(st_boundaries, regular_st_bound, st_guide_line,
                   &st_graph_debug);
// 1.Initialize the related helper classes
InitSTBoundsDecider(*frame, reference_line_info);
// 1.1st_driving_limits_初始化
st_obstacles_processor_.MapObstaclesToSTBoundaries(path_decision);
st_driving_limits_.Init(max_acc, max_dec, max_v,
                        frame.PlanningStartPoint().v());
// 2.GenerateRegularSTBound,Sweep the t-axis, and determine the s-boundaries step by step
// 2.1依据运动学driving_limits更新s_lower, s_upper
auto driving_limits_bound = st_driving_limits_.GetVehicleDynamicsLimits(t); 
// 2.2.依据自车运动学约束，剔除不可达的障碍物decision
 
STBoundsDecider是来计算ST Graph中st_drivable_boundary_。由三个模块的计算组成：

依据障碍物st_boundary约束，更新可通行s_bounds以及对应的决策(

GetBoundsFromDecisions),对于同一障碍物的st_boundary可能存在多个决策，比如overtake或yield,见GetSBoundsFromDecisions函数;
本模块障碍物的ST图计算耗时也较高，但st_boundary精度较低;同理最终生成的st_drivable_boundary_边界精度也较低。
如果在下游gridded_path_time_graph模块中FLAGS_use_st_drivable_boundary不置位时,则无需使用该模块输出的st_drivable_boundary；因此可结合实际项目需求，来精简该模块的冗余计算。

7.2 SPEED_BOUNDS_PRIORI_DECIDER

作用：
得到ST Graph中的st_boundaries
产生速度可行驶边界

所形成的区域是非凸的，不能用之前
凸优化的方法去做，需要用动态规划的方法去做。
（1）将规划路径上障碍物的st bounds 加载到路径对应的st 图上
（2）计算并生成路径上的限速信息

// 输入：
// retrieve data from frame and reference_line_info
const PathData &path_data = reference_line_info->path_data();
const TrajectoryPoint &init_point = frame->PlanningStartPoint();
const ReferenceLine &reference_line = reference_line_info->reference_line();
PathDecision *const path_decision = reference_line_info->path_decision();
// 输出:
boundaries, min_s_on_st_boundaries, speed_limit
st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,
                        speed_limit, reference_line_info->GetCruiseSpeed(),
                        path_data_length, total_time_by_conf);

SpeedBoundsPrioriDecider对应的Decider或者说class是SpeedBoundsDecider，是计算每个障碍物的STBoundary得到ST Graph中的st_boundaries_和speed_limit_(SpeedLimit)。其中每个障碍物的STBoundary的计算其实是根据遍历规划时间内障碍物和规划路径的碰撞关系来计算的，并且会根据是否对障碍物做过纵向决策以及决策类型设置其STBoundary的
characteristic_length。
speed_limit_(SpeedLimit)则是根据三个方面计算的：

地图限速；

double speed_limit_from_reference_line =
    reference_line_.GetSpeedLimitFromS(reference_line_s);

向心加速度限制；sqrt(a/kappa) a= 2

const double speed_limit_from_centripetal_acc =
    std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() /
              std::fmax(std::fabs(discretized_path.at(i).kappa()),
                        speed_bounds_config_.minimal_kappa()));

对于近距离nudge障碍物的减速避让；

//1.先判断是否左/右靠太近
//2.然后根据动静态障碍物设置限速比例
speed_limit_from_nearby_obstacles =
    nudge_speed_ratio * speed_limit_from_reference_line;

7.3 PathTimeHeuristicOptimizer

非凸变凸 -> DP
动态规划
动态规划——通过把原问题分解为相对简单的子问题，再根据子问题的解来求解出原问题解的方法

状态转移方程

1.目标：产生粗糙速度规划曲线

PathTimeHeuristicOptimizer是速度规划的DP过程。速度规划需要在凸空间内求解，然而由于障碍物等原因会使求解空间变成非凸的，因此需要DP算法得到对不同障碍物的决策，从而得到一个凸空间。另一方面，最优化问题一般都仅有局部寻优能力，可能会收敛到某个局部最优解。为了保障优化问题的求解质量，使用全局“视野”的DP算法快速生成粗糙的初始解，并从该初始解的领域开展局部优化可以使最优化问题收敛到全局近似解。

SPEED_HEURISTIC_OPTIMIZER 是lanefollow 场景下，所调用的第 11个 task，属于task中的optimizer 类别，它的作用主要是：

apollo中使用动态规划的思路来进行速度规划，其实更类似于使用动态规划的思路进行速度决策；
首先将st图进行网格化，然后使用动态规划求解一条最优路径，作为后续进一步速度规划的输入，将问题的求解空间转化为凸空间

// 输入:
init_point,  path_data
// 输出
(使用speed_bounds_decider的reference_line_info_->st_graph_data())
speed_data（s, t, v）

2. 流程

基于动态规划的速度规划的流程如下：
对路程和时间进行采样2.搜索出粗略的可行路线3.选出代价最小的一条

速度规划在ST图进行采样，在t 的方向上以固定的间隔进行采样，在s方向上以先密后疏的方式进行采样（离主车越近（10m），所需规划的精度就需更高；离主车越远，牺牲采样精度，提升采样效率）

// 时间采样的一般参数设置
unit_t: 1.0  //采样时间
dense_dimension_s: 101  // 采样密集区域的点数
dense_unit_s: 0.1   //采样密集区域的间隔
sparse_unit_s: 1.0  //采样系数区域的间隔

代码总流程如下：

1、遍历每个障碍物的boundry，判度是否有碰撞风险，如果有碰撞风险使用fallback速度规划；
2、初始化cost table
3、按照纵向采样点的s，查询各个位置处的限速
4、搜索可到达位置
5、计算可到达位置的cost
6、搜索最优路径

2.1采样：
如上
2.2状态转移方程
一个点的total_cost由四部分构成：

obstacle_cost：障碍物cost
spatial_potential_cost：空间位置cost，于终点s的差值
前一个点的total_cost
EdgeCost：边界约束由三部分构成Speedcost、AccelCost、JerkCost

在更新父节点时同样有一个剪枝操作，使用限速信息FLAGS_planning_upper_speed_limit得到pre_lowest_s，进而将寻找范围限制在[r_low, r]，其中r为当前行号，因为EM Planner主要是前进场景，不会考虑倒车情况，那么S值是递增或不变，不会下降，所以r最大也就当前行号

2.2.2.1 障碍物cost计算

S_safe_overtake超车的安全距离S_safe_follow跟车的安全距离在设计状态转移方程时，要求不能与障碍物发生碰撞以及和障碍物不发生碰撞。于是可以得到以下方程：

如果在障碍物距离之内，则cost设为无穷；如果在安全距离之外，则cost设为0；如果在安全距离与障碍物之间，则按按之间的距离计算。
2.2.2.2 距离cost计算
目的是更快的到达目的地

距离cost计算方式如下：

2.2.2.3 状态转移cost计算
状态转移cost计算分为三个部分：

越靠近0，代价值越小；越靠近目标值，代价值越大，满足舒适性与平滑性。

加加速度的计算方式如下：

7.4 SpeedDecider

产生速度决策

根据粗规划出的速度曲线，依据曲线在障碍物的上方还是下方，采取不同的决策。

// 输入：
init_point_， adc_sl_boundary_， reference_line_，
MakeObjectDecision(reference_line_info->speed_data(),
                        reference_line_info->path_decision())
// 输出：
mutable_obstacle->AddLongitudinalDecision()

SpeedDecider 是lanefollow 场景下，Apollo Planning算法所调用的第12个 task，属于task中的decider 类别它的作用主要是：

1、对每个目标进行遍历，分别对每个目标进行决策
2、或得mutable_obstacle->path_st_boundary()
3、根据障碍物st_boundary的时间与位置的分布，判断是否要忽略
4、对于虚拟目标 Virtual obstacle，如果不在referenceline的车道上，则跳过
5、如果是行人则决策结果置为stop
6、SpeedDecider::GetSTLocation() 获取障碍物在st图上与自车路径的位置关系
7、根据不同的STLocation，来对障碍物进行决策
8、如果没有纵向决策结果，则置位ignore_decision；

SpeedDecider根据DP算法生成的速度曲线s ( t ) s(t)s(t)和障碍物的STBoundary的位置关系生成Ignore、Stop、Follow、Yield、Overtake的纵向决策。

Ignore：

障碍物的STBoundary在ST Graph的范围内；
已经有纵向决策的障碍物；
类型是禁停区的obstacle的STBoundary位于速度曲线的下方；

Stop：

前方是行人的停车决策；
类型是禁停区的obstacle的STBoundary位于速度曲线的上方；
在ADC前方低速行驶的堵塞道路的障碍物；
STBoundary和速度曲线相交的障碍物；

Follow：

STBoundary位于速度曲线上方，且不需要Stop的障碍物；

Yield：

STBoundary位于速度曲线上方，且不需要Stop和Follow的障碍物；

Overtake：

STBoundary位于速度曲线下方的障碍物；
**13 SPEED_BOUNDS_FINAL_DECIDER **产生速度规划边界

在障碍物的上方或下方确定可行使区域。
SpeedBoundsFinalDecider对应的Decider同样是SpeedBoundsDecider，和SpeedBoundsPrioriDecider不同的是配置参数，从Apollo中的默认配置参数来看，

7.5 SpeedBoundsFinalDecider

会根据DP过程生成的决策结果和更小的boundary_buffer生成更加精确的STBoundary。

// 输入：
const PathData &path_data = reference_line_info->path_data();
const TrajectoryPoint &init_point = frame->PlanningStartPoint();
const ReferenceLine &reference_line = reference_line_info->reference_line();
PathDecision *const path_decision = reference_line_info->path_decision();
// 输出：
std::vector<const StBoundary *> boundaries,const double min_s_on_st_boundaries,
SpeedLimit speed_limit,
st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,
                        speed_limit, reference_line_info->GetCruiseSpeed(),
                        path_data_length, total_time_by_conf);

配置
// 1. Map obstacles into st graph
// 2. Create speed limit along path
// 3. Get path_length as s axis search bound in st graph
// 4. Get time duration as t axis search bound in st graph

SPEED_BOUNDS_FINAL_DECIDER 是lanefollow 场景下，所调用的第 13 个 task，属于task中的decider 类别它的作用主要是：

（1）将规划路径上障碍物的st bounds 加载到路径对应的st 图上(并且根据障碍物决策生成boundary类型)
（2）计算并生成路径上的限速信息

7.5 PiecewiseJerkSpeedOptimizer

产生平滑速度规划曲线

根据ST图的可行驶区域，优化出一条平滑的速度曲线。满足一阶导、二阶导平滑（速度加速度平滑）；满足道路限速；满足车辆动力学约束。
PIECEWISE_JERK_SPEED_OPTIMIZER 基于二次规划的速度规划
PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER 基于非线性规划的速度规划
两者二选一即可
速度规划的优化求解即是按照上述的算法原理实现的。
可以看到，在此会依据纵向决策结果生成纵向位移的约束边界，将每个时刻和cruise speed的误差作为优化目标的一部分，并且根据DP结果在每个时刻处位移的速度约束作为优化问题的速度约束边界，因此将每个时刻的位移和DP的位置之差作为了优化目标的一部分，但是这样只能实现速度的软约束。
PiecewiseJerkSpeedOptimizer 是lanefollow 场景下，所调用的第 14个 task，属于task中的decider 类别它的作用主要是：

1、根据之前decider决策的speed decider和 speed bound，以及纵向约束，将最优速度求解问题转化为二次型规划问题；
2、调用osqp库求解最优路径；

//输入：
PathData& path_data(get 曲率)，
common::TrajectoryPoint& init_point，
SpeedData* const speed_data
//优化变量x dx ddx 
//目标函数set_x_ref，set_dx_ref，set_weight_ddx，set_weight_dddx
//构建约束set_x_bounds，set_penalty_dx，set_dx_bounds，set_ddx_bounds，set_dddx_bound
//优化器求解piecewise_jerk_problem.Optimize()
penalty_dx.push_back(std::fabs(path_point.kappa()) *
                        config.kappa_penalty_weight());
piecewise_jerk_problem.set_penalty_dx(penalty_dx);                        


//输出：s ds dds
// Update STBoundary by boundary_type影响s上下边界
speed_data->AppendSpeedPoint(s[i], delta_t * i, ds[i], dds[i],
                            (dds[i] - dds[i - 1]) / delta_t);
配置
default_task_config: {
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_process_type: ON_REFERENCE_LINE
  piecewise_jerk_speed_optimizer_config {
    acc_weight: 1.0                # ddx
    jerk_weight: 3.0               # dddx
    kappa_penalty_weight: 2000.0   # kappa penalty_dx
    ref_s_weight: 10.0             # x
    ref_v_weight: 10.0             # dx
  }
}
default_task_config: {
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_process_type: ON_REFERENCE_LINE
  piecewise_jerk_nonlinear_speed_optimizer_config {
    acc_weight: 2.0                # ddx
    jerk_weight: 3.0               # dddx
    lat_acc_weight: 1000.0         # kappa
    s_potential_weight: 0.05       # add
    ref_v_weight: 5.0              # dx
    ref_s_weight: 100.0            # x
    soft_s_bound_weight: 1e6       # add
    use_warm_start: true           # add
  }
}

动态规划得到的轨迹还比较粗糙，需要用优化的方法对轨迹进行进一步的平滑。基于二次规划的速度规划的方法与路径规划基本一致。

确定优化变量
设计目标函数
设计约束

二次规划

这些成员函数之间的相互依赖关系见下图，其中Optimize( )是入口
调用FormulateProblem( )完成二次规划问题的建模，针对二次规划问题CalculateKerne( )完成目标函数中的H矩阵计算，CalculateOffset( )完成目标函数中的g矩阵计算，CalculateAffineConstraint( )完成约束条件中的A矩阵计算
最后借用osqp求解器完成求解

1.1.确定优化变量

1.2设计目标函数

对于目标函数的设计，我们需要明确以下目标：

1.3.要满足的约束条件

1.4.转化为二次规划问题求解

代入OSQP求解器进行求解，输出一条平稳、舒适、能安全避开障碍物并且尽快到达目的地的速度分配曲线。
二次规划速度规划算法的问题

基于二次规划的速度规划中，p i 是曲率关于时间t tt的函数，但实际上路径的曲率是与s 相关的。二次规划在原先动态规划出来的粗糙ST曲线上将关于s 的曲率惩罚转化为关于t 的曲率惩罚，如此，当二次规划曲线与动态规划曲线差别不大，规划出来基本一致；若规划差别大，则会差别很大。就如图所示，规划出来的区间差别较大。限速/曲率的函数是关于s 的函数，而s 是我们要求的优化量，只能通过动态规划进行转化，如此就会使得二次规划的速度约束不精确。
二次规划与非线性的选择：
二次规划在dp的ST曲线基础上将关于s的曲率惩罚转化为关于t的曲率惩罚，导致对于道路限速的反应不准确，如下图，sv图，绿线是地图实际限速，蓝线为二次规划求解所得。但是求解效率高。
非线性优化主要考虑曲率的约束，惩罚是关于s的函数，对于限速和曲率是更加准确和严格的；二次规划是关于t的函数，从dp的st图转化过来的，是对t的约束

非线性优化

// 输入：
scont PathData& path_data，const common::TrajectoryPoint& init_point SpeedData* const speed_data


const auto problem_setups_status = SetUpStatesAndBounds(path_data, *speed_data);
const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration, st_graph_data->mutable_st_graph_debug());
const auto speed_limit_smooth_status = SmoothSpeedLimit();
const auto nlp_smooth_status =OptimizeByNLP(&distance, &velocity, &acceleration);
 
// 输出：
x,dx,ddx
speed_data->AppendSpeedPoint(
    distance[i], delta_t_ * i, velocity[i], acceleration[i],
    (acceleration[i] - acceleration[i - 1]) / delta_t_);

2.1优化变量
基于非线性规划的速度规划步骤与之前规划步骤基本一致。

采样方式：等间隔的时间采样。s l o w e r s_{lower}与 s_{upper}为松弛变量，防止求解失败。2.2目标函数

2.3制定约束

将所有的限速函数相加，得到下图的限速函数，很明显，该函数既不连续也不可导，所以需要对其进行平滑处理。

对于限速曲线的平滑，Apollo采样分段多项式进行平滑，之后采样二次规划的方式进行求解。限速曲线的目标函数如下：

如此，我们就有了连续且可导的限速曲线。再回到约束中,为了避免求解的失败，二次规划中对位置的硬约束，在非线性规划中转为了对位置的软约束。提升求解的精度。

同时还需满足基本的物理学原理

2.4求解器求解
最后代入Ipopt中进行非线性规划的求解。 Ipopt（Interior Point Optimizer）是一个用于大规模非线性优化的开源软件包。它可用于解决如下形式的非线性规划问题：

Apollo星火计划学习笔记——Apollo速度规划算法原理与实践
https://blog.csdn.net/sinat_52032317/article/details/128456949

8. CombinePathAndSpeedProfile

将SL曲线、ST曲线合成为完整轨迹，之后作为Planning的输出。

8.1 trajectory plan

更好的位形组成在上面的3个变量基础上加入车辆的即时转向曲率κ：如果车辆的导向轮保持一定的角度，车辆会做圆周运动。这个圆周运动的半径就是即时转向曲率 κ。加入κ，有助于控制模块获得更准确的控制反馈，设计更为精细平稳的控制器。
轨迹规划的正式定义，计算一个以时间t为参数的函数S，对于定义域内（[0, t_max]）的每一个t，都有满足条件的状态 s：满足目标车辆的运动学约束，碰撞约束，以及车辆的物理极限。

https://www.cnblogs.com/liuzubing/p/11051390.html

8.2 FallBack

GenerateFallbackPathBound
fallback是其他3种场景计算PathBound失败时的备选，只考虑自车信息和静态道路信息，不考虑静态障碍物。因此，speed decider负责让自车在障碍物前停车。

Status PathBoundsDecider::GenerateFallbackPathBound(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
  // 1. Initialize the path boundaries to be an indefinitely large area.
  if (!InitPathBoundary(reference_line_info, path_bound)) { ... }
  // 2. Decide a rough boundary based on lane info and ADC's position
  std::string dummy_borrow_lane_type;
  if (!GetBoundaryFromLanesAndADC(reference_line_info,
                                  LaneBorrowInfo::NO_BORROW, 0.5, path_bound,
                                  &dummy_borrow_lane_type)) { ... }
  return Status::OK();
}

GenerateFallbackSpeed

SpeedData SpeedProfileGenerator::GenerateFallbackSpeed( const EgoInfo* ego_info, const double stop_distance) // s的最大范围应该符合车辆动力学，即车辆按照最大a和da进行刹车时，车辆在此范围内可以停下来，否则会造成优化问题无解 // 对于乘用车来说100m的刹车距离是足够了

『自动驾驶之心知识星球』欢迎加入交流！重磅，自动驾驶之心科研论文辅导来啦，申博、CCF系列、SCI、EI、毕业论文、比赛辅导等多个方向，欢迎联系我们！

① 全网独家视频课程

端到端自动驾驶、仿真测试、自动驾驶C++、BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、CUDA与TensorRT模型部署、大模型与自动驾驶、NeRF、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内外最大最专业，近4000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（端到端自动驾驶、世界模型、仿真闭环、2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频