4.在STDR中使用amcl进行仿真机器人定位
转自链接：http://www.corvin.cn，有兴趣请移步学习。
0x00 为何需要对机器人进行定位？
要想实现移动式机器人的自动导航需要完成以下六点，当然这些信息是我自己总结的仅供大家参考：
（1）一张完整的高精度全局地图，机器人需要在该地图下完成自动导航，这是进行自动导航的必要且重要的前提;
（2）机器人在地图的哪个位置上，确定机器人当期位姿，用于确定机器人的起始状态;
（3）机器人需要到达的目的地在地图上哪里，确定机器人在地图上的最终位姿，用于确定机器人的结束状态;
（4）根据机器人在地图上的当前位姿和目的地位姿两点信息，如何规划出一条完美的移动路径使其可以从当前位置移动到目的位置;
（5）尽可能根据全局地图来移动机器人同时需要进行局部路径的规划，防止全局路径上突然出现障碍物，因此需要不断小距离的规划局部路径来移动机器人使其避障。
（6）随着机器人的不断移动，全局路径也需要不断的重新规划，因为从当前位置移动到目的位置需要时间，全局路径规划只在某一确定时间点下是有效的，随着机器人位置不断变化和时间的流逝，开始时候规划的全局路径已经变得不可靠，因此需要不断的更新全局路径。
对于第一点的建立一张全局地图，我们在第3篇教程在STDR仿真器中使用gmapping构建2D栅格地图中已经介绍过了，本次教程主要就是为了解决第二点，我们如何确定机器人在一张确定的地图上的位姿状态，在这里就用到了AMCL软件包，AMCL的全称是Adaptive Monte Carlo Localization自适应蒙特卡罗定位，是移动机器人在二维环境下的概率定位算法，它实现了自适应（或kld采样）的蒙特卡罗定位方法，作用是针对已有的地图使用粒子滤波器跟踪确定一个机器人的位姿信息。对机器人的定位是非常重要的，因为若无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的，那么按照错误的路径来移动必定是无法到达目的地的，因此我们需要一种资源消耗低，定位速度快，定位精度好，可以稳定定位机器人当前位置的算法是重中之重。
我们可以感受到要想做好机器人的自动导航发现每一步都是非常重要的，第一步就是需要有高精度的全局地图，接下来对机器人在当前地图下的定位就是第二件重要的事，只有一步一步的做好每一步才能做出最完美的自动导航系统。
0x01 AMCL是如何实现对机器人进行定位的呢？
AMCL使用的是粒子滤波的方式来实现对机器人的定位，下面我使用一个简单形象的例子来介绍这个定位过程是如何实现的。你可以想象这样一个场景，你站在一个商业广场上，这个广场就相当于一张全局地图。我在广场上随机的分布了100个妹子，这些妹子就是粒子用来对你进行跟踪定位的，当然现在这些妹子都不知道要跟踪定位的目标就是你啊。你也不认识妹子们，你现在漫无目的的往前移动5步，你突然发现你面前有一个火锅店，你就饿了想吃火锅，就顺便在广场上吼了一嗓子，“我往前走了5步就发现前面有个火锅店啊，我要去吃火锅“。妹子们根据你的信息也来同样的移动，然后就开始观察自己的前面有没有火锅店啊，那肯定是前面有火锅店的妹子附近才有要跟踪的目标啊。此时，由于开始的时候妹子们是随机分布在广场上，面朝的方向肯定也是各部相同，有的肯定就不能发现自己前面有火锅店，那这个妹子就肯定很伤心，自己就退出这个任务了。那些可以发现了火锅店的妹子就很开心，距离目标很近啊，所以她就开始再生出一个妹子跟自己一起来完成这个任务，因为自己找到目标的概率很大啊，可以想象现在这群妹子们肯定有很多就在要跟踪的目标附近了。
那接下来你吃了火锅出来后，继续移动，你向前走了10步，做转弯后发现一个兰州拉面，你就又吼了一嗓子，”我往前走了10步，左拐个弯就发现个兰州拉面啊“。那妹子们又开始根据这个信息来也做同样的移动，此时有的妹子移动后就发现前面没有兰州拉面啊，很伤心就退出跟踪定位任务。那些移动后发现自己前面有兰州拉面的就更开心了啊，距离目标又近了一点啊，因为两次概率都在目标的附近，那她就再生出一个妹子跟着自己一起来定位目标。那可以想象一下，现在肯定广场上有更多的妹子都聚集在一起，她们都在目标的附近。
那你吃完面后出来继续往前走20步，突然发现前面有个珠宝店，你就又吼一嗓子，”我往前走了20步，发现一个珠宝店啊“。妹子们又根据这个信息开始移动，并查看自己的前面有没有珠宝店，那些没有看到珠宝店的就给淘汰了，那发现又珠宝店的妹子就再生出一个妹子，可以想象现在基本上妹子差不多都聚集在目标的附近了，那经过多次这样的循环，即使妹子们不知道要跟踪的目标在哪里，但是可以确定自己现在所处的位置基本上跟目标位置也差不到哪里去了。所以以后我要想定位你的位置那就容易多了，即使我不知道你在哪里，我只要知道我的那群妹子在哪里就行，你基本上也就在那附近了，说实话我感觉这个过程都有点像遗传算法的过程了。
在这里我只是简单介绍了一下例子滤波定位的原理，当然以上这个例子仅供玩乐，不必拘泥是否科学严谨，重在理解这个定位的过程，在ROS下的AMCL只实现了在laser扫描的创建的地图上，使用laser来进行定位，其实该定位方法还可以修改使用其他类型的传感器来定位，例如可以使用超声波或红外这些测距的传感器也可以实现定位。
0x02 创建stdr_amcl测试软件包
（1）在自己的ros工作空间src目录下创建stdr_amcl软件包，在其中编写相关launch文件来加载stdr仿真器和amcl节点来对仿真机器人在地图上定位，首先来创建软件包：
（2）在launch目录下创建stdr_amcl.launch文件，在stdr_amcl.launch文件中添加如下代码，首先将stdr仿真器加载启动，然后打开rviz并加载配置文件，然后通过map_server来加载gmapping建立的地图，最后加载amcl节点启动：
|
|
（3）配置rviz显示，由于rviz的配置文件过长，因此现在提供压缩包下载大家解压后放到rviz目录下即可：stdr_amcl_config.rviz.tar.gz，按照下图来解压：
0x03 编译并测试stdr_amcl软件包
（1）编译软件包需要在catkin_ws目录下执行catkin_make命令，如果想只编译stdr_amcl软件包就加上–pkg就可以了，如下图所示：
当编译完成后需要执行source devel/setup.bash来配置环境变量，这样才能使用如下命令来启动stdr_amcl软件包下的stdr_amcl.launch文件：
roslaunch stdr_amcl stdr_amcl.launch
在robot0旁边的那一大群零散的东西就是初始化的粒子，用来定位机器人的，在初始状态下可以看到定位精度很差，粒子定位在机器人周围2米范围内，随着机器人的不断移动，粒子会逐渐收敛于机器人的位置上。
（2）下面我们使用teleop_twist_keyboard软件包下的teleop_twist_keyboard.py来键盘遥控机器人往前走，会看到整个粒子收敛定位的过程：
（3）查看rqt_graph节点和话题之间的关系图，这样可以对AMCL的正常运行的依赖条件理解更为全面：
（4）来仔细研究一下amcl节点订阅的各个话题：
-
/robot0/laser_0 (sensor_msgs/LaserScan)：这是stdr中的仿真机器人的激光雷达输出的话题，在进行amcl定位时，这个是必不可少需要订阅的话题，因为我们要知道目标周围的环境状态。amcl节点默认是订阅的scan话题，这里由于robot0仿真器发布的雷达话题是/robot0/laser_0，因此需要在launch文件中使用remap来重新映射下订阅的话题。
-
/tf (tf/tfMessage)：订阅了各坐标系转换的话题，用于查询各坐标系的转换。
-
/initialpose (geometry_msgs/PoseWithCovarianceStamped)：用于（重新）初始化粒子滤波器的平均值和协方差，简单来理解就是先预估计一下机器人的初始位姿。
-
/amcl/map (nav_msgs/OccupancyGrid)：当在launch文件中设置了use_map_topic为true时，amcl则订阅该话题获取地图，然后使用基于激光来进行定位，当然设置use_map_topic为false时不订阅该话题也是可以的。
amcl节点发布的话题如下：
-
amcl_pose (geometry_msgs/PoseWithCovarianceStamped)：机器人在地图上带有协方差的位姿估计，这个是话题是整个粒子滤波定位的最终输出结果，该话题输出的位姿信息是根据全局坐标系/map的坐标转换后的位置。
-
particlecloud (geometry_msgs/PoseArray)：在粒子滤波器维护下的一组粒子位姿估计，可以直接在rviz中显示，查看粒子的收敛效果。
-
tf (tf/tfMessage)：发布从odom坐标系到map坐标系的转换，当然该odom坐标系可以使用odom_frame_id参数来重新映射为自定义的坐标系名称，在stdr中由于没有odom坐标系，因此使用/map_static来代替odom坐标系。
（5）下面来看看amcl节点提供的service:
-
global_localization (std_srvs/Empty)：通过调用该服务将初始化全局定位，将所有的粒子重新打散随机的分散在地图的空闲地方，可以通过以下命令来调用该服务，当调用后可以发现所有的粒子被重新打散随机分布在地图上，然后使用键盘遥控机器人旋转，这样可以自动重新对机器人进行定位，调用效果如下图所示：
rosservice call /global_localization "{}"
-
request_nomotion_update (std_srvs/Empty)：该服务是手动的来更新粒子并发布新的粒子，可以使用如下命令来调用服务执行更新粒子的操作，该服务一般需要多次调用才能逐渐看到粒子收敛的效果：
rosservice call /request_nomotion_update "{}"
amcl调用的服务：
-
static_map (nav_msgs/GetMap)：我们从前面话题和节点图得知，amcl可以通过订阅/map话题来获取地图，如果在launch文件中设置use_map_static为false的话，就可以通过调用该服务来获取地图了，效果与订阅/map话题是一样的。
0x04 测试amcl的定位效果
（1）通过查看/amcl_pose话题输出来看定位位姿，可以通过以下命令来获取话题的输出：
rostopic echo /amcl_pose
（2）在机器人的移动过程中，突然将机器人搬起来更换位置，看看能否重新定位成功：
通过上面动图可以看到在我们小范围内（3米）来移动机器人，amcl的定位效果很好，粒子收敛速度很快，但是当大范围的移动时效果就不理想了，这个在机器人移动过程中如果突然将其搬起来放置到距离当前较远位置的话，会导致机器人定位突然失灵，所以对于大范围的定位算法还是有提升空间的。
0x05 launch文件中各参数意义解析
（1）滤波器可以设置的所有参数
-
~min_particles (int, default: 100)：滤波器中的最少粒子数，值越大定位效果越好，但是相应的会增加主控平台的计算资源消耗。
-
~max_particles (int, default: 5000)：滤波器中最多粒子数，是一个上限值，因为太多的粒子数会导致系统资源消耗过多。
-
~kld_err (double, default: 0.01)：真实分布与估计分布之间的最大误差。
-
~kld_z (double, default: 0.99)：上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率，默认0.99。
-
~update_min_d (double, default: 0.2 meters)：在执行滤波更新前平移运动的距离，默认0.2m(对于里程计模型有影响，模型中根据运动和地图求最终位姿的似然时丢弃了路径中的相关所有信息，已知的只有最终位姿，为了规避不合理的穿过障碍物后的非零似然，这个值建议不大于机器人半径，否则因更新频率的不同可能产生完全不同的结果)。
-
~update_min_a (double, default: π/6.0 radians)：执行滤波更新前旋转的角度。
-
~resample_interval (int, default: 2)：在重采样前需要滤波更新的次数。
-
~transform_tolerance (double, default: 0.1 seconds)：tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的。
-
~recovery_alpha_slow (double, default: 0.0 (disabled))：慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值。
-
~recovery_alpha_fast (double, default: 0.0 (disabled))：快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.1是个不错的值。
-
~initial_pose_x (double, default: 0.0 meters)：初始位姿均值（x），用于初始化高斯分布滤波器。（initial_pose_参数决定撒出去的初始位姿粒子集范围中心）。
-
~initial_pose_y (double, default: 0.0 meters)：初始位姿均值（y），用于初始化高斯分布滤波器。（同上）
-
~initial_pose_a (double, default: 0.0 radians)：初始位姿均值（yaw），用于初始化高斯分布滤波器。（粒子朝向）
-
~initial_cov_xx (double, default: 0.5*0.5 meters)：初始位姿协方差（x*x），用于初始化高斯分布滤波器。（initial_cov_参数决定初始粒子集的范围）
-
~initial_cov_yy (double, default: 0.5*0.5 meters)：初始位姿协方差（y*y），用于初始化高斯分布滤波器。（同上）
-
~initial_cov_aa (double, default: (π/12)*(π/12) radian)：初始位姿协方差（yaw*yaw），用于初始化高斯分布滤波器。（粒子朝向的偏差）
-
~gui_publish_rate (double, default: -1.0 Hz)：扫描和路径发布到可视化软件的最大频率，设置参数为-1.0意为失能此功能，默认-1.0。
-
~save_pose_rate (double, default: 0.5 Hz)：存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。-1.0失能。
-
~use_map_topic (bool, default: false)：当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。
-
~first_map_only (bool, default: false)：当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图。
（2）可以设置的所有激光模型参数
请注意无论使用哪种混合权重都应该等于1，对于laser_model_type是beam时会用到4个参数z_hit，z_short，z_max和z_rand，如果是likelihood_field模型仅使用2个：z_hit和z_rand。这4个laser_z_参数，是在动态环境下的定位时用于异常值去除技术的参数。
-
~laser_min_range (double, default: -1.0)：最小扫描范围，参数设置为-1.0时，将会使用激光上报的最小扫描范围。
-
~laser_max_range (double, default: -1.0)：最大扫描范围，参数设置为-1.0时，将会使用激光上报的最大扫描范围。
-
~laser_max_beams (int, default: 30)：更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准）。
-
~laser_z_hit (double, default: 0.95)：模型的z_hit部分的混合权值，默认0.95(混合权重1.具有局部测量噪声的正确范围–以测量距离近似真实距离为均值，其后laser_sigma_hit为标准偏差的高斯分布的权重)。
-
~laser_z_short (double, default: 0.1)：模型的z_short部分的混合权值，默认0.1（混合权重2.意外对象权重（类似于一元指数关于y轴对称0～测量距离（非最大距离）的部分：–ηλe^(-λz)，其余部分为0，其中η为归一化参数，λ为laser_lambda_short,z为t时刻的一个独立测量值（一个测距值，测距传感器一次测量通常产生一系列的测量值）），动态的环境，如人或移动物体）。
-
~laser_z_max (double, default: 0.05)：模型的z_max部分的混合权值，默认0.05（混合权重3.测量失败权重（最大距离时为1，其余为0），如声呐镜面反射，激光黑色吸光对象或强光下的测量，最典型的是超出最大距离）。
-
~laser_z_rand (double, default: 0.05)：模型的z_rand部分的混合权值，默认0.05（混合权重4.随机测量权重–均匀分布（1平均分布到0～最大测量范围），完全无法解释的测量，如声呐的多次反射，传感器串扰）。
-
~laser_sigma_hit (double, default: 0.2 meters)：被用在模型的z_hit部分的高斯模型的标准差，默认0.2m。
-
~laser_lambda_short (double, default: 0.1)：模型z_short部分的指数衰减参数，默认0.1（根据ηλe^(-λz)，λ越大随距离增大意外对象概率衰减越快）。
-
~laser_likelihood_max_dist (double, default: 2.0 meters)：地图上做障碍物膨胀的最大距离，用作likelihood_field模型（likelihood_field_range_finder_model只描述了最近障碍物的距离，（目前理解应该是在这个距离内的障碍物膨胀处理,但是算法里又没有提到膨胀，不明确是什么意思）.这里算法用到上面的laser_sigma_hit。似然域计算测量概率的算法是将t时刻的各个测量（舍去达到最大测量范围的测量值）的概率相乘，单个测量概率：Zh * prob(dist,σ) +avg，Zh为laser_z_hit,avg为均匀分布概率，dist最近障碍物的距离，prob为0为中心标准方差为σ（laser_sigma_hit）的高斯分布的距离概率。
-
~laser_model_type (string, default: "likelihood_field")：激光模型类型定义，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征–官网的注释），默认是“likehood_field” 。
（3）里程计模型参数
-
~odom_model_type (string, default: "diff")：odom模型定义，可以是"diff", "omni", "diff-corrected", "omni-corrected"，后面两个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小。
-
~odom_alpha1 (double, default: 0.2)：指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2（旋转存在旋转噪声）。
-
~odom_alpha2 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2（旋转中可能出现平移噪声）。
-
~odom_alpha3 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计平移的期望噪声，如果你自认为自己机器人的里程计信息比较准确那么就可以将该值设置的很小。
-
~odom_alpha4 (double, default: 0.2)：机器人运动部分的旋转分量估计的里程计平移的期望噪声，你设置的这4个alpha值越大说明里程计的误差越大。
-
~odom_alpha5 (double, default: 0.2)：平移相关的噪声参数（仅用于模型是“omni”的情况，就是当你的机器人是全向移动时才需要设置该参数，否则就设置其为0.0）
-
~odom_frame_id (string, default: "odom")：里程计默认使用的坐标系。
-
~base_frame_id (string, default: "base_link")：机器人的基坐标系。
-
~global_frame_id (string, default: "map")：由定位系统发布的坐标系名称。
-
~tf_broadcast (bool, default: true)：设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换。
0x06 AMCL中的坐标转换
首先我们来看一下在amcl中tf树，总共有多少个坐标系参与到这个过程中：
在amcl实现定位的一开始，从激光雷达获得测距信息，然后开始将laser_frame相对于base_link做tf转换，并且将永久保存该转换，这样就导致你安装在机器人上的激光雷达位置需要固定，不能激光雷达一下在移动底盘上，一会又跑到机器人头顶上，再过一会又跑机器人屁股上了，即保证laser_frame到base_link的转换是固定的才行，如下图所示来理解laser_frame测距信息与base_link之间转换关系：
接下来开始考虑如何实现定位的，下面来对比两种方法Odometry Localization和AMCL Map Localization：
我们一直再说轮式里程计来得到的测距信息会存在累计误差，我相信很多人都不太清楚为什么，这就跟汽车打滑差不多，如果只通过轮式里程计来计算移动距离的话就会出错：
在amcl中已经通过粒子滤波定位得到了base_frame相对应global_frame的位姿信息，但是为了保证base_frame只有一个父节点就没有来直接发布这个转换，因为base_frame的父节点已经是odom_frame了，那amcl就只能发布map_frame到odom_frame的转换了，这样还得到一个好处就是我们可以得到odom的累计误差了。
这是因为你可以想象一个场景，机器人直线往前走，通过轮式里程计得到结果应该走了10米，那么距离odom_frame就应该是10米了，但是由于原地打滑，实际上机器人通过amcl定位发现根本就没有动，那么amcl就需要发布map_frame到odom_frame的转换将这个10米的误差去除掉，这样才能维护整个tf树正常。
0x07 参考资料
[1].ROS WiKi上AMCL的主页[OL]. http://wiki.ros.org/amcl
[2].ros的navigation之amcl(localization)应用详解[OL]. https://blog.csdn.net/chenxingwangzi/article/details/50038413
[3].ros amcl参数设置[OL]. https://www.cnblogs.com/dyan1024/p/7825988.html