概述
最近找了一些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 pp的状态空间为:[x p ( l , d ) ,y p ( l , d ) ,θ 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进行微调,我对此持保留意见,因为该做法难以保证微调后的曲线能够通过碰撞检测,除非额外再要做一次碰撞检测。