（仅作学习笔记，参考书籍《ROS机器人编程》）
四、SLAM理论篇
1、SLAM
SLAM（Simultaneous Localization And Mapping），翻译成中文就是“同时定位与地图构建”。换句话说，这意味着机器人在未知的环境中探索，仅通过安装在机器人上的传感器估计机器人本身的位置的同时绘制未知环境的地图。这是导航及自主驾驶的关键技术。
通常用于位置估算的传感器有编码器（Encoder）和惯性测量单元（IMU）。编码器测量车轮的旋转量，并通过导航推测（dead reckoning）推算机器人的大致位置。在这种情况下会发生一定的误差，此时用惯性传感器测得的惯性信息补偿位置信息的误差。根据目的，位置也可以不用编码器，只用惯性传感器来估算。
该位置估计根据通过在创建地图时使用的距离传感器或相机获得的周围环境的信息再次校正位置。这种位置估计方法包括卡尔曼滤波（Kalman filter）、马尔可夫定位（Markov localization）、利用粒子滤波（Particle filter）的蒙特卡罗定位（MonteCarlo Localization）等等。
距离传感器广泛用于测绘，如超声波传感器、光探测器、无线电探测器、激光测位仪和红外扫描仪。除了距离传感器之外，还使用相机，诸如将立体相机当作距离传感器，或使用普通相机的视觉SLAM。而且，有的研究者提出了通过给环境贴上标记（marker）来识别环境的方法。例如，通过将标记安装在天花板上，用相机区分标记。近来，出现了多种深度相机（RealSense、Kinect和Xtion等），利用这些相机可以获得接近距离传感器的距离值，因此有很多相关的研究。
2、多种位置估计（localization）方法论
位置估计方法是机器人工程的一个重要研究领域，它目前也在被人们积极地研究。只要能对机器人的位置进行足够正确的估计，则能够容易地解决基于位置的地图绘制的问题，如SLAM。但是目前还有许多问题，比如，传感器捕捉到信息不确定、为了在实际环境中工作需要保证实时性，等等。为了解决这个问题，有各种位置估计方法在被研究当中。在本部分中，作为位置估计的代表性例子，讨论了卡尔曼滤波器（Kalman filter）和粒子滤波器（Particle filter）方法论。
卡尔曼滤波器（Kalman filter）
由Rudolf E. Kalman博士开发的卡尔曼滤波器（Kalman filter）因其在美国宇航局的阿波罗计划中的应用而广为人知，该滤波器指，在有噪声的线性系统中，跟踪目标值状态的递归滤波器。它基于贝叶斯（Bayes）概率，它预先假定了一个模型，并使用这个模型从以前的状态预测（Prediction）当前状态。然后，使用这个预测值与由外部测量仪器获得的实际测量值之间的误差来执行一个补偿（update）过程，这个过程利用误差值推定更准确的状态值。它持续地重复迭代，以此提高准确性。这个过程的简化说明如图所示。
但是，卡尔曼滤波器仅适用于线性系统。我们的机器人和传感器大部分都是非线性系统，扩展和改进卡尔曼滤波的EKF（扩展卡尔曼滤波）被广泛应用。此外，还有许多KF变体，例如无损卡尔曼滤波器（UKF，Unscented Kalman Filter）和快速卡尔曼滤波器（Fast Kalman filter），这些都提高了EKF的精度。它也经常与其他算法一起使用，例如会与粒子滤波器一起使用的Rao-Blackwellized粒子滤波器（RBPF，Rao-Blackwellized Particle Filter）一起使用。
粒子滤波器
粒子滤波（Particle Filter）是目前最流行的目标跟踪算法。典型的例子是使用粒子滤波器的蒙特卡罗定位（Monte Carlo Localization）。前面描述的卡尔曼滤波器存在一个问题，即在具有高斯噪声的线性系统中能保证准确度，但是对于其他的系统无法保证准确度。但现实世界中的大部分问题都是非线性系统。
机器人和传感器也是如此，所以粒子滤波器通常用于位置估计。如果卡尔曼滤波器是一个分析方法，假设目标是一个线性系统，那么卡尔曼滤波器通过线性运动搜索参数，而粒子滤波器是一种基于尝试和错误（try-and-error）的方法通过仿真进行预测的技术。称为粒子滤波器是因为将由目标系统中的概率分布随机生成的估计值看作为粒子。这也被称为顺序蒙特卡罗（SMC，Sequential Monte Carlo）方法或蒙特卡罗方法。
像其他位置估计算法一样，用粒子滤波方法推定目标物体的位置时，也假设输入信息中包含误差。当使用SLAM时，使用机器人的测位（odometry）值和用距离传感器获得的环境测量值作为观测值来估计机器人的当前位置。
在粒子滤波方法中，位置不确定性是用称为样本的粒子群来描述的。我们根据机器人的运动模型和概率将粒子移动到一个新的估计位置和方向，并根据实际测量值对每个粒子的权重进行调整，逐渐降低噪声，得到比较精确的位置。在移动机器人的情况下，每个粒子（particle）是姿态（pose）和权重（weight）的函数。其中姿态（pose）是机器人的位置（x，y）和方向（i）的函数。
这个粒子滤波器经历以下5个步骤。除了第一步的初始化之外，重复执行第二至第五步，以估计机器人的位置值。换句话说，是一种用测量值反复更新粒子分布（在X，Y坐标平面上把机器人的位置用概率表达的粒子的分布），以此推测机器人的位置的方法。
➊ 初始化（initialization）
由于一开始根本无法知道初始机器人的姿态（位置和方向），因此在可以用N个粒子求得的所有可能的姿态的范围内随机安排机器人的姿态。每个初始粒子权重为1 / N，总和为1。N以经验确定，通常为数百。如果初始位置已知，则将粒子放置在其附近。
➋预测（prediction）
根据描述机器人运动的系统模型（system model），给被观察到的移动量（如机器人的测位（Odometry）信息等）加上噪声（noise），用这种方法移动各个粒子。
➌ 调整（update）
基于所测量到的传感器信息，计算每个粒子的概率，并且通过反映这个值来更新每个粒子的权重值。
➍ 姿态估计（pose estimation）
利用所有粒子的位置、方向和权重值来计算加权平均值、中央值和最大权重的粒子值，并用这些估计机器人的姿态。
➎ 重采样（Resampling）
这是生成新粒子的步骤，是去除权重小的粒子，以权重大的粒子为中心创建继承了现有例子的特性（姿态信息）的新粒子。这里，必须保持粒子数量N不变。
另外，如果样本数量足够，粒子滤波器的位置估计比卡尔曼滤波器改进版的EKF或UKF更准确，但如果数目不够，则可能不准确。为了解决这个问题，同时使用粒子滤波和卡尔曼滤波的基于Rao-Blackwellized粒子滤波器（RBPF）的SLAM算法被广泛使用。
五、导航实战篇
导航所需的机器人硬件：移动机器人使用TurtleBot3，传感器使用LDS。测量环境也与SLAM相同。在本节中将了解的导航是指：利用前面在SLAM的中创建的地图，让机器人移动到指定的目的地。
1、用于导航的ROS功能包
本节中使用的与导航相关的ROS功能包包括：turtlebot3元功能包；前一个SLAM课程中编写的turtlebot3元功能包；navigation元功能包中的move_base、amcl和map_server功能包。安装已经在前面的SLAM中做好了。这一节只描述执行方法。
2、运行导航
运行导航的顺序如下。以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
运行导航功能包
在[Remote PC]中，运行turtlebot3_navigation.launch启动文件。turtlebot3_navigation功能包由几个启动文件组成。运行后，robot_state_publisher节点（将TurtleBot3的3D模型信息、双轮及各关节的三维位置和方向信息以TF形式发布）、用于加载先前创建的地图的map_server节点、AMCL（自适应蒙特卡罗定位，AdaptiveMonte Carlo Localization） 节点和move_base节点等4个节点会一起被运行。
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
运行RViz
先运行ROS的可视化工具RViz，以便在导航中可以直观地确认目标指定命令和结果。
当使用以下选项运行RViz时，会从一开始添加显示插件，因此非常方便。
$ rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz
运行后，可以看到如图所示的画面。在右边的地图上，可以看到很多绿色的箭头，这是SLAM理论中描述的粒子滤波器的粒子。这是因为导航也使用粒子滤波器，稍后再解释。可以确认机器人在绿色箭头的中间。
初始位置估计
首先，要做的是估计机器人的初始位置。当在RViz的菜单中按下[2D Pose Estimate]时，会出现一个非常大的绿色箭头。将其移动到机器人在给定的地图中所在的位置，并按住鼠标键的同时，拖动绿色箭头使其指向机器人的前方。这是一种在初期为了估计机器人位置的命令。然后用turtlebot3_teleop_keyboard节点等来回移动机器人，搜集周围的环境信息，找出机器人当前位于地图上的位置。经过了这个过程后，机器人将绿色箭头指定的位置和方向作为初始位置，推定自己的位置和方向。
设置目的地且移动机器人
一切准备就绪后，下面下达移动命令。如果在RViz的菜单中按[2D Nav Goal]，会出现一个非常大的绿色箭头。该绿色箭头是指定机器人的目的地的标记，箭头的起点是机器人的x、y位置，箭头方向是机器人的i方向。将此箭头移动到机器人的目的地，然后拖动，以设置方向。机器人将根据创建的地图躲避障碍物，移动到目的地（见下图）
六、导航应用程序
包括turtlebot3元功能包、作为LDS驱动程序的turtlebot3_lds节点、turtlebot3的三维模型信息（turtlebot3_description）、加载先前创建的地图的map_server节点、ACML（自适应蒙特卡罗定位）节点和和move_base节点。
1、导航
导航是在给定的环境中将机器人从当前位置移动到指定的目的地。为此，需要有包含给定环境的家具、物体和墙壁的几何信息（geometry，geo-：土地，metry：测量）的地图，正如前面的SLAM所述， 机器人可以从自己的姿态信息和从传感器获得的距离信息来获得地图。
在导航中，机器人利用这个地图和机器人的编码器、惯性传感器和距离传感器等资源，从当前位置移动到地图上的指定目的地。这个过程如下。
传感（sensing）
在地图上，机器人利用编码器和惯性传感器（IMU传感器）更新其测位（odometry）信息，并测量从距离传感器的位置到障碍物（墙壁、物体、家具等）的距离。
姿态估计（localization / pose estimation）
基于来自编码器的车轮旋转量、来自惯性传感器的惯性信息以及从距离传感器到障碍物的距离信息，估计机器人在已经绘制的地图上的姿态（localization / poseestimation）。此时用到的姿态估计方法有很多种，本节中将使用粒子滤波器定位（particle filter localization），以及蒙特卡罗定位（Monte Carlo Localization）的变体ACML（Adaptive Monte Carlo Localization，自适应蒙特卡罗定位）。
运动规划
也称为路径规划，它创建一个从当前位置到地图上指定的目标点的轨迹。对整个地图进行全局路径规划，以及以机器人为中心对部分地区进行局部路径规划。我们计划使用基于一种避障算法Dynamic Window Approach（DWA）的ROS move_base和nav_core等路径规划功能包。
移动/躲避障碍物（move / collision avoidance）
如果按照在运动规划中创建的移动轨迹向机器人发出速度命令，则机器人会根据移动轨迹移动到目的地。由于感应、位置估计和运动规划在移动时仍在被执行，因此使用动态窗口方法（DWA）算法可避免突然出现的障碍物和移动物体。
导航所需的信息
如图显示了运行ROS导航功能包所需的必要节点和话题之间的关系。我们将重点介绍导航所需的信息（话题）。作为参考，在描述图中的话题时，分别显示了话题名称和话题消息类型。例如，在测位（odometry）中，“/odom”是话题名称，“nav_msgs/Odometry”是话题消息的类型。
测位（‘/odom’，nav_msgs/Odometry）
机器人的测位信息用于局部路径规划，利用接收到的机器人的当前速度等信息，产生局部移动路径或避开障碍物。
坐标变换（‘/tf , tf/tfMessage）
由于机器人传感器的位置根据机器人的硬件配置而变化，所以ROS使用tf相对坐标变换。这只是简单地利用测位获得机器人的位置，例如测位描述“以机器人的位置为原点，传感器在x、y、z坐标坐标系中位于某某位置”。例如，经过odom→base_footprint→base_link→base_scan的变换后以话题来发布。它从move_base节点接收这些信息，并根据机器人的位置和传感器的位置执行移动路径规划。
距离传感器（‘/scan , sensor_msgs/LaserScan or sensor_msgs/PointCloud）
意味着从传感器测量得到的距离值。通常使用LDS和RealSense、Kinect、Xtion等。该距离传感器使用AMCL（adaptive Monte Carlo localization，自适应蒙特卡罗定位）来估计机器人的当前位置，且规划机器人的运动。
地图(‘/map , nav_msgs/GetMap）
导航使用占用网格地图（occupancy grid map）。我们使用map_server功能包来发布我们之前编写的“map.pgm”和“map.yaml”文件。
目标坐标（‘/move_base_simple/goal , geometry_msgs/PoseStamped）
目标坐标由用户直接指定。可以使用如平板电脑的设备创建和使用单独的目标坐标命令功能包，但在本节中，将在ROS的可视化工具RViz中设置目标坐标。目标坐标由二维坐标（x，y）和姿态θ组成。
速度命令（‘/cmd_vel , geometry_msgs/Twist）
根据最终规划的移动轨迹发布移动机器人的速度命令，而机器人根据该命令移动到目的地。
3、turtlebot3_navigation的各节点和话题状态
在[turtlebot]中执行turtlebot3_robot.launch文件和turtlebot3_navigation.launch文件，就具有了导航的必要条件。
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
在这种状态下，运行rqt_graph，则可以在ROS环境中查看正在运行的节点和话题信息，如图所示，上述导航所需的信息分别以/odom、/tf、/scan、/map和/cmd_vel的话题名称发布和订阅，而move_base_simple/goal是在ROS的可视化工具RViz中直接指定目标坐标时才会被发布。
4、turtlebot3_navigation设置
turtlebot3_navigation功能包需要多种文件：启动与导航节点有关的功能包的launch文件、xml文件、设置各种参数的yaml文件、地图相关文件和rviz配置文件。下面介绍这些配置文件。
■ /launch/turtlebot3_navigation.launch
turtlebot3_navigation.launch一个文件可以启动与导航相关的所有功能包。
■ /launch/amcl.launch.xml
amcl.launch.xml文件是一个包含自适应蒙特卡罗定位（AMCL）的各种参数设置的文件。它与turtlebot3_navigation.launch一起使用。
■ /param/move_base_params.yaml
统筹管理运动规划的move_base的参数设置文件。
■ /param/costmap_common_params_burger.yaml
■ /param/costmap_common_params_waffle.yaml
■ /param/global_costmap_params.yaml
■ /param/local_costmap_params.yaml
导航使用占用网格地图（occupancy grid map）。基于该占用网格地图，利用机器人姿态和从传感器获得的周围信息，将每个像素计算为障碍物、不可移动区域和可移动区域。这时用到的概念是costmap。costmap的配置参数就是这些文件，其中包括公用的costmap_common_params.yaml文件、全局区域运动规划所需的global_costmap_params.yaml文件以及本地区域所需的local_costmap_params.yaml文件。其中，costmap_common_params.yaml根据机器人的型号具有burger和waffle等两种后缀，并且各模型的内容包含不同的外观信息。需要留意的是，TurtleBot3
Waffle Pi除了相机以外其他部分与TurtleBot3 Waffle相同，因此使用waffle后缀，沿用waffle型号的设置。
■ /param/dwa_local_planner_params.yaml
dwa_local_planner是最后将移动速度命令传给机器人的功能包，上面文件则是设置该功能包的参数的文件。
■ base_local_planner_params.yaml
因为turtlebot3已将dwa_local_planner用作base_local_planner的设置值，因此在此不使用。这是因为已经在move_base节点中更改了参数设置，如下所示：
<param name=”base_local_planner” value=”dwa_local_planner/DWAPlannerROS” />
■ /maps/map.pgm
■ /maps/map.yaml
将以前创建的占用网格地图（occupancy grid map）保存在/maps目录中。
■ /rviz/turtlebot3_nav.rviz
它是一个包含ROS的可视化工具RViz的设置信息的文件。将加载RViz的显示插件中的Grid、RobotModel、TF、LaserScan、Map、Global Map、Local Map和AMCLParticles。
七、导航理论篇
1、Costmap
机器人的位置是根据从编码器和惯性传感器（IMU传感器）获得的测位来估计的。然后，通过安装在机器人上的距离传感器来计算机器人与障碍物之间的距离。导航系统将机器人位置、传感器姿态、障碍物信息和作为SLAM地图的结果而获得的占用网格地图调用到固定地图（static map），用作占用区域（occupied area）、自由区域（free area）和未知区域（unknown area）。
在导航中，基于上述四种因素，计算障碍物区域、预计会和障碍物碰撞的区域以及机器人可移动区域，这被称为成本地图（costmap）。根据导航类型，成本地图又被分成两部分。一个是global_costmap，在全局移动路径规划中以整个区域为对象建立移动计划，其输出结果就是global_costmap。而另一个被称为local_costmap，这是在局部移动路径规划中，在以机器人为中心的部分限定区域中规划移动路径时，或在躲避障碍物时用到的地图。然而，这两种成本图的表示方法是相同的，尽管它们的目的不同。
costmap用0到255之间的值来表示。数值的含义如图所示，简单地说，根据该值可以知道机器人是位于可移动区域还是位于可能与障碍物碰撞的区域。
■■ 000：机器人可以自由移动的free area（自由区域）
■■ 001~127：碰撞概率低的区域
■■ 128~252：碰撞概率高的区域
■■ 253~254：碰撞区域
■■ 255：机器人不能移动的占用区域（occupied area）
例如，实际的costmap如图11-20所示。具体的，机器人模型位于中间，其周围的黑色边框对应于机器人的外表面。当这个轮廓线碰到实际的墙壁时，意味着机器人会发生碰撞。绿色用从激光传感器获得的距离传感器值表示的障碍物，灰度的颜色越深的位置意味着碰撞的可能性越高。这同样适用于用颜色表示的情况，粉红色区域是实际的障碍物，蓝色区域是机器人中心位置进入该区域则会发生碰撞的位置，且边框用粗红色像素绘制。这些颜色可以由用户在RViz中修改，因此可以说没有太大的意义。
2、AMCL
正如在4节的SLAM理论篇的粒子滤波（particle filter）的说明，蒙特卡罗定位（MCL）位置估计算法在位置估计领域中被广泛运用。AMCL（自适应蒙特卡罗定位）可以被看作蒙特卡罗位置估计的改进版本，它通过在蒙特卡罗位置估计算法中使用少量样本来减少执行时间，以此提高实时性能。
3、Dynamic Window Approach(DWA)
动态窗口方法（Dynamic Windows Approach，DWA）是在规划移动路径和躲避障碍物时常用的方法，具体是指在机器人的速度搜索空间（velocity search space）中选择适当的速度，以躲避可能碰撞的障碍物的同时能迅速到达目的地。在ROS中，局部移动规划中曾广泛使用Trajectory planner，而最近由于DWA的性能优越，因此DWA在逐渐代替Trajectory planner。
首先，如图所示，用平移速度v和旋转速度w为轴的速度搜索空间（velocitysearch space）来表示机器人，而不是用x轴和y轴的位置坐标系表示。在这个空间中，由于硬件限制，机器人具有最大允许速度，这被称为动态窗口（Dynamic Window）
v：平移速度 (meter/sec)
~：旋转速度 (radian/sec)
Vs：最大速度区域
Va：允许的速度区域
Vc：当前速度
Vr：动态窗口中的速度区域
amax：最大加/减速度
G(v, ~) = v(a·heading(v, ~) + b·dist(v, ~) + c·velocity(v, ~))：目标函数
heading(v, ~)：180 -（机器人的方向与目标点的方向之差）
dist(v, ~)：离障碍物的距离
velocity(v, ~)：选择的速度
a, b, c：权重常数
v(x) ：Smooth Function
在这个动态窗口中，通过使用目标函数，获得满足条件的平移速度和旋转速度。满足条件意味着使考虑了机器人的方向、速度和碰撞的目标函数达到最大化。如果绘制出来，则如图所示，我们可以在各种和 中找到最优速度并移动机器人。