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

参考与前言

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

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{array}{c}\dot{x}(t)=f\left(x(t),u(t),z(t)\right).\end{array}\text{\hspace{1em}(1)}$$

使用 $\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{array}{rl}& \left(|a_{\text{s,max,b}}|<|a_{\text{s,max}}|\right)\wedge \left(v_b^{\ast }<v_{\text{ego}}\right)\wedge \left(\frac{v_{\mathrm{ego}}}{|a_{\mathrm{s},\max }|}<\frac{v_b^{\ast }}{|a_{\mathrm{s},\max ,\mathrm{b}}|}\right),\end{array}\text{\hspace{1em}(2)}$$

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

$$\begin{array}{rl}{\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{array}$$

其中前面是一个整体 即$\frac{(v-0{)}^2}{2a}$；后面是另一个整体 在反应时间内的行驶相对距离 即 $0+vt+\frac{1}{2}a{t}^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\le 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}}\text{\hspace{1em}(5)}$$

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

$$\begin{array}{rl}\Delta s_b& :=v_b{t}_b-\frac{1}{2}|a_{\mathrm{s},\max ,\mathrm{b}}|t_b^2\\ t_b& :=\min (t_{\mathrm{eva}},\frac{v_b}{|a_{\mathrm{s},\max ,\mathrm{b}}|}).\end{array}\text{\hspace{1em}(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\le 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{array}{c}\frac{d^4}{d{t}^4}s(t)=u_{\mathrm{lon}}(t).\end{array}\text{\hspace{1em}(8)}$$

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

$$\begin{array}{rl}{s}_{\min }(t)\le & x_{\mathrm{lon}}^{(0)}(t)\le s_{\max }(t)\\ v_{\min }\le & x_{\mathrm{lon}}^{(1)}(t)\le v_{\max }\\ a_{\min }\le & x_{\mathrm{lon}}^{(2)}(t)\le a_{\max }\\ j_{\min }\le & x_{\mathrm{lon}}^{(3)}(t)\le j_{\max }\end{array}\text{\hspace{1em}(10)}$$

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

$$\begin{array}{rl}{x}_{\mathrm{lon}}^{(2)}(t)& \ge a_{\lim ,1}-{\varsigma }_{\mathrm{lon},1}\\ x_{\mathrm{lon}}^{(2)}(t)& \ge a_{\lim ,2}-{\varsigma }_{\mathrm{lon},2}.\end{array}\text{\hspace{1em}(11)}$$

纵向的cost

$$\begin{array}{rl}{J}_{\mathrm{lon}}\left(x(t),u(t)\right)=& w_a{x}_{\mathrm{lon}}^{(2)}(t{)}^2+w_j{x}_{\mathrm{lon}}^{(3)}(t{)}^2+w_{{\varsigma }_1}{\varsigma }_{\mathrm{lon},1}+w_{{\varsigma }_2}{\varsigma }_{\mathrm{lon},2}^2.\end{array}\text{\hspace{1em}(12)}$$

横向 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{array}{rl}{\dot{x}}_{\mathrm{lat}}=& \left(\begin{array}{cccc}0& v(t)& 0& 0\\ 0& 0& v(t)& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right)x_{\mathrm{lat}}(t)+\left(\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right)u_{\mathrm{lat}}(t)\\ & +\left(\begin{array}{c}-v(t)\\ 0\\ 0\\ 0\end{array}\right)z_{\mathrm{lat}}(t).\end{array}\text{\hspace{1em}(13)}$$

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

$$\begin{array}{c}{d}_i=d+\frac{i-1}{2}\ell \sin (\theta -{\theta }_{\Gamma })\approx d+\frac{i-1}{2}\ell (\theta -{\theta }_{\Gamma }).\end{array}\text{\hspace{1em}(14)}$$

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

横向的cost function：

$$\begin{array}{c}\begin{array}{rl}{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{array}\end{array}\text{\hspace{1em}(18)}$$

生成Fail-Safe

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

下列公式中 ${\Delta }_{cor}$为由车辆中心到后轴中心的距离修正项，$s_{min}\le s\le s_{max}$

s m i n ( t ) = sup ⁡ { s − Δ c o r   ∣   ∀ b ∈ B f o l : s − Δ c o r < s 0 ∧ ( s , d ) T ∈ O b , c l s ( t ) } . \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*} smin​(t)=sup{s−Δcor​∣∀b∈Bfol​:s−Δcor​<s0​∧(s,d)T∈Ob,cls​(t)}.​​(20)​

s m a x ( t ) = inf ⁡ { s − Δ c o r   ∣   ∀ b ∈ B p r e : s − Δ c o r > s 0 ∧ ( s , d ) T ∈ O b , c l s ( t ) } . \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*} smax​(t)=inf{s−Δcor​∣∀b∈Bpre​:s−Δcor​>s0​∧(s,d)T∈Ob,cls​(t)}.​​(19)​

整体生成流程：

collision avoidance through braking

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

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

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

$$\begin{array}{c}{t}_{\mathrm{GTTC}}:={\text{argmin}}_{t\in [0,t_h]}|(s_0+v_{0}t)-s_{\max }(t)|.\end{array}$$

Evasice Acceleration可以由此表示：Proposition 5

$$\begin{array}{c}{a}_{\mathrm{eva}}=\frac{2\left(d_{\mathrm{eva}}-|v_{\mathrm{lat}}|t_{\mathrm{GTTC}}\right)}{(t_{\mathrm{GTTC}}-{\delta }_{\mathrm{steer}}{)}^2}.\end{array}$$

需要满足的距离曲线 示意

使用h段g的函数去近似安全距离 Δ s a f e ∈ { Δ s a f e , 1 , Δ s a f e , 2 } \Delta_{safe}\in\{\Delta_{safe,1},\Delta_{safe,2}\} Δsafe​∈{Δsafe,1​,Δsafe,2​} 然后分速度区间去得到h段

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

$$\begin{array}{c}{\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{array}$$

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

$$\begin{array}{r}{x}_{\mathrm{lon}}^{(0)}(t)+{\tilde{\Delta }}_{\mathrm{safe}}(x_{\mathrm{lon}}^{(1)})\le s_{\max }(t).\end{array}\text{\hspace{1em}(26)}$$

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

$$\begin{array}{r}{x}_{\mathrm{lon}}^{(0)}(t)+\left(g_i(x_{\mathrm{lon}}^{(1)})+{\delta }_{\mathrm{brake}}{x}_{\mathrm{lon}}^{(1)}\right)\le s_{\max }(t).\end{array}\text{\hspace{1em}(27)}$$

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