粒子滤波和蒙特卡洛
蒙特卡洛：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。
粒子滤波：粒子数代表某个东西的可能性高低。通过某种评价方法（评价这个东西的可能性），改变粒子的分布情况。比如在机器人定位中，某个粒子A，我觉得这个粒子在这个坐标（比如这个坐标就属于之前说的“这个东西”）的可能性很高，那么我给他打高分。下次重新安排所有的粒子的位置的时候，就在这个位置附近多安排一些。这样多来几轮，粒子就都集中到可能性高的位置去了。
重要性采样
就像转盘抽奖一样，原本分数高（我们给它打分）的粒子，它在转盘上对应的面积就大。原本有100个粒子，那下次我就转100次，转到什么就取个对应的粒子。这样多重复几次，仍然是100个粒子，但是分数高的粒子越来越多了，它们代表的东西（比如位姿）几乎是一样的。
机器人绑架
举例，机器人突然被抱走，放到了另外一个地方。类似这种情况。
自适应蒙特卡洛
自适应体现在：
1解决了机器人绑架问题，它会在发现粒子们的平均分数突然降低了（意味着正确的粒子在某次迭代中被抛弃了）的时候，在全局再重新的撒一些粒子。
2解决了粒子数固定的问题，因为有时候当机器人定位差不多得到了的时候，比如这些粒子都集中在一块了，还要维持这么多的粒子没必要，这个时候粒子数可以少一点了。
KLD采样
就是为了控制上述粒子数冗余而设计的。比如在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。
(AMCL原理概述_ethan_guo的博客-CSDN博客 https://blog.csdn.net/ethan_guo/article/details/81809054)
amcl
adaptive Monte Carlo localization
AMCL将传入的激光扫描数据转为里程计结果（odom_frame_id),因此必须存在从激光发布到里程计的tf树转换。即2D的概率定位系统，输入激光雷达数据，里程计数据，输出机器人在地图中的位姿，用的是自适应蒙特卡洛定位方法，这个方法是在已知地图中使用粒子滤波方法得到位姿的。
而mcl（蒙特卡洛定位）法使用的是粒子滤波的方法来进行定位的。而粒子滤波很粗浅的说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了。
这幅图模拟了一个一维机器人的粒子更新，机器人下面那些想条形码一样的竖条就是粒子的分布了，可以看到粒子随着机器人的移动与更新会逐渐的收敛到机器人的正确位置上。
mcl算法步骤图：
ros里的amcl
ros使用自适应的蒙特卡洛定位法，即amcl
订阅的主题
scan (sensor_msgs/LaserScan)
Laser scans.
tf (tf/tfMessage)
Transforms.
initialpose (geometry_msgs/PoseWithCovarianceStamped)
Mean and covariance with which to (re-)initialize the particle filter.
map (nav_msgs/OccupancyGrid)
When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization. New in navigation 1.4.2.
实际上初始位姿可以通过参数提供也可以使用默认初始值，我们主要是要将scan（激光）、tf和map主题提供给amcl。
发布的主题
amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
Robot’s estimated pose in the map, with covariance.
particlecloud (geometry_msgs/PoseArray)
The set of pose estimates being maintained by the filter.
tf (tf/tfMessage)
Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map.
如果纯粹是为了显示一下机器人的位姿（in rviz）我们只需要tf主题就够了。
tf tree
move_base package的系统介绍：
ROS提供的move_base 包让我们能够在已建立好的地图中指定目标位置和方向后，move_base根据机器人的传感器信息控制机器人到达我们想要的目标位置。它主要功能包括：结合机器人码盘推算出的odometry信息，作出路径规划，输出前进速度和转向速度。这两个速度是根据你在配置文件里设定的最大速度和最小速度而自动作出的加减速决策。
在此框架中白色和灰色的都是ROS已经封装好的功能包：
map_server
gmapping：根据激光数据+里程odom数据构建地图http://wiki.ros.org/gmapping
hector：根据激光数据构建地图http://wiki.ros.org/hector_slam
acml：根据激光数据在已有的地图上定位http://wiki.ros.org/amcl
move_base：路径规划，根据地图，激光数据，使得机器人从一个位置到达另外一个位置，可以避开障碍物http://wiki.ros.org/move_base
这其中gmapping 和hector的功能是一样的，都是用来构建地图的，区别在于gmapping在构建过程中依赖于机器人的里程odom数据，hector则不需要odom，算法不同，在实际使用中选用其中的一个就可以了。
而蓝色标识的功能包则需要根据选用硬件平台的不同来定制。
图中我们可以看到move_base package 的输入和输出。要使得它能运行起来，我们就得构建好这些输入和输出。
必要的输入：
goal ： 期望机器人在地图中的目标位置。
tf ： 各个坐标系之间的转换关系。（具体/map frame --> /odom frame ，/odom frame --> /base_link frame）
odom：根据机器人左右轮速度推算出的航向信息（即/odom 坐标系中机器人x,y坐标以及航向角yaw，下面会具体介绍）
LaserScan：激光传感器的信息，用于定位。
输出：
cmd_vel：在cmd_vel这个主题上发布Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。
再整理下思路：move_base收到goal以后，将目标goal通过基于actionlib的client（客户端）向服务器发送，服务器根据你的tf关系以及发布的odom消息不断反馈机器人的状态（feedbackcall）到客户端, 让move_base做路径规划和控制twist。
知道了move_base的这些外围消息接口以后，move_base的运行还需要一些内部的配置参数，如机器人的最大最小速度，已经路径规划时的最大转弯半径等等
Twist 消息转化为机器人左右轮期望速度。
首先，假设move_base能够正常工作了，它将把控制命令Twist发布到cmd_vel这个主题上。我们现在来解决如何利用这个Twist消息来对机器人进行控制。
在ROS by example 一书中的第七章开头就规定了机器人自身的坐标系系统，如下图。注意两个坐标系的建立都是右手坐标系，左图中的右手就是机器人本身，x轴就是前进的方向，垂直于两轮之间的轴连线，Y轴就是两个轮之间的轴连线。右图表示机器人的旋转坐标系，大拇指指向Z轴，逆时针方向为正值。
清楚了坐标系以后，再来看看Twist这个消息里包含的是什么东西。
使用ctrl + alt + t 打开一个新的终端以后，输入如下命令，就可以查看Twist的消息类型了。
rosmsg show geometry_msgs/Twist
其中linear 的x就是代表前进方向的速度，单位为m/s。angular 的z就代表机器人的绕中心旋转的角速度，单位为 弧度/s (rad/s)。
最主要的是如何将指定的转速转化为两轮的差速。主程序中订阅了cmd_vel主题，一旦收到move_base发出的twist消息，就调用callback函数进行转化。如果linear.y 不为0，说明小车要沿着y轴运动，这会导致两轮的差速，但是对于两轮控制的移动机器人，twist.linear.y = 0 是在move_base的配置文件base_local_planner_params.yaml中有明确指定，所以不需关注linear.y的转换。
只要关注前进速度linear.x和旋转速度angular.z的转换。
在直线行驶时，前进速度linear.x就是左右轮的期望速度。最主要的是将转速转化为左右轮的差速，一个轮子转的快，一个轮子转的慢，就有了转速。
下面介绍如何将指定的转速转化为两轮差速：
1.dsp采样的是单位时间内的码盘值，将码盘值转化为左右轮速度值后(Lwheelspeed,Rwheelspeed)通过串口发送给电脑端。
2.关于航迹推演(Odometry) 的公式中有一个关于如何有左右轮差速转化为旋转速度的计算公式。即yaw_rate = (Rwheelspeed - Lwheelspeed) / d .其中d为两轮间的间距，得到的转速单位rad/s。(如果不知道什么是Odometry 方法，可以点击这里。)或者也可以看看这个，还有这个链接。直接用这个公式可以计算，但是在测量这个公式中的d的时候有测量误差。因此，楼主这里采用的拟合的方法得到这个差速到转速之间的转换系数。
（参考：move_base 控制机器人(1) - arbain - 博客园 https://www.cnblogs.com/arbain/p/7850838.html）
