【论文阅读】Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments

这个系列会把读论文的一些关键内容和新的体会做一个总结。会陆续更新其他文章。

Open Source Integrated Planner for Autonomous Navigation inHighly Dynamic Environments

Abstract

OpenPlanner包含三个部分

• a global path planner 全局规划器
• a behavior state generator 行为状态生成器
• a local planner 局部规划器

OpenPlanner的输入：

• map
• goal position

OpenPlanner可以做到的事情：

计算出一条全局路径，并进行避障。触发行为，比如在红绿灯前停止等。

3个planner的作用

global planner的作用：

在权衡地图标注中的交通成本后，生成一条平滑的全局路径作为参考。

local planner的作用：

生成一条平滑的，无障碍的局部轨迹，被一个轨迹追踪器使用来实现low level control，现在还不知道这个low level control具体是什么意思。

behavior state generator的作用：

处理一些情景，比如path tracking（路径跟踪），object following（目标跟随），obstacle avoidance（避障），emergency stopping（急刹），stopping at stop signs（停止），traffic light negotiation（交通灯处理）。

实验数据表明OpenPlanner可以自动生成全局和局部路径，在高精环境中流畅导航，及时，高可靠。

1. Introduction

这部分集中在两种类型的规划上，path planning和behavior planning。

OpenPlanner的结构：

包括一个global planner从vector map（road network）生成一个参考路径。
然后从这个参考路径，生成一个平滑的无障碍局部路径。
在规划器的中心，行为生成器使用预定义的交通规则和传感器数据作为协调器，使用碰撞和交通规则成本计算，选择最佳轨迹，重新规划命令和速度城市概况。

OpenPlanner已经集成在Autoware中，基于ROS开发。是ROS的外部library。

本文使用的机器人是，基于Ackerman的转向机器人。

2. Related Work

这部分没有仔细看，不影响理解OpenPlanner

3. System Overview

3.1 Overview of Global Planner

Global Planner处理路径规划。
输入：

• vector map 矢量地图
• start position 起点
• goal position 终点
  输出：
  最短，最少cost的路径

支持复杂的vector map，但这篇论文里用的是简单的。

3.2 Overview of Behavior State Generation

状态生成模块为OpenPlanner系统做出决策。是一个有限状态机，每个状态代表一种交通情景。
状态间的交换由中间变量控制，中间变量是根据当前交通信息和提前编程好的交通规则共同计算得出的。

3.3 Overview of Trajectory Generation

local path planner的输入是global reference path和current position。选出几条备选路径，local planner选出一条cost最小的路径。如下图所示：

4. Global Planner

OpenPlanner将vector map作为主要输入。

4.1 Vector Maps

vector map有时又被称为high definition map（高精地图），用来区别于GIS中的map。
OpenPlanner用的是2.5D map，意思是，它只会在需要海拔信息的时候使用海拔信息。
因为大部分情况下planning都是在2D维度上进行的，而需要用到3D的场景很少，比如遇到了非常陡峭的斜坡。

使用高阶多项式来表示vector map中的车道线的好处是，这可以帮助我们插入所需要的密集度的路径点。但缺点是多项式曲线的计算会过载。如果map只需要从文件中加载一次的话，这样就没问题；但是如果map是从map服务器中更新加载的，这样做就使planning的速度变慢。出于这个原因，OpenPlanner团队开发了高效算法来调整车道线的density。在附录B的表格6里面。中心线由于是人手画的，因此有一些误差，因此会有额外的连接方法来平滑轨迹。
OpenPlanner支持简单和复杂的vector map，复杂的比如道路网络兼容矢量图（不知道这是啥，就是下图里面的东西）

4.2 Global Planner

自动驾驶的路径规划被分为两种主要类型。
第一种类型的planning包括非结构化场景，比如越野驾驶和停车时，这些位置我们不使用vector map。这些场景最适合的是cost map。
第二种类型的场景包含结构化场景，比如清晰定位的道路，交通灯，十字路口，交通标识等，这些都可以在vector map中表述。
规划的主要目的是找到起点到终点的路径，但是在结构化场景中，我们必须遵循交通规则，比如在车道中间行驶，朝正确的方向行驶，只在被允许的情况下变道，并且驶入正确的道路以实现左拐右拐。
在使用动态编程来寻找最优路径时，我们追寻当前位置到目标位置的可能路径。在路径追踪的过程中，我们根据既有规则构建一个可能路径树，直至到达终点，如Fig7a所示。

一旦找到终点，就从终点回溯到起点，为该路径注释所有的local planner需要的信息来生成一条local trajectory，如图Fig7b

local planner需要知道交通方向，车道变换位置，停止线的位置，交通灯位置，和限速。

5. Local Planner

一个local trajectory planner（局部路径规划器）是一组方法，可以生成一条平滑的轨迹，这条轨迹可以被路径跟随算法跟踪，比如Pure Pursuit。在OpenPlanner中，我们使用roll-out生成方法，behavior generator（状态生成器）可以随时重新规划产生一条新的、平滑的一束roll-out路径。 re-planning（重规划）会在下个部分详细讨论。

5.1 Roll-Out Generation

roll-out的生成必须及时，这是local planner能够实时工作的基础。目标处理时间是0.1秒，这样控制器才能够快速地调整速度。
roll-outs generation算法的输入：

• 当前位置
• 规划距离
• roll-outs个数
• 下一部分global path
  roll-outs generation的输出：
  n个平滑的路径，ranges from车中间线到最大规划距离。

roll-outs的抽样分为三个部分，如Fig.10所示。

• car tip： 最近的部分是车的边缘，这段就是从车的中心位置到抽样旁边的距离。这段距离的长度决定了车轮在切换轨迹时的顺滑程度。
• roll in：从roll-outs的采样点开始到平行采样roll-outs终点的这一段，这段的距离与车的速度成正比。
• roll out： 从roll-in部分的最外侧到local trajectory的终点这一段。roll-out这部分的sampling按照固定距离垂直的产生几条路径，称为roll-out密度。

roll-outs的生成分三个步骤：

• 第一步：根据车辆的当前位置和最大规划距离，从global path里面分出section of interest（感兴趣的范围）
• 第二步：根据选出的这一段global path，垂直取样出路径点。这部分取样的路径，从车的中心开始，逐渐扩散到roll-outs的终点。
• 第三步：用conjugate gradient（共轭梯度）平滑生成的轨迹，这是一项非线性插值优化技术，能够消除取样roll-outs步骤中产生的不连续性。同样能提升曲率，带来更顺滑的驾驶操作。
  
  轨迹顶点（路径点）的密度由分段插值法进行调整，如附录B表6所示。很多参数插值方法都对输入的噪音非常敏锐，并会将其传播到输出。（例子：在输入顶点距离很近的情况下，三次样条在输出中会引入很大的随机振荡） 因此我们采用一组分段插值方法和共轭梯度来产生更平滑的轨迹。
  我们的输出轨迹在运动动力学上是可行的，因为我们使用了vector map，因此我们假定所有的lane都是在运动动力学上可行的。Fig.11展示了roll-out的产生过程，实现方法在附录B的表格9中有体现。

附录B表格9如下所示：

5.2 Cost Calculation

除了roll-out的生成，对于local planner而言很重要的另一个功能是lane内部的避障，例如转向。避障就是在生成的所有roll-outs里面选择一条最好的trajectory。避障程序的输入是roll-outs的结果和检测到的障碍物，输出是选择的trajectory。我们使用额外的cost function来衡量每个trajectory，这个function计算三中常规的cost度量，priorty cost， collision cost和transition cost，选择cost最小的trajectory。
避障在Autoware的另一个模块中实现，输出两种类型的障碍物表示，bounding box和点云束。障碍物的表示对精确度和performance都很重要，用bounding box可以明显的提升障碍物检测performance，但是以牺牲准确性为代价。用点云表示障碍物则极大地提升了准确性，但却彻底牺牲了performance。我们解决了这个权衡问题，通过只用点云轮廓一束数据，每个障碍物最多只取16个点的方法。
轮廓的最多点数是local planner的重要参数之一，通过增加这个数值，可以得到更好的表述，更精确的避障效果。Fig12展示了用8个轮廓点进行的障碍物检测。轮廓的表述在3个阶段进行计算：首先把平面分为几个部分（这里是8个）；第二步找到每个点和中心点之间的距离和角度，然后用这个角度将点分配给每个部分；第三步选择最终的轮廓点，就是每个部分里面距离中心点最远的点。

5.2.1 Center Cost

Center cost约束车辆总是行驶在lane的中间，每一个roll-out都是用和lane中间线的绝对距离计算出来的。

5.2.2 Transition Cost

Transition cost约束车辆跳出roll-outs，这可以带来更顺滑的驾驶。这个cost是计算当前选择的trajectory和roll-outs的规范化后的垂直距离。

5.2.3 Collision Cost

Collision cost被分为两步计算来提升表现。
第一阶段，通过计算障碍物轮廓点到每个生成的trajectory的距离，测试每个trajectory。因为所有生成的trajectory都是平行于roll-in之后的中心线的，所以不需要精确测试roll-in之后的距离。障碍物的测试是用“点在圈内”方法，每个轮廓边提供测试点，圆的中心是way-pioints，每个圆的半径是车宽的一半加检测误差范围。
第二阶段是检查roll-in限制之后，生成轨迹和检测到的障碍物之间的距离。在roll-in limit之后，所有轨迹都是平行的，所以不需要给每个轨迹都分别计算collision cost。计算障碍物轮廓点到中心线之间的距离，然后用每个生成轨迹和中心线之间的符号距离来得到每条轨迹的collision cost。Fig13展示了颜色表示的center costs，Fig14阐述了有障碍物时的规范化总cost。

6. Behavior Generation Using State Machine

各种情景，比如在红绿灯前停下，决定变道，在停止标识前停止和等待，已经礼让行人等，很难用一个算法来解决。就像交通规则，这些事情是自然规定的，但是国家和国家之间却各有不同。另外，一些特别的交通规和目标可能会被随时添加或取消。我们将对这些事情的反应称为行为、任务、目标、状态或情境。这篇文章采用“behavior state”表达所有状态，以及状态之间的切换。前面的Fig3展示了所有的状态类型，表2展示了所有状态间转化的条件。

有几个参数控制状态间的转换。这几个参数在每个interation都会被决定性的计算。理论上，概率方法会带来更平滑的转换，但是它却更慢，而且更复杂，在实现和维护大范围应用时。解决这个问题的方法之一是引入counter和timer。比如，障碍物移动接近阈值时，状态生成机快速在转向跟随之间切换。conter或者timer可以打断这个循环。另一种counter可以给出更好方案的场景是，当交通灯从红灯变成绿灯而红绿灯探测器的结果不够可靠来处理这种情况时。在这类情况中，很必要收到多次信息来确认信号的可信度来切换到下一状态。因此在每个行为状态初始化时，我们都设置了一个最小过渡时间，这样状态会持续保持运行，直到过了设定时间，或者遇到紧急的状态切换。

7. Experimental Setup and Results

7.1 Qualitative Results

这部分提到，quality的第一部分是稳定性，openplanner必须一直保持运行，即便是有错误信息被提供，openplanner必须永远保持运行，不能崩溃，这部分通过了测试。
第二部分是完整性。意味着系统必须在各个状态之间顺滑的切换，绝对不能卡住。实验证明可以做到，不过切换的顺滑性也依赖于障碍物检测节点的可靠性。

7.3 Tsukuba RWRC Experiment

planner会自动走更远的路来规避拥堵。

8. Conclusion

第一段总结了一下这篇论文的主要内容。
第二段：这篇文章的主要贡献是为机器人社区提供了OpenPlanner的开源代码。这部分开源代码集成到Autoware项目的ROS的包里面，可以作为独立的包使用，也可以在Autoware项目里使用。可以使用.kml格式的RNDF地图文件，这种地图文件容易创建和修改，且被Autoware支持。OpenPlanner的基础功能是可以作为share library使用，这样用户就可以在ROS环境以外的地方使用这个库。
第三段：实验的内容…成功的自动巡航要求能够规避障碍物和行人，在道路上跟随移动物体的能力，在狭窄走廊里通行，协调自动门，在红绿灯和停止符号前停止等。我们的实验中，OpenPlanner可以完成上述所有任务。

Appendix A. Implementation

这部分介绍了OpenPlanner的代码。OpenPlanner的代码是Autoware代码框架的一部分，这部分被分为两个ROS node：

• way planner
• DP planner
  这两个node都使用了两个共享的library，utilityh.so和plannerh.so，包含了可以被用到任何planning任务的可复用的方法。基于doxygen的文档和wiki都提供了这些node和libraries。由于OpenPlanner的ROS node基本都是用这两个库的核心功能，所以basic planner可以在ROS之外的其他任何平台使用。

A.1. Way Planner Node

way planner node的作用是global planner，用来产生到目标位置的参考路径，可以根据要求运行。参考路线包含多条trajectories供local planner执行lane的变换。如果关闭lane change选项，只有一条trajectory会被返回，如果可能的话。way planner node的输入包括vector map，当前位置和目标位置。输出是最短路径，或考虑了预定义交通成本的地图路径。如图8，9。
way planner node 的 launch file 提供了一些参数供用户控制node的行为和表现，如表4。

另外，这个way planner全局规划node可以和任何local planner一起使用。任务规划器负责调用路径规划器节点并向其发送目标位置。现在，目标位置也可以在rviz中指定，在simulation模式中，起点和终点被用这种方式指定。

A.2. DP Planner Node

DP planner node的作用是一个local planner。当这里有一个global path时，DP planner会生成一束roll-outs然后选择最好的一个作为输出，取决于障碍物的检测情况。它也输出行为状态信息（当前状态，最大速度，最小速度，停止距离，跟随距离，跟随速度）。这些信息的值被计算来支持controller。在我们的测试环境中，使用一个前馈PID控制器，这个controller只使用当前轨迹，状态和最大速度。其他控制器可能会使用跟随距离或速度来生成更顺滑的控制信号。用户可以选择关掉way-point跟随器或使用Pure Pursuit节点来跟随生成的轨迹。DP Planner的重要参数描述如表5。

Appendix B. Algorithms

伪代码描述了相关算法。

单词速览

cornerstone 基石
non-holonomic 非完整的
orchestrator 协调器
scooter 小型摩托车
finite state machine 有限状态机
elaborate 阐述；精心制作的
elevation 海拔
high order polynomial 高阶多项式
interpolate 插
margin 边缘
lateral 旁边，侧
perpendicularly 垂直的
conjugate gradient 共轭梯度
curvature 曲率
piece wise interpolation 分段插值
kinodynamically 运动动力学
swerving 转向
drastically 彻底地
trade-off 权衡
contour 轮廓
explicit 明确
signed distance 符号距离
deterministically adv确切地
swerve v. 转向
transition 过渡
elapsed 消逝
curb n. 路沿石
feed-forward 前馈