（学习笔记）机器人自主导航从零开始第六步——FAR Planner全局路径规划算法
本文参考资料：
GitHub - MichaelFyang/far_planner：在已知和未知环境中导航的快速，可尝试的路线规划器
https://github.com/michaelfyang/far_planner【泡泡前沿专栏•重磅】全网首发，系列(四)：CMU团队开发的全套开源自主导航算法——FAR Planner全局路径规划算法 (qq.com)https://mp.weixin.qq.com/s?__biz=MzI5MTM1MTQwMw==&mid=2247525152&idx=1&sn=f917e31efcb48be92b9989b82efd7ca6&chksm=ec13dd24db64543291ce9d61a7855e1ccb4b2761c2c420fe3b5403e86795616504dfa744bb4f&scene=178&cur_album_id=1954649120803995651#rd
前言
（以下转载自【泡泡机器人SLAM】微信公众号）
机器人路径规划（Route Planning）是一个经典问题。常见的方式类似与RRT基于采样(Sample-Based)，或者 A*基于离散化搜索(Search-Based)的路径规划算法。同时这两种方法大多限制在已经地图条件下的路经规划计算。
第三类基于环境特征（Feature-Based）的路径规划方法如Visibility Graph Planning， 其基于对环境特征的提取与几何关系分析生成路径图（Roadmap）再在生成的路径图上进行规划（Planning）。这种基于环境特征进行建图与规划的方式可生成理论最短路径。相比较与RRT其随机性而使其生成的路径图高度重叠（Redundence），稠密（Dense），而难以动态维护，基于环境特征所生成的Visibility Graph拥有更少的节点，可在环境变化发生时，或在未知环境中，随着机器人移动进行快速的动态更新，也最大限度降低路径规划的计算复杂度。
然而这种基于环境特征的方式因为对环境输入的要求，很难被应用的实际中所以在一定程度上限制了其发展。本项目旨在建立一种可在现实环境中实时提取环境几何特征并动态建立Visbility Graph进行路径规划及导航的算法，同时该算法支持在没有已知地图的环境下根据实时传感器输入对路径图以及导航路径进行实时动态调整， 可在1-2ms实现对300m环境内的全局路径规划及动态调整。本项目所提算法是我们团队(Team Explorer, CMU&OSU)参加DARPA地下挑战赛 (Subterranean Challenge)使用的全局路径规划算法。
方法论
障碍物的几何提取 （Enclosed Polygons Extraction）
为了生成Visibility Graph，需要从传感器输入信息中提取障碍物的封闭几何特征形成封闭的多边形 （Enclosed Polygon）。这里我们的传感器输入采用常见的点云图。首先，从点云中提取出障碍物的角点作为生成Enclosed Polygon的vertex point， 然后将vertex points连接形成闭合的多边形用来代表障碍物的几何形状。
动态可视性更新（Dynamic Visibility Update）
我们采取动态维护的方式更新地图， 并建立全局Visibility Graph。对于未知环境，算法对每一帧传感器输入进行处理；提取障碍物Polygon特征，并通过检测vertex points之间的相互可视性（Visibility）建立一个局部 Visibity Graph用于全局Graph更新。这样，将建立大范围全局Visibility Graph的计算量分散，并且实现在未知环境下对环境的动态更新，使得Planner可以进行在未知环境下探索式地路进规划。
动态障碍（Dynamic Obstacle）物检测与路径更新
通过对于多帧传感器输入的比较，检测环境中的动态障碍物（如：行人，车辆），算法会对受到动态障碍物遮挡的Graph进行更新，并在动态障碍物移开后恢复之前Graph内相对应vertex points之间的链接。
在未知环境中的尝试性（Attemptable）路径规划
如导航点（Goal Point）处在当前没有先验地图（Prior Map）的环境中，算法会根据当前已知信息规划出多个可行路径进行尝试，并根据之后新环境信息进行更新。
应用场景
已知环境下的快速路径规划以及导航
在已知环境中快速建立路径图（Visibility Graph）并进行路径规划，如在已知环境信息的工厂，配送中心的自主机器人路径规划及导航，同时系统可根据实时输入的环境以及出现的动态障碍物对于已知地图进行更新，并实时更新导航路径。用户可根据不同任务需且在此规划算法基础上设置更高层的任务规划算法，生成结合任务的时序导航点，实现并处理不同场景的任务需求。并且快速部署到现实机器人平台。
未知环境中的定向探索及导航
在某些应用场景中例如搜救任务中，需要对未知环境中的某一特定区域进行搜索，该算法可根据当前已知环境障碍物信息规划出多个可行性路径进行尝试，并在探索前往目标位置过程不断更新优化路径，并在以探索区域建立全局路径地图，为后续机器人导航提供环境信息，比且可直接为后续其他机器人或人员提够到达目标区域的导航路径。
机器学习以及仿真训练
该算法可生成基于环境特征的导航路径，可用于生成大量导航及动态环境数据用于网络训练。结合Matterport3D Potorealistic仿真环境及AI Habitat Engine可提供多种传感器输出，例如，RGB相机图片，深度图，语义图像以及LiDAR点云, 可以为例如，基于激光雷达或者视觉感知的状态估计， 局部避障，全局路径规划，探索等Sim-to-Real网络训练提供学习数据源。例如基于，训练基于网络的视觉导航及避障策略（Visual Planning Policy），训练端到端的定位及状态估计网络（End-to-End Localization network）。
用法
（以下翻译自GitHub - MichaelFyang/far_planner）
该存储库已在Ubuntu 18.04中使用ROS Melodic和Ubuntu 20.04与ROS Noetic进行了测试。按照上一个博客的说明设置开发环境。确保签出与计算机设置匹配的分支，编译并下载模拟环境。
要设置 FAR Planner，转到autonomous_exploration_development_environment文件夹下，并克隆存储库：
cd autonomous_exploration_development_environment
git clone https://github.com/MichaelFYang/far_planner在终端中，转到far_planner文件夹并进行编译：
cd far_planner
catkin_make要运行代码，请转到终端中的开发环境文件夹，获取 ROS 工作区，然后启动：
cd ..
source devel/setup.sh
roslaunch vehicle_simulator system_indoor.launch
打开一个新的终端，转到 FAR Planner 文件夹，获取 ROS 工作区，然后启动：
cd autonomous_exploration_development_environment
cd far_planner
source devel/setup.sh
roslaunch far_planner far_planner.launch 
现在，用户可以通过按RVIZ中的"Goalpoint"按钮，然后单击一个点来设置目标来发送目标。车辆将导航到目标，并沿途建立能见度图（青色）。可见性图覆盖的区域变为可用空间。在自由空间中导航时，计划者使用构建的可见性图，而在未知空间中导航时，计划者尝试发现实现目标的方法。通过按"重置可见性图表"按钮，计划员将重新初始化可见性图表。通过取消选中"规划可尝试"复选框，规划师将首先尝试通过可用空间找到路径。路径将显示为绿色。如果这样的路径不存在，规划者将一起考虑未知空间。路径将显示为蓝色。通过取消选中"更新可见性图表"复选框，计划员将停止更新可见性图表。要从/向文件读取/保存可见性图表，请按"读取"/"保存"按钮。室内环境的可见性图表文件示例可在'src/far_planner/data/indoor.vgh'上找到。
在导航过程中的任何时候，用户都可以使用控制面板通过单击黑框中的来导航车辆。系统将切换到智能操纵杆模式 - 车辆尝试遵循虚拟操纵杆命令并同时避免碰撞。要恢复 FAR 计划器导航，请按"继续导航到目标"按钮或使用"目标点"按钮设置新目标。请注意，用户可以使用PS3 / 4或Xbox控制器而不是虚拟操纵杆。有关更多信息，请参阅我们的开发环境页面。
要使用其他环境启动，请使用下面的命令行，并将"
roslaunch vehicle_simulator system_.launch
roslaunch far_planner far_planner.launch 要在 Matterport3D环境中运行 FAR Planner，请按照开发环境页面上的说明设置 Matterport3D 环境。然后，使用下面的命令行启动系统和 FAR Planner。
roslaunch vehicle_simulator system_matterport.launch
roslaunch far_planner far_planner.launch config:=matterport
用户可以选择定义自定义导航边界。为此，用户需要提供边界文件和轨迹文件。示例在"src/far_planner/data"文件夹中以"boundary.ply"和"trajectory.txt"的形式提供。文本编辑器可以查看这些文件。具体而言，边界文件包含用户定义的多边形，其中每个多边形都有一个索引。轨迹文件包含姿势，其中每个姿势都是一行，具有 x （m）、y （m）、z （m）、滚动 （rad）、俯仰 （rad）、偏航（rad）和从运行开始的时间持续时间（秒）。轨迹文件的格式与开发环境保存的轨迹文件的格式相同，并且需要包含至少一个姿势来确定导航边界的可遍历侧。用户可以使用下面的命令行在同一文件夹中生成可见性图形文件（提供为"boundary_graph.vgh"），该文件可以使用"读取"按钮加载到FAR Planner。
roslaunch boundary_handler boundary_handler.launch配置
FAR 规划器设置保存在 default.yaml 的 'src/far_planner/config' 文件夹中。对于 Matterport3D 环境，设置位于同一文件夹中的 matterport.yaml 中。
- is_static_env（默认值：true） - 如果环境包含动态障碍物，则设置为 false。
- is_attempt_autoswitch（默认值：true） - 设置为 false 可关闭从不可尝试的导航（对于已知环境）到可尝试的导航（对于未知环境）的自动切换。
- is_viewpoint_extend（默认值：true） - 设置为 false 以停止将可见性图形顶点从对象扩展到远离对象以获得更好的视点。
- is_pub_boundary（默认值：false） - 设置为 true 可将自定义导航边界发送到开发环境中的本地计划程序。