目录
一、机器人导航实现
1.1 本章简介
1.1.1 本章主要内容介绍
导航是机器人系统中最重要的模块之一，比如现在较为流行的服务型室内机器人，就是依赖于机器人导航来实现室内自主移动的，本章主要就是介绍仿真环境下的导航实现，主要内容有:
- 导航相关概念
- 导航实现:机器人建图(SLAM)、地图服务、定位、路径规划....以可视化操作为主。
- 导航消息:了解地图、里程计、雷达、摄像头等相关消息格式。
1.1.2 预期达成的学习目标
- 了解导航模块中的组成部分以及相关概念
- 能够在仿真环境下独立完成机器人导航
1.1.3 导航模块简介
1.概念
在ROS中机器人导航(Navigation)由多个功能包组合实现，ROS 中又称之为导航功能包集，关于导航模块，官方介绍如下：
一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令。
更通俗的讲: 导航其实就是机器人自主的从 A 点移动到 B 点的过程。
2.作用
秉着"不重复发明轮子"的原则，ROS 中导航相关的功能包集为机器人导航提供了一套通用的实现，开发者不再需要关注于导航算法、硬件交互... 等偏复杂、偏底层的实现，这些实现都由更专业的研发人员管理、迭代和维护，开发者可以更专注于上层功能，而对于导航功能的调用，只需要根据自身机器人相关参数合理设置各模块的配置文件即可，当然，如果有必要，也可以基于现有的功能包二次开发实现一些定制化需求，这样可以大大提高研发效率，缩短产品落地时间。总而言之，对于一般开发者而言，ROS 的导航功能包集优势如下:
安全: 由专业团队开发和维护
功能: 功能更稳定且全面
高效: 解放开发者，让开发者更专注于上层功能实现
3.导航涉及的五点关键技术
全局地图
自身定位
路径规划
运动控制
环境感知
机器人导航实现与无人驾驶类似，关键技术也是由上述五点组成，只是无人驾驶是基于室外的，而我们当前介绍的机器人导航更多是基于室内的。
1.全局地图
在现实生活中，当我们需要实现导航时，可能会首先参考一张全局性质的地图，然后根据地图来确定自身的位置、目的地位置，并且也会根据地图显示来规划一条大致的路线.... 对于机器人导航而言，也是如此，在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:
SLAM(simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。
在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM ....
当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头...
SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server
另外注意: SLAM 虽然是机器人导航的重要技术之一，但是 二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。
2.自身定位
导航伊始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，那么 GPS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽 GPS 信号的区域，由于 GPS 信号弱化甚至完全不可用，那么就必须另辟蹊径了，比如前面的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl
amcl(adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。
3.路径规划
导航就是机器人从A点运动至B点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要时时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。在 ROS 中提供了 move_base 包来实现路径规则,该功能包主要由两大规划器组成:
全局路径规划(gloable_planner)
根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线
本地时时规划(local_planner)
在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物... 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径
全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。
4.运动控制
导航功能包集假定它可以通过话题"cmd_vel"发布
geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令转换为电机命令并发送。5.环境感知
感知周围环境信息，比如: 摄像头、激光雷达、编码器...，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。
在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。
现实案例：
我进入学校后，怎么找到主楼？
Ⅰ.需要一张地貌图，比对周围环境看看有什么样的楼、有什么样的湖，然后拿标志物跟地图比对定位自己和最终结果。
Ⅱ.根据地图进行最短的全局路径的路径规划
Ⅲ.走过去？跑过去？（运动控制）
Ⅳ.找到路口？看看往哪里走？感知环境变化？ （环境感知）
1.1.4 导航之坐标系
1.简介
定位是导航中的重要实现之一，所谓定位，就是参考某个坐标系(比如:以机器人的出发点为原点创建坐标系)在该坐标系中标注机器人。定位原理看似简单，但是这个这个坐标系不是客观存在的，我们也无法以上帝视角确定机器人的位姿，定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种:
- 通过里程计定位:时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系。
- 通过传感器定位:通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。
两种方式在导航中都会经常使用。
2.两种定位方式的优缺点
两种定位方式都有各自的优缺点。
里程计定位:
- 优点:里程计定位信息是连续的，没有离散的跳跃。
- 缺点:里程计存在累计误差，不利于长距离或长期定位。
传感器定位:
- 优点:比里程计定位更精准；
- 缺点:传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣。
两种定位方式优缺点互补，应用时一般二者结合使用。
3.坐标系变换
上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)，里程计定位时，父级坐标系一般称之为 odom，如果通过传感器定位，父级参考系一般称之为 map。当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> doom -> base_link 或 base_footprint。
1.1.5 导航条件要求
1.硬件
虽然导航功能包集被设计成尽可能的通用，在使用时仍然有三个主要的硬件限制：
它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x速度分量，y速度分量，角速度(theta)分量。
它需要在底盘上安装一个单线激光雷达。这个激光雷达用于构建地图和定位。
导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。 它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间。
2.软件
导航功能实现之前，需要搭建一些软件环境:
毋庸置疑的，必须先要安装 ROS
当前导航基于仿真环境，先保证上一章的机器人系统仿真可以正常执行
在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕
后续导航实现中，我们主要关注于: 使用 SLAM 绘制地图、地图服务、自身定位与路径规划。
1.2 导航实现
1.2.1 引言
1.本节主要实现
本节内容主要介绍导航的完整性实现，旨在掌握机器人导航的基本流程，该章涉及的主要内容如下:
SLAM建图(选用较为常见的gmapping)
地图服务(可以保存和重现地图)
机器人定位
路径规划
上述流程介绍完毕，还会对功能进一步集成实现探索式的SLAM建图。
2.准备工作
请先安装相关的ROS功能包:
安装 gmapping 包(用于构建地图):
sudo apt install ros-melodic-gmapping安装地图服务包(用于保存与读取地图):
sudo apt install ros-melodic-map-server安装 navigation 包(用于定位以及路径规划):
sudo apt install ros-melodic-navigation新建功能包，并导入依赖: gmapping map_server amcl move_base
3.安装教程：
基本安装教程
https://blog.csdn.net/xtdx_xty/article/details/110195907
报错unuseablehttps://blog.csdn.net/qq_33406883/article/details/100971183安装到80%报错https://blog.csdn.net/zacharyzqc/article/details/967261314.新建功能包，包含依赖 gmapping map_server amcl move_base
1.2.2 导航实现01_SLAM建图
1.gmapping简介
gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:
- 该移动机器人可以发布里程计消息
- 机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)
关于里程计与雷达数据，仿真环境中可以正常获取的。
2.gamapping节点说明
gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。
Ⅰ.订阅的Topic
tf (tf/tfMessage)
- 用于雷达、底盘与里程计之间的坐标变换消息。
scan(sensor_msgs/LaserScan)
- SLAM所需的雷达信息。
Ⅱ.发布的Topic
map_metadata(nav_msgs/MapMetaData)
- 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。
map(nav_msgs/OccupancyGrid)
- 地图栅格数据，一般会在rviz中以图形化的方式显示。
~entropy(std_msgs/Float64)
- 机器人姿态分布熵估计(值越大，不确定性越大)。
Ⅲ.服务
dynamic_map(nav_msgs/GetMap)
- 用于获取地图数据。
Ⅳ.参数
~base_frame(string, default:"base_link")
- 机器人基坐标系。
~map_frame(string, default:"map")
- 地图坐标系。
~odom_frame(string, default:"odom")
- 里程计坐标系。
~map_update_interval(float, default: 5.0)
- 地图更新频率，根据指定的值设计更新间隔。
~maxUrange(float, default: 80.0)
- 激光探测的最大可用范围(超出此阈值，被截断)。
~maxRange(float)
- 激光探测的最大范围。
.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。
Ⅴ.所需的坐标变换
雷达坐标系→基坐标系
- 一般由 robot_state_publisher 或 static_transform_publisher 发布。
基坐标系→里程计坐标系
- 一般由里程计节点发布。
Ⅵ.发布的坐标变换
地图坐标系→里程计坐标系
- 地图到里程计坐标系之间的变换。
3.gmapping的使用
Ⅰ.编写gmapping节点相关launch文件
建立功能包 nav01_demo 建立 nav01_slam.launch文件
<launch> <!-- 仿真环境下 ,将该参数设置为true --> <param name="use_sim_time" value="true"/> <!-- gmapping节点 --> <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> <!-- 设置雷达话题 --> <remap from="scan" to="scan"/> <!-- 设置基坐标系 --> <param name = "base_frame" value = "base_footprint" /> <!-- 设置地图坐标系 --> <param name = "map_frame" value = "map" /> <!-- 设置里程计坐标系 --> <param name = "odom_frame" value = "odom" /> <param name="map_update_interval" value="5.0"/> <param name="maxUrange" value="16.0"/> <param name="sigma" value="0.05"/> <param name="kernelSize" value="1"/> <param name="lstep" value="0.05"/> <param name="astep" value="0.05"/> <param name="iterations" value="5"/> <param name="lsigma" value="0.075"/> <param name="ogain" value="3.0"/> <param name="lskip" value="0"/> <param name="srr" value="0.1"/> <param name="srt" value="0.2"/> <param name="str" value="0.1"/> <param name="stt" value="0.2"/> <param name="linearUpdate" value="1.0"/> <param name="angularUpdate" value="0.5"/> <param name="temporalUpdate" value="3.0"/> <param name="resampleThreshold" value="0.5"/> <param name="particles" value="30"/> <param name="xmin" value="-50.0"/> <param name="ymin" value="-50.0"/> <param name="xmax" value="50.0"/> <param name="ymax" value="50.0"/> <param name="delta" value="0.05"/> <param name="llsamplerange" value="0.01"/> <param name="llsamplestep" value="0.01"/> <param name="lasamplerange" value="0.005"/> <param name="lasamplestep" value="0.005"/> </node>ame="maxUrange" value="16.0"/> <param name="sigma" value="0.05"/> <param name="kernelSize" value="1"/> <param name="lstep" value="0.05"/> <param name="astep" value="0.05"/> <param name="iterations" value="5"/> <param name="lsigma" value="0.075"/> <param name="ogain" value="3.0"/> <param name="lskip" value="0"/> <param name="srr" value="0.1"/> <param name="srt" value="0.2"/> <param name="str" value="0.1"/> <param name="stt" value="0.2"/> <param name="linearUpdate" value="1.0"/> <param name="angularUpdate" value="0.5"/> <param name="temporalUpdate" value="3.0"/> <param name="resampleThreshold" value="0.5"/> <param name="particles" value="30"/> <param name="xmin" value="-50.0"/> <param name="ymin" value="-50.0"/> <param name="xmax" value="50.0"/> <param name="ymax" value="50.0"/> <param name="delta" value="0.05"/> <param name="llsamplerange" value="0.01"/> <param name="llsamplestep" value="0.01"/> <param name="lasamplerange" value="0.005"/> <param name="lasamplestep" value="0.005"/> </node> <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" /> <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" /> <node pkg="rviz" type="rviz" name="rviz" /> </launch>Ⅱ.执行
①先启动gazebo仿真环境
roslaunch nav_demo demo03_env.launch②启动地图绘制的launch文件
roslaunch nav_demo nav01_slam.launch③启动键盘控制节点，用于控制机器人运动建图
rosrun teleop_twist_keyboard teleop_twist_keyboard.py④在rviz中添加组件，显示栅格地图
添加如下组件，更改话题并观察
观察到，这三个坐标系重合
创建地图，添加map功能栏
控制运动，绘制地图
1.2.3 导航实现02_地图服务
1.上节回顾
上一节我们已经实现通过gmapping的构建地图并在rviz中显示了地图，不过，上一节中地图数据是保存在内存中的，当节点关闭时，数据也会被一并释放，我们需要将栅格地图序列化到的磁盘以持久化存储，后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在ROS中，地图数据的序列化与反序列化可以通过 map_server 功能包实现。
2.map_server简介
map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。
map_server安装前面也有介绍，命令如下:
sudo apt install ros-<ROS版本>-map-server
3.map_server使用之地图保存节点(map_saver)
Ⅰ.map_saver节点说明
订阅的topic:
map(nav_msgs/OccupancyGrid)
- 订阅此话题用于生成地图文件。
Ⅱ.地图保存launch文件
地图保存的语法比较简单，编写一个launch文件，内容如下:
<launch> <arg name="filename" value="$(find mycar_nav)/map/nav" /> <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" /> </launch>其中 mymap 是指地图的保存路径以及保存的文件名称。
SLAM建图完毕后，执行该launch文件即可。
Ⅲ.测试
首先，参考上一节，依次启动仿真环境，键盘控制节点与SLAM节点；
然后，通过键盘控制机器人运动并绘图；
最后，通过上述地图保存方式保存地图。
结果：在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml
①新建launch文件nav02_map_sava.launch
<launch> <!--nav是被保存文件的名称 自动生存--> <arg name="filename" value="$(find nav_demo)/map/nav" /> <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" /> </launch>②绘图，然后执行nav02_map_sava.launch
发现在map目录下有两个文件
第一个文件是一个图像文件，打开即可看见地图
4.map_server使用之地图读取(map_server)
Ⅰ. map_server节点说明发布的话题
map_metadata（nav_msgs / MapMetaData）
- 发布地图元数据。
map（nav_msgs / OccupancyGrid）
- 地图数据。
服务
static_map（nav_msgs / GetMap）
- 通过此服务获取地图。
参数
〜frame_id（字符串，默认值：“map”）
- 地图坐标系。
Ⅱ.地图读取
通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:
<launch> <!-- 设置地图的配置文件 --> <arg name="map" default="nav.yaml" /> <!-- 运行地图服务器，并且加载设置的地图--> <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/> </launch>args是被解析的地图的配置文件yaml。
Ⅲ.地图显示
在 rviz 中使用 map 组件可以显示栅格地图：
Ⅳ.效果
编写launch文件
<launch> <node pkg ="map_server" type = "map_server" node = "" name = "map_server" args = "$(find nav_demo)/map/nav.yaml"/> </launch>执行launch文件
打开rviz，添加map节点，更改话题
5.关于保存map生成的两个文件
Ⅰ.一个是pgm文件，一个是yaml文件；pgm文件可以直接用图片查看器打开。本节主要介绍yaml文件的相关知识
Ⅱ.nav.yaml文件
image ：声明地图图片资源的路径
resolution：刻度尺（米/每像素）
origin ：地图的位姿信息（相对于rviz原点的距离）
occupied_thresh：占用阈值
free_thresh：空闲阈值
negate：取反
Ⅲ.演示参数
①resolution 改成0.1
表示以前这张图片里面一个像素对应现实生活中的5cm，现在变成10cm
发现地图变大了，边长扩大了一倍了
②origin 右下角的点相对于原点的偏移量（右 -50 下 -50）
若修正为 0 0
第三个是偏航角度 修改为0.3 偏航0.3个弧度
③occupied_thresh && free_thresh
与地图中的障碍物判断有关：
规则：白色可通行区，黑色是障碍物，蓝灰是未知区域
判断规则：
地图中的每个像素都有取值[0-255],白色 255 黑色 0，像素值设为x
根据像素值计算一个比例 p = (255-x)/255 白色是0 黑色是1
判断是否是障碍物 p>occupied_thresh 是障碍物 p<free_thresh是可以自由通行的
④取反
白的变黑的、黑的变白的
1.2.4 导航实现03_定位
所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。
1.amcl简介
AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。
2.amcl节点说明
amcl 功能包中的核心节点是:amcl。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。
订阅的Topic
scan(sensor_msgs/LaserScan)
- 激光雷达数据。
tf(tf/tfMessage)
- 坐标变换消息。
initialpose(geometry_msgs/PoseWithCovarianceStamped)
- 用来初始化粒子滤波器的均值和协方差。
map(nav_msgs/OccupancyGrid)
- 获取地图数据。
发布的Topic
amcl_pose(geometry_msgs/PoseWithCovarianceStamped)
- 机器人在地图中的位姿估计。
particlecloud(geometry_msgs/PoseArray)
- 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。
tf(tf/tfMessage)
- 发布从 odom 到 map 的转换。
服务
global_localization(std_srvs/Empty)
- 初始化全局定位的服务。
request_nomotion_update(std_srvs/Empty)
- 手动执行更新和发布更新的粒子的服务。
set_map(nav_msgs/SetMap)
- 手动设置新地图和姿态的服务。
调用的服务
static_map(nav_msgs/GetMap)
- 调用此服务获取地图数据。
参数
~odom_model_type(string, default:"diff")
- 里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)
~odom_frame_id(string, default:"odom")
- 里程计坐标系。
~base_frame_id(string, default:"base_link")
- 机器人极坐标系。
~global_frame_id(string, default:"map")
- 地图坐标系。
坐标变换
里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。
- 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
- amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。
3.amcl的使用
Ⅰ.编写amcl节点相关的launch文件
获取现成的差速机器人的amcl文件
<launch> <node pkg="amcl" type="amcl" name="amcl" output="screen"> <!-- Publish scans from best pose at a max of 10 Hz --> <param name="odom_model_type" value="diff"/> <param name="odom_alpha5" value="0.1"/> <param name="transform_tolerance" value="0.2" /> <param name="gui_publish_rate" value="10.0"/> <param name="laser_max_beams" value="30"/> <param name="min_particles" value="500"/> <param name="max_particles" value="5000"/> <param name="kld_err" value="0.05"/> <param name="kld_z" value="0.99"/> <param name="odom_alpha1" value="0.2"/> <param name="odom_alpha2" value="0.2"/> <!-- translation std dev, m --> <param name="odom_alpha3" value="0.8"/> <param name="odom_alpha4" value="0.2"/> <param name="laser_z_hit" value="0.5"/> <param name="laser_z_short" value="0.05"/> <param name="laser_z_max" value="0.05"/> <param name="laser_z_rand" value="0.5"/> <param name="laser_sigma_hit" value="0.2"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_model_type" value="likelihood_field"/> <!-- <param name="laser_model_type" value="beam"/> --> <param name="laser_likelihood_max_dist" value="2.0"/> <param name="update_min_d" value="0.2"/> <param name="update_min_a" value="0.5"/> <param name="odom_frame_id" value="odom"/> <!--里程计坐标系--> <param name="base_frame_id" value="base_footprint"/> <!--机器人基坐标系--> <param name="resample_interval" value="1"/> <param name="transform_tolerance" value="0.1"/> <param name="recovery_alpha_slow" value="0.0"/> <param name="recovery_alpha_fast" value="0.0"/> </node> </launch>Ⅱ.编写测试launch文件
①amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，内容示例如下:
<launch> <!-- 运行rviz --> <node pkg="rviz" type="rviz" name="rviz"/> <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" /> <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" /> <!-- 加载地图服务 --> <include file="$(find nav_demo)/launch/nav03_map_server.launch" /> <!-- 启动AMCL节点 --> <include file="$(find nav_demo)/launch/nav04_amcl.launch" /> </launch>②执行
先启动 Gazebo 仿真环境(此过程略)；
roslaunch nav_demo demo03_env.launch
启动键盘控制节点：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；
roslaunch nav_demo test_amcl.launch在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；
通过键盘控制机器人运动，会发现 posearray 也随之而改变。观察结果
1.2.5 导航实现04_路径规划
毋庸置疑的，路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。
1.move_base简介
move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。move_base主要由全局路径规划与本地路径规划组成。
2.move_base节点说明
move_base功能包中的核心节点是:move_base。为了方便调用，需要先了解该节点action、订阅的话题、发布的话题、服务以及相关参数。
动作订阅
move_base/goal(move_base_msgs/MoveBaseActionGoal)
- move_base 的运动规划目标。
move_base/cancel(actionlib_msgs/GoalID)
- 取消目标。
动作发布
move_base/feedback(move_base_msgs/MoveBaseActionFeedback)
- 连续反馈的信息，包含机器人底盘坐标。
move_base/status(actionlib_msgs/GoalStatusArray)
- 发送到move_base的目标状态信息。
move_base/result(move_base_msgs/MoveBaseActionResult)
- 操作结果(此处为空)。
订阅的Topic
move_base_simple/goal(geometry_msgs/PoseStamped)
- 运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。
发布的Topic
cmd_vel(geometry_msgs/Twist)
- 输出到机器人底盘的运动控制消息。
服务
~make_plan(nav_msgs/GetPlan)
- 请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。
~clear_unknown_space(std_srvs/Empty)
- 允许用户直接清除机器人周围的未知空间。
~clear_costmaps(std_srvs/Empty)
- 允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。
3.move_base与代价地图
Ⅰ.概念：
机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：
- SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
- 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入...
所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。加上这些的地图称为代价地图。
Ⅱ.代价地图的组成
代价地图有两张:global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划（起点到终点），后者用于本地路径规划（到其中一个小地方）。
两张代价地图都可以多层叠加,一般有以下层级:
Static Map Layer：静态地图层，SLAM构建的静态地图。
Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
Other Layers：自定义costmap。
Ⅲ.碰撞算法
在ROS中，如何计算代价值呢？请看下图:
上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。
- 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
- 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
- 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
- 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
- 自由区域:栅格值为0，此处机器人可以自由通过；
- 未知区域:栅格值为255，还没探明是否有障碍物。
膨胀空间的设置可以参考非自由空间。
Ⅳ.movebase使用
路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同....最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。具体实现如下:
- 先编写launch文件模板
- 编写配置文件
- 集成导航相关的launch文件
- 测试
编写launch文件模板
<launch> <!-- clear_params 将之前所有参数删除，初始化--> <!-- respawn 将节点删除后就不会再次执行了 --> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"> <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" /> <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" /> <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" /> </node> </launch>编写配置文件
costmap_common_params.yaml ：该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:
#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint robot_radius: 0.12 #圆形 # footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状 obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图 raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物 #膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物 inflation_radius: 0.2 #代价比例系数，越大则代价值越小 cost_scaling_factor: 3.0 #地图类型 map_type: costmap #导航包所需要的传感器 observation_sources: scan #对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。 scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}global_costmap_params.yaml ：该文件用于全局代价地图参数设置
global_costmap: global_frame: map #地图坐标系 robot_base_frame: base_footprint #机器人坐标系 # 以此实现坐标变换 update_frequency: 1.0 #代价地图更新频率 publish_frequency: 1.0 #代价地图的发布频率 transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间 static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.local_costmap_params.yaml 该文件用于局部代价地图参数设置:
local_costmap: global_frame: odom #里程计坐标系 robot_base_frame: base_footprint #机器人坐标系 update_frequency: 10.0 #代价地图更新频率 publish_frequency: 10.0 #代价地图的发布频率 transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间 static_map: false #不需要静态地图，可以提升导航效果 rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化 width: 3 # 局部地图宽度 单位是 m height: 3 # 局部地图高度 单位是 m resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致base_local_planner_params：基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。
TrajectoryPlannerROS: # Robot Configuration Parameters max_vel_x: 0.5 # X 方向最大速度 min_vel_x: 0.1 # X 方向最小速速 max_vel_theta: 1.0 # min_vel_theta: -1.0 min_in_place_vel_theta: 1.0 acc_lim_x: 1.0 # X 加速限制 acc_lim_y: 0.0 # Y 加速限制 acc_lim_theta: 0.6 # 角速度加速限制 # Goal Tolerance Parameters，目标公差 xy_goal_tolerance: 0.10 yaw_goal_tolerance: 0.05 # Differential-drive robot configuration # 是否是全向移动机器人 holonomic_robot: false # Forward Simulation Parameters，前进模拟参数 sim_time: 0.8 vx_samples: 18 vtheta_samples: 20 sim_granularity: 0.05集成导航相关的launch文件
<!-- 集成导航相关的launch文件 --> <launch> <!-- 地图服务 --> <include file = "$(find nav_demo)/launch/nav03_map_server.launch"/> <!-- amcl --> <include file = "$(find nav_demo)/launch/nav04_amcl.launch"/> <!-- move_base --> <include file = "$(find nav_demo)/launch/nav05_path.launch"/> <!-- rviz --> <node pkg="rviz" type="rviz" name="rviz" /> <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" /> <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" /> </launch>测试
1.先启动 Gazebo 仿真环境(此过程略)；
roslaunch nav_demo demo03_env.launch2.启动导航相关的 launch 文件；
roslaunch nav_demo nav06_test.launch3.添加Rviz组件(参考演示结果),可以将配置数据保存，后期直接调用；
添加这些组件后是这样的
点击这个
机器人就去了那个地方，成功！
Ⅴ.其他琐碎小知识
全局代价地图与本地代价地图组件配置如下:
全局路径规划与本地路径规划组件配置如下:Path
Ⅵ.配置文件具体实现
在代码中已有标注
Ⅶ.参数配置小技巧
以上配置在实操中，可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？
全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:
- 全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；
- 本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。
这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。
1.2.6 导航与slam建图
1.背景
场景: 在导航实现01_SLAM建图中，我们是通过键盘控制机器人移动实现建图的，而后续又介绍了机器人的自主移动实现，那么可不可以将二者结合，实现机器人自主移动的SLAM建图呢？
上述需求是可行的。虽然可能会有疑问，导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过SLAM建图，那么如何发布地图信息呢？SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。
该过程实现比较简单，步骤如下:
- 编写launch文件，集成SLAM与move_base相关节点；
- 执行launch文件并测试。
2.编写launch文件
当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点，示例内容如下:
<!-- 集成slam与导航，实现机器人自主移动的地图构建 --> <launch> <!-- slam实现 --> <include file = "$(find nav_demo)/launch/nav01_slam.launch" /> <!-- 导航中的move_base实现 --> <include file = "$(find nav_demo)/launch/nav05_path.launch" /> </launch>
3.测试
1.首先运行gazebo仿真环境；
roslaunch nav_demo demo03_env.launch2.然后执行launch文件；
roslaunch nav_demo nav07_slam_auto.launch3.在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；
4.最后可以使用 map_server 保存地图。
<launch> <!--nav是被保存文件的名称 自动生存--> <arg name="filename" value="$(find nav_demo)/map/nav" /> <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" /> </launch>
552
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
