SLAM+语音机器人DIY系列：（六）SLAM建图与自主避障导航——3.ros-navigation机器人自主避障导航
温馨提示
摘要
通过前面的基础学习，本章进入最为激动的机器人自主导航的学习。在前面的学习铺垫后，终于迎来了最大乐趣的时刻，就是赋予我们的miiboo机器人能自由行走的生命。本章将围绕机器人SLAM建图、导航避障、巡航、监控等内容展开。本章内容：
2.google-cartographer机器人SLAM建图
3.ros-navigation机器人自主避障导航
前面的学习教程打好了必须的基础，现在就正式开始探讨ROS系统最强大的特性之一，让我们的机器人能自主导航和避障。这都得益于开源社区和共享代码，使ROS拥有大量可用的导航算法。将这些导航算法集大成者，便是ros-navigation导航功能包集。了解更多ros-navigation的信息，请参考官方wiki教程：http://wiki.ros.org/navigation/。
3.1.机器人自主避障导航原理分析
要分析导航功能包的原理，肯定需要先参考下面这张ros-navigation官方给出的系统框图，如图30。最中心的是move_base节点，是导航过程运动控制的最终执行机构，move_base订阅用户发布的导航目标move_base_simple/goal，并将实时运动控制信号cmd_vel下发给底盘以实现最终运动控制，move_base中的各种导航算法模块都是以插件的形式进行调用的，这样可以很方便的替换不同的算法以适应不同的应用，其中global_planner用于全局路径规划、local_planner用于局部路径规划、golobal_costmap是全局代价地图用于描述全局环境信息、local_costmap是局部代价地图用于描述局部环境信息、recovery_behaviors是恢复策略用于机器人碰到障碍后自动进行逃离恢复。然后是amcl节点，amcl节点利用粒子滤波算法实现机器人的全局定位，为机器人导航提供全局位置信息。再然后是map_server节点，map_server节点通过调用前面SLAM建图得到的地图为导航提供环境地图信息。最后就是要提供机器人模型相关的tf信息、里程计odom信息、激光雷达信息scan。
（图30）ros-navigation导航功能包集系统框图
机器人全局定位amcl粒子滤波算法：
amcl的全称是自适应蒙特卡洛粒子滤波，这里通过讲解粒子滤波、重要性采样、机器人绑架、自适应蒙特卡洛这几个概念来说明机器人全局定位的原理。由于amcl的数学理论比较复杂，限于篇幅这里不展开讲解，感兴趣的朋友可以参考《Probabilistic Robotics》这本书，里面有详细的推导过程，如图31。
（图31）概率机器人
粒子滤波，是一种思想，比如要计算一个矩形里面一个不规则形状的面积，这个问题不好直接计算，但是可以拿一把豆子均匀撒到矩形中，统计落在不规则形状中豆子的占比就能算出其面积了。在机器人定位问题中，我们在地图的任意位置撒上许多粒子点，然后通过传感器观测数据按照一定的评价方法对每个粒子点进行打分，评分高的粒子点表示机器人有更大的可能在此位置；在下一轮撒点时，就在评分高的粒子点附近多撒一些点，这样通过不断的迭代，粒子点就会聚拢到一个地方。这个粒子点聚集的地方，就是机器人位置的最优估计点。如图32，红色的粒子点慢慢聚拢到一团。
（图32）粒子滤波
重要性采样，在粒子滤波的迭代过程中，评分高的粒子点会被下一轮迭代时更加看重，这样不断迭代真实估计值附近的粒子点会越来越多。
机器人绑架，当机器人被突然从一个地方抱走到另一个地方，这个时候前一轮迭代得到的粒子点完全不能在新的位置上试用，这样继续迭代下去就会发生位置估计的错误。
自适应蒙特卡洛，自适应主要体现在两个方面。通过判断粒子点的平均分突变来识别机器人绑架问题，并在全局重新撒点来解决机器人绑架问题；通过判断粒子点的聚集程度来确定位置估计是否准确，在估计比较准确的时候降低需要维护的粒子点数目，这样来降低算法的计算开销。
代价地图costmap：
代价地图用于描述环境中的障碍物信息，代价地图利用激光雷达、声呐、红外测距等探测传感器的数据来生成，大致的原理是通过建立不同的图层Layer然后叠在一起，被填充的栅格点表示有障碍物，如图33，想要了解代价地图的更多细节可以阅读《Layered Costmaps for Context-Sensitive Navigation》这篇论文。机器人导航中用到了两个代价地图，全局代价地图global_costmap和局部代价地图local_costmap。
（图33）代价地图
路径规划：
机器人导航中核心的就是路径规划，路径规划是通过利用环境障碍物信息找到一条到达目标并且开销小的路劲。导航中会用到两种路径规划，即全局路径规划global_planner和局部路径规划local_planner。全局路径规划更像是一种战略性策略，需要考虑全局，规划处一条尽量短并且易于执行的路径。在全局路径的指导下，机器人在实际行走时还需要考虑周围实时的障碍物并制定避让策略，这就是局部路径规划要完成的事，可以说机器人的自主导航最终是由局部路径规划一步步完成的。ROS中推荐的全局路径规划器是global_planner，基于A*算法，如图34。
（图34）基于A*算法的全局路径规划
由于ROS中的局部路径规划器比较老了，所以我推荐大家使用比较新的局部路径规划器teb_local_planner。teb_local_planner是基于弹性时间带碰撞约束的算法，算法将动态障碍物、运行时效、路径平滑性等约束做综合考虑，在复杂环境下有更优秀的表现，如图35。
（图35）基于弹性时间带碰撞算法的局部路径规划
3.2.ros-navigation安装
ros-navigation的安装有两种方法，方法一是直接通过apt-get安装编译好的ros-navigation库到系统中，方法二是下载ros-navigation源码手动编译安装。由于后续可能需要对ros-navigation中的算法做修改和改进，所以我采用方法二进行安装。
这里将建立一个叫catkin_ws_nav的ROS工作空间，专门用于存放机器人导航相关的功能包。关于创建ROS工作空间的操作，请参考前面相应部分内容，这里就不做讲解。
安装命令很简单，执行下面的命令编译安装就行了。
首先前往https://github.com/ros-planning/navigation，将分支切换到kinetic-devel然后下载源码到本地，将navigation-kinetic-devel.zip解压备用。将解压好的navigation-kinetic-devel文件夹拷贝到~/catkin_ws_nav/src/后，就可编译安装了。
#编译过程可能会遇到找不到Bullet等依赖问题，解决这些依赖简便办法 #先apt-get安装ros-navigation包，这样依赖会被自动装上 sudo apt-get install ros-kinetic-navigation* #再apt-get卸载掉ros-navigation包 sudo apt-get remove ros-kinetic-navigation ros-kinetic-navigation-experimental cd ~/catkin_ws_nav/ catkin_make
然后需要安装teb_local_planner，前往https://github.com/rst-tu-dortmund/teb_local_planner，将分支切换到kinetic-devel然后下载源码到本地，并解压源码包到~/catkin_ws_nav/src/后，就可以编译安装了。
#安装依赖 source ~/catkin_ws_nav/devel/setup.bash rosdep install teb_local_planner cd ~/catkin_ws_nav/ catkin_make -DCATKIN_WHITELIST_PACKAGES="teb_local_planner"
值得一提的是，teb_local_planner源码中关于plugin的配置文件均已写好，直接编译源码就能完成plugin的注册及插入，非常方便。
3.3.ros-navigation使用
ros-navigation功能包集是一个很强大的导航框架，支持几乎所有的移动机器人。我们只需要按照适当的配置就可以将ros-navigation应用到我们自己的机器人。为了更好的管理配置文件和启动节点，我们需要先为miiboo机器人建立一个功能包miiboo_nav，用于专门存放我们机器人的导航配置与启动文件。miiboo_nav功能包文件结构，如图36。不难发现，miiboo_nav是一个不包含任何可执行源码的功能包，里面只包含config和launch。config中存放导航中各算法模块的参数配置，launch中存放启动导航所需的各种节点的启动文件。下面依次介绍这些文件的作用。
（图36）miiboo_nav功能包文件结构
机器人全局定位AMCL算法配置amcl.launch.xml：
导航中用到的全局定位AMCL算法功能包包含很多可以配置的参数，这里建立配置文件amcl.launch.xml对这些参数进行配置，内容如图37。配置参数分为3类：粒子滤波参数、雷达模型参数、里程计模型参数。由于参数比较多，关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：http://wiki.ros.org/amcl。
（图37）全局定位AMCL算法配置amcl.launch.xml
代价地图公有参数配置costmap_common_params.yaml：
全局代价地图和局部代价地图公有的参数被放置在costmap_common_params.yaml这里面，这样只需要载入这个配置文件就能完成共有参数的配置，避免了在全局代价地图和局部代价地图配置文件中重复配置一些相同参数的冗余，内容如图38。配置参数分为2类：机器人形状和代价地图各Layer图层。机器人形状可以用多边形或圆形描述，我们的miiboo机器人是矩形的，所以选多边形描述。代价地图各Layer图层包括：静态层static_layer（由SLAM建立得到的地图提供数据）、障碍层obstacle_layer（由激光雷达等障碍扫描传感器提供实时数据）、全局膨胀层global_inflation_layer（为全局代价地图提供膨胀效果）、局部膨胀层global_inflation_layer（为局部代价地图提供膨胀效果）。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：http://wiki.ros.org/costmap_2d。
（图38）代价地图共有参数配置costmap_common_params.yaml
全局代价地图参数配置global_costmap_params.yaml：
全局代价地图是以插件的形式组建的，在costmap_common_params.yaml中定义的各Layer图层都可以通过插件的形式放入全局代价地图中，各Layer图层的组合可以根据需求自由选择，如图39。我们的miiboo机器人的全局代价地图只用了static_layer和global_inflation_layer两个图层。
（图39）全局代价地图参数配置global_costmap_params.yaml
局部代价地图参数配置global_costmap_params.yaml：
局部代价地图和全局代价地图类似，就不展开讲解了，如图40。我们的miiboo机器人的局部代价地图只用了obstacle_layer和local_inflation_layer两个图层。
（图40）局部代价地图参数配置local_costmap_params.yaml
全局路径规划器参数配置global_planner_params.yaml：
全局路径规划器也是以插件的形式被使用，这里主要对全局路径规划器GlobalPlanner插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图41。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：
http://wiki.ros.org/global_planner。
（图41）全局路径规划器参数配置global_planner_params.yaml
局部路径规划器参数配置teb_local_planner_params.yaml：
局部路径规划器也是以插件的形式被使用，这里主要对局部路径规划器TebLocalPlannerROS插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图42。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：
http://wiki.ros.org/teb_local_planner。
（图42）局部路径规划器参数配置teb_local_planner_params.yaml
导航核心控制参数配置move_base_params.yaml：
导航核心控制由move_base节点最终实现，move_base在整个导航框架中处于核心地位，全局代价地图、局部代价地图、全局路径规划、局部路径规划、恢复策略等等都将在这里得到具体实现的调用。配置内容如图43。不难发现，最开始两行便对调用的全局路径规划器和局部路径规划器进行了申明，然后就是一些运动控制方面的参数。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：http://wiki.ros.org/move_base。
（图43）导航核心控制参数配置move_base_params.yaml
启动导航所需的各种节点的启动文件miiboo_nav.launch：
在config目录中将各个导航模块的参数配置好后，就可以在一个启动文件中将导航所需的各种节点进行启动，并载入相关配置。启动文件miiboo_nav.launch就是来完成这个任务的，如图44。不难发现，第一个启动项是map_server，将SLAM建图中得到的地图进行载入后发布到ROS中供导航中需要的模块使用；第二个启动项是amcl，amcl开始运行后将为导航中的模块提供机器人在地图中的全局位置；第三个启动项是move_base，在这里将载入代价地图公有参数、全局代价地图参数、局部代价地图参数、move_base参数、全局路径规划参数、局部路径规划参数。
（图44）启动导航所需的各种节点的启动文件miiboo_nav.launch
到这里，我们已经准备好了导航的各个配置文件和启动文件，现在就正式来进行导航。这里需要特别提醒，我们假设已经在SLAM建图过程得到了一张比较好的环境地图，并保存到了正确的路径。启动导航分为3步：启动机器人上所有传感器、启动导航所需各个节点、发送导航目标点。
首先，启动机器人上所有传感器，打开终端，通过下面的命令直接启动就行了。
source ~/catkin_ws/devel/setup.bash
roslaunch miiboo_bringup miiboo_all_sensor.launch
然后，启动导航所需各个节点，打开终端，通过下面的命令直接启动就行了。
source ~/catkin_ws_nav/devel/setup.bash
roslaunch miiboo_nav miiboo_nav.launch
最后，发送导航目标点。这里有两种方式来下发导航目标点，方法一是通过rviz交互界面，方法二是通过手机APP交互界面。下面对两种方法分别介绍。
方法一：
在PC端打开rviz，打开一个新终端，运行rviz启动命令。
rosrun rviz rviz
订阅/map、/scan、/tf等信息，并观察机器人的初始位置是否正确，如果机器人的初始位置不正确，需要用[2D Pose Estimate]按钮手动给定一个正确的初始位置，如图45。操作方法很简单，先点击[2D Pose Estimate]按钮，然后将鼠标放置到机器人在地图中实际应该的位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。
（图45）在rviz中手动给定机器人初始位置
初始位置设置正确后，就可以用[2D Nav Goal]按钮手动指定导航目标点了，如图46。操作方法很简单，先点击[2D Nav Goal]按钮，然后将鼠标放置到地图中任意的想让机器人到达的空白位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。这样机器人就会开始规划路径并自动导航到该指定目标点。
（图46）在rviz中手动指定导航目标点
方法二：
直接打开手机上的miiboo机器人APP，就可以看到地图和机器人在地图中的位置了。和方法一中的一样，需要观察机器人的初始位置是否正确，如果机器人的初始位置不正确，用[Set Pose]按钮手动给定一个正确的初始位置，如图47。操作过程和方法一类似，就不展开了。
（图47）在miiboo机器人APP中手动给定机器人初始位置
初始位置设置正确后，就可以用[Set Goal]按钮手动指定导航目标点了，如图48。操作过程和方法一类似，就不展开了。
（图48）在miiboo机器人APP中手动指定导航目标点
后记
------SLAM+语音机器人DIY系列【目录】快速导览------
第1章：Linux基础
第2章：ROS入门
第3章：感知与大脑
第4章：差分底盘设计
第5章：树莓派3开发环境搭建
第6章：SLAM建图与自主避障导航
2.google-cartographer机器人SLAM建图
第7章：语音交互与自然语言处理
第8章：高阶拓展
2.centos7下部署Django(nginx+uwsgi+django+python3)
参考文献
[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.
