当前位置：网站首页>【论文阅读】TRO 2021: Fail-Safe Motion Planning for Online Verification of Autonomous Vehicles Using Conve

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

2022-08-03 22:47:00 【Kin__Zhang】

参考与前言

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 能保证自动驾驶车辆不会出现事故，但是现有的方法存在下列缺点：

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

Related work

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

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)$ ，我们遵循一下规定：

Formal safe distances[79]：不管前方车辆状态如何包括前方车辆急刹等情况；在这个距离下，我们总是能刹停下来
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*}$