文档维护:Arvin
网页部署:Arvin
▶
写在前面:本文内容是作者在深蓝学院自动驾驶预测与决策规划学习时的笔记,作者按照自己的理解进行了记录,如果有错误的地方还请执政。如涉侵权,请联系删除。
时空联合规划
传统的时空分离规划只考虑了路径规划和速度规划,分别对应横向控制和纵向控制。而时空联合规划算法则同时考虑空间和时间来规划轨迹,在路径基础上再求解速度从而形成轨迹,能够能直接在x−y−t(即平面和时间)三个维度的空间中直接求解最优轨迹。
时空分离规划:
时空联合规划:
基于搜索(Hybrid A*)的时空联合规划方法
构建三维时空联合规划地图
-
构建二维栅格地图
二维x-y栅格地图只具有几何属性,无法直接搜索带有时间属性的可行驶轨迹。
(灰色圆形区域部分:在最大转向角下也会发生碰撞 或 不可能到达目的地 ;
半径大小:最小转弯半径;
方向:车速方向相切;)
-
沿时间轴扩展生成三维时空地图
如下图所示,多个地图层相互平行;相邻图层中的两个状态根据时间步长由有向边相连;蓝色有向边连接的点序列表示以Δt为时间步长离散化的时空轨迹;绿色部分:动态障碍物在Δt为时间步长离散化的位置(由轨迹预测模块给出)
三维时空地图中包含了如下信息:
- 车辆位置状态信息
- 车辆运动学信息:
- 蓝色有向线段在X-O-T平面的投影斜率为横向速度。
- 蓝色有向线段在Y-O-T平面的投影斜率为纵向速度。
- 蓝色有向线段在X-O-Y平面的投影倾斜角为偏航角
- 根据相邻状态可以计算动作空间
- 根据动作空间扩展时空地图(图中由蓝色实线、虚线所示)
基于Hybrid A*的时空节点扩展
离散化前轮转角集合Δ :
Δ={δi∣−δmax+(i−1)⋅N−1δmax−(−δmax)}
其中δi为离散后的前轮转角。
离散加速度集合A:
A={−amax,...,0,...,amax}
状态更新方程:
⎩⎨⎧si=sp+vp⋅dt⋅cosθili=lp+vp⋅dt⋅sinθiθi=θp+vi⋅dt⋅tanδi/Lvi=vp+ai⋅dtti=tp+dt
上面是将车辆运动连续动作根据运动学模型离散化,接下来进行Hybrid A的算法设计(Hybrid A算法原理参阅之前文章)
-
设计启发函数
Hi=wH1⋅(sgoal−si)2+(lgoal−li)2+wH2⋅(tgoal−ti)
其中sgoal、lgoal分别是目标节点的纵向、横向位置。tgoal为目标节点对应的相对时间,避免陷入时间停滞,加快搜索过程;wH1与wH2是对应权重。
-
设计成本函数
Gi=Gp+wg1⋅Ei+wg2⋅Ci+wg3⋅Si
其中Gp为父节点成本,wg1,wg2,wg3为权重系数。
Ei为子节点目标代价:
Ei=∣vi−vexpected∣
Ci为子节点舒适性代价:
Ci=∣li−lcenterline∣
Si为子节点安全性代价:
Si=R(s,l)⋅exp(cosθr)
R(s,l)=exp[−21×(σsg2(s−us)2+σlg2(l−ul)2)]
r=(s−us,l−ul)
(s,l),(ul,us)分别表示节点和障碍物的位置;σsg,σlg分别表示障碍物在s向、l向的权重系数;r为节点相对于障碍物的位置向量;θ+r为向量r与障碍物速度v之间的夹角。
算法流程:
-
以当前自车所在位置(s1,l1,θ1,v1,t1)为起点进行时空节点扩展;
-
扩展过程中考虑节点无碰撞和道路边界约束问题进行检查;
-
通过节点代价评估寻找最优扩展节点,直至达到目标节点;
-
通过各个最优节点得到初始粗轨迹。
基于迭代的时空联合规划方法
由于时空联合规划方法加上了时间的维度,因此基于搜索的方法搜索的轨迹空间十分庞大。减少时间和空间上的搜索复杂度,使规划聚焦于最优解可能出现的区域。
基于时空走廊的规划方法
参考文献:Multipolicy and Risk-aware Contingency Planning for Autonomous Driving
在高度交互的环境中完成安全和高效的规划任务是一项具有挑战的任务,这是由于交通参与者的随机性以及它们与自车的隐式互动,无法直接观测它们的多模态的意图。对于这种交互决策问题,一般有两种主流方法:第一种是马尔可夫决策过程(MDP:Markov decision process)和 部分可观察马尔可夫决策过程(POMDP: partially observable Markov decision process),这种方法存在一个缺点,随着问题规模的增加(也就是参与交互的障碍物越多),计算量剧增;为了减少计算量,可以使用规则进行剪枝,将原始问题分解成有限数量的闭环策略评估。然而,剪枝的缺点是,很难设计出好的规则去适应所有的场景。
第二种方法是数值优化方法(如:contigency planning),它是在运动规划层为多种可能的未来生成应急计划。通过优化树状(tree-structured)轨迹来处理环境不确定性,其中每个分支都好考虑了潜在的风险,但contigency planning 对周围障碍物的选择是一个问题。
而为了解决上述问题,MARC算法,结合前面两种方法的优点,将行为和运动规划层进行组合。首先利用前向模拟(Forward Simulations)生成基于语义级别的关键场景集(具有动态分支点的场景树,使用分叉点减少相同部分的计算量),然后使用Risk-aware Contiency Planning(使用线性规划器LP和迭代线性二次调节器iLQR)进行优化,生成考虑多样化风险和未来场景的轨迹树,最后进行cost计算,选择出合适的场景和轨迹。
**语义时空走廊(Semantic Spatio-Temporal Semantic Corridor)**一系列被语义要素约束的相互连接的无碰撞立方体。
语义时空走廊构建
障碍物要素:
约束要素:
-
速度约束:道路限速,停止标志
-
时间约束:变道时长
-
软约束与硬约束
-
语义边界的概念:特定语义约束开始和停止生效的位置,例如速度约束的纵向范围[sbegin,send],变道时长约束的横向范围[dbegin,dend]
输入数据:
-
其他车辆的未来状态(MPDM前向模拟)
-
自车初始状态
-
语义边界
-
s−l−t构型空间
时空走廊轨迹轨迹规划
其中具体的方法
-
保证安全性和可行性的轨迹生成
-
轨迹表示
每个时空走廊中的立方体对应着一段贝塞尔曲线,每个维度σ∈{s,l}各用一条n段的贝塞尔轨迹表示,pij对应着第j段轨迹的第i个控制点,t0,t1,...,tn是每段轨迹的初始和末尾时刻,使用尺度因子αj保证轨迹定义在[0,1]的区间上
α1⋅i=0∑mpi1⋅bmi(α1t−t0),t∈[t0,t1]fσ(t)=⎩⎨⎧α2⋅∑i=0mpi2⋅bmi(α2t−t1),t∈[t1,t2]⋮αn⋅∑i=0mpin⋅bmi(αnt−tn−1),t∈[tn−1,tn]
-
轨迹优化(QP问题)
最小化jerk,目标函数:
Ji=ωs∫tj−1tj(dt3d3fs(t))2dt+ωl∫tj−1tj(dt3d3fl(t))2dt
约束:
-
利用凸包性质得到的安全和动力学约束,βj,−σ,(k),βj,+σ,(k)表示第j段轨迹k阶导数的上下界
βj,−σ,(k)≤dtkdkfjσ(t)≤βj,+σ,(k),σ∈{s,l},k=0,1,2
-
首末状态约束
dtkdkf0σ(t0)=σt0(k),dtkdkfnσ(tn)=σtn(k)
-
连续性约束
dtkdkfjσ(tj)=dtkdkfj+1σ(tj)
参考代码:https://github.com/HKUST-Aerial-Robotics/EPSILON/blob/master/util/ssc_planner/src/ssc_planner/ssc_planner.cc