【论文阅读】TRO 2021: Fail-Safe Motion Planning for Online Verification of Autonomous Vehicles Using Convex Optimization

参考与前言

Last edited time: August 3, 2022 10:04 AM
Status: Reading
Type: TRO
Year: 2021
论文链接：https://ieeexplore.ieee.org/document/9302873

1. Motivation

safe motion planning对自动驾驶很重要，其中online verification 能保证自动驾驶车辆不会出现事故，但是现有的方法存在下列缺点：

  1. online verification并不高效，一般需要20HZ或更高的频率
  2. 在一些极端的情况下 全时间段内不能完全避免碰撞
  3. 在不安全场景，大多方法都缺少提供alternative motion plans

因聪明需要 所以总结了一下，感兴趣可看，大致了解一下无人车的规划相关工作

trajectory planning

discrete planning比较受欢迎，主要是离散化搜索空间、状态和输入空间等，motion primitives主要就是提前计算好轨迹快，然后通过search-based方案concatenaated到一起，这一步骤通常是offline的，所以可以应用于较为复杂的车辆模型，比如multibody modesl[19]

OS: 估计是将轨迹点预录下来的形式，或者建完图后有free的全局轨迹点 搜素空间

  • Sampling-based: RRT为主的，比较适合高维的搜索空间，但是由于随机采样，通常无法在时间内获得optimal motion
  • Graph-search: 比如state lattices，同属discrete planning。获取一系列轨迹，其goal states是预定义好的grid上的点，使用 lattice 结构表示。state lattices使用了优化来实现jerk-optimal。通常lattices能构建 可行驶的轨迹，但是 due to grid, it lack optimality and completeness

虽然discrete planning是比较容易实现并有效的解决motion问题的，但是缺点也很明显：也正是因为离散，所以可能在safety-critical scenarios with small and convoluted solution spaces 无法求解；同样的原因 也会导致在fail to determine trajectories ending in small safe terminal sets

故连续的model predictive control通过minimizing cost function 来生成无碰撞的轨迹，比如混合整数优化、sequential quadratic programs；因为求解的问题通常是非凸的，所以求解起来较为困难无法做到实时性；另一个则是可能造成local minimal

通常可以把非凸问题转成凸问题，比如线性化 vehicle dynamics，将motion分为横向和纵向的；[47]-[51] 说明了这一方法可以在全局收敛的情况下有效解决优化问题；但因为方法上分横纵，可能导致无法在复杂场景下获得可行轨迹。我们通过focusing on simple evasive maneuvers and providing safe fallback solutions来解决这一问题 → 对应Section VI.

safety verification

In theorem proving, desired system properties are formulated using logical formulas，然后verification步骤主要是checking the satisfiability of the logical formulas

如果从未进入inevitable collision states 则认为是安全的，ICS是一种状态：所有车辆自身可能的轨迹都会和障碍物相撞，ICS reason over infinite time horizons，在随机的交通场景 确定ICS是比较耗时的，大部分工作为提高效果，仅考虑其他参与者的一条轨迹。

为补充ICS，controlled invariant sets 保证了persistent feasibility，根据定义就是根据其他参与者，如果在CIS内的每个状态都存在一个无碰撞轨迹，则说明车辆是安全的。因为对障碍物未来状态的未知，计算CIS在动态环境中比较challenging

Set-based reachability analysis 通过计算所有的feasible轨迹，检查是否存在无碰撞轨迹；但是这一方法中，因为对未来所有的可能状态进行考虑，unsafe regions may grow rapidly。 导致的结果就是 刚规划的轨迹会被人为potentially unsafe，leaving AV without a safe trajectory. 在本篇文章中，我们通过结合fail-safe planning来结合reachable sets在verficiation中以解决这一问题

responsibility-sensible safety RSS 也是一种formal safety model

Contribution

主要贡献是展示了一种 verification technique 来保证车辆的安全性 弥补上面提到的以下不足，从上面抽取出来的：

  • 相关规划在方法上分横纵，可能导致无法在复杂场景下获得可行轨迹。本文通过focusing on simple evasive maneuvers and providing safe fallback solutions来解决这一问题 → 对应Section VI.
  • 计算CIS在动态环境中比较challenging
  • 因为对未来所有的可能状态进行考虑，unsafe regions may grow rapidly。 导致的结果就是 刚规划的轨迹会被人为potentially unsafe，leaving AV without a safe trajectory

2. Method

首先定义了问题，及 相关sets，这一部分有点多，建议看原文，主要是介绍所有变量的意义等，定义了set Z R^1 是自车的possible disturbances z，运动的微分公式为：

\[\begin{equation*} \dot{x}(t) = f\left(x(t),u(t),z(t)\right). \tag{1} \end{equation*} \]

使用 \(\chi\left(t_{h}, x\left(t_{0}\right), u\left(\left[t_{0}, t_{h}\right]\right)\right)\) 来表示 此公式的解，也就是满足motion下的轨迹集合

## 2.1 Invariably Safe Sets 计算

在他们之前的工作中有提到[68]，为计算 \(\mathcal S(t)\)，我们遵循一下规定：

  1. Formal safe distances[79]：不管前方车辆状态如何 包括前方车辆急刹等情况；在这个距离下，我们总是能刹停下来
  2. Evasive distances[80]：这个距离下 即使前方车辆急刹，我们也可以通过向相邻车道转向来避免碰撞

后面ab给我解释，两公式用了点时间，首先有个条件：

\[\begin{align*} & \left(|a_{\text{s,max,b}}| < |a_{\text{s,max}}| \right)\wedge \left(v^*_b < v_\text{ego} \right)\wedge\left(\frac{v_{\mathrm{ego}}}{|a_{\mathrm{s,max}}|} < \frac{v_b^*}{|a_{\mathrm{s,max,b}}|}\right), \tag{2} \end{align*} \]

如果满足这个条件，那么安全距离为：

\[\begin{align*} \Delta _{\mathrm{safe,1}}(v_{\mathrm{ego}},b):=&\frac{(v_b-|a_{\text{s,max,b}}|\delta _{\mathrm{brake}}-v_\text{ego})^2}{-2(|a_{\text{s,max,b}}|-|a_{\text{s,max}}|)}+v_\text{ego}\delta _{\mathrm{brake}}-v_b\delta _{\mathrm{brake}}+\frac{1}{2}|a_{\text{s,max,b}}|\delta _{\mathrm{brake}}^2\end{align*} \]

其中前面是一个整体 即\(\frac{(v-0)^2}{2a}\)；后面是另一个整体 在反应时间内的行驶相对距离 即 \(0+vt+\frac{1}{2}at^2\)

否则安全距离为： 即大家都以最大加速度减到速度为0的时候，外加一个自车的反应时间

\[\Delta _{\mathrm{safe,2}}(v_{\mathrm{ego}},b):=\frac{v_b^2}{-2|a_{\text{s,max,b}}|}-\frac{v_\text{ego}^2}{-2|a_{\text{s,max}}|} + v_\text{ego}\delta _{\mathrm{brake}}. \]

2.2 Safe& Evasive Distance Set

safe distance set

分横纵向考虑安全距离，纵向距离在[81,82] 二文中有详细考虑与证明：

\[\mathcal{S}^{1}(t)=\left\{(s, d, v)^{T} \in \mathcal{X} \mid \forall\left(s_{b}, d_{b}\right)^{T} \in \mathcal{O}_{b, \mathrm{cls}}(t): s \leq s_{b}-\Delta_{\mathrm{safe}, 2}(v, b)\right\} \]

而这里主要解释横向的evasive distance [感觉翻译成逃逸 逃离距离都不太好 所以就用原文把]，首先引入 \(d_{\mathrm{eva}}\) 作为车辆完全换道所需的横向距离，a为横向加速度

\[t_{\mathrm{eva}}:= \sqrt{\frac{2d_{\mathrm{eva}}}{a_{\mathrm{d,max}}}} + \delta _{\mathrm{steer}}\tag{5} \]

同时考虑前方车辆在这个时间内的行驶距离(纵)

\[\begin{split} \Delta s_b &:= v_bt_b-\frac{1}{2}|a_{\mathrm{s,max,b}}|t_b^2\\ t_b&:= \min (t_{\mathrm{eva}},\frac{v_b}{|a_{\mathrm{s,max,b}}|}). \end{split} \tag{6} \]

evasive distance set

定义如下，证明在[80]

\[\mathcal{S}^{2}(t)=\left\{(s, d, v)^{T} \in \mathcal{X} \mid \forall\left(s_{b}, d_{b}\right)^{T} \in \mathcal{O}_{b, \mathrm{cls}}(t): s \leq s_{b}-\Delta_{\text {eva }}^{t}(v, b)\right\} \]

Underapproximation of S

即前两者的set取并集，得到S

2.3 生成fail-safe Trajectories

纵向 Longitudinal Motion

首先是 纵向运动公式表示：\(x_{\text {lon }}=(s, v, a, j)^{T}\)，纵向距离、速度、加速度、jerk；我们使用加加速度作为input，将车辆纵向运动描述为linear time-invariant system

\[\begin{equation*} \frac{d^4}{dt^4}s(t)=u_{\mathrm{lon}}(t). \tag{8} \end{equation*} \]

然后加入约束，a 纵向前后的车辆距离，b 车辆本身的最大小 速度、加速度和jerk

\[\begin{align*} s_{\mathrm{min}}(t) \leq &x_{\mathrm{lon}}^{(0)}(t) \leq s_{\mathrm{max}}(t)\\ v_{\mathrm{min}}\leq &x_{\mathrm{lon}}^{(1)}(t) \leq v_{\mathrm{max}}\\ a_{\mathrm{min}}\leq &x_{\mathrm{lon}}^{(2)}(t) \leq a_{\mathrm{max}}\\ j_{\mathrm{min}}\leq &x_{\mathrm{lon}}^{(3)}(t) \leq j_{\mathrm{max}} \tag{10} \end{align*} \]

我们演示了刹车，但是因为是>0的加速度，所以此处我们引入两个deceleration limits \(a_{\min }<a_{\lim , 2}<a_{\lim , 1}<0\)，其中 \(\varsigma _{\mathrm{lon},1},\varsigma _{\mathrm{lon},2}\geq0\) 是slack variables 松弛约束 （JG: 优化的小trick 可以加速 更容易求解）

\[\begin{align*} x_{\mathrm{lon}}^{(2)}(t)&\geq a_{\mathrm{lim},1} - \varsigma _{\mathrm{lon},1}\\ x_{\mathrm{lon}}^{(2)}(t)&\geq a_{\mathrm{lim},2} - \varsigma _{\mathrm{lon},2}. \tag{11} \end{align*} \]

纵向的cost

\[\begin{align*} J_{\mathrm{lon}}\left(x(t),u(t)\right) =&\,w_ax_{\mathrm{lon}}^{(2)}(t)^2+w_jx_{\mathrm{lon}}^{(3)}(t)^2+w_{\varsigma _1}\varsigma _{\mathrm{lon},1}+w_{\varsigma _2}\varsigma _{\mathrm{lon},2}^2. \tag{12} \end{align*} \]

横向 Lateral Motion

同理 横向运动表示：\(x_{\text {lat }}=(d, \theta, \kappa, \dot{\kappa})^{T}\)，沿frenet坐标系下的离参考线的横向距离、orientation、曲率、自车曲率的变化

横向的运动描述为下列13公式，其中 \(u_{\mathrm{lat}}(t)=\ddot \kappa(t)\) 来源于 time-invariant linear

\[\begin{align*} \dot{x}_{\mathrm{lat}} =& \begin{pmatrix}0&v(t)&0&0\\ 0&0&v(t)&0\\ 0&0&0&1 \\ 0&0&0&0 \\ \end{pmatrix}x_{\mathrm{lat}}(t) + \begin{pmatrix}0\\ 0\\ 0\\ 1 \end{pmatrix}u_{\mathrm{lat}}(t)\\ &+ \begin{pmatrix}-v(t)\\ 0\\ 0\\ 0 \end{pmatrix}z_{\mathrm{lat}}(t). \tag{13} \end{align*} \]

对于collision free 将车辆看做三个圆，每个圆离参考线的距离可以用下列公式表示

\[\begin{equation*} d_i = d + \frac{i-1}{2}\ell \sin (\theta -\theta _\Gamma)\approx d+\frac{i-1}{2}\ell (\theta -\theta _\Gamma). \tag{14} \end{equation*} \]

再加上一系列的和纵向类似的约束，就不在此赘述了

横向的cost function：

\[\begin{equation*} \begin{split} J_{\mathrm{lat}}\left(x(t),u(t)\right) =&\,w_d x_{\mathrm{lat}}^{(0)}(t)^2+w_\theta \left(x_{\mathrm{lat}}^{(1)}(t)-\theta _\Gamma (t)\right)^2\\ &+w_\kappa x_{\mathrm{lat}}^{(2)}(t)^2+w_{\dot{\kappa }}x_{\mathrm{lat}}^{(3)}(t)^2. \end{split} \tag{18} \end{equation*} \]

生成Fail-Safe

首先是根据TTR生成初始状态，再由初始状态去计算接下来的fail-safe轨迹

下列公式中 \(\Delta_{cor}\)为由车辆中心到后轴中心的距离修正项，\(s_{min}\leq s \leq s_{max}\)

\[\begin{equation*} \begin{split} s_{\mathrm{min}}(t) = \sup \left\lbrace s-\Delta _{\mathrm{cor}}\,|\,\forall b \in \mathcal {B}_{\mathrm{fol}}: s-\Delta _{\mathrm{cor}}< s_0\right.\\ \left.\wedge (s,d)^T\in \mathcal {O}_{b,\mathrm{cls}}(t)\right\rbrace . \end{split} \tag{20} \end{equation*} \]

\[\begin{equation*} \begin{split} s_{\mathrm{max}}(t) = \inf \left\lbrace s-\Delta _{\mathrm{cor}}\,|\,\forall b \in \mathcal {B}_{\mathrm{pre}}:s-\Delta _{\mathrm{cor}}>s_0\right.\\ \wedge \left.(s,d)^T\in \mathcal {O}_{b,\mathrm{cls}}(t)\right\rbrace . \end{split} \tag{19} \end{equation*} \]

整体生成流程：

collision avoidance through braking

在初始状态下 能够紧急刹停的条件：Proposition 4

\[\begin{equation*} \begin{split} \forall t\in [t_0,t_h]: &\,s_0+v_0(\tau)-\frac{1}{2}|a_{\mathrm{max}}| \max (\tau -\delta _{\mathrm{brake}},0)^2 \\ &\leq s_{\mathrm{max}}(t), \tau :=\min (t,\frac{v_0}{|a_{\mathrm{max}}|}+\delta _{\mathrm{brake}}). \end{split} \end{equation*} \]

如果碰撞一定会发生，那么guaranteed Time-To-Collision是：

\[\begin{equation*} t_{\mathrm{GTTC}}:= \text{argmin}_{t\in [0,t_h]} \big |(s_0+v_0t) - s_{\mathrm{max}}(t)\big |. \end{equation*} \]

Evasice Acceleration可以由此表示：Proposition 5

\[\begin{equation*} a_{\mathrm{eva}}= \frac{2\left(d_{\mathrm{eva}}-|v_{\mathrm{lat}}|t_{\mathrm{GTTC}}\right)}{(t_{\mathrm{GTTC}}-\delta _{\mathrm{steer}})^2}. \end{equation*} \]

需要满足的距离曲线 示意

使用h段g的函数去近似安全距离 \(\Delta_{safe}\in\{\Delta_{safe,1},\Delta_{safe,2}\}\) 然后分速度区间去得到h段

为了将此加入到优化过程，可以表示为maximum function[88]

\[\begin{equation*} \tilde{\Delta }_{\mathrm{safe}}(x_{\mathrm{lon}}^{(1)}) = \max \left(g_1(x_{\mathrm{lon}}^{(1)}), g_2(x_{\mathrm{lon}}^{(1)}), \dots, g_h(x_{\mathrm{lon}}^{(1)})\right). \end{equation*} \]

然后加入纵向距离的约束中 公式(9)

\[\begin{align*} x_{\mathrm{lon}}^{(0)}(t) + \tilde{\Delta }_{\mathrm{safe}}(x_{\mathrm{lon}}^{(1)}) \leq s_{\mathrm{max}}(t). \tag{26} \end{align*} \]

因为求解器无法求解带max的函数，所以加入h个小于等于的约束, h不应该太大 也不能太小，前者会增加计算负担，后者会有较大误差

\[\begin{align*} x_{\mathrm{lon}}^{(0)}(t) + \left(g_{i}(x_{\mathrm{lon}}^{(1)})+\delta _{\mathrm{brake}}x_{\mathrm{lon}}^{(1)}\right) \leq s_{\mathrm{max}}(t). \tag{27} \end{align*} \]

3. 实验及结果

使用了Python和C++，优化求解库使用的是：CVXPY, ECOS and CVXPY-CODEGEN

Invariably safe set of the scenario

然后主要是实验了几个特定的场景 去验证 fail-safe 的轨迹运行情况：

静态障碍物、前方车辆急刹、行人冲出

4. Conclusion

提出了fail-safe motion planning 来保证AV不会发生事故

对比于其他的verification，我们的方法是第一个能在arbitrary traffic situations使用的；同时保证了40ms内的计算时间，可以与任何其他的motion plan相结合

我们的方案并不会使AV在过度的保守策略中

如果相关法规被扩展，我们的verification也可以自动适应。通过提供的occupancy sets来捕获其他交通参与者的所有合法行为

碎碎念

一开始，ab说这篇算是保守策略的，也就是考虑最差情况；但是初读的时候发现他们在摘要就说了 不会采取过度保守策略，后面一开始没get到这个点，直到问jg的时候发现slack variable和weights的一些trick设计，也就是并不是 非0即1 而是可以超过约束

对于fail-safe的概念也是先由TTR生成状态，然后根据流程图去判断纵向刹停是否可以，不可以的话换横向避障；整体算是一个求解优化问题，只是约束条件等由一系列的状态去决定

赠人点赞 手有余香 😆；正向回馈 才能更好开放记录 hhh

posted @ 2022-08-03 10:16  Kin_Zhang  阅读(355)  评论(0)  编辑  收藏  举报