移动机器人的导航
（仅作学习笔记，参考书籍《ROS机器人编程》）
一、导航及其组成要素
导航是指机器人运动到一个指定的目的地，这说起来很容易，但完成它所需的技术一个个都不是容易的任务：要知道机器人本身在哪里，并要有一个给定的周围环境的地图，在各种路径中找出最优路径，在
行驶中避免障碍物（如墙壁、家具、物体）等。
机器人实现自主导航都需要哪些？根据导航算法会有不同，但应该至少需要如下几种：
➊ 地图
➋ 测量或估计机器人的姿态的功能
➌ 识别障碍物，如墙壁和物体的功能
➍ 能够计算出最优路线并行驶的功能
1、地图
第一是地图。导航仪从购买时起就配备有非常准确的地图，并且可以定期下载更新的地图，以便可以根据地图将汽车引导到目的地。但是在使用服务机器人的房间里是否会有地图呢？服务机器人也像导航仪一样，需要一个地图，所以需要人创建一个地图，并把它给到机器人，或者需要机器人自己创建一个地图。
SLAM（Simultaneous localization and mapping）就是为了让机器人自己（或接受人的一些帮助）绘制地图而出现的技术。用中文应该是“同步定位和绘制地图”。这是在机器人移动到未知空间时通过探测周围环境来估计当前位置并同时绘制地图的方法。
2、测量或估计机器人姿态的功能
第二，机器人需要自己能够测量和估计姿态（位置+方向）。汽车会用GPS估计自己的位置，但在室内无法使用GPS。即使说可以在室内使用，误差较大的GPS无法用于测量精细的移动。最近，虽然有DGPS等高精度的定位系统，但在室内还是无法使用的。为了克服这种问题，人们引进了标志识别方式及室内定位系统等技术，但在成本或精确度方面还不足以投入实际应用。当前的室内机器人用的最多的是导航推测（dead reckoning）。它的缺点是只能估算相对位置，但因为仅用廉价的传感器就能实现，且已有较长时间的研究进展，因此可以得到一定水平的位置估计值，因此被广泛使用。导航推测技术用机器人的车轮的旋转量来估计机器人本身的移动量。但车轮的旋转量具有不少的误差。
因此还利用IMU传感器等获取惯性信息来补偿位置和方向值，以此减小误差。
姿态（位置+方向）
在ROS中，机器人的位置（position：x，y，z）和方向（orientation：x，y，z，w）被定义为姿态。
该位置由x、y和z三个向量描述，而方向使用四元数形式的x、y、z和w。有关消息pose的说明，请参阅
以下地址。需要注意，有一些术语隐含地包括方向信息，例如位置估计或特定物体的位置等。
http://docs.ros.org/api/geometry_msgs/html/msg/Pose.html
导航推测所需要的信息（中心位置（x，y），车轮间距离D，车轮半径r）
导航推测（dead reckoning）
下面简要介绍一下导航推测。当有如图移动机器人时，设D是车轮之间的距离，r是车轮的半径。如图所示，当机器人在时间Te内移动很短距离时，利用左右电机旋转量（当前编码器值Elc、Erc和Te之前的编码器值Elp 和Erp）来计算出左右车轮的转速 （vl，vr），如式11-1和11-2所示。
求出左右车轮的移动速度（Vl， Vr）后，并如式11-5和11-6求出机器人的平移速度（linear velocity：vk）和旋转速度（angular velocity：wk）。
最后，通过这些值，利用式11-7至11-10的计算来求得机器人的位置（x( k+1), y( k+1 )）和方向
3、识别障碍物，如墙壁和物体
第三是一种利用传感器检测墙壁和物体等障碍物的方法。此时用到距离传感器、视觉传感器等多种传感器。其中距离传感器有基于雷达的距离传感器（常用的是LDS、LRF和LiDAR）、超声波传感器和红外距离传感器等，而视觉传感器包括立体相机、单镜相机、360度相机，以及经常用作深度摄像头的RealSense、Kinect和Xtion也都用于识别障碍物。
4、计算最优路径和行驶功能
第四是导航（Navigation）功能，这是计算到达目的地的最优路径，并且驱动机器人按照最优路径到达目的地的功能。实现这个功能的算法有很多种：称为路径搜索和规划的A*算法、势场算法、粒子过滤算法和RRT（Rapidly-exploring Random Tree）算法等。
二、SLAM运行
1、对于使用SLAM的机器人的硬件限制
与SLAM相关的常用的功能包有gmapping、cartographer和rtabmap。在这里使用gmapping。使用Gmapping有几种硬件限制。对于常见的移动机器人不成问题。
移动方式
机器人必须能够用X、Y轴平面上的平移速度（linear velocity）和theta旋转速度（angular velocity）指令进行操作。比如有左右两个可以单独驱动的差动驱动式移动机器人（differential drive mobile robot），或者具有三个以上的全向轮的全向移动机器人（omni-wheel robot）。
测位(Odometry)
需要能获得测位信息。换句话说，要可以通过导航推测方法（dead reckoning）来推算机器人移动的距离和旋转量，或者通过使用IMU传感器的惯性信息推定姿态补偿，或用IMU传感器测量平移速度和旋转速度，最终要可以测量及推断机器人本身的位置。
检测用传感器
为了实现SLAM和导航，机器人需要有LDS （Laser Distance Sensor） 、LRF（Laser Range Finder）和LiDAR来测量XY平面上的障碍物。深度相机（如RealSense、Kinect和Xtion）也可以将3D信息转换为XY平面上的信息。换句话说，有必要安装一个能够测量二维平面的传感器。使用超声波传感器、PSD传感器和摄像机的可视SLAM也属于这个概念。
机器人的形态
只考虑正多边形、正方形和圆形机器人。不考虑沿某一方向太长的机器人、无法从房门通过的过大的机器人、双足人形机器人、多关节移动机器人和飞行机器人等。在本篇中，我们将使用官方ROS平台TurtleBot3。图中的TurtleBot3满足上面提到的所有四个SLAM约束条件。
2、SLAM的实验环境
如下几种环境从Gmapping算法的角度来看缺少特征要素，因此这些不适合应用SLAM。➊没有任何障碍物的方形环境。➋由两个长长而平行的墙壁形成的走廊。➌无法反射激光或红外线的玻璃窗。➍散射镜。➎由于传感器的特性，无法获取障碍物信息的环境，如湖泊或海边等。
3、用于SLAM的ROS功能包
本篇中使用的SLAM相关的ROS功能包是turtlebot3元功能包、slam_gmapping元功能包中的gmapping功能包，以及navigation元功能包中的map_server功能包。如果是TurtleBot3开发环境应该都安装过。暂时只描述运行方法。为了便于参考，此处将分开说明[Remote PC]和[TurtleBot]两种环境下运行的命令以避免混淆。
4、运行SLAM
SLAM运行顺序如下。在这个例子中，我们将使用TurtleBot3 Waffle作为参考。如果你的是Burger，则只需改变名字。如果是在使用Burger或Waffle Pi，只需将命令中的‘TURTLEBOT3_MODEL’项目从‘waffle’改为‘burger’或‘waffle_pi’。
roscore
在[Remote PC]中，运行roscore。
roscore
启动机器人
在[TurtleBot]中，运行turtlebot3_robot.launch文件并运行turtlebot3_core和turtlebot3_lds节点。
roslaunch turtlebot3_bringup turtlebot3_robot.launch
运行SLAM功能包
在[Remote PC]中，运行turtlebot3_slam.launch启动文件。turtlebot3_slam功能包只包含一个launch文件。运行后，将运行robot_state_publisher节点和slam_gmapping节点，其中robot_state_publisher节点将两个轮子和每个关节的三维位置和方向信息以TF形式发布，而slam_gmapping节点用于绘制地图。另外，描述包含机器人的外观信息的URDF的robot_model也会被设置。
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_slam turtlebot3_slam.launch
运行RViz
运行RViz可视化工具RViz，以便在SLAM过程中可以直观地确认结果。运行时如果附加如下所示的“-d”选项，则从一开始就会添加有关显示（display）的插件，会比较方便。
export TURTLEBOT3_MODEL=waffle
rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz
保存话题信息
下一步，用户将直接遥控机器人并执行SLAM操作，此时发出的/scan和/tf话题存储在名为scan_data的bag文件中。您可以在后期使用此文件创建地图，也可以重现绘制地图过程中的/scan和/tf话题，而无需重复做实验。可以把它想象成来自实验的话题数据（下一个例子中的/scan和/tf话题）的副本。以下命令的-O选项是指定输出文件名称的选项，这将会把输出内容保存为叫做“scan_data.bag”的bag文件。保存话题消息在SLAM过程中不是必要功能，所以如果不需要保存消息，则可以跳过它。
rosbag record -O scan_data /scan /tf
遥控机器人
以下命令允许用户手动遥控机器人并执行SLAM操作。这里重要的是不要过快改变机器人的速度，也不要以过快的速度前进、后退或旋转。移动机器人时，机器人必须扫描要测量的环境的每个角落。这需要经验和技巧，因此需要在大量的SLAM实验中积累经验。
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
绘制地图
现在已经完成了所有的准备工作，让我们运行map_saver节点来创建一个地图吧。当移动机器人时，机器人会根据测位（odometry）、tf信息和传感器的扫描信息来创建地图。这可以在我们刚刚运行的RViz中看到。创建的地图保存在运行map_saver的目录中。除非指定了文件名，否则保存为实际地图文件map.pgm和包含地图信息的和map.yaml文件。如下命令中的“-f”选项是指定保存地图文件的目录及文件名的选项。例如，如果指定为"~/map",
"~"意味着用户目录，而“map”意味着要保存为map.pgm和map.yaml文件。
rosrun map_server map_saver -f ~/map
可以通过上述过程创建地图。如图所示,绘制地图所需的节点和话题可以通过使用rqt_graph来查看,绘制地图过程，得到最终的地图。我们可以确认，上面提到的实验环境地图已正确绘制。
5、利用预先准备好的bag文件运行的SLAM
为了可以在没有TurtleBot3和LDS传感器的情况下尝试SLAM，我们将利用录制的bag文件。首先，下载本节要用到的文件。
wget https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/master/turtlebot3_slam/bag/
TB3_WAFFLE_SLAM.bag
以下内容类似于上面的SLAM运行方法。唯一的不同点是对rosbag进行回放（play），而不是保存。如此操作就和实际实验相同。
roscore
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_bringup turtlebot3_remote.launch
export TURTLEBOT3_MODEL=waffle
rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz
roscd turtlebot3_slam/bag
rosbag play ./TB3_WAFFLE_SLAM.bag
rosrun map_server map_saver -f ~/map
三、SLAM应用
本节详细剖析SLAM中使用的ROS功能包以及创建和配置它的方法。我们将仔细看看turtlebot3元功能
包、slam_gmapping元功能包中的gmapping功能包和navigation元功能包中的map_server功能包。
1、地图
首先，由于slam最终要得到的结果是地图，因此我们有必要更详细了解有关地图的内容。如果我们给机器人一张我们使用的纸质地图，机器人会理解吗？ 应该不会。应该给机器人一个易于理解和易于计算的数字文件。人们对于这种机器人导航地图的定义已经讨论了很长时间，现在也没有结束。尤其是，近年来出现了各种形式的地图，有些不仅包括二维信息，而且还包括三维信息，或者有些地图不仅包含有关移动的信息，还包含各物体的分割（segmentation）的信息。
在这，我们将使用在ROS社区中常用的二维占用网格地图（OGM，Occupancy Grid Map）。如上面运行slam地结果图所示，白色是机器人可以移动的自由区域（free area），黑色是机器人不能移动的占用区域（occupied area），灰色是未被确认的未知区域（unknown area）。
这些区域用从0到255表达的灰度（gray scale）值表示。该值是通过贝叶斯定理的后验概率获得的，该概率代表占用状态的占用概率。占用概率occ表示为“occ =（255- color_avg）/255.0”。如果图像是24位，则是“color_avg =（一个单元的灰度值/0xFFFFFF×255）”。这个occ越接近1，它被占用的概率越大，越接近0，被占用的概率就越小。
当它以ROS消息（nav_msgs/OccupancyGrid）发布时，会被重新定义为占有度，是[0〜100]之间的整数。越接近“0”就越接近移动自由的区域（free area），而越接近“100”就越是不可移动的已占用的区域（occupied area）。此外，“-1”是定义为未知区域（unknown area）。
在ROS中，地图信息以*.pgm文件格式（portable graymap format）存储和使用。它还包含一个*.yaml文件，它也包含地图信息。例如，如果我们查看在第二部分中所写的地图信息（map.yaml），则结果类似如下所示。image是地图的文件名，而resolution是地图的分辨率，单位是meters/pixel。
image: map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
也就是说，每个像素意味着5厘米。origin是地图的原点，origin的每个数字代表x、y和yaw。地图的左下角是x = -10m，y = -10m。negate会反转黑白。每个像素的颜色如下确定：由当占用概率超过占用阈值（occupied_thresh）时表示为黑色的占用区域，而当占用概率小于自由阈值（free_thresh）时表示为白色的自由区域（free area）。
2、SLAM所需的信息
我们已经了解过地图了，那下面让我们来看看为了绘制地图，SLAM都需要哪些信息。首先，“制作地图时需要什么？” 首要的是距离值。例如，可以以自己为中心来判断“那个沙发离我有2m远”的距离值。可以说这个距离值是用LDS或深度摄像机来扫描XY平面的结果值。
其次是我的位置值。在这里，“我”是指“传感器”，因为这个传感器的位置固定在机器人上，所以如果机器人移动，传感器也会一起移动。因此，传感器的位置值依赖于机器人的移动量，也就是测位（odometry）。有必要计算它并将其作为位置值来提供。
这里提到的距离值在ROS中被称为scan，并且姿态（位置+方向）信息会根据相对坐标关系而改变，因此被称为tf（transform）。如图所示，我们根据两个信息scan和tf来运行SLAM，并创建我们想要的地图。
3、SLAM的处理过程
为了创建地图，除了turtlebot3_core节点之外，笔者还为SLAM创建了turtlebot3_slam功能包。该功能包没有源文件，但是通过将需要的功能包捆绑成launch文件来运行。这个过程如图所示，后面有详细的说明。
➊sensor_node(例：turtlebot3_lds)
turtlebot3_lds节点运行LDS传感器，并将SLAM所需的scan信息发送到slam_gmapping节点。
➋ turtlebot3_teleop(例：turtlebot3_teleop_keyboard)
turtlebot3_teleop_keyboard节点是可以接收键盘输入并控制机器人的节点。向turtlebot3_core节点发送移动速度和旋转速度命令。
➌ turtlebot3_core
turtlebot3_core节点接收用户的命令并移动机器人。此时在内部发送测得的机器人自己的位置odom信息，且还会以odom→base_footprint→base_link→base_scan的顺序将odom的相对坐标变换信息以tf形式发布。
➍ turtlebot3_slam_gmapping
在turtlebot3_slam_gmapping节点中，根据scan信息（由传感器测量的距离值）和tf值（传感器的位置值）来创建地图。
➎ map_saver
map_server功能包中的map_saver节点将利用这个地图的信息生成一个可保存的map.pgm文件和一个信息文件map.yaml。
4、坐标变换（TF）
在SLAM中使用的两种信息是如上所述的距离值和测量该距离值的位置。距离值可以从传感器节点接收，并且距离值被测量的位置是相应传感器的位置。传感器安装在机器人的某个地方，因此机器人的移动会带动传感器移动。换句话说，机器人和传感器在物理上是固定的，并且传感器的姿态（位置+方向）根据机器人的移动而相对变化。可以将其视为相对坐标变换。在ROS中，这个过程被称为tf。我们以树的形式看看当前的相对坐标，命令如下：
rosrun rqt_tf_tree rqt_tf_tree
如果执行上述命令，则可以使用tf的tree查看器检查机器人和传感器的相对位置变换信息（tf），如图所示。换句话说，如果仅考虑从机器人位置到安装LDS的位置这一段，则位置信息会按照odom→base_footprint→base_link→base_scan的顺序相对连接。机器人会根据从
turtlebot3_teleop_keyboard节点收到的平移速度和转速命令来移动，并且按照前面所述的导航推测（dead reckoning）估计机器人的测位（odometry），如此生成的odom会以tf形式发布。之后的base_footprint→base_link→base_scan是在结构上固定的状态。这与在turtlebot3_description功能包中的/urdf/turtlebot3_waffle.urdf.xacro中描述的一样，描述了各坐标变换，并定期地通过robot_state_publisher节点发布tf。
5、turtlebot3_slam功能包
turtlebot3_slam功能包中的turtlebot3_slam.launch的内容如下。这个启动文件主要分为两种，一种包含turtlebot3_remote.launch文件，另一种运行本节讨论的turtlebot3_slam_gmapping节点。
首先，我们来看看turtlebot3_remote.launch文件。该文件描述了用户指定的机器人模型的加载和robot_state_publisher节点的执行，该节点将两个轮子和每个关节的姿态信息以TF形式。
剩下的一个节点turtlebot3_slam_gmapping实际上是将gmapping功能包中的slam_gmapping节点改名后运行。为了使这个节点正常工作，需要根据自己的机器人和传感器修改各种选项，如下所示。下面的设置值都是为TurtleBot3 Waffle设置的。如果想使用TurtleBot3以外的其他机器人，请参考以下说明并根据您的机器人和传感器进行修改。
<param name="base_frame" value="base_footprint"/> 机器人基本框架
<param name="odom_frame" value="odom"/> 测位（Odometry）框架
<param name="map_update_interval" value="2.0"/> 地图更新时间间隔（sec）
<param name="maxUrange" value="4.0"/> 使用的激光传感器的最大范围（meter）
<param name="minimumScore" value="100"/> 考虑到扫描匹配结果的最低分数
<param name="linearUpdate" value="0.2"/> 处理所需的最小移动距离
<param name="angularUpdate" value="0.2"/> 处理所需的最小旋转角度
<param name="temporalUpdate" value="0.5"/> 如果从最后一次扫描时刻开始超过了此更新时
间，则执行扫描。如果这个值小于0，则不使用它。
<param name="delta" value="0.05"/> 地图分辨率：距离/像素
<param name="lskip" value="0"/> 在每次扫描中跳过的光束数量
<param name="particles" value="120"/> 粒子滤波器中的粒子数
<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="srr" value="0.01"/> 测位误差（平移→移动）
<param name="srt" value="0.02"/> 测位误差（平移→旋转）
<param name="str" value="0.01"/> 测位误差（旋转→平移）
<param name="stt" value="0.02"/> 测位误差（旋转→旋转）
<param name="resampleThreshold" value="0.5"/> 重新采样阈值
<param name="xmin" value="-10.0"/> 初始地图大小（最小x）
<param name="ymin" value="-10.0"/> 初始地图大小（最小y）
<param name="xmax" value="10.0"/> 初始地图大小（最大x）
<param name="ymax" value="10.0"/> 初始地图大小（最大y）
<param name="llsamplerange" value="0.01"/> 似然估计的范围（平移）
<param name="llsamplestep" value="0.01"/> 似然估计的步幅（平移）
<param name="lasamplerange" value="0.005"/> 似然估计的范围（旋转）
<param name=”lasamplestep” value=”0.005”/> 似然估计的步幅（旋转）
扫一扫
7917
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
