移动机器人多模态自主导航详解
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者丨ROSrookie
来源丨古月居
点击进入—>3D视觉工坊学习交流群
想象我们要去某个地方游玩，你是不是会先在脑海中勾勒出一条路线，然后出发前往这个地方？
人类的导航如此，那对于机器人来讲，该如何实现导航功能呢？
1.机器人自主导航
我们先来理清自主导航的框架，其关键是自主定位和路径规划。针对这两个核心功能，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
2.机器人定位
导航功能的顺利进行，离不开机器人的精准定位。自主定位即机器人在任意状态下都可以推算出自己在地图中所处的位置。ROS为开发者提供了一种自适蒙特卡罗定位方法（Adaptive Monte Carlo Localization，amcl），这是一种概率统计方法，针对已有地图使用粒子滤波器跟踪一个机器人的姿态。
给定初始位姿后，AMCL会在机器人周围随机撒一些粒子，随着机器人的运动，每个粒子也会实时跟随机器人的速度更新位姿，当粒子周边的环境状态与机器人差距较大时，就会被逐渐淘汰，反之，则会在机器人周边产生更多粒子。以此类推，粒子都集中在机器人所在位置可能性高的地方，也就是定位的结果。
为了调教以上粒子滤波算法，AMCL功能包中可配置的参数很多，一般初次上手不建议，只需要注意订阅和发布的话题名匹配即可，感兴趣的小伙伴可以在官网（http://wiki.ros.org/amcl）查看各个参数的详细介绍，相关的理论算法请参考《概率机器人》一书。
3.机器人路径规划
机器人知道自己的位置后，如何像人一样根据自己的经验规划出一条路径来呢？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）查看。
4.代价地图的配置
人在导航去某个地方的时候可以通过眼睛和经验判断哪里有墙、哪里有水坑、哪里能走和哪里不能走。机器人如何判断呢？
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算法 》
5. 差速移动机器人自主导航
打开机器人的启动文件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如图所示，可以通过途中黄色箭头所指的工具设定机器人的初始位姿，通过红色箭头所指示的工具设定机器人的目标位姿。机器人实现效果如下：
6. 全向移动机器人自主导航
更改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用上文提到的方法设定移动机器人的初始位姿和目标位姿。机器人导航的效果如下：
7. 阿克曼移动机器人自主导航
更改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依然使用上文提到的方法设定移动机器人的初始位姿和目标位姿。机器人导航的效果如下：
8. 总结
以上只演示了各种运动模态下使用某一种本地实时规划算法的导航效果，每种本地实时规划算法都有其优缺点，各个算法介绍如下图，可以根据需求选择相应的算法。
本文仅做学术分享，如有侵权，请联系删文。
点击进入—>3D视觉工坊学习交流群
干货下载与学习
后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件
后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf
后台回复：3D视觉课程，即可学习3D视觉领域精品课程
3D视觉工坊精品课程官网：3dcver.com
1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进
重磅！粉丝学习交流群已成立
交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。
扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿，微信号：dddvisiona
3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看，3天内无条件退款
高质量教程资料、答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~