Abstract

OpenPlanner包含三个部分

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

OpenPlanner的输入：

map
goal 
position
obstacles
traffic-light

OpenPlanner可以做到的事情：

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

3个planner的作用(两种类型的规划：path planning和behavior planning)

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.Open planner结构

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

2.相关工作

2.1 Global planning

全局规划器，满足最最优函数，并从当前的位置到目标位置的路径，通常使用的是Dijkstra中的距离约束。RNDF文件用于描述了全局路径的信息，比如交通信号灯，车道线等，它是基于的Vector map。
2.2 Behavior State machine

将规划抽象成一组离散的行为
2.3 Local planning

生成很多的Rollouts，Open Planner不需要cost map，只需要矢量地图就能完成全局规划，局部规划和行为状态生成也只需要检测当前位置和检测到的障碍物。

3.系统概述

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最小的路径。如下图所示：

Local Path的输入是全局的路径和当前的位置，几个候选轨迹，计算cost，选择cost最近的路径。

4.Global Planner

4.1 Vector map

Open Planner使用2.5D地图，这意味着它包含仅在需要时使用的高程信息。这可以提高规划性能，因为大多数规划是在二维中完成的，但三维信息也可以在极少数情况下使用，例如遇到非常陡峭的斜坡时。

Open Planner，为了加快规划的速度，减少计算量，用了共轭梯度（CG），进行平滑曲线。

使用高阶多项式来表示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步骤中产生的不连续性。同样能提升曲率，带来更顺滑的驾驶操作。

Roll-Out的部分算法输入是当前位置，规划距离，Roll-Out Numbers，截取的全局路径的下一部分，输出n条平滑的轨迹。图中的Car tip是车辆中心到横向采样点的距离，Car tip的长度决定了在两条轨迹之间切换时转向的平稳性。Roll in是指从Car tip到外部极限的平行横向采样点的距离，这部分的长度和速度成正比。Roll out，从Roll in到局部路径的末端，通过从全局路径垂直移动固定距离来执行直接的横向采样。

局部轨迹规划算法的滚动生成包括三个主要步骤，第一步是利用车辆的当前位置和最大规划距离从全局路径中提取感兴趣的部分。第二步是对新的垂直路径点进行采样，这些点对应于全局路径的扩展部分。采样从横向距离为零的车辆端部边缘开始，然后逐渐增加，以达到使用Roll in末端的每个轨迹指数计算的滚出密度。第三步是使用共轭梯度平滑每个采样轨迹，这是一种非线性迭代优化技术，可消除采样步骤导致的滚动不连续性。这也提高了曲率，从而使转向更平稳。

轨迹顶点（路径点）的密度由分段插值法进行调整，如附录B表6所示。很多参数插值方法都对输入的噪音非常敏锐，并会将其传播到输出。（例子：在输入顶点距离很近的情况下，三次样条在输出中会引入很大的随机振荡） 因此我们采用一组分段插值方法和共轭梯度来产生更平滑的轨迹。
我们的输出轨迹在运动动力学上是可行的，因为我们使用了vector map，因此我们假定所有的lane都是在运动动力学上可行的。Fig.11展示了roll-out的产生过程，实现方法在附录B的表格9中有体现。

附录B表格9如下所示：

5.2 Cost Calculation

在避障过程中输入的是Roll out和检测到的障碍物，输出是选定的轨迹。 我们使用加性成本函数来评估每个轨迹，它计算三种不同的归一化成本度量，优先级成本、碰撞成本和转移成本，选择其中最小的一个。

除了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。

5.3 Collision 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

在本文中，我们解释了我们的方法论、矢量图设计和全局规划器的动态编程。 我们还解释了局部规划器如何生成轨迹和安全轨迹选择标准。 我们使用状态机转换矩阵概述了行为状态生成，并解释了相关 ROS 节点的功能和用法。 我们进行了模拟实验，然后进行了两次实地测试，一次在名古屋大学校园内进行，另一次在筑波真实世界机器人挑战赛中进行。 我们的结果表明，OpenPlanner 能够通过复杂的矢量地图规划轨迹并在动态环境中导航，同时处理各种离散行为，例如在停车标志和交通灯处停车以及避开障碍物。 如果没有进一步的优化，即使障碍物的折痕数量增加到100个左右，OpenPlanner仍可以以超过10 Hz的速度实时运行。OpenPlanner还可以生成非常平滑的轨迹，从而可以进行更平滑的控制。

本文的主要贡献是将 Open Planner 作为开源资源提供给机器人社区使用和增强。 源代码可作为 Autoware 项目中 ROS 包的集合提供。 它可以用作独立包或在 Autoware 框架内使用。 它可以使用.kml格式的RNDF映射文件（可以轻松创建和修改）以及Autoware支持的矢量映射。 OpenPlanner 的基本功能可作为共享库使用，因此用户可以将其用于 ROS 环境之外的开发。

在这项研究中，我们证明了 OpenPlanner 可以在动态环境中有效和安全地运行，方法是使用改进的电动滑板车成功地通过筑波真实世界机器人挑战赛和在名古屋大学校园内的类似环境中导航。 成功的自主导航需要避开移动物体和行人，能够沿着路径跟随移动物体，在狭窄的走廊中导航，通过自动门，以及在红绿灯和停车标志前停车。 在我们的现场测试中，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。

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。

参考

https://blog.csdn.net/ninnyyan/article/details/129727513