路径规划算法

三、路径规划算法

（⼀）路径规划算法总体介绍

⾸先进⾏路径规划，然后进⾏速度规划

         

路径规划主要对于静态障碍物，速度规划主要针对动态障碍物配置⽂件中，DECIDER都是决策器，OPTIMIZER都是优化器下⾯具体讲路径规划task做什么

⾸先是四种决策

（1）Task: LANE_CHANGE DECIDER

         

如果有⼀条参考线就不换道，如果多条就判断需不需要换道所有条件满⾜会产⽣换道决策

（2）

         

路径规划中，每⼀帧都规划，如果感知识别出来的障碍物bounding box不稳定，每⼀帧也会随着bounding box的改变⽽上下跳动，带来的问题就是⾏驶中的不稳定，路径根据障碍物上下跳动

所以Apollo引⼊路径重⽤决策，如果上⼀帧路径没和障碍物发⽣碰撞，这⼀帧路径直接复⽤上⼀帧路径，不会再进⾏接下来的路径规划，直接跳到速度规划的部分。

如果上⼀帧的路径和障碍物存在碰撞，就继续进⾏后⾯的路径规划

（3）

主要处理⾃⻋道前⽅有没有⼀个阻挡⾃⾝的障碍物

         

决策也是⼀些判断条件：

如果只有⼀条⻋道，就没法借道， 如果其他⻋道有障碍物，

如果障碍物距离路⼝⽐较近，也不借道。

障碍物是够⻓期存在。旁边⻋道是实线还是虚线。条件满⾜后会产⽣向左或者向右的决策。

（4）

         

SL坐标系内，确定路径可以⾛的边界范围，使⽤前⼏个决策器产⽣候选边界：

如果只是⾃⻋道⾏驶，就按照⾃⻋道边界

如果是Nudge障碍物，（Nudge障碍物指的是前⽅障碍物没有完全阻挡住⾃⾝⻋道的障碍物），我们会采⽤障碍物的边界以及⾃⻋道边界

向左借道超⻋，边界采⽤被超⻋的边界和换道的边界组成换道决策，采⽤当前⻋道和换道的边界组成

（5）

最后是⼀个优化算法

         

规划出⼀条平滑路径

（6）Task: PATH ASSESSMENT_DECIDER

路径规划后，产⽣多条路径，再对你每条路径进⾏打分

         

⽐如向左向右和当前⻋道的路径，最后进⾏评价，基于规则，选出最优路径从上到下按照规则选择

（）

         

根据路径对障碍物赋予决策，⽐如最后是向左绕⾏的路径，会对绕⾏障碍物进⾏决策，就产⽣⼀个停⽌的决策，这个决策主要是对速度规划使⽤

（⼆）基于⼆次规划的路径规划算法

⼆次规划标准型

         

求解⼆次型的，线性约束的优化问题

P是⼆次项系数，P是⼀个正定矩阵，为什么是正定矩阵呢？

答：正定矩阵保证了x^TPx⼤于等于0，这样才会有⼀个极⼩值点

q是⼀次项系数，是⼀个n维实数向量A是约束函数的⼀次项系数，

l和u是m维向量，上边界和下边界

常⽤开源的求解器：OSQP，qpOASES OSQP使⽤ADMM求解，Apollo主要采取OSQP

⼆次规划流程

         

定义优化变量

⾸先需要把障碍物投影到参考线上

先把障碍物投影到参考线，然后再以delta s进⾏离散成s0到sn-1

         

选择纵向的l作为优化的变量

此外还需要选取l的⼀阶导dl和⼆阶导ddl作为优化变量

         

⽬标函数设计

根据实际需求来设计⽬标函数：安全性，舒适性，以及接近终点

         

         

w为惩罚项

三阶导可以使⽤相邻两个⼆阶导求得

         

         

         

         

         

然后写成矩阵形式

         

然后说⼀下约束设计

（1）主⻋必须在道路边界内

       

         

         

bound由之前task确定的

（2）运动学限制

         

对于曲率的公式做⼀些假设，

       

         

         

得到ddl的约束

此外还需要满⾜曲率变化率的要求，也就是规划出的路径， 能让⽅向盘最⼤打的过来

         

         

         

假设在路径上是匀速的，带⼊三阶导公式

       

         

         

得到了三阶导边界约束总结：

         

此外要满⾜基本物理原理

         

也就是三阶导恒定，⼆阶三阶导连续转化为约束矩阵

第⼀部分l约束，然后dl的约束，以及ddl约束，然后是连续性约束最后是起点硬性约束

         

然后是带⼊OSQP进⾏求解

OSQP是⼀种求解QP问题的数值优化开源软件求解分为四步

         

设置求解器参数，这⾥都是⽤的默认参数

         

         

然后是计算QP系数矩阵

         

CalculateKernel是计算⼆次型系数P矩阵， AffineCons是A矩阵，Offset是q向量

         

算完之后调⽤setuo函数去求解

       

         

         

获取优化值

         

         

（三）路径规划算法执⾏流程

         

         

         

         

⾸先获取参考线平滑结果，然后⽣成⼀系列任务每⼀个决策都是⼀个Process

         

         

输⼊的为参考线info，frame（这⼀帧的总的数据结构）

         

⾸先是换道决策

         

         

         

结果保存在injector中，是⼀个单例类，保存的变量会贯穿始终，因为好⼏帧都要依靠它来

然后是借道决策

         

判断结果使⽤truefalse来保存，向左向右保存在injector中

       

         

         

第三步是路径边界产⽣

         

       

         

         

最后把它保存在参考线info内

         

最后进⾏路径⽣成，调⽤优化器

         

每条路径保存在候选路径内最后进⾏路径结果⽐较

       

         

         

路径规划算法实践