论文解读——A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles

A Real-Time Motion Planner with Trajectory Optimization forAutonomous Vehicles

参考资料来源：《A Real-Time Motion Planner with Trajectory Optimization forAutonomous Vehicles》一文

注：本文章在于《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》一文的解读，大家在学习过程中请关注以下几个问题：

• 轨迹如何产生
• cost函数设计过程
• iterative trajectory optimization algorithm的优势

1 基础知识

1.1 多项式螺旋线

多项式螺旋线如上图所示。它代表了一类曲率可以用弧长（对应轨迹的s方向）的多项式函数来表示的曲线簇。
使用三阶或者五阶多项式螺旋线，其曲率$\kappa$和轨迹弧长$s$的关系$\kappa (s)$如下。
三阶：$\kappa (s)={\kappa }_0+{\kappa }_{1}s+{\kappa }_2{s}^2+{\kappa }_3{s}^3$
五阶：$\kappa (s)={\kappa }_0+{\kappa }_{1}s+{\kappa }_2{s}^2+{\kappa }_3{s}^3+{\kappa }_4{s}^4+{\kappa }_5{s}^5$
基于这种使用三阶（五阶）螺旋线连接的轨迹，其参数可以通过梯度下降的方法快速有效地进行搜索

2 轨迹产生

该文章依旧采用自然坐标系下解耦规划+多项式螺旋线分别规划path和speed,从而产生多条候选轨迹，后续在采用cost函数进行选择。

2.1 path规划

采用如上图的path规划手段。蓝色车代表当前车的状态，灰色车代表采样点，红色轨迹代表四次多项式螺旋线，绿色代表三次多项式螺旋线。连接方式：1.当前车与endpoints进行连接（红色）；2.不同endpoints间进行连接（绿色）。

path规划中，采样+多项式连接属于老套路，在该文章中比较新颖的是

• 计算四次多项式螺旋线是耗时的，所以只有一部分通过四次多项式螺旋线连接，其余使用三次多项式螺旋线连接。
• 三次多项式螺旋线虽然计算快，单存在如下的缺点，该方法的在每个周期内的曲率变化是连续的，但在cycles间的junction point不连续
• 为了解决cycles间的junction point不连续问题，引入一个新的约束：即当前车辆姿态点处曲率的一阶导数。那么此时就应当是四次多项式了

注：关于这个不同cycles之间的拼接问题，这个先mark住，回头学习完Apollo代码回来再补充，这个论文结论可以先记住。

2.2 speed规划

采用如上图speed规划手段，绿色点代表采样点，蓝色点代表当前状态点，红色为无效曲线（超出了加速度限制），灰色代表有效曲线，关于这个speed规划有如下几个点需要关注

• 一般的speed规划都是s-t规划，而这个是speed-s规划（这里使用的是四次多项式连接，纵坐标是速度，横坐标是弧长）。其目的是为了与path规划相对应（横坐标都是弧长）

3 cost函数设计

设计如上图的cost函数，各个指标如下，该文章将其分为了静态和动态指标

该文章其余指标大家应该理解都没问题，比较新颖的是有关障碍物的指标$c_{obs}^s$和$c_{obs}^d$,下面详细说明一下障碍物指标的计算过程

使用多个circles代表无人车的结构，这样与传统的包络盒相比，这样的碰撞检测精度更高，因此采用如下公式计算$c_{obs}^s$和$c_{obs}^d$中的$s_i$和$d_i$

当无人车和障碍物距离小于某一阈值时，直接给障碍物cost赋值无穷大，否则使用（7）和（8）公式进行计算，$g_j$是障碍物和覆盖汽车的第$j^{t}h$，${\lambda }_{obs}^s$和${\lambda }_{obs}^d$表示常数。

4 iterative trajectory optimization algorithm

该文章的planning框架如上，path和speed解耦规划，随后合并轨迹并使用cost函数选择最佳的轨迹（博客2，3所介绍的过程）。紧接着需要对生成的轨迹优化，下面主要说一下轨迹优化的东西。

• 该文章使用的是速度和路径分别优化并非同时优化，虽然同时优化解的质量更高，但是计算效率很低（维数变大，耗时很大）。而分别优化，虽然解次优，但是计算效率很高，同时得到的解也是可以接受的。（EM planner也是采用如上的策略）
• Compared with the planner without optimization, this framework can reduce the planning time by 52% and improve the trajectory quality.（摘要结论，时间计算大幅度减少）