AMCL原理概念
AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位，A也可以理解为augmented，是机器人在二维移动过程中概率定位系统，采用粒子滤波器来跟踪已经知道的地图中机器人位姿，对于大范围的局部定位问题工作良好。对机器人的定位是非常重要的，因为若无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的。详细原理参考链接
AMCL配置文件
<launch>
<!--当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说当设置为true时，有另外一个节点实时的发布map话题，
也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。-->
<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan"/> <!--扫描话题-->
<arg name="initial_pose_x" default="0.0"/> <!--初始位姿均值x,用于初始化高斯分布滤波器-->
<arg name="initial_pose_y" default="0.0"/> <!--初始位姿均值y,用于初始化高斯分布滤波器-->
<arg name="initial_pose_a" default="0.0"/> <!--初始位姿均值（yaw）,用于初始化高斯分布滤波器-->
<arg name="odom_frame_id" default="odom"/> <!--里程计默认坐标-->
<arg name="base_frame_id" default="base_link"/> <!--机器人基坐标-->
<arg name="global_frame_id" default="map"/> <!--全局坐标-->
<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/> <!--选择odom模型，diff差速模型，omni全向模型-->
<param name="odom_alpha5" value="0.1"/> <!--移相关的噪声参数（仅用于模型是“omni”的情况,默认0-->
<param name="gui_publish_rate" value="10.0"/> <!--扫描和路径发布到可视化软件的最大频率-->
<param name="laser_max_beams" value="120"/> <!--更新滤波器时，每次扫描中多少个等间距的光束被使用-->
<param name="laser_max_range" value="80.0"/> <!--最大扫描范围，参数设置为-1.0时，将会使用激光上报的最大扫描范围-->
<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"/> <!--上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率-->
<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"/> <!--模型的z_hit部分的混合权值-->
<param name="laser_z_short" value="0.05"/> <!--模型的z_short部分的混合权值-->
<param name="laser_z_max" value="0.05"/> <!--模型的z_max部分的混合权值-->
<param name="laser_z_rand" value="0.5"/> <!--模型的z_rand部分的混合权值-->
<param name="laser_sigma_hit" value="0.2"/> <!--被用在模型的z_hit部分的高斯模型的标准差-->
<param name="laser_lambda_short" value="0.1"/> <!--模型z_short部分的指数衰减参数-->
<param name="laser_model_type" value="likelihood_field"/> <!--激光模型类型定义，可以是beam, likehood_field, likehood_field_prob-->
<!-- <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.4"/> <!--执行滤波更新前旋转的角度-->
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="0.5"/> <!--在重采样前需要滤波更新的次数-->
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="0.1"/> <!--tf变换发布推迟的时间-->
<param name="recovery_alpha_slow" value="0.0"/> <!--慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover-->
<param name="recovery_alpha_fast" value="0.0"/> <!--快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover-->
<<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
对于各个参数解释参考Nav2中文网
http://dev.nav2.fishros.com/doc/configuration/packages/configuring-amcl.html
关于amcl定位模块wiki上有详细说明 http://wiki.ros.org/amcl
订阅的主题
scan (sensor_msgs/LaserScan)
amcl 需要默认订阅的topics 可以在diff.launch 重映射一下<remap from="scan" to="filter_scan">
发布的主题
amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
amcl通过粒子滤波后 估计出机器人最佳的位姿 （x,y,theta） 以及各分量的方差 ***amcl 最终定位结果输出***
particlecloud (geometry_msgs/PoseArray)
粒子云 用于rviz可视化显示
tf (tf/tfMessage)
整个amcl 定位结果体现在这里，这个tf发布的是 odom坐标系(机器人开机那个坐标原点) 在map坐标系下的转换
amcl 定位结果与其他模块是通过发布 doom 与map 的tf转换关系发生联系
其中调参用的多参数
dom_model_type (string, default: "diff") 里程计模型 可选 "diff", "omni", "diff-corrected" or "omni-corrected".
diff：2轮差分 omni：全向轮 y方向有速度
min_particles，max_particles
控制AMCL节点中使用的粒子数。 增加粒子的数量增加了精度，但由于计算量的增加，它也减慢了计算速度。原则上越大，定位精度越好，但会影响占用的CPU。
AMCL收敛速度不够快，在初值比较好的情况下，如果设置min_particles为300，max_particles为5000，重采样需要14~20次才能收敛到300个粒子。如果设置得更小，收敛速度就更慢。 重采样几次时，有的粒子权重还是另一些粒子权重的三四倍。
min_particles不能太大，应该比pf_resample_limit函数返回值小，否则set_b偏大，位姿不够精确。 我见过pf_resample_limit最小返回值是66，一般都大于100，所以min_particles取100即可。
max_particles不能太小，一方面难以筛选权重大的粒子，另一方面在初值不太准确时，粒子集不能有效覆盖真实初值的的周边。但也不能太大，否则占用CPU上升。经过反复试验，max_particles取3000，此时AMCL占用CPU 2%左右
kld_err: 初始粒子真实分布和估计分布之间的最大误差，默认0.01
kld_z: 上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率，默认0.99，但实际在pf_alloc为3
recovery_alpha_slow: 慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值
recovery_alpha_fast: 快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.1是个不错的值
这两个参数在AMCL失效恢复中，用于短期和长期的似然平均，要求 0 < alpha_slow << alpha_fast，用于计算w_slow和w_fast，最终判断是否触发recover机制，增加随机位姿， 用以削弱粒子滤波器的粒子匮乏问题的影响。
update_min_d : 在执行滤波更新前平移运动的最小距离，默认0.2m，可改为0.05
update_min_a: 执行滤波更新前旋转的最小角度，单位弧度，默认 pi/6
设置更新粒子滤波器所需的最小平移距离(m)和旋转角(rad）。 使用较小的值可以更频繁地进行更新，但计算量也会增加。
resample_interval : 在重采样前需要等待的滤波次数，默认2。越大，重采样频率越低
<!--初始位姿均值（x），用于初始化高斯分布滤波器。0.0 -->
<param name="initial_pose_x" value="-3.13841"/>
<!--初始位姿均值（y），用于初始化高斯分布滤波器。 0.0-->
<param name="initial_pose_y" value="2.20967"/>
<!--初始位姿均值（yaw），用于初始化高斯分布滤波器。 -->
<param name="initial_pose_a" value="2.611510"/>
<!--初始位姿协方差（x*x），用于初始化高斯分布滤波器。 -->
<param name="initial_cov_xx" value="0.5*0.5"/>
<!--初始位姿协方差（y*y），用于初始化高斯分布滤波器。 -->
<param name="initial_cov_yy" value="0.5*0.5"/>
<!--初始位姿协方差（yaw*yaw），撒粒子的方差，使粒子均匀的围绕初始化点,用于初始化高斯分布滤波器 -->
<param name="initial_cov_aa" value="(π/12)*(π/12)"/>
初始化位姿，初始点位姿参数有6个，均在handleMapMessage函数中使用，最终是在 pf_pdf_gaussian_sample函数。每次电脑重启后，在同一个点的坐标是不同的，每次都要手动定位。参考链接
粒子收敛会使得covariance参数变小：
covariance是一个6x6的协方差矩阵，6个状态量是x,y,z,绕x轴的旋转(roll),绕y轴的旋转(pitch),绕z轴的旋转(yaw)。矩阵对角线的6个元素就是6个状态量的方差，由于z,pitch,roll都是0，所以只看第1,2和最后的元素，也就是矩阵的第1,8和最后的元素，分别是x,y,yaw的方差
在pf_pdf_gaussian_alloc函数中，对协方差矩阵进行了特征值分解。也就是协方差矩阵cx=cr cd crT ，cd是对角矩阵，取cd的三个对角元素开平方后，传入pf_ran_gaussian函数，作为set_a粒子集的初始化的标准差。yaw的方差在对角矩阵cd里不变，cd的最后一个元素仍然是initial_cov_aa
主要是前三项参数，在建好地图后，选择一个启动点的坐标，将朝向转换获得yaw，以后每次启动机器人时，都要让机器人在这个位姿。因为每次启动move_base.launch后，只调整机器人位姿无法定位准确，还需要rviz手动定位，有了这个初始位姿之后，不必rviz手动定位了，只调整机器人位姿就可以了，调整方式是先让机器人到某个点。
在机器人移动时，才会发布/amcl_pose的消息，类型为geometry_msgs/PoseWithCovarianceStamped，也就是带协方差的消息。robot_pose_publisher发布了/base_link坐标系和/map坐标系的转换， 一般放在机器人开机启动的节点里，发布话题robot_pose(geometry_msgs/Pose)。常用这两个话题获取机器人当前位置
在远离初始点的位置,执行rosparam get amcl/initial_pose_x，发现它的值实际是当前的位姿x坐标， 而不是amcl.launch中的参数了，显然是参数服务器在更新这个参数
如何接受地图数据：
use_map_topic: 导航时设置为false。 当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图
first_map_only: 用于mapReceived函数，可以不看。当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图，导航时设置为false
transform_tolerance: tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的，默认0.1
gui_publish_rate: 扫描和路径发布到可视化软件的最大频率，默认参数-1.0表示disable，可以设置为10
save_pose_rate: 存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。默认参数-1.0表示disable，可以用0.5
激光模型参数，都在测量模型中使用
laser_model_type: 模型使用，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征），默认是 likehood_field
laser_min_range: 被考虑的最小扫描范围；默认参数-1.0，将会使用激光上报的最小扫描范围
laser_max_range: 被考虑的最大扫描范围；默认参数-1.0时，将会使用激光上报的最大扫描范围
laser_max_beams: 更新滤波器时，每次扫描中多少个等间距的光束被使用，默认30
laser_z_hit: 似然域模型的高斯噪声的最大权值，默认0.95
laser_z_rand: 似然域模型的随机噪声的最大权值，默认0.05
laser_sigma_hit: 高斯噪声模型的标准差，默认0.2m
以下几个参数不在似然域模型中使用
laser_z_short: 模型的z_short部分的最大权值，默认0.1
laser_z_max: 模型的z_max部分的最大权值，默认0.05
laser_lambda_short: 模型z_short部分的指数衰减参数，默认0.1
laser_likehood_max_dist: 地图上做障碍物膨胀的最大距离，用作likehood_field模型，默认2m
最重要的是里程计误差模型参数
AMCL中里程计模型参考链接
假如对AMCL算法不清楚第一眼看过去大概就会有点懵。然后就会去翻AMCL的算法原理，一看发现原来在《概率机器人》中文版一书上讲过，但书上没有特别直接，所以还是得看程序具体怎么写的。但程序虽然是最直接的，为了理解程序最好还是要从算法原理上有一点基础的认识，所以这一小节简单概括一下里程计模型以及其采样算法，算法伪代码可以参考《概率机器人》书上103页。
里程计diff模型，也就是最常见的差速模型，两平行轮作驱动，另外附加一个万向轮作支撑。在AMCL算法中将机器人的每一步的广义运动进行了拆解，拆解成包含三个动作的序列，即：a.在起始点旋转，转向终止点的方向；b.沿着该方向做直线运动到终止点；c.在终止点进行旋转，转到目标方向，其运动示意图如下所示。学过机器人学大的会问，平面上的刚体运动可以拆分为一个旋转和一个平移，为什么这个要做3次运动。这个忽略了一个大前提，就是平面上的刚体有3个自由度：x、y、yaw，但是差速模型机器人虽然可以抵达平面上的任意位置和姿态，但是并不具备3个自由度的概念。想一下，差速模型的机器人只能直行和转弯，并不能沿着驱动轮的轴线进行运动吧？而且这用做的一大好处是可以提取相对距离便于定位校核的。
其中，
代表在起始点的旋转，
代表着第二段平移过程，
代表着在终止点的旋转
里程计噪音
要讨论里程计噪音，首先来讨论一下怎么样里程记没有噪声，也就是说，里程计读轮子的转圈和机器人的位移是精确对应的。当然理论上和仿真中可以做到：首先假设轮子和地面之间不打滑、地面绝对平整且光滑、轮子绝对圆润且不变形、里程计和机器人轮轴以及机器人轮子之间没有摩擦和迟滞可以做到完全时间同步…等等。当然现实中做不到，所以在差速模型的里程记计算中需要对机器人进行噪音估计，因为我们读到的直很可能不是真值。
在AMCL中，里程计是作为状态预测器存在的，通过接受当前的控制信号，从上一帧机器人状态对这一帧机器人状态进行预测，并与当前观测所得结果对当前预测进行加权打分。所以通过输入和输出我们知道，里程计在AMCL中的作用就是根据当前控制信号更新上一帧的能表征机器人状态的粒子集合。在AMCL中关于odom的伪代码如下所示：
详细理论代码分析参考
https://blog.csdn.net/qq_17032807/article/details/97650380?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167030841116800180610007%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=167030841116800180610007&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-1-97650380-null-null.142^v67^wechat_v2,201^v4^add_ask,213^v2^t3_control1&utm_term=odom_alpha2%E6%80%8E%E4%B9%88%E7%A1%AE%E5%AE%9A&spm=1018.2226.3001.4187
AMCL中轮式里程计误差模型参数
odom_alpha1，odom_alpha2，odom_alpha3，odom_alpha4这些如何参数如何根据我们轮子的精度来给出理论上比较准的值参考链接
odom_alpha1 考虑里程计只有旋转累计误差，那么理论上AMCL中的里程计实现粒子预测更新应该主要只表现粒子整体横向变胖。增大该值，机器人发生有旋转运动时，就会出现扇形噪声粒子云。
odom_alpha2 位移的变化引起角度变化的系数。假如我只控制小车直行，理论上小车运行是一条直线（此时odom_alpha2=0.0）但由于底盘精度，小车运行轨迹可能是条斜线（odom_alpha2=0.04）。
odom_alpha3考虑里程计只有位移累计误差，那么理论上AMCL中的里程计实现粒子预测更新应该只表现粒子整体纵向拉长。平移分量估计的里程计平移的期望噪声。
第一张图初始生成的粒子，角度的误差给的很小，所以看起来像🏹。
第二张图直线行走，更新的粒子逐渐被拉长，表示位移的误差越来越多，此处给的odom_alpha3 = 0.04，折算到我们通常说的轮式里程计累计误差在20%（别忘了代码中的odom_alpha3有求开根号的），基本满足我们理论中的预设。其中还是变胖了，主要是即使角度误差为0作为高斯分布的标准差，也会产生不为0 的误差。
odom_alpha4 (double, default: 0.2) 机器人运动部分的旋转分量估计的里程计平移的期望噪声。 斜角方向上的运动噪声 0.5参数如下图参考链接
dom_alpha5 (double, default: 0.2) 该参数只对全向运动模型有用，理解为y方向的位移误差。
odom_frame_id: 里程计默认使用的坐标系，默认odom
base_frame_id: 机器人的基坐标系，默认base_link
global_frame_id: 由定位系统发布的坐标系名称，默认map
tf_broadcast: 设置为false会阻止amcl发布全局坐标系和里程计坐标系之间的tf变换，一般为true
问题总结
amcl全局定位失败的解决办法参考链接
（1）调用global_localization (std_srvs/Empty)服务
通过调用该服务将初始化全局定位，将所有的粒子重新打散随机的分散在地图的空闲地方，可以通过以下命令来调用该服务，当调用后可以发现所有的粒子被重新打散随机分布在地图上，然后使用键盘遥控机器人旋转，这样可以自动重新对机器人进行定位
rosservice call /global_localization "{}"
（2）调用request_nomotion_update (std_srvs/Empty)服务
该服务是手动的来更新粒子并发布新的粒子，可以使用如下命令来调用服务执行更新粒子的操作，该服务一般需要多次调用才能逐渐看到粒子收敛的效果
rosservice call /request_nomotion_update “{}”
（3）清理代价地图
rosservice call / move_base / clear_costmaps“{}”
（4）机器人再进行旋转重定位
里程计的精度在AMCL中很重要
假如里程计精度不高，那么对应的参数odom_alpha值就大，这从理论上就表明你的下一可能位置分散程度就大（好比落在一个半径1m的圆内），假如里程计精度高，那么对应的参数odom_alpha值就小，这从理论上就表明你的下一可能位置分散程度就小（好比落在一个半径0.1m的圆内），然后你都是用min_particles个粒子来描述这两个圆，那1m圆内出现更接近真实位姿的粒子可能性小很多，久而久之，有可能出现粒子退化，定位跟踪失败。（此情况下，或许你可以在odom预测阶段增加粒子）。
假如你的AMCL运行过程中，激光SCAN不能完全跟踪地图，总是差点，不一定是里程计误差模型的问题，有可能只是你的update_min_d update_min_a这两个参数不够小。
AMCL出现定位抖动，很可能是里程计误差
2029
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
