自动驾驶规划与决策之动态规划思想
自动驾驶规划与决策之动态规划思想
动态规划(Dynamic Programming)是一种解决多阶段决策问题的算法,其基本思想是将复杂的原始问题分解成若干个易于解决的子问题,然后求解子问题的最优解,最后通过组合子问题的最优解得到原问题的最优解。
使用动态规划做路径决策的一般步骤为:
1.以车辆在参考线上的投影点为坐标原点,建立 Frnet 坐标系,将静态障碍物投影到该坐标系上,即构建 S-L 图,并确定规划起点的坐标。车辆在参考线上的投影点坐标为(0,l0),考虑到规划 周期与控制周期的不一致以及控制是不完美的,本周期规划起点的坐标取决于本周期车辆实际位置与上个规划周期车辆期望位置之间的误差,若误差过大,则以车辆投影点(0,l0)为规划起点,强制修正误差;若误差不大,则可用上一规划周期车辆期望位置(s0,l0)为规划起点;
2.构建用于评价路径的代价函数。代价函数一般考虑三个要素:路径要尽可能平滑、尽可能贴近参考线、与障碍物要保持一定距离,设 S-L 图中的路径为l=f(s),代价函数的一般形式为:
其中,为平滑代价,对于一个五次多项式,其平滑代价由一阶导数项目、二阶导数项目和三阶导数项组成,即希望该曲线弧长较小贴近直线且曲率和曲率的变化率较小,由于积分处理较为复杂,故此处使用离散化求和的方式代替,一般形式为:
为障碍物距离代价,设障碍物坐标为(sobj,lobj),首先设计如式(52)的函数,其输入路段点与障碍物之间的距离,输出该路段点的障碍物代价,
其中,dlb为距离下界,小于该阈值则认为发生碰撞,代价无穷大;dub为距离上界,距离大于该阈值则认为不会发生碰撞,代价为 0;距离在上下界之内时,则认为有碰撞的可能,代价随距离的减小而增大。故此可得障碍物代价为:
3.在 S-L 图上以规划起点为中心网格化取点,如图(1)所示,使用五次多项式连接路段点。
五次多项式使用六个边界条件确定其六个系数,以此获得连接两个路段点的轨迹。对于规划起点到第一层中路段点P1
的路径,由于规划起点有朝向 heading 约束,故其边界条件一般为:
其中,为规划起点在大地坐标系下与 x 轴方向的夹角,表示车辆当前的朝向,
表示一种几何关系,用于限制车辆在 S-L 图中的朝向。
对于网格中间的路段点(sij,lij)和(sik,lik)其中j不等于k,由于此处求的粗解仅用于开辟凸空间,故边界条件可设为:
4.使用动态规划算法求解出连接从规划起点到当前规划周期终点 s 所在层中某一点的代价最小的一条路径,即为粗解。首先考虑如图(13)左子图的简单情形,假设Pij表示第 i 行第 j 列的路段点,cost(P0,Pij),则有:
假设三者中cost(P0,P21)最小,则P0到第一层的最优路径为进而可以求得P0到P12的最小代价,和 同理可求,比较三者大小,假设最小,则可求得规划起点P0到第 2 层的最优路径为,如图(2)右子图所示。
求解从规划起点到第 n 层的最优路径,即求一点集使得路径代价
最小。即可类比上述的简单情形,要求起点到第 n 层的最优路径,可先求到第 n-1 层的最优路径,并以此类推到求起点到第一层的最优路径,求解起点到第一层的最优路径是一个简单的问题,故此可以反推出起点到第 n 层的最优路径,如图(3)所示。
图 3 动态规划决策结果
需要注意的是,由于龙格现象的存在,即使用高次多项式拟合点集会出现震荡,故决策路径和规划路径中的路段点都应尽可能使用分段低次多项式去拟合,而非高次多项式。
5.基于粗解构建凸空间。根据步骤 4 求解得到的点集,其中每个路段点都对应一个上下区间
就构成了用于路径优化求解的凸空间,如图(4)所示。
文章详情
Details
넳
넲