自动驾驶之轨迹规划7——A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles文献阅读

概述

最近找了一些EM Motion Planner相关的参考文献阅读了一下以便深刻理解。本篇为：A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles

Abstract

规划器首先把规划空间离散化，并根据一组代价函数搜索最优轨迹，然后对轨迹的path和speed进行迭代优化。文中提出了一种“post-optimization”的方法，可以弥补离散化难以求出最优解的问题（离散越密，越接近最优解），并可以减少52%的规划时间。

Introduction

文中举了一些前人的工作，基本都是用多项式（curvature polynomials or quintic polynomials）+frenet farame（这里的reference line是车道中心线，后面会在文献《Optimal trajectory generation for dynamic street scenarios in a frenet frame》中详细说）生成path或trajectory的，但会存在一些不足：加速度不连续或计算量过大，（注意，此时的trajectory是粗略解）。

下面为本文算法的架构图：由trajectory planning（trajectory generation + cost function）和轨迹优化（trajectory optimization）两部分组成：首先参考《Reactive nonholonomic trajectory generation via parametric optimal control》生成path和speed，然后用cost function求最优trajectory。最后会用《GNU Scientific Library Reference Manual》（a collection of numerical routines for scientific computing）中的方法对该trajectory的path和speed进行迭代优化。

ALGORITHM FRAMEWORK - Trajectory generation

path和speed分别生成。然后结合path和speed，得到一个trajectory。

Path generation：
通过《Motion Planning for Autonomous Driving with a Conformal Spatiotemporal Lattice》中的方法进行采样：先定义采样点$p$的状态空间为：[$x_p(l,d)$，$y_p(l,d)$，${\theta }_p(l,d)$，$k_p(l,d)$]，即x坐标、y坐标、航向角、曲率，其中$l$为$S$方向坐标，$d$为$L$方向坐标，再用四次曲率多项式连接各个采样点和当前车辆位置。最后文中在这里比较了3次曲率多项式和4次曲率多项式的区别：
3次多项：$k(s)=r_0+r_{1}s+r_2{s}^2+r_3{s}^3$，有5个参数$r_0$、$r_1$、$r_2$、$r_3$、$s$（弧长），就需要5个约束方程来求解，可以用起点和终点的相对$x$、$y$、航向角变化、起点曲率和终点曲率作为约束。该方法的在每个周期内的曲率变化是连续的，但在2个plan cycles的junction point不连续（可以理解为相邻周期计算出的path没有继承性，转向会有突变。为了解决这个问题可以在引入一个新的约束：即当前车辆姿态点处曲率的一阶导数。那么此时就应当是4次多项式了），这里需要用4次多项式，但4次多项式又很耗费计算量，因此仅用于计算距离车辆当前位置较近的几段trajectory。

备注：
1、这里是想要写出一个曲率kappa关于位置s的多项式，而多项式的阶数是根据约束的个数定下的；
2、原文中上段有错误，$s$（弧长）不应是参数，因此方程里有4个参数，对应5个约束。

Speed generation：
每个path都会生成一个speed profile：先把speed space离散化，然后再用多项式曲线连接起来：$v(s)=p_0+p_{1}s+p_2{s}^2+p_3{s}^3$，每个采样点的状态空间为：[$s$，$v$，$a$]，即弧长、车速、加速度。

ALGORITHM FRAMEWORK - Cost function set

文中会从安全、舒适、效率、能耗和行为等方面衡量每一段trajectory的静动态代价：从每段trajectory中提取$n$个点来代表其cost

ALGORITHM FRAMEWORK - Trajectory optimization

如果同时优化trajectory的path和speed，计算量太大了，如果path的参数个数为$M$，speed的参数个数为$N$，则整个计算量为$O(opt(M+N))$。而且离散化会导致无法兼顾效率和最优解，基于此，本文提出了一种“post-optimize”方法：

Path optimization：
path的离散化限制了其的最优性：采样点的横向偏移、方向和曲率被约束了。因此，放松这些约束（参见下图）并在新端点之间重新生成path可以提高path的质量。这里用到了《Fast collision checking for intelligent vehicle motion planning》的单纯形算法。

Speed optimization：同上

总结

个人觉着文中较为新颖的就是最后章节的“post-optimize”，但意义不大，其本质是放开了对采样点的约束，对已经得到的path profile和speed profile进行微调，我对此持保留意见，因为该做法难以保证微调后的曲线能够通过碰撞检测，除非额外再要做一次碰撞检测。