论文研读笔记--"Hybrid Trajectory Planning for Autonomous Driving in Highly Constrained Environments"

Basic info

Author: Yu Zhang
Date: 2018
Juounal: IEEE Acess
Institute: 北理

Abstract

共四方面贡献:

1. trajectory planning framework: 克服 geometry constraints(free space), nonholonomic constraints(vehicle kinematics and dynamics) and dynamics constraints, task constraints (requirements to visit certain intermediate targets)实现 curvature continuous , kinodynamically feasible, smooth and collision-free trajectories in real-time.
2. derivative-free global path modification algorithm to extract high-order state information in free space for state sampling.
3. multi-phase deterministic state space sampling method that is able to approximate complex maneuvers.
4. car footprint approximation strategy and a two-phase collision checking routine.
   后面这些名词概念都会得到详细解释.

Keywords

trajectory planning, motion planning, autonomous driving, obstacle avoidance, kinodynamic constraints, collision checking

Introduction

作者理解的规划的本质是:

In general, a trajectory planning problem refers to finding an optimal path with time stamped positions, orientations, and velocities from the current configuration to the goal configuration by minimizing certain objectives subject to geometry constraints (feasible paths must lie in the free space), task constraints (requirements to visit certain intermediate targets), and nonholonomic constraints (vehicle kinematics and dynamics).

规划问题是 PSPACE-hard 无法直接数值求解( no polynomial-time algorithm able to solve all instances of the problem).

算法分类介绍

Sampling-based methods

1.random sampling-based methods

包括 probabilistic roadmaps and rapidly exploring random trees(RRT).

• 特点

  1. sub-optimal.
  2. good at exploring the reachability of the free space using control space paths and rapid collision checking in complex environments.
  3. 不需要考虑 freespace 模型, without having access to the explicit geometry model of the free space.
  4. Some random sampling-based methods such as PRM and RRT are known to be probabilistically complete(概率上完备的), ensuring a path will eventually be found if one exists.
  5. 生成结果需要优化, resulting paths tend to be jerky, redundant and not curvature continuous.
  6. run-time is also unlimited and unpredictable.

• review 文献推荐:M. Elbanhawi and M. Simic, “Sampling-Based Robot Motion Planning: A Review,” IEEE Access, vol. 2, pp. 56–77, 2014

2.deterministic sampling-based methods

也被叫做 local search methods.
分类: control state sampling methods and state space sampling methods.
具体过程:

1. Control space sampling methods simply run discrete control inputs for a certain duration to generate the shifted curves.
2. State space sampling methods instead get goal states from a reference path or a center line of the road within a certain horizon, then solve a two-point boundary value problems with predefined curve models connecting the initial state of the car and the goal state set.

sampling curve: arcs, spirals, splines, polynomials.
特点:

1. 需要参考线定义曲率等 state 信息. All the curve models except arcs required goal states such as position, orientation, and curvature to be defined from a reference path in order to construct candidate paths.
2. 依赖终点设置: goal state plays an import role in affecting the shape of the seeds paths.
3. 比随机采样优点是采样更合理, reduce the solution space dramatically by using preferred actions (control space sampling) or taking advantages of the structure of roads (state space sampling), which makes behaviors of the planner predictable and also avoids unreasonable sampling seeds.
4. 搜索深度始终为 This methods can be thought of as a class of planners whose sampling depth is one, regardless of their look-ahead horizon.
   下图是对 sampling depth 与 horizon 的解释示意图:

5. 无法应对稠密障碍物场景. 原因: The presence of dense obstacles break the original geometry property of the road exploited by deterministic sampling methods. 我觉得用我的理解来描述的话, 因为只能搜一层所以很可能得到的前段部分的采样最优点导致全局的无解. 还不如选择前期不是那么优的解, 最终得到全局可行解.

2.Search-based method

分类: A*, Hybrid A*, Lattice search, Dijkstra
原理: This class of methods usually constructs a directed graph to sample the configuration space uniformly and then searches the graph with different search strategies. Search-based methods discretize the configuration space with fixed number of motion primitives and build the search graph in advance.
与 RRT 的不同:

1. The graph that is built has a better coverage of the whole free space with less configuration states compared to RRTs at the early stages of planning, which must construct their search tree from scratch.
2. This distinction ensures that search-based methods have a better global view of the geometry model of free space before searching for a path. With a well designed search space and admissible heuristics, searched-based methods are able to find a globally optimal or sub-optimal path in real-time.

特点:

1. good at solving path planning problems with geometric constraints but poor at dealing with kinodynamic constraints of the system in general.
2. the solution space will be dramatically reduced if the search space is not well designed.
3. not good at high dimensional problems.

3.optimization-based methods

特点:

• 参数化的曲线拟合: usually represent the path of a vehicle with certain parametrized curve models, such as splines, spirals, polynomials or piecewise lines. Then optimize for the prescribed objectives over a finite dimensional parameter vector space to get smooth paths or trajectories.
• 不直接适用于 free space 输入. There is no optimization-based method that incorporates the explicit geometry model of the free space into the trajectory planning problem formulation.
• 不适合避障. Some of the methods can incorporate nonholonomic constraints in the optimization, but including obstacle avoidance within the optimization is still problematic.

本论文方案

输入:

1. drivable region and the global path, which may be or may not be collision-free, are provided.
2. 障碍物.

步骤:

1. 解偶 path 与 speed 规划.
2. path: The spatial path planning problem is further decomposed to a global path modification layerand a multi-phase state space sampling planner layer.
3. speed: The speed planning sub-problem is solved by an optimization-based speed planning layer along the fixed path.

解偶+分阶段的原因: 组合各类方法, 扬长避短.

• 对不同的 constraints 使用不同的方法

1. geometry constraints: using a search-based global path modification layer.
2. nonholonomic constraints: 先对不可行区域进行剪枝再进行 sampling, 即分阶段: a multi-stage state sampling method, samples in the neighbourhood around the shortest path. It prevents the state space sampling method from spending computation resources on generating high-cost kinematically-feasible paths to sample the infeasible region that is limited by constraints.
3. Once the best path is found in the sampling stage, the dynamical constraints of the vehicle (lateral accelerations and longitudinal accelerations and decelerations) are imposed to define a speed profile.
   整体而言: 先找粗 path 再采样满足动力学约束的路径: finding the best coarse path through the environment first, and then refining the path to meet kinodynamic constraints. 而不像 Hybrid A* 以及 RRT * 那样从动力学约束出发进行图的拓展/搜索, 然后满足几何(静态避障)的约束. 本文的方法的顺序是反过来的.

• 高阶曲线拟合, 并且不依赖精细的场景信息(unstructured environment), free space 足矣. derivative free path smoothing algorithm based on the curve energy function and fit a B-spline to get the precise orientation and curvature information of the path. Only require traversable region information.

• multi-phase deterministic state space sampling method 生成多段曲率连续的三次螺旋线. curvature-continuous piecewise cubic spirals.
• 障碍物检测算法: footprint approximation strategy and a two-phase collision checking routine.
• speed planning: optimization-based time-optimal speed planning.

Methodology

A. GLOBAL PATH MODIFICATION

输入依赖: original global path provided by a high-level route planning.

输出结果: does not necessarily have to consider the nonholonomic constraints, but needs to be as continuous and short as possible.

思想: To find a sequential overlapped circle path in workspace using a heuristic A graph search* algorithm. The centers of the circles are waypoints of the path.

方法步骤

1. heuristic A* search
   建图:
   $n_i = (p_i, r_i, g_i, h_i, f_i)$, $p_i$ 为节点 $(x_i,y_i)$, $r_i$ 为节点距离障碍物/边界相切圆半径(clearance radius of the circle, 存在上下限 $R_{max},R_{min}$),$g_i$ 为起点到 $n_i$ 点的距离(the length of edges between nodes), $h_i$ 为到终点欧式距离, $f_i=g_i+h_i$.
   Algorithm 1: The space exploration algorithm 基本的 A* 搜索算法
   
   其中拓展新节点的函数为: The $\text{ExpandNode }\left(n_{i}, j, R_{\min }, R_{\max }\right)$ operation generates the children node set $S_{\text {children }}$ for $n_{i}$ , with $j$ elements evenly distributed in a circle around $n_{i}$ with radius 如下(不是传统的方形网格状的图):
   $$
   r_{i}=\left{\begin{array}{l}
   R_{\text {clearance }}, \text { if } R_{\text {clearance }} \in\left[R_{\min }, R_{\text {max }}\right] \
   R_{\text {min }}, \text { if } R_{\text {clearance }}<R_{\min } \
   R_{\text {max }}, \text { if } R_{\text {clearance }}>R_{\text {max }}
   \end{array}\right. \text {, }
   $$
   图解如下:
   
   $R_{\text {clearance }}$ is the max distance from $n_i$ to the closest obstacle in clearance map. 可以使用 OpenCV 的 distanceTransform() 函数求得.
   是否达到终点用下面的函数确定. Algorithm 2: Overlap(ni; nj) 判断中止, 如果 $n_i$ 与终点 overlap, 更新能到达终点的最小代价, 能够到达终点且代价最小的 route (node sequence)是所求. 基本思想是终点(倒数第一个点)不能在倒数第二点的距离障碍物的圆内, 同时两者的距离也不能太远.
   
   用直接连接所有 $p_i$ 点得到搜索后的粗轨迹. More precisely, a discrete curve is an ordered tuple $\left(p_{0}, p_{1}, \cdots, p_{n-1}\right) \in \mathbb{R}^{2 n}$ of vertices $p_{i} \in \mathbb{R}^{2}$, with $p_{i+1} \neq p_{i}$ for all $i$. The consecutive vertices are connected by straight lines that are called edges. 因此光滑度为 $G^0$.

2. 光滑粗轨迹
   主题思想是缩短长度的同时保持光滑. The core idea of path smoothing is reducing the slackness of the path (shortening the length) while increasing the smoothness. 采用迭代的形式进行光滑. 迭代的结束条件使用如下方式目标函数.
   Energy function for a discrete curve to measure the progress of smoothing:
   $$J_d = \sum_{i=2}^N \frac {(\kappa_{p_{i−1}}^2 + \kappa_{p_i}^2)∆s_{p_i}}2$$
   $∆s_{p_i}$ 是 $p_{i−1}$ 与 $p_{i}$ 的间距. $\kappa_{p_i}$ 是曲率.
   是下面式子的离散化形式:
   $$J = \int_0^s \kappa^2ds$$
   约束条件: triangle inequality rule(sss theorem), curvature upper boundary $k_{max}$ and the clearance constraint.
   目标函数: 所有点处的累计曲率平方和最小.
   方法: 迭代计算点的位置直到达到点位置调整的距离小于 resolution.
   Algorithm 4: PathSmoothing(fpig, Rmin,Rmax) 迭代找最小的目标函数值, 通过 Algorithm 3 来减少.
   
   to decrease the slackness of the path by performing a binary search for a feasible updated circle center, subject to the triangle inequality rule, curvature upper boundary and the clearance constraint.
   

   其中用的 GetNewCircleCenter 函数为以下算法: Algorithm 3: GetNewCircleCenter (pi, pi+1, pi+2)
   计算曲率的原理: calculates the discrete curvature according to the SSS Theorem(边边边定理).
   
   计算每个点新位置(满足约束的前提下)
   最后使用 B-spline 拟合直线段序列. 问题是拟合不是插值会不会导致拟合出来的结果太离谱导致之前的结果白做了(例如, 破坏了避障结果). 论文里的回答是: Our algorithm generates waypoints based on clearance, which provides enough support waypoints for curve fitting. Thus, the fit B-spline still maintains a good approximation of the geometry of the circle path. 说实话, 我没太理解. 如果让我解决此问题的话, 我会在拟合的约束中加入之前的约束进去, 例如 clearance 约束/动力学约束等. 极端情况下, 拟合求解不出结果, 那就是真的没有可行的路径.

光滑前后的效果对比示意图:

B. MULTIPLE-PHASE DETERMINISTIC STATE SAMPLING

本阶段的目的: Given an initial state of a vehicle and a reference path, plan a smooth collision-free path aligned with the reference path while respecting nonholonomic constraints of vehicles. 分阶段 sampling

具体思想: To divide state sampling into several consecutive stages. In every stage, state sampling is employed to generate a finite set of seeding paths with different lateral offsets shifting from the reference path, which results in solving several two-point boundary value problems (TPBVPs). The closest collision-free path to the reference path at every stage is selected as the start state of the next stage. 采样点是参考线横向上的不同偏差. 每个阶段通过两点边值问题求解, 然后选取距离参考线最近的点. 采用贪心策略(greedy search strategy).

1.Reference Path Segmenting

计算效率考虑, 因此对 Path 分段.
分段方法:

1. 曲线长度: arc-length (distance along a curve) of the motion primitives
2. 曲率: the curvature profile of the reference path
   论文采用方法1, 每段 6m.
   生成为 piece-wise cubic spirals(3 阶).

2.Single Phase State Space Sampling

1. 先判断 sampling 点可用与否再生成曲线,美其名曰 an adaptive offset sampling to determine feasible goal states to generate a kinematic-feasible path. 为什么能做到不对 infeasible 的地方进行采样呢? 因为有 modified global path. 下图是有无 adaptive sampling 的对比示意图.
   
2. 求解 TwoPoint-Boundary-Value problem (TPBVP)非线性规划.
3. 轨迹为三阶螺旋线(前提: A curve is said to be parameterized by arc-length if its speed is always unit speed)
   $\kappa(s) = a + bs + cs^2 + ds^3$
   不使用其他类型的曲线模型是因为: The B-spline, Bezier curve, piecewise linear curve and polynomial curve do not satisfy kinematic constraints of the car model according to their curve definitions and independent parametrization for $x$ and $y$ in Cartesian coordinates.
4. 车辆运动学模型
   

$$
x(s) = x_0 + \int_0^{s_f} cos(θ(s))ds \\
y(s) = y_0 + \int_0^{s_f} sin(θ(s))ds \\
θ(s) = θ_0 + \int_0^{s_f} \kappa(s)ds
$$

更多细节:

1. The $κ(s)$ can be mapped to a steering angle $\phi(s)$ of the vehicle using $\phi(s) = \arctan(L · κ(s))$, where $L$ is the wheelbase.
2. Vehicle configuration at $s$ by $q(s) = x(s), y(s), θ(s)$. Since any configuration can be transformed to the origin of a local frame by a homogeneous transformation in $SE(2)^3$, the initial configuration $q_0 = (x_0, y_0 , θ_0 )$ is set to $(0, 0, 0)$.

定义 configuration space:$q(s) = x(s),y(s),θ(s)$
挑战:无法解析算积分,通过 Trapezoidal rule or Simpson’s rule数值拟合积分.

Cartesian coordinates 坐标系下的螺旋线拟合优化问题模型:

$$
r(p) = (x(p),y(p)) \\
s.t.\quad x(p) = \int_0^{s} cos (\int_0^{s}\kappa(p)ds)ds \\
y(p) = \int_0^{s} sin (\int_0^{s}\kappa(p)ds)ds \\
\kappa(p) = a + bs + cs^2 + ds^3 \\
\forall s \in [0,s_f]
$$

$a$ is the known initial curvature, while $b, c, d, s_f$ are the unknown optimization parameters.
系数向量: $p_c = (b, c, d, s_f)^T$, 使用 curve knot spacing technique(leads to fast convergence) 对其进行变换如下:

$$
\boldsymbol{p}=\left(\begin{array}{c}
m_{1} \
m_{2} \
m_{3} \
m_{4}
\end{array}\right)=\left(\begin{array}{c}
\kappa\left(\frac{s_{f}}{3}\right) \
\kappa\left(\frac{2 s_{f}}{3}\right) \
\kappa\left(s_{f}\right) \
s_{f}
\end{array}\right)=T \cdot \boldsymbol{p}_{\boldsymbol{c}}
$$

where,

$$
T=\left(\begin{array}{cccc}
\frac{s_{f}}{3} & \left(\frac{s_{f}}{3}\right)^{2} & \left(\frac{s_{f}}{3}\right)^{3} & 0 \
\frac{2 s_{f}}{3} & \left(\frac{2 s_{f}}{3}\right)^{2} & \left(\frac{2 s_{f}}{3}\right)^{3} & 0 \
s_{f} & \left(s_{f}\right)^{2} & \left(s_{f}\right)^{3} & 0 \
0 & 0 & 0 & 1
\end{array}\right) .
$$

可以通过方向盘的上下限确定 $\kappa(s)$ 的上下限, 因为方向盘转角 $\phi(s) = \arctan(L · \kappa(s))$.

a.Path generation problem formulation

$$
\begin{aligned}
\text { minimize } \quad & J=J_{s m o o t h n e s s}\left(\boldsymbol{p}{r}\right)=\int{0}^{s_{f}}\left|\boldsymbol{\kappa}\left(\boldsymbol{p}{\boldsymbol{r}}\right)\right|^{2} d s \
\text { s.t. } \quad & q\left(r\left(\boldsymbol{p}{r}\right)\right)=q_{i n i t} \text { when } s=0 \
& q\left(r\left(\boldsymbol{p}{r}\right)\right)=q{e n d} \text { when } s=s_{f} \
& 0 \leq s_{f} \leq s_{\max }
\end{aligned}
$$

目标方程的原理: Least Energy Curve, least bending energy.
实现效果: generates a smooth path that distributes the steering efforts evenly along the path while satisfying the boundary constraints. The initial guess for $p_r$and $s_{min}$ can be easily obtained by solving a Dubin path planning problem.

b.Path Selection in Single Phase

方法: 剔除 collsion -> 选择距离最近, single objective, the distance to the reference path.

c.Speed Planning Along The Fixed Path

目标: finding a minimum time speed profile traveling along a fixed path subject to the vehicle dynamics constraints, slip circle constraints($Ru = mr̈$), and actuator limits(最后一项约束在公式里没怎么体现,实际仿真时考虑的?).

这是一个 convex optimization problem. 最终的形式如下:

$$
\begin{aligned}
\min_{\alpha(s), \beta(s), \boldsymbol{u}(s)} & T=\int_{f(0)}^{f\left(t_{f}\right)} \frac{1}{\dot{f}} d s=\int_{0}^{s_{f}} \beta(s)^{-\frac{1}{2}} d s \
\text{s.t.} & \quad R\boldsymbol{u} = mr̈, \
& β{‘}(s) = 2α(s), s ∈ [0, s_f]. \
& (\alpha(s), \beta(s), \boldsymbol{u}(s)) \in\left{\left(\ddot{r}(s), \dot{r}^{2}(s), \boldsymbol{u}(s)\right) \mid\right.\left.|\boldsymbol{u}| \leq \mu m g, u_{\text {long }} \leq m a_{\max }^{\text {long }}\right}
\end{aligned}
$$

下面进行推导:

1. workspace path: $r$ of the body point, $b$, at the center of the rear axle. $r : [0, s_f ] → \mathbf{R}^2$. orientation and curvature encoded implicitly by the path.

$$
r(s) = (x(s), y(s)), s ∈ [0, s_f]
$$

因为 $s = f (t)$ 建立时域关系:

$$
r̃(t) = (x̃(t), ỹ(t)), t ∈ [0, t_f]
$$

2. speed vector $\overrightarrow{\boldsymbol{v}}$ in Cartesian coordinates:

$$
\overrightarrow{\boldsymbol{v}} = ṙ(s) = r’(s)\dot{f},
$$

where $r’(s)$ is the unit tangent vector of the path $r(s)$ at $s$ that represents the direction of the speed of a car by assuming no sliding, $\dot{f}$ is the corresponding longitudinal speed of the car in ego frame.

3. heading of the car at $s$ of the path $r$: $ θ(s)$

$$
r’(s) =(\cos(θ(s)) , sin (θ(s))) = x’(s), y’(s)
$$

4. acceleration vector $\overrightarrow{\boldsymbol{a}}$ in Cartesian coordinates:

$$
\overrightarrow{\boldsymbol{a}}=\ddot{r}(s)=r^{\prime \prime}(s) \dot{f}^{2}+r^{\prime} \ddot{f},
$$

where $f$ is the longitudinal acceleration and $r(s)$ is the principle normal vector of the path, which is also called the curvature vector.

5. scalar of the curvature

$$
\kappa = ||r’’||
$$

6. control force: $\boldsymbol{u} = (u_{long} , u_{lat})$. The dynamics of the car:

$$
R\boldsymbol{u} = mr̈ \
R=\left[\begin{array}{cc}
\cos (\theta(s)) & -\sin (\theta(s)) \
\sin (\theta(s)) & \cos (\theta(s))
\end{array}\right]
$$

where $R$ is the rotation matrix that maps forces from the ego frame to the global Cartesian coordinate system.

7. The control forces are limited by the friction circle:

$$
||\boldsymbol{u}|| ≤ μmg, \
u_{long} ≤ ma^{long}_{max}.
$$

或者显式地展示横向加速度限制:

$$
||\boldsymbol{u}|| ≤ (ma^{long}{max})^2 + (ma^{lat}{max})^2 ≤ (μmg)^2
.
$$

8. 引入简化记号:

$$
α(s) = \ddot f, β(s) = \dot f^2
$$

Then, $\dot{\beta}(s)=2 \ddot{f} \dot{f}=2 \alpha(s) \dot{f}=\beta^{\prime} \dot{f}$. Thus,

$$
\beta^{\prime}(s)=2 \alpha(s), s \in\left[0, s_{f}\right]
$$

$$
\dot{r}^{2}(s)=\left(r^{\prime}(s)\right)^{2} \beta(s) \text { and } \ddot{r}(s)=r^{\prime} \alpha(s)+r^{\prime \prime} \beta(s)
$$

9. 沿着 $s$ 进行积分希望得到最短时间内完成即可得到最上面的模型.

使用 MTSOS 求解器求解.

D. COLLISION CHECKING

1. 栅格地图通过 distance transform 生成 clearance map.

2. 用多个圆包络车形状: The footprint of the car is approximated by a cluster of circles. 步骤: 先求得整体的大圆 footprint rectangle called the bounding circle. 然后分解成 6 个小圆(是对之前用 4 个圆分解的改进, $x$ 方向上检测更准, 虽然在 $y$ 方向上检测相对于 4 圆较差, 但也可以忍受 absolute error remains relatively low (0.1m)). 如下图所示:

3. two-phase collision checking routine, 先接测大圆碰撞, 如果大圆有碰撞风险再用小圆检测. 同时作者的这种分 2 阶段并且 6 个小圆的的检测与 单阶段的 4 个小圆进行了对比性能测试. 只有在障碍物特别稠密的环境下才会进行 6(加上大圆, 应该是 7 个) 个圆的检测会降低性能. 对比测试结果如下:

最终的算法流程如下, 注意其中的自车坐标系与 grid map 的坐标系转换过程.

SIMULATION RESULTS

• 对比对象: Autoware Hybrid A*, conjugate-gradient (CG) descent path smoother(三个可调参数 obstacle avoidance term, the change of headings term, and the square of curvatures term).

• 仿真平台: ROS, Lanelet maps, 122 way points(roughly with 0.5 m for an interval), Intel Xeon E3 processor at 2.8GHz and 8GB RAM in a Linux system.

• 场景, 比较 path 路径长短, 光滑程度(Heading Profiles,Curvature Profiles), 运算时间, 消耗内存

  1. 无障碍物弯曲道路
  2. 静止障碍物弯曲道路(算法跑了约 60ms)

验证的结果省略.

感触

1. 完全是一个缝合怪, 能看出来作者的确是调查尝试过很多种算法的, 并对大部分环节的算法都做了一些小改进(基本上都是加个预处理的阶段). 参考文献可能是个宝库.
2. 前提是 free space 能给出这么复杂的参考路径信息, 也就是 reference line.
3. 没有对动态障碍物的仿真结果(原文承认).
4. 仿真结果里也没有说到速度的设置以及最终的 speed profile 与运行结果.