文献阅读之高速场景的分层运动规划框架

大家好，欢迎大家关注我的知乎专栏- 慢慢悠悠小马车。

---

原文《A Hierarchical Motion Planning Framework for Autonomous Driving in Structured Highway Environments》。

摘要、简介、算法框架

本文提出了一种分层运动规划框架，适用于结构化的含动静态障碍物的高速环境。为了解决高计算复杂度和局部极小值问题，提出了解耦的规划方法如图1，分为2步：long-term planning（10s） and short-term planning（3s）。

1、long-term planner产生far-sighted行为（behavior），也是分2步：

a、 沿车道采样构造搜索空间，动态规划搜索出一条最优路径（path），考虑到道路的形状和障碍物（附近车辆）。这一步决定了在哪片区域行驶，体现了纵向和横向的lane-level决策信息（是否变道）。

b、构造加速度的搜索空间，应用Jump Point Search（JPS）搜索一条带时间信息的轨迹（trajectory）。JPS只适用于uniform step cost，但本文的non-uniform step cost可以产生次优解。

2、short-term planner应用particle swarm optimization（PSO）优化一条short-sighted轨迹。steering angle set被编码为1个particle，考虑了车辆运动学模型。

vehicle-in-the-loop仿真验证，动静态障碍物都是通过Intelligent Driver Model生成的。planning horizon是10s，规划周期100ms。

三 long-term planning

A 最优path生成

搜索空间是frenet坐标系下沿车道（lane-level）的采样产生。采样间隔是固定的，取决于当前速度和时间间隔，最短的采样间隔设置为10m，以防低速、减速时过密。如图2、3，方程2-3。

2个相邻layer间的2个node所连接构成的边，其代价值通过方程4来计算，第1项是欧式距离（方程5），第2项是对相邻2条最优path之间的横向偏离的惩罚（方程6），第3项是对自车预测位置和障碍物预测位置的碰撞可能性的惩罚（方程7-8）。障碍物的预测采用constant velocity（CV）模型（预测模型太简单，不适用复杂环境），自车预测位置在2个node连接的边中。若在同一车道内任一时间片有碰撞的可能，就施加一个足够大的常数cost以惩罚碰撞（碰撞一票否决）。至此，该最短路径搜索问题可以采用动态规划（Dynamic Programming）来解决了。

B 轨迹生成模型

轨迹生成构造为一个最优化问题，目标函数如方程10。

状态转移方程以离散的形式体现为方程12，采用的恒加速度模型，加速度来自备选加速度集合。

环境中的障碍物以其与自车路径的重合覆盖范围来表示，与Apollo的S-T Graph相同，如方程13、14和图4。

 状态转移方程12的输入是加速度，因此将方程10的最优化问题，转换为离散的加速度构成的采样空间中搜索最短路径的问题，就可以应用图搜索算法。基于最优的加速度和时间间隔，计算出速度和path结合，构成trajectory。

代价函数15-16同方程10，各项代价分别为：

1、速度和参考速度的差异惩罚。限速通过最大横向加速度和车道曲率计算，结合交通规则限速取最小值，如方程17-18。

2、惩罚加速度的大小，主要考虑舒适性。 

3、障碍物代价，取决于在distance-time plane（即S-T Graph）和path是否有重合覆盖区域，如方程19-20。

C 轨迹求解

JPS只能求解step cost is uniform的最短路径问题。但本文限定了如下条件：

当障碍物预测位置和自车规划位置存在重叠时，如果某一时刻a=0被采用了（minimum action），则之后会一直保持a=0，不会再接受其他action。

使得可以使用JPS来计算次优解。（不懂）

如图5（a）是最优解的示意，（b）是本文的次优解计算方式，即在后半段始终保持a=0，如果发生碰撞，最后加速的node不被加入open list，那么保持匀速的上面一条速度曲线被保留作为次优解。

JPS的算法内容见算法1-2，不做赘述。算法中的节点状态表示为{state, path cost, heuristic, action, parent node}。启发函数如方程21-22。

 四 short-term planning

A 重采样

Long-term输出的path是long time horizon的behavioral strategy，需要在2个相邻node间线性插值，以满足接下来的优化分辨率需求。

B 轨迹优化

PSO常用来求解非线性优化问题，具有快速收敛的优势，但无法保证是全局最优解，有局部极小值问题。

如图7，1个particle就是1个可行解（1组采样值），包含了Npso个steering angle，即Npso维的向量，使用fitness function评估。Particle考虑了运动学约束，满足non-holonomic constraint。状态转移见方程26-28.

C fitness function

PSO中的particle沿令fitness function取最小值的方向移动。fitness function见方程29。第1项是惩罚对参考轨迹横纵向的偏离（方程30），参考轨迹即重采样后的输出。第2-5项是舒适度的acceleration and jerk costs（方程31-34），acceleration and jerk采用前向差值计算。需要限定横纵向加速度的上限，如方程35.第6项是碰撞代价，使用2个圆近似车辆外形做碰撞检测，若发生碰撞，cost设置为无限大。优化示意如图8.

五 车辆在环仿真

C++代码运行在Intel Core i7-6700K CPU的Ubuntu 16.04系统，执行周期100ms，可以执行变道、跟车、避障、停车等行为。没有实车验证和开源代码。

总结

本文将path和speed的生成解耦，先使用采样+图搜索算法构造粗略解，再应用PSO优化以得到更平滑安全的轨迹，完成了变道决策+运动规划的工作。但因为采样比较稀疏（尤其是车道横向）、动态障碍物预测采用CV模型、自车状态转移采用CA模型、PSO可能陷入局部极小、碰撞检测采用2圆粗略近似等限制，无法产生多样灵活的决策输出（如借道），在障碍物较多的复杂环境中容易规划失败、输出轨迹可行度低，难以在实际应用中使用。