移动机器人多模态自主导航详解

人类的导航如此，那对于机器人来讲，该如何实现导航功能呢？

我们先来理清自主导航的框架，其关键是自主定位和路径规划。针对这两个核心功能，ROS提供了一套完整的框架支持，收到导航目标位置后，机器人只需要发布必要的传感器信息，框架中的功能包即可帮助机器人完成导航。

其中，move_base功能包实现机器人导航中的最优路径规划，amcl实现二维地图中的机器人定位。

为了实现机器人全局最优路径规划与实时避障路径规划，move_base需要订阅机器人发布的深度传感器信息（sensor_msgs/LaserScan或 sensor_msgs/PointCloud）和里程计信息（nav_msgs/Odometry），同时完整的TF坐标变换也是实现路径规划的重要基础。

导航框架最终的输出是控制机器人的速度指令（geometry_msgs/Twist），这就要求机器人控制节点具备解析控制指令中线速度、角速度的能力，并且控制机器人完成相应的运动。

注：

导航框架所包含的功能包很多，可以直接使用如下命令安装：

$ sudo apt-get install ros-melodic-navigation

导航功能的顺利进行，离不开机器人的精准定位。自主定位即机器人在任意状态下都可以推算出自己在地图中所处的位置。ROS为开发者提供了一种自适蒙特卡罗定位方法（Adaptive Monte Carlo Localization，amcl），这是一种概率统计方法，针对已有地图使用粒子滤波器跟踪一个机器人的姿态。

给定初始位姿后，AMCL会在机器人周围随机撒一些粒子，随着机器人的运动，每个粒子也会实时跟随机器人的速度更新位姿，当粒子周边的环境状态与机器人差距较大时，就会被逐渐淘汰，反之，则会在机器人周边产生更多粒子。以此类推，粒子都集中在机器人所在位置可能性高的地方，也就是定位的结果。

为了调教以上粒子滤波算法，AMCL功能包中可配置的参数很多，一般初次上手不建议，只需要注意订阅和发布的话题名匹配即可，感兴趣的小伙伴可以在官网（http://wiki.ros.org/amcl）查看各个参数的详细介绍，相关的理论算法请参考《概率机器人》一书。

机器人知道自己的位置后，如何像人一样根据自己的经验规划出一条路径来呢？move_base功能包就是负责这样的功能，主要由全局路径规划器和本地实时规划器（局部路径规划器）组成。

全局规划就好比我们依靠经验（或地图数据）规划点到点最优路径的过程；局部规划可以理解为去往目的地途中，不断调整机器人姿态、躲避障碍物，以贴合最优路径的过程。

全局路径规划常用Dijkstra算法和A*算法。Dijkstra算法深度优先，往往可以找到全局最优路径，不过搜索时间长、消耗资源多，而A*算法加入了启发函数，虽然不一定可以找到全局最优路径，但搜索时间更快，适合大空间范围的规划。移动机器人大部分是在室内有限范围内使用，两者搜索时间和消耗资源的差距并不明显，一般使用Dijkstra算法即可。

本地实时规划常用Dynamic Window Approaches（DWA）和Time Elastic Band（TEB）算法，两种算法的核心思想如下，具体算法实现大家可以在网上搜索相关的论文。

为了完成以上这些规划器算法的配置，move_base功能包的启动也需要配置不少参数，比如下图所示的这些参数文件：

包含通用配置文件（costmap_common）、全局规划配置文件（global_costmap）、本地规划配置文件（local_costmap）、本地规划器（planer）、路径规划算法（全局base_global_planner和局部base_local_planner）和控制频率（controller_frequency）、规划频率（planner_frequency）等一些参数。这些参数的详细介绍可以去官网（http://wiki.ros.org/move_base）查看。

人在导航去某个地方的时候可以通过眼睛和经验判断哪里有墙、哪里有水坑、哪里能走和哪里不能走。机器人如何判断呢？

move_base导航使用两种代价地图存储周围环境中的障碍信息：一种用于全局路径规划（global_costmap），一种用于本地实时路径规划（local_costmap）。两种代价地图需要使用一些共用的或独立的配置文件：通用配置文件（costmap_common_params）、全局规划配置文件（global_costmap_params）和本地规划配置文件（local_costmap_params）。

通用配置文件costmap_common_params.yaml的代码和关键参数的含义如下：

注意话题名不要设置错了，footprint也需要根据实际机器人的大小进行设置。

全局规划配置文件global_costmap_params.yaml的代码和关键参数的含义如下：

由于我们这里是已经有地图了，所以将static_map设置true。若我们想使用机器人探索未知的地方并且进行建图的话，则将static_map设置为false。

本地规划配置文件local_costmap_params.yaml的代码和关键参数的含义如下：

其中width、height、resolution参数用于设置代价地图的长（米）、高（米）和分辨率（米/格）。rolling_window参数用来设置在机器人移动过程中是否需要滚动窗口，以保持机器人处于中心位置。

既然导航的各个问题都已经梳理清楚，那么是否真的可以实现导航呢？以及在不同运动模态下，机器人的导航效果又会怎样？接下来我们就用LIMO机器人试一试。

注：关于该机器人运动模态的配置与SLAM，请参考以下内容：

《 细数移动机器人那些常用的运动模态 》

《 如何在移动机器人中部署Gmapping SLAM算法 》

打开机器人的启动文件limo_start.launch，将use_mcnamu设置为false，如下图所示。

局部规划器选用的算法为TEB，由于相关参数比较多，这里就不一一展示和说明，若想了解每个参数，可以前往官网查看相关说明：http://wiki.ros.org/teb_local_planner

下面是一些参数的配置与介绍：

注意，当运动模式为差速运动时，y方向的速度必须设置为0。为了导航效果更好，这里将最小转弯半径（min_turning_radius）设置为0。

注意min_obstacle_dist不要设置的比costmap_common_params.yaml中的膨胀半径小，不然会报错。

然后运行下面的命令，开启导航功能。

$ roslaunch limo_bringup limo_start.launch$ roslaunch limo_bringup limo_navigation_diff.launch

启动的rviz如图所示，可以通过途中黄色箭头所指的工具设定机器人的初始位姿，通过红色箭头所指示的工具设定机器人的目标位姿。机器人实现效果如下：

更改LIMO机器人的运动模态，打开机器人的启动文件limo_start.launch，将use_mcnamu设置为true，如下图所示：

为了更好地体现出全向运动的效果，这里局部规划器选用的算法为TEB。下面是一些参数的配置与介绍：

与差速模式不同，全向模式可以横向移动，所以y方向的速度不必设置为0，而是可以设置为任意速度。

 weight_kinematics_nh用于调整顺应纵向运动和非顺应横向运动（扫射）之间的权衡。当此参数设置比较大时，将倾向于纵向运动；当此参数设置较小时，将更倾向于与横向运动。weight_optimaltime参数为最优时间权重，如果设置较大时，那么机器人会在直道上快速加速，并且路径规划的也会切内道。

$ roslaunch limo_bringup limo_start.launch$ roslaunch limo_bringup limo_navigation_mcnamu.launch

更改LIMO机器人的运动模态，打开机器人的启动文件limo_start.launch，将use_mcnamu设置为false，如下图所示。

由于阿克曼运动只适用TEB算法，所以我们这里仍然选用TEB算法。下面是一些参数的配置与介绍：

我们这里也将y方向的速度设置为0，但是最小转弯半径此时不能设置为0，而是需要根据机器人的实际最小转弯半径来设置。为了方便测量出机器人的最小转弯半径，我们可以用遥控器让机器人以最大角速度做圆周运动，然后就可以用尺子测量出机器人的最小转弯半径。注意设置的最小转弯半径不能小于最小转弯半径。

$ roslaunch limo_bringup limo_start.launch$ roslaunch limo_bringup limo_navigation_ackerman.launch

以上只演示了各种运动模态下使用某一种本地实时规划算法的导航效果，每种本地实时规划算法都有其优缺点，各个算法介绍如下图，可以根据需求选择相应的算法。