【论文解读】Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments
这篇论文介绍了一种名为OpenPlanner的自动驾驶系统，旨在解决复杂动态环境中移动机器人的导航问题。以下是对其主要内容的总结：
背景与挑战
自动驾驶的重要性：在智能交通和城市导航中，自动驾驶可以显著提高效率并减少事故。
现有系统的局限性：
- 开源计划者如OMPL无法直接用于实时导航。
- 自动驾驶汽车需要处理复杂的环境（如交叉路口、停车标志等）。
- 动态环境中的实时路径规划需求未得到充分解决。
OpenPlanner的核心结构
模块化设计：
- 全局规划器：基于矢量地图生成从起点到目标的无碰撞路径。
- 本地规划器：从全局路径生成局部轨迹并避免障碍物。
- 行为状态生成器：根据交通规则和障碍物状态切换行为。
2.1 矢量地图
矢量地图由车辆位置、交通信号灯、停车线、障碍物等组成。
支持复杂的道路网络（如交叉路口、曲线等），适用于动态环境。
2.2 全局规划算法
基于动态编程的方法计算最短路径。
支持复杂场景下的路径选择（如过马路）。
2.3 本地规划算法
使用采样轨迹优化方法生成平滑轨迹。
避免障碍物并优化行驶成本（距离、时间等）。
2.4 行为状态生成
使用有限状态机（FSM）处理离散行为（如停车、转向）。
根据当前状态和传感器数据切换行为模式。
实现与性能
实现细节：
- 使用C++开发，并支持ROS节点集成。
- 提供扩展功能以增强功能（如成本映射）。
性能表现：- 在模拟环境中完成每秒约14次迭代的任务。
- 在真实测试中成功过马路并避障，在筑波RWRC比赛中完成了多个关键任务。
结论
OpenPlanner通过模块化设计解决了复杂自动驾驶任务中的关键问题。
它支持离散事件驱动控制框架，并能够处理多种动态环境中的导航任务。
系统在模拟和真实环境中均表现出色，未来可以通过进一步优化提升性能。这篇论文展示了如何将复杂的自动驾驶任务分解为可管理的部分，并实现了集成的解决方案。OpenPlanner不仅适用于学术研究，还能在实际应用中推广。
标题：开源集成式规划器用于高动态环境下的自主导航系统开发
作者：Hatem Darweesh、Eijiro Takeuchi、Kazuya Takeda、Yoshiki Ninomiya、Adi Sujiwara、Yoichi Morales、Naoki Akai、Tetsuo Tomizawa和Shinpei Kato
来源：《机器人学与机电》2017年8月号
代码：https://github.com/Autoware-AI/autoware.ai（已用斜体突出显示）
摘要
运动规划是自主机器人实现高效导航的关键环节。本文介绍了一种用于移动机器人导航的开源算法框架"OpenPlanner"。该框架由全局路径规划算法、行为状态生成器及局部轨迹规划模块组成，并命名为"OpenPlanner"。该系统接收环境地图与目标位置信息后，在动态环境中完成避障与路径优化计算，并提供规避障碍物的有效解决方案。系统能够根据预先标注的道路成本信息动态生成平滑且可操作的安全全局路径方案作为参考依据，并通过局部轨迹生成模块完成平滑连续的局部运动规划任务，并由轨迹执行模块将其转化为低层控制指令以实现精确跟随与避障操作。在复杂的交通场景中进行仿真实验与实际测试表明，在基于Ackermann转向驱动的非完整移动机器人平台上"OpenPlanner"展现出卓越的表现：系统能够在复杂环境下自动生成合理的动态全局与局部路径方案，在高度动态变化中维持良好的导航性能，并保证了运行效率与可靠性水平。此外该框架已在Autoware开源自动驾驶框架中成功集成并在实际应用中取得显著成效。
关键词：自动驾驶, 运动规划, 开源软件
文章目录
-
摘要
-
1.简介
-
2.相关工作
-
- 2.1 全局规划
- 2.2 行为状态机
- 2.3 局部规划
- 2.4 开源
-
3.系统概述
-
- 3.1 全局规划器概述
- 3.2 行为状态生成概述
- 3.3 轨迹生成概述
-
4. 全局规划器
-
- 4.1 矢量地图
- 4.2 全局规划器
- 局部规划模块
5.1 展开式生成
5.2 成本计算过程- 5.2.1 核心成本
- 5.2.2 切换成本
- 5.2.3 冲突成本
- 6 基于状态机的行为生成机制
-
7.实验设置和结果
-
- 7.1 定性结果
- 7.2 大学校园实验
- 7.3 筑波RWRC实验
- 7.4 性能
- 7.5 准确性
-
8.结论
1.简介
实现自主机器人导航需要完成感知、定位、控制以及规划任务。 现有的开源资源主要集中在感知与定位方面[1]。 虽然存在一些专注于特定领域（如路径规划）的工具链或框架[2,3] ，但通用性强到可以直接应用或方便扩展为完整解决方案的开源规划系统仍比较稀缺[4] 。 由于规划模块作为整个系统的核心环节其开发通常受限于具体应用场景的需求[5] 。 因此它在现有的应用中往往难以满足多样化需求进而被限制了进一步的发展空间[6] 。 结合这一背景 我们开展的研究工作聚焦于两个关键方向：路径规划与行为规划 这两类问题共同构成了实现完全自主移动机器人导航的基础要素[7] 。 其中路径规划主要用于解决在静态或动态环境下如何生成安全可靠的运动轨迹 行为规划则关注如何根据环境条件动态调整运动策略以适应复杂多变的情况[8] 。 拥有良好性能的表现不仅体现在对既定目标环境的有效应对上 更应在真实场景下展现出较高的泛化能力[9] 。 例如博物馆导游机器人这一典型的室内自主导航应用实例已在多个国际比赛中取得了优异成绩[10] 。 同时基于现有技术 在城市道路的实际运行环境中实现类似车辆的自动驾驶能力已取得显著进展[11] 。 尽管当前在某些特殊场景下已经实现了高度智能化的道路跟随功能 但在复杂的交通环境中仍面临诸多挑战包括动态障碍物检测与避让实时决策支持等技术瓶颈尚未得到彻底解决[12-13] 。 因此具备良好的扩展性和可维护性的开放源代码平台仍是当前研究领域的关键目标之一。 本研究旨在构建一个开放源代码平台支持的自主移动机器人导航系统 该平台将提供完整的功能模块化支撑并支持多种算法框架之间的灵活集成 最终形成一套具有高扩展性且易于社区协作完善的解决方案框架。
该研究最初源于自动驾驶技术领域的开发需求，在这一背景下提出的系统也可作为Autoware框架的一个组成部分被采用[a]。 Autoware系日本筑波大学所研发的一种开源自动驾驶技术平台[6] ，已被众多研究人员应用于自动驾驶相关研究与实践[7] 。 该平台基础架构源自中所述的机器人操作系统（ROS）[8] ，它由一系列相关的软件包构成 ，例如OpenPlanner以及辅助功能库等基本组件 。 OpenPlanner具备高度通用性 ，仅需微调参数即可实现与各类移动机器人系统的兼容性 ，尤其适用于差动驱动型及非完整运动机器人体型 。 在本研究项目中我们采用了基于Ackerman转向策略的移动机器人 该设备基于滑板车底盘设计 并在筑波真实世界机器人挑战赛（RWRC）[b] 中得到实际应用 。 此次竞赛要求参赛机器人具备在动态环境中的精准定位与自主导航能力 须能实时识别交通信号灯 并应对路口过境情况 还需依靠自动滑门导航至商场内寻找特定人群 。 我们的参赛目标即通过OpenPlanner实现尽可能多的上述功能 。 每年RWRC竞赛都会产生大量创新高效的规划算法 但遗憾的是这些算法大多为独有方案 每位新参赛者均需从零开始构建自己的规划系统 在现有文献报道中通常仅对其轮廓及细节进行简要描述 因此我们致力于开发一个开源代码可操作性强的规划算法 以便供行业界人士参考并持续改进 。
本论文第二部分将综述相关最新研究成果 第三部分将概述OpenPlan ner系统及其总体架构 在第四部分我们将详细阐述全局路径规划方法 包括图表示法的应用及其理论基础 第五部分介绍本地规划器的工作原理 第六部分阐述行为状态机的设计思路 。 第七章将介绍实验装置及其性能测试结果 最后第八章总结研究成果 同时附录A介绍了当前实现版本及相关兼容平台信息 并附录B列出了核心算法伪代码。
2.相关工作
在机器人技术领域中确定从起始点至目标点无冲突路径并指导导航决策的过程被称为路径规划任务。涵盖从简单避障场景[8]到复杂环境下的高级算法[9,10]的研究已获得广泛重视。通常涉及全局与局部规划方案的综合计算。其中全局规划系统一般采用基于距离约束的方法（如Dijkstra算法[11]）以确保优化效果。而任务调度人员则通过启动、终止计划、创建新任务以及切换至紧急状态等方式进行系统协调。在以下各小节中我们列举了拟议计划系统每个特定方面的相关工作示例
2.1 全局规划
一些全局路径规划器（例如A * [12]）使用启发式函数，而其他一些全局规划器（例如Anytime dynamic A * [13]和D *算法[14]）也使用重新规划。 也有拓扑方法，例如计算无碰撞路径的Voronoi图[15]。 这些技术基于用传感器信息更新的网格图，称为代价图。 这样的技术创造了用于非结构化环境（如越野导航和停车状况）的全球飞机。 另一类全球规划环境是延伸数公里的结构化道路网络。 使用这种大小的地图，成本地图变得不切实际，并且需要不同的环境表示方法。
在2007年DARPA城市挑战赛上，车队收到了完整课程的路网定义文件（RNDF）。 团队使用此文件通过挑战目标全局计划其运动，如[4，5]中所述。 斯坦福大学的团队如[16]中所述使用了用于全局规划的动态编程技术，该技术涉及具有加速节点的动态编程。
显然，这种结构化的环境信息使团队可以规划全局路径，包括变道，交叉路口协商，停车标志，交通信号灯和停车位。 从那时起，RNDF已成为自主导航的基本要素。 在[17]中，引入了用于自动驾驶的优化RNDF，许多公司正在研究基于RNDF的精确地图，即矢量地图。 尽管这些地图的结构与开放式街道地图[8]相似，但它们更为精确，并包含其他定期更新的信息。
2.2 行为状态机
除了路径规划这一功能之外，在另一个关键功能上实现了智能行为生成的能力。该方法一般采用状态机模型来描述任务，并通过定义任务间的切换规则来进行管理。研究者在文献[18]中将连续驾驶的行为状态转化为离散的状态空间模型，并进而通过搜索算法确定最优的任务序列从而在符号空间中实现目标条件的满足。
2.3 局部规划
已提出了好几种类型的地方计划者。 势场方法将排斥力分配给障碍物，将吸引力分配给无障碍空间[14]。 其他成功的避障算法在预测车辆的未来位置时会考虑车辆的约束[19]。 全局动态窗口方法集成了全局路径信息，并将其用于避障[20，21]。 最近开发的计划人员考虑到人为因素，以便计算出使乘客感到舒适的路径[16、22]。 还引入了一种有效的本地规划方法，该方法可以从车辆的中心开始，沿水平方向运行到参考路径，从而产生多个展开画面[4，23]。然后对这些展开画面进行线性采样并进行优化，以达到最佳效果且满足车辆运动学。
对象跟踪是包括我们在内的许多本地计划方法的重要组成部分。 嘈杂的传感器，错误的检测算法和天气状况会导致误报和误报。 本地计划方法必须具有可靠的对象跟踪功能，尤其是在户外自主导航应用中，这一点至关重要。 在[24]中，多重假设跟踪（MHT）用于实现多个目标跟踪，而其他研究人员则使用了概率过滤器，例如卡尔曼或粒子过滤器。
2.4 开源
当前可用的主要开源运动规划库包括Open Motion Planning Library（OMPL）[25]和Navigation Stack [c]。 OMPL既可以作为一个与ROS兼容的API集合使用，也可以独立于ROS运行。 Navigation Stack则作为基于ROS开发的一个模块集，在其生态系统中具有高度集成性。 OpenPlanner则与其相似地提供了一系列基于C++语言开发的API集合，并具备跨平台适用性及模块化设计特点。 此外，在功能扩展方面 OpenPlanner支持通过简单的接口直接集成Autoware框架或其他基于ROS的操作系统节点实现功能拓展 Open-rdc则是在现有的 ROS 导航堆栈基础上构建的一套开源机器人导航软件[d] [26] 。 该软件特别针对差速驱动移动机器人设计，在此基础上可实现导航堆栈功能的扩展并提供完整的开发支持 Open-rdc项目起源于2015年筑波RWRC会议期间
3.系统概述
图1展示了OpenPlan ner的整体架构，在其核心模块中包含了全局规划模块、行为状态生成模块以及本地规划模块。
3.1 全局规划器概述
全局规划程序负责路径路由处理任务。该系统通过动态规划算法计算出最优路径[4]。OpenPlanner支持复杂矢量地图的全局规划功能，在本次研究中所采用的地图相对简单。Tsukuba RWRC的地图结构完整地展示在图2中。从起始点至终点我们根据交通规则及关键特征（如交通信号灯和停车区）对地图进行了详细标注具体信息将在第四章详细阐述。
3.2 行为状态生成概述
OpenPlanner的行为状态生成模块负责系统的决策任务。 该系统采用有限状态机模型，在各个状态下均能准确反映交通状况特征。 各个状态间的转换关系基于当前编队流量以及预设编程流量规则所计算出的关键参数值进行调控。 图3直观展示了当前系统可运行的状态配置情况。 为便于理解相关机制运作原理，在第六节部分将提供更为详实的技术解析内容。
3.3 轨迹生成概述
局部路径规划器接收两个关键参数：全局基准轨迹和当前位置信息。随后系统将生成多个候选轨迹作为扩展过程，并随后由本地规划者选择最优轨迹。如图4所示，在这7种可能的扩展轨迹中存在一条中间路径。为了提高算法效率我们参考文献[4]中提出了一种基于斯坦福方法的经典算法并在其基础上进行了优化和改进。具体实现细节将在第五节中进行阐述
4. 全局规划器
在第2节中, 我们阐述了多种路径规划方案及其应用场景. OpenPlanner主要采用了矢量地图作为全局规划的核心输入, 并同时采用该地图作为本地规划者生成展开轨迹的计算参考路径. 本节将深入讲解矢量地图与全局规划的相关内容.
图4展示了运行状态下的本地规划者配置情况：
- （a）当中心路径具有较高的自由度时，
- （b）当存在障碍物阻挡了中心路径时，
- （c）最优路径是左侧第二个选项。
表1列出了按重要性排序的矢量地图组件及其在大多数规划算法中的应用情况。
4.1 矢量地图
在自动驾驶领域中应用最广泛的是矢量地图方法，在某些情况下也被称作高清地理信息系统（GIS）地图，并特别适用于区分其与其他GIS应用中使用的常规地理图形（例如开放式街道图形）。这些图形通常包含若干关键组件及其功能描述如表1所示。其中值得注意的是OpenPlanner采用的是2.5D地形图，在这种情况下程序运行效率相对较高但仍然存在一定的计算开销特别是在需要实时更新地形数据时表现尤为明显这可能导致整体计划效率下降为此我们开发了一种高效算法用于调节车道中心线的空间密度分布如附录B所示这种方法能够有效平衡计算资源消耗与图形精度之间的关系同时还能有效避免由于人工操作导致几何不连续性的问题此外还需要额外施加平滑处理以提升整体图形质量该过程利用共轭梯度法[27]进行优化。尽管测试表明筑波RWRC地形图结构简单明了（见图2）但其并不能完全满足复杂道路网络需求为此我们进一步开发了支持道路网络兼容性的高级矢量图形格式如图6所示。
4.2 全局规划器
基于驾驶环境的应用场景下,自动驾驶系统可将其划分为两类不同的规划系统。 第一类涉及非结构化的道路环境,例如越野行驶或停车等场景中遇到的位置点,这些区域无法借助传统的矢量地图进行定位。 在此类环境下,最适宜采用的成本映射模型能够提供有效的路径信息。 第二类则适用于结构化道路系统,例如明确的道路线条、车道标线以及交通标志等均能在矢量地图中得到精确标注。 在这种情况下,路径规划的主要任务是从起始点到目标点寻找到达的最佳路径,但必须遵循交通法规,例如必须在车道中间行驶,在允许转向的方向上做出正确的转向动作等。 当动态规划算法确定最佳轨迹时,系统会追踪从当前车辆位置到目标的所有可能路线选项;随后按照既定规则构建所有可能路径的树状结构,直到车辆到达目的地为止(如图7(a)所示)。 当最终抵达目标位置后,系统会回溯从目标回到起始点的具体路线;并利用本地轨迹生成器来获取关于路线所需的所有细节信息(如图7(b)所示)。 本地轨迹生成器需要获取以下关键信息:当前的道路方向、即将执行的变道动作位置、停车线的位置、交通信号灯的状态以及设定的速度限制值等数据参数以确保安全可靠的操作执行。 为了验证算法的有效性,我们选取了两组具有代表性的复杂矢量地图进行了全局路径规划模拟实验(见图5和6);其中第8号图展示了包含换道操作的情况;而第9号图则是一个不包含换道操作的情况(见附录B中的表7和表8详细数据)
图表7展示了如何计算最短路线，在地图上使用线条颜色来表示各条路线的成本。这些复杂矢量图形及其全局路径示例分别展示于图5和6。编号8的部分则包含转向操作。我们通过图表5展示了其中一个测试环境的整体路径规划结果。该测试环境简明扼要地展示了与相同向量图形一致的结果。附录B列出了用于查找全局路径所使用的算法列表。
5. 局部规划器
局部运动轨迹规划系统是一组负责生成平滑运动轨迹的功能模块。其中一种实现方式是利用路径跟踪算法（例如Pure Pursuit [28]）。对于OpenPlanner框架来说，在该框架中支持动态重规划能力的实现方式是通过一种基于推导的生成方法（如图4所示）。在该框架中支持动态重规划能力的具体实现是允许行为生成器在运行时随时触发重规划过程以获得新的平滑推导轨迹的能力。在下一小节中将深入探讨这种重新规划机制的具体实现细节。
5.1 Roll-Out Generation
Roll-Out Generation需要实时执行，因为这是所有局部规划都必须能够实时工作的基本要求。因此，目标处理时间最多为0.1秒，以便控制器可以快速响应速度变化。展开生成算法的输入是当前位置，计划距离，展开数量和全局路径的下一部分。输出为n条平滑轨迹，从车辆中心一直延伸到最大规划距离。
如图10所示，将采样的展品划分为三个部分。最接近车辆的部分是Car Tip，它是从机器人中心到横向采样点的距离，其长度确定在切换轨迹之间时转向的平滑度。下一部分称为Roll in，即从车头余量的外部界限到平行横向采样点的距离，其长度与车辆速度成正比。车辆行驶的速度越快，此区域产生平滑变化的时间就越长。离车辆最远的部分称为Roll out部分，该区域从Roll in区域的外部边界一直延伸到局部轨迹长度的末端。通过从全局路径垂直移动固定距离（称为Roll out密度）来执行直接横向采样。
通过局部轨迹规划器算法生成roll-outs包括三个主要步骤，第一步是使用车辆的当前位置和最大规划距离从全局路径中提取所需的部分。 第二步是对与全局路径的提取部分相对应的新的垂直航路点进行采样。 采样从横向距离为零的车头边缘开始，然后逐渐增加，以达到在滚动边际末尾使用每个轨迹指数计算得出的滚动密度。 第三步是使用共轭梯度对每个采样轨迹进行平滑处理，这是一种非线性迭代优化技术，可消除采样步骤导致的滚动不连续性。 这也改善了曲率，从而使转向更平稳。
使用分段插值调整轨迹顶点的密度（航点），如附录B的表6所示。许多参数插值技术对输入噪声非常敏感，并将其传播到输出（例如， ，三次三次样条可以随着输入顶点的靠近而导致输出中的任意大的振荡
彼此）[29]。 因此，我们结合使用分段插值和共轭梯度平滑来生成更平滑的轨迹。 由于使用矢量地图，因此生成的轨迹通常在动力学上是可行的，因此我们假设所有车道在动力学上都是可行的。 图11显示了推出生成的步骤，并在附录B的表9中演示了实现。
5.2 代价计算
除了roll-out生成机制之外，在城市道路设计中占据重要地位的是本地规划者的核心能力——避让交通参与者以实现安全顺畅的道路通行。 该过程基于附录B表9中的算法生成流程提取出最优路径，并将其作为主要决策依据进行规划操作。 在避障过程中涉及的是探测到的障碍物及其处理方案，在这种情况下系统会根据预设的安全标准进行优化调整以确定最终行驶路线
图11.生成局部轨迹的步骤：（a）原始地图，（b）从全局路径提取的路径部分，（c）采样，（d）使用共轭梯度平滑。
使用Autoware [a]中的另一个模块可以实现障碍物检测，该模块输出两种类型的障碍物表示，边界框和点云数据簇。 障碍物表示对于准确性和性能都是必不可少的，通过使用边界框，我们可以显着提高障碍物检测性能，但会牺牲准确性。 使用点云数据可以大大提高检测精度，但会大大降低性能。 我们通过仅使用来自点云数据簇的轮廓点样本来解决此折衷问题，每个障碍物最多16个点。
轮廓点的最大数量是局部规划器的参数之一，通过增加该数量，我们可以实现更精细的表示，从而可以更准确地避开障碍物。 图12显示了使用8个轮廓点进行障碍物检测的示例。 巡回演出的计算分为三个阶段：首先，我们将xy平面划分为n个扇区。 其次，我们找到每个点和中心点之间的距离和角度，并使用该角度将点分配给一个扇区。 第三，我们选择最终的轮廓点，它们是距每个扇形中心最大距离的点。
基于仅点云轮廓点的数据表示障碍物。在图（a）中展示了一个样本随机生成的点云数据集；计算的第一步在图（b）中进行展示，在这一过程中我们首先确定了点云的整体中心位置，并将其划分为8个相等的部分——四分之一数目是一个可调节参数值，在获得更高分辨率轮廓时可以对其进行修改。接着，在图（c) 中展示了整个计算过程中的第二步操作：对于每一个划分好的四分之一区域，在其中计算该区域中心与每个数据点之间的欧几里得距离，并标记出具有最大距离的那个关键特征点。最后，在图（d)中展示了最终得到的结果轮廓结构，在每个象限中只包含关键特征点。
5.2.1 中心代价
在某些情况下，中心成本会导致车辆持续沿着车道居中行驶；此外，在每次roll-out过程中，系统会根据当前车辆与目标车道中心线之间的绝对距离来进行调整。
5.2.2 过渡代价
过渡成本抑制了车辆转向入弯的速度，从而实现了转弯过程中的稳定性。 该方法通过计算不同展开层之间的归一化垂直距离与当前轨迹的选择来综合评估成本。
5.2.3 碰撞代价
碰撞代价分两个阶段计算，以提高性能。在第一阶段，我们通过测量从障碍物轮廓点到每个生成的轨迹的距离来测试每个轨迹。由于在插入部分之后所有轨迹均与中心轨迹平行，因此我们无需在插入距离之后进行显式测试。障碍物测试是通过“圆内的点”测试完成的，其中每个轮廓边提供了测试点，圆心是道路点，每个圆的半径是车辆宽度的一半加上检测误差。
碰撞代价计算的第二阶段是检查滚入限制后检测到的障碍物与生成轨迹之间的距离。在滚入限制之后，所有生成的轨迹都是平行的，因此我们无需分别计算每个轨迹的碰撞成本。我们计算从检测到的障碍物的轮廓点到中心轨迹的距离，然后使用从每个轨迹到中心轨迹的符号距离来找到每个轨迹的碰撞代价。图13示出了以颜色编码的中心代价，而图14示出了存在障碍物时的标准化总代价。
图13展示了中间位置的机器人维持在车道中央。选定的路径位于中间位置,而其他路径采用颜色渐变表示其对应的代价。
图14.障碍对成本计算的影响。
6 使用状态机生成行为
采用一种算法难以应对诸如在交通信号灯前停车，在路口变更车道，在停车标识处停车以及在行人面前停下的情况。类似于其他交通法规则一样，这些事件本质上是确定性的现象；然而各国的法规则略有不同。值得注意的是几个参数用于调节过渡状态之间的转换频率。理论上概率方法能够使过渡更为平滑；但在实际应用中实现这种效果较为缓慢且复杂化程度较高。为此我们采用了引入计时器和计数器的方法来解决此问题。具体而言当障碍物距离阈值非常近时行为生成模块会在Swerve和Follow两种状态之间快速切换以规避潜在危险；而通过设置计数器或计时器可以有效中断这种自动切换过程以保障系统安全运行。此外当遇到如红绿灯变化及光检测精度不足等情况时基于信号可靠性需求系统必须反复采集信号信息才能确保正确切换至下一个状态。因此在每个行为状态初始化阶段我们设定了一个基本保持时间参数；只有当经过该设定时间或者紧急状况得到缓解后系统才会自动切换到下一个预定义的行为模式进行操作。
图15.带有停车线和交通信号灯的实验矢量地图。
7.实验设置和结果
第一次现场测试是在名古屋大学校园内完成的，并将路线绘制在附图15上；第二次则是在筑波发生的RWRC事件中完成，并将矢量图形绘制在附图2上。这些实验旨在评估全球计划绩效——确定目标位置并评估避障性能；同时，在停车标志处停下并在交通信号灯处也停下。此外我们还考察了平滑度 性能 准确度 实用性和可靠性。在模拟环境中使用了一个由线条 交叉路口 停车线及交通标志构成的真实世界矢量地图 如上文所述 参考附图6和7 对于现场实验部分 在地图中标注了实际路况信息
实验平台如图16所示，在[30]中使用了相同的平台。 它是一种改进的机动踏板车，因此可以由计算机控制。 它包括一个HDL32 Velodyne LIDAR传感器，用于本地化和物体检测。 除了3D LIDAR，我们还使用三个2D LIDAR进行路缘和附近障碍物检测。 在软件部分，我们有多个ROS节点，用于本地化，障碍物检测，控制，全局规划，局部规划和路径跟踪。 在附录A中，我们为ROS用户提供有关OpenPlanner的技术信息。
在本节中，我们将首先讨论定性结果，然后介绍我们的模拟，在名古屋大学校园进行的实验以及参加筑波RWRC活动的主要结果。 模拟和现场测试均取得了积极的成果。
表3列出了用于仿真和现场实验的参数配置。
7.1 定性结果
实验的第一个定性维度是系统稳定性的验证。这意味着OpenPlanner必须持续运行而不崩溃或停止工作。即使输入数据存在错误或异常情况出现时，在计划器内部也必须能够生成有意义的错误提示信息，并且系统应始终保持活性状态以便及时响应相关问题。为此，在模拟环境中我们进行了相关测试：从地图上任意选定起点启动规划算法，在规划任务局部化能力达到一定阈值后主动终止本地化功能并切换至自主导航模式。在此过程中观察到的所有异常情况均能被系统正确捕获并有效处理
图17.每个航路点的角度表示在有和没有平滑的情况下生成的路径的曲率。
7.2 大学校园实验
在此我们将重点聚焦于校园现场测试地图的一部分如图15所示该地图由两条直线一条曲线一条停车线和一个交通信号灯构成其中图17(a)展示了生成路径的曲率特征而图17(b)揭示了其平滑输出特性值得注意的是这种平滑性有助于提升路径跟踪效果无需对控制参数进行松动即可实现
图表18.无障行测试模拟结果。
行为代码及其对应动作如下：
- 2：前行
- 5：停止红灯通行
- 6：等待红灯期间
- 7：在停车标志处停下
- 8：等待停车标志指示
- 11：避让障碍物
图19. 在无阻碍条件下对模拟矢量地图进行导航时的状态流动
如图18所示，在图15中的地图部分结果展示为无阻碍条件下对该地图部分的导航模拟结果。该图表展示了无阻碍条件下对该地图部分的导航模拟结果。通过蓝线和行为ID数值来表示这些行为：其中橙色线条代表轨迹索引（即当前所选择的目标编号）。本实验采用了7个卷展栏来实现这一功能：其中3个卷展栏代表中心轨迹的数量。如图19所示，则展示了整个实验过程中行为状态转换的过程。在仿真模式下使用OpenPlanner工具能够插入不同大小和形状的随机障碍物。为了测试系统的鲁棒性，在机器人移动过程中增加了两个具体障碍物：一个位于停车标志前方的位置；另一个位于停车标志后方的位置；随后又重复了先前的实验过程（如前所述）。
如图20所示，当使用障碍物进行导航测试时状态转换与轨迹切换均为平滑过渡（即车辆停泊于停车线及交通信号灯处，并根据情况避让障碍物）。值得注意的是，在输入数据较为嘈杂的情况下（例如传感器噪声或通信延迟），系统可能会出现状态转换。
图20展示了障碍物导航过程中的模拟测试结果。行为代码标识中：
- 2代表机器人执行前行动作；
- 5表示机器人在遇到红灯时临时停止通行；
- 6对应机器人在绿灯允许下等待红灯通行；
- 7表明机器人根据停车标志指示停下车辆；
- 8指机器人在停车标志指示期间等待绿灯通行；
- 11代表机器人采取绕过障碍物的行动。
图21.用障碍物导航模拟矢量地图时的行为状态流。
图22 现场测试结果
7.3 筑波RWRC实验
筑波活动期间的一项最困难的任务就是过马路，在此之前需要先在交通信号灯处停车并等待绿灯后才可正常通行。这一过程包括快速掉头，在第二个交通信号灯处停车后再次通过马路以继续前行。为了实现这一流程的有效性，在使用矢量地图进行导航时我们能够动态选择继续当前阶段还是跳过该阶段以便寻找更优路径。通常情况下规划器会优先选择从当前位置到目标位置的最短路线因此常规路线将导致如图23（a）所示的具体路径走向。然而如果我们希望通过增加捷径相关的成本来尝试不同的任务那么可能会迫使导航系统避开那些费用高昂的关键节点从而生成一条更长但可能更灵活的道路网络（如图23b）。这条全局路径最终会体现在整体行驶路线中如图1和2所示其中详细展示了两种不同情况下的行驶轨迹变化趋势：一种是在不考虑捷径费用的情况下生成的标准路径另一种则是经过费用优化后的替代方案（如图24a和b）。通过模拟分析这两种路径的行为状态我们能够更好地理解系统决策背后的逻辑机制（见图5和6）。
该图表展示了图2中用矩形表示的筑波挑战路径的剖面。其中，在全局规划动态成本计算时：（a）未采用任何捷径；（b）采用更高程度的优化。
如图24所示，在图2中以矩形形式标记的波阵挑战路径呈现其剖面特征。在整体规划中，默认情况下该区域未被包含（a），而通过优化设计可显著提升该区域的表现（b）。此外，在整体布局中，默认情况下该区域未被包含（a），而通过优化设计可显著提升该区域的表现（b）。
7.4 性能
在校园现场实验中，该系统能够达成每秒14.6次迭代的实时计算能力，并支持本地计划和行为状态生成功能的同时统计得到62个障碍物和1255个轮廓点（见图26）。每次迭代过程中，系统会计算障碍物的轮廓并使用卡尔曼滤波器跟踪动态物体，在完成成本计算后生成新的展开方案。在图26中可以看到执行成本计算所需的时间（以黄色曲线表示）与障碍物数量（蓝色曲线表示）呈现出高度相关性：当障碍物数量增加时两者的变化趋势几乎一致。这表明成本计算环节构成了系统的性能瓶颈。值得注意的是，在不引入任何性能优化技术和未启用编译器优化指令的情况下仍能维持实时性能水平。OpenPlanner下一步的主要目标是提升障碍物呈现的流畅度以及生成更多样化的展开方案
图25展示了当选择快捷路径（顶部）或跨越马路（底部）时的行为状态结果。行为编号说明如下：
- 行为ID 2：直行。
- 行为ID 5：停车等待红灯。
- 行为ID 6：等待红灯通过。
- 行为ID 7：停在禁止掉头线前。
- 行为ID 8：停在路口指示线处等待绿灯。
- 行为ID 11：绕开障碍物前行。
图26展示了校园现场测试的性能数据曲线图。左边区域以秒为时间单位标记了四个关键计算环节所需的时间（包括物体追踪、路径成本评估、行为决策以及路径生成），这些数值与右边标记的检测到物体的数量呈现相关性。
7.5 准确性
我们的机动踏板车能够有效规避静电与移动障碍物的影响程度取决于定位系统的准确性。
图27展示了在校园测试区域设置了专用的狭窄道路用于考察或检测避开周围障碍时的性能。
图28. 在圆锥体与树木之间行驶具有较高的转弯频率。 在(a)配置中采用最低的 roll-in 裕度时，在电动踏板车上实现更快的转向速度但缺乏稳定性，在(b)配置中通过增大 car tip 和 roll-in 参数实现了更平滑的转向效果。
8.结论
在执行全局规划、本地规划和行为计划方面，OpenPlanner是一个集成式的规划工具。它是一个机器人社区能够有效利用、修改和构建的开源规划程序。本文中我们详细阐述了我们的方法——基于矢量地图的设计以及动态规划算法的应用。此外我们还深入探讨了本地规划者如何生成轨迹，并阐述了安全轨迹选择的标准依据。我们介绍了基于状态机转换矩阵的行为状态生成方法，并详细说明了相关ROS节点的功能与操作流程。通过模拟实验验证后又进行了两次实际机器人测试——一次在名古屋大学校园内进行自主导航测试另一次则在筑波真实世界机器人挑战赛中完成系统应用测试。研究结果表明该系统能够在复杂的平面地图上生成可行的路径并在动态环境中实现精确避障功能的同时还能有效处理多种离散行为如停车标志前停车以及红绿灯路口避让障碍物等动作即使障碍数量增加至100个以上其实时运行速度仍维持在10Hz以上水平并且能够输出非常平滑的轨迹曲线从而保证了系统的稳定性和可靠性