论文解读——Trajectory Planning for BERTHA - a Local, Continuous Method

论文解读——Trajectory Planning for BERTHA -a Local, Continuous Method

参考资料来源：《Trajectory Planning for BERTHA -a Local, Continuous Method》

注：本文按照论文原文顺序讲述，此方法是一个比较经典的在XYT优化的paper

A Preliminaries

• 轨迹规划是在全局坐标系进行
• map提供道路的左右边界
• Obstacle- and object data由立体视觉和雷达采集而来，静态障碍物为a set of stixels形式，动态障碍物为带速度矢量的矩形
  

Figure1中，白色车代表BERTHA，紫红色代表静态障碍物，带运动方向的蓝色为动态障碍物。

B Objective function

• 问题描述：轨迹规划的是无人车后轴中心点的运动轨迹$X(t)=(x(t),y(t){)}^T$
• 目标函数的建立：
  

  建立如（1）式的泛函指标，积分区间为$[t_0,t_0+T]$

  具体而言，$L$由五项构成，分别为期望中心线项、期望速度项、加速度项、jerk项、tangent angle项

  期望中心线项：$d$代表距离函数

  推荐速度项

  $v_{des}$计算公式

  加速度项

  jerk项

  tangent angle项

  tangent angle计算

注：后续会详细说明距离函数和推荐速度详细计算

• 目标函数离散化

  等时间间隔（间隔为$h$）划分规划时间

  利用有限差分近似速度和加速度项

  泛函指标可以由原来积分形式转换为求和形式

注：轨迹规划的是无人车后轴中心点的运动轨迹$X(t)=(x(t),y(t){)}^T$（而运动轨迹由一系列散点组成，并非连续）

C.Constraint functions

• 曲率约束
  
  曲率的计算公式
  
• 加速度约束
  
• 外部约束：主要考虑道路边界、静态障碍物、动态障碍物的影响，其都用polygons的形式给出，类似于下图中（车辆动态障碍物），在每一个离散时间范围$[t_i,t_{i+1}]$内以一系列的多边形给出
  
    为了考虑无人车的的几何特性，可以用一系列的等距圆来表征车的几何特性，后轴中心距离最后一个圆距离为$l_{min}$,后轴中心距离最前面一个圆距离为$l_{max}$,(a)图为一辆车情形，(b)图为一辆车前后时刻的情形，（c）图为一辆车整个轨迹的情形
  
    最后可以得到如（11）式的外部约束，$d(p,x_c)$代表圆中心$x_c$到多边形$d$的距离，$r_{veh}$代表圆的半径。
  

D Building constraint polygons from sensor data

• 先决策后规划：（a）图所示，从A到B规划一条优化某种平滑标准的轨迹，可能会产出许多局部最优点。（b）图所示，若先决策（对于障碍物往左或右nudge），最后规划的曲线是一个全局最优的

注：这个有点像EM planner一样，DP决策开辟凸空间

• 如何从传感器数据中建立$polygons$多边形：如（a）中所示，将障碍物（single obstacle points）和边界（left and right bound）当成图的节点（其中较节点大的为道路边界）。如（b）中所示，节点之间比较相近，所以要切割最小数量的节点来区分。最后如（c）中所示，构建带有一定安全裕量的多边形。
  

E.Distance function

这一节主要介绍距离函数的设计过程，如下图（a）所示，$p_1,p_2$代表一个多边形中相邻的两个点，可以根据有限差分的方法计算其切向量$t_1,t_2$,$x$为我们的投影点，所谓的的距离即是$n_{\lambda }$的模长（同时也具有方向），其需要满足$t_{\lambda }{n}_{\lambda }$=0(即垂直)

点$p_{\lambda }$和$t_{\lambda }$计算公式如下：


所谓的梯度场，即是对$n_{\lambda }$单位化，（b）图为构造的梯度场，红色代表（多边形的边），黑色代表梯度，蓝色代表真实的点

注：按照原文的说法这样做是为了保证约束函数的连续可微

F.Re-planning scheme

因为车的前轮转角与车的曲率相关（车辆运动学模型），曲率由轨迹二阶导计算，所以希望轨迹三阶连续（$C_{2}continuity$）,同时有因为传感器不断获取数据，所以需要不断的re-planned轨迹。

如下图所示，黑色轨迹代表上一次迭代得到的轨迹，从$t_{now}$开始，无人车沿着红色轨迹运动（be guided by the trajectory controller），假设新的轨迹规划时间小于$t_{process}$，这样就意味着旧轨迹控制器跟踪在$t_{now}+t_{process}$之前有效，为了保证反馈控制的连续性，所以新轨迹只能改变绿色的部分。同时为了保证（$C_{2}continuity$），新轨迹在规划起点时应该选用上一个迭代得到的轨迹的三个点（$x(t_0),x(t_1),x(t_2)$）

G.Constrained optimization

这个主要讲的是求解的技巧，这个先mark住，还是涉及到最优化和代码的问题，学会之后开一手专栏。