论文阅读笔记-Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor

[TOC]

本文为论文《Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor》的阅读后总结.
作者:Wenchao Ding,Lu Zhang 等
学校:香港科技大学
主题: frenet 坐标系下建图后, 利用 QP 进行局部轨迹规划.
论文网址: https://ieeexplore.ieee.org/document/8740885

I. INTRODUCTION

问题的提出:

1. 自动驾驶的规划算法,对于形形色色的约束,如何统一数学描述它们呢?
2. 同时还要保证算法的安全与可靠性(guarantee that the entire trajectory is safe and constraint-satisfied)

前者问题本文用 Spatio-temporal Semantic Corridor(SSC) 来解决(本质上是对时空信息进行建图的过程),对于后面的问题,如下:

为了保障规划结果,常规优化算法或者 lattice-based 算法都是通过采样点来约束,明显有漏洞.与之对比,本文不是,而是通过优化算法解析求得最优解.The SSC is motivated by the fact that most semantic elements can be either rendered as spatio-temporal obstacles or constraints within a certain range of the spatio-temporal domain.

因此, SSC consists of a series of mutually connected collision-free cubes with dynamical constraints posed by the semantic elements.即最终是靠在 frenet 坐标系下的 TS/LS 图中构造出一个个相互连接的 cube 区域作为可通行的 free space.

论文特点:

1. SSC structure,即对约束的数学描述框架(建图)
2. quadratic programming (QP) 求解 piecewise Bézier curve(特点: convex hull and hodograph properties) parameterization.
3. 开源,求败.

实际交通环境转换成 SSC 的图解:

II. RELATED WORKS

A. Spatio-temporal motion planning for AVs

1. spatio-temporal state lattice + search-based approach, 论文1网址,论文2网址.特点都是基于采样求解.
2. specially designed cost functions and aggregate the cost terms as a potential field, 问题:local minimums. 论文网址
3. 基于的论文.

B. Corridor generation for AVs

Corridor(可通行通道)概念的参考:
convex elastic smoothing algorithm
convex tube
find a convex feasible set around the reference trajectory

III. SYSTEM OVERVIEW

本文为 motion planning layer 领域的算法.
整体架构如下:

semantic elements (e.g., occupancy grid map, dynamic agents, lanes, traffic rules, etc.).
决策模块: multi-policy decision making (MPDM) method, 不限于这个决策模块, as long as the behavior planner provides a preliminary initial guess about the future states.

四个输入(常规输入依赖):

• a semantic map which consists of the semantic elements: 感知输入
• predicted trajectories for dynamical agents: 预测输入
• forward simulated states: 决策输入
• a reference lane given by the route information: 全局规划输入

IV.PRELIMINARIES ON MULTI-POLICY DECISION MAKING

MPDM算法决策, 然后 we use the forward simulated states of the ego vehicle as the seeds in the corridor generation process.

V. SPATIO-TEMPORAL SEMANTIC CORRIDOR

A. Semantic Elements And Frenét Frame Representation

$slt 3-D$ configuration space :

• longitudinal direction $s$, Frenét frame
• lateral direction $l$ , Frenét frame
• time $t$

如下图限速下的合流场景:

语义元素的分类:

1. Obstacle-like semantic elements
   not allowed to be driven in -> configuration space is a 3-D occupancy grid
2. Constraint-like semantic elements
   dynamical constraints or time constraints, i.e. speed limits and stop signs.
   命名为: semantic boundaries, 例子:

• a speed limit: a longitudinal range $[s_{begin}, s_{end}]$
• lane change duration constraint: time constraint applied to the lateral range $[d_{begin}, d_{end}]$ of the current lane.

hardness of the constraints,例如交规与变道时长限制

B. Semantic Corridor Generation

SSC 生成的图解如下图:

橙色为限速区域(因此绿色的自车轨迹斜率在此区间降低,驶出此区域后又增加).

(a) 中的绿色 cube 为行为决策给出的初始值(seeds),规划依据此进行进一步优化

(b) 对初始值的 cube 进行膨胀, 膨胀到接触到障碍物(静态/动态的外扩)以及 constraint (例如速度限制)

(c) 重复进行膨胀得到最小的区域,同时注意到拓展的方向有些是要省略的,例如第三个 cube 的向下向左拓展(也就是已经拓展过的方向). 还有一点要注意,那就是下方橙色的线变成了蓝色的虚线,有一条新的蓝色实线要比橙色线位置更高,这个就是 relaxation 的过程,对 soft constraints 放松一些.

(d) 在得到的 cube 区域中拟合 piecewise 贝塞尔曲线得到规划轨迹.

Seed Generation

The seeds of the semantic corridor are generated by projecting the forward simulated states of the behavior planner to the $slt$ configuration space.

首先要保证 seeds 是 collision-free 的, 即上图中(a) 中的初始 cube 的大小确定.

Cube Inflation with Semantic Boundaries

注意:each inflated cube corresponds to one piece of the trajectory,拓展后的大 cube 与 cube 之间的时间间隔不做限制.

Cube Relaxation

上面的 inflation 其实只考虑了 hard constraints 例如交通规则,碰撞.对于 soft constraints 其实可以适当地放松一些. 也就是 relaxation 过程.

可以放松的地方例如: $s$ 方向上距离, $t$ 上对执行时间的放松等.

VI. TRAJECTORY GENERATION WITH SAFETY AND FEASIBILITY GUARANTEE

[前人](Optimal trajectories for time-critical street scenarios using discretized terminal manifolds - Moritz Werling, Sören Kammel, Julius Ziegler, Lutz Gröll, 2012 (sagepub.com)),也是大名鼎鼎的提出使用 frenet 坐标系进行研究的Moritz Werling,Sören Kammel等人,此研究是本文的算法效果的最终 benchmark 对象.

他们提出的单条五次多项式曲线缺点:

1. 单条很可能求不出来解
2. 多项式曲线的拟合只考虑了采样点约束,无法考虑到整个 convex hull 的约束,总会有漏网之鱼

因此本文采用了 a piecewise B´ezier curve for the two-dimensional trajectory(the longitudinal direction $s(t)$ and lateral direction $l(t)$) along the reference lane.

A. B´ezier Basis and Its Properties

好处2点:

1. 贝塞尔曲线从数学上保证是在 convex hull 里的,具体可以参考本人的学习笔记.

1. 如其他 spline 曲线一样,$m$ 阶贝塞尔曲线的 $m-1$导数后的曲线仍旧为低阶的贝塞尔曲线,这为我们求导数提供了便利.吐槽:这不就是曲线的连续性概念吗?

B. Piecewise B´ezier Curve Representation

多段贝塞尔曲线表达式如下:

$$ f_{j}^{\sigma}(t)=\left\{\begin{array}{cc} \alpha_{1} \cdot \sum_{i=0}^{m} p_{i}^{1} \cdot b_{m}^{i}\left(\frac{t-t_{0}}{\alpha_{1}}\right), & t \in\left[t_{0}, t_{1}\right] \\\\ \alpha_{2} \cdot \sum_{i=0}^{m} p_{i}^{2} \cdot b_{m}^{i}\left(\frac{t-t_{1}}{\alpha_{2}}\right), & t \in\left[t_{1}, t_{2}\right] \\\\ \vdots & \vdots \\\\ \alpha_{n} \cdot \sum_{i=0}^{m} p_{i}^{n} \cdot b_{m}^{i}\left(\frac{t-t_{n-1}}{\alpha_{n}}\right), & t \in\left[t_{n-1}, t_{n}\right] \end{array}\right. $$

其中 $σ ∈ {s, l}$, 两个维度的曲线.

求解过程中的 cost function 是 time integral of the square of the jerk:
$$
J_{j}=w_{s} \int_{t_{j-1}}^{t_{j}}\left(\frac{d^{3} f^{s}(t)}{d t^{3}}\right)^{2} d t+w_{l} \int_{t_{j-1}}^{t_{j}}\left(\frac{d^{3} f^{l}(t)}{d t^{3}}\right)^{2} d t
$$
为第 $i$ 段的曲线的 cost function.

化成矩阵形式:

$$ J_{j}^{\sigma}=\int_{0}^{1} \alpha_{j} \cdot\left(\frac{d^{3}\left(\alpha_{j} \cdot y_{j}^{\sigma}(t)\right)}{d\left(u \cdot \alpha_{j}\right)^{3}}\right)^{2} d u=\frac{1}{\alpha_{j}^{3}} \cdot \mathbf{p}_{j}^{\mathrm{T}} \mathbf{Q} \mathbf{p}_{j} $$

$\mathbf{p}_j$: the control points

$\mathbf{Q}$: the Hessian matrix of the non-scaled B´ezier curve

$u = \frac{t−t_{j−1}}{\alpha_j}$: normalized time of the non-scaled B´ezier curve

C. Enforcing Safety and Dynamical Constraints

拟合目标是五次,这一节的公式看起来很唬人,但是都是贝塞尔曲线的基本公式,为了构造 QP 问题求解,使用了如下约束:

1. Desired state constraints 始终点约束从 $0$ 阶到 $k$ 阶

initial state $[σ^{(0)}_{t_0} , σ^{(1)}_{t_0} , σ^{(2)}_{t_0} ]$, goal state $[σ^{(0)}_{t_n} , σ^{(1)}_{t_n} , σ^{(2)}_{t_n} ]$, $\frac{d^{k} f_{0}^{\sigma}\left(t_{0}\right)}{d t^{k}}=\sigma_{t_{0}}^{(k)}, \quad \frac{d^{k} f_{n}^{\sigma}\left(t_{n}\right)}{d t^{k}}=\sigma_{t_{n}}^{(k)}$

2. Continuity constraints 连续性约束

$$
\frac{d^{k} f_{j}^{\sigma}\left(t_{j}\right)}{d t^{k}}=\frac{d^{k} f_{j+1}^{\sigma}\left(t_{j+1}\right)}{d t^{k}}
$$

3. Free-space constraints 边界约束

$$
\beta_{j,-}^{\sigma,(k)} \leq \alpha_{j}^{1-k} \cdot q_{j, i}^{\sigma,(k)} \leq \beta_{j,+}^{\sigma,(k)}, \forall i \Rightarrow \beta_{j,-}^{\sigma,(k)} \leq \frac{d^{k} f_{j}^{\sigma}(t)}{d t^{k}} \leq \beta_{j,+}^{\sigma,(k)} \\
\frac{d^{k} f_{j}^{\sigma}\left(t_{j-1}\right)}{d t^{k}}=\alpha_{j}^{1-k} \cdot q_{j, 0}^{\sigma,(k)}, \frac{d^{k} f_{j}^{\sigma}\left(t_{j}\right)}{d t^{k}}=\alpha_{j}^{1-k} \cdot q_{j, m}^{\sigma,(k)} \\
q_{j, i}^{\sigma,(0)}=p_{i}^{j}, q_{j, i}^{\sigma,(k)}=\frac{m !}{(m-k) !}\left(q_{j, i+1}^{\sigma,(k-1)}-q_{j, i}^{\sigma,(k-1)}\right)
$$

这三行公式第一行是约束的最终表达式,后面2行是为了求解第一行中的 $q_i$所需要的公式,$k=0$ 代表的含义是 inflated cubes 的边界，如果需要限制曲率或者更高阶导数的化可以对 $k=2,3…$ 的情况下进行约束.

4. 加速度上下限限制： maximum acceleration and the maximum deceleration are set to $2 m/s^2$ and $3 m/s^2$, respectively.

求解器使用 OOQP 库.

VII. EXPERIMENTAL RESULTS

A. Implementation Details

测试场景来源:real satellite maps via QGIS

硬件: Intel I7-8700K CPU

运行频率: 20 Hz

感知距离: ego vehicle only has a limited sensing range (around 100 m)

B. Qualitative Results

选择一些较难的场景验证如下:

1. Merging into congested traffic due to road construction 由于道路施工换道到拥挤车道中
2. Overtaking on an urban expressway 高速超车
3. An unprotected left turn at an intersection 直行左转灯不分的路口进行左转

C. Comparisons and Analysis

a local target state with a certain resolution in the $slt$ domain, and for different behaviors, the strategy for choosing the local target is different.

=> 问题是如何分的呢??? 这不正是 SSC 的精华所在吗???

结果:

1. Collision-avoidance in cluttered environments： 更快实现目标, 跟 benchmark 比较
2. Precise stop with a high entry speed: 实现指定位置高速状态下刹停, benchmark 最大的问题是采样点可能会错过完美的终点导致刹停点前移
3. Collision avoidance under a low speed limit:

最后小结

• 作者很会包装概念,其实整体思路就2点: 1. 五次分段贝塞尔曲线的 QP 求解, 2. 所谓的 SSC 就是, 为在 frenet 的 TS 图中以及 TL 图中求解贝塞尔曲线而进行的建图工作.说实话这些在 Apollo 里都有… 虽然没有直接的代码, 但是贝塞尔曲线的生成以及 TS/TL 图的建立都可以在 Apollo 找到(具体很可能不一样,思路都是一样的). 你说作者没有借鉴 Apollo 我是绝对不信的,后面看他们代码吧.

• 最值得借鉴的部分其实是建图, 但是作者用 for different behaviors, the strategy for choosing the local target is different 打发了. 没明说(数学建模)的意思就是需要手动 tuning 的意思吧.

• 另外分段 B 样条曲线拟合的最大问题是可能出现突然大曲率拐动的问题,需要看源码作者的应对方法.

• 配图做的真不错,做到了一图胜千言.

• 即便是 QP 问题,也无法保证必然能求出最优可行解的,没有 backup 方案？