自适应蒙特卡洛定位-AMCL

自适应蒙特卡洛定位，简称AMCL，主要提供定位功能并以/tf形式输出

蒙特卡洛算法的基本思想：当所要求的问题是某种事件出现的概率或者是某个变量的期望值时，它们可以通过某种"试验"的方法，得到这种事件出现的概率，并用它们作为问题的解

粒子滤波

粒子滤波就是使用了蒙特卡洛思想的方法。把粒子滤波用在定位上就成为蒙特卡洛定位。其核心思想是通过一组随机采样粒子（即假设的状态）及其权重来近似后验概率分布，并通过重采样避免粒子退化。

粒子滤波实现的蒙特卡洛定位过程如下：

1. 初始化：生成初始粒子集，表示机器人可能的初始位姿分布，注意一般粒子要均匀分布在环境的自由空间（非障碍物区域）。如果有初始位姿（有先验，比如初始GPS或人工指定位置），则粒子集中在初始位姿附近的高斯分布中。这里，生成的粒子数量作为参数可调，粒子数量 NN 影响计算效率和定位精度
2. 权重计算：根据传感器观测数据（如激光雷达、深度相机）调整粒子权重，反映其与真实位姿的匹配程度。计算每个粒子的观测似然 p(zt∣xt(i))，即当前传感器数据在该粒子位姿 xt(i)xt(i)​ 下与地图的匹配程度。以雷达为例，对每个激光束，在地图中计算其端点与最近障碍物的距离误差，误差越小权重越高。
3. 重采样：淘汰低权重粒子，复制高权重粒子，避免粒子退化（即多数粒子权重趋近0）。抛弃旧粒子重新生成相同数量的新粒子，新粒子集中出现在权重大的旧粒子位置上，并且有大量的新粒子叠加在同一个位置。因为所有的新粒子都是重采样得到的，所以它们现在有相同的权重
4. 状态转移：根据机器人运动模型（如里程计）传播粒子，预测下一时刻的位姿分布。传感器不是绝对精准的，也就是存在误差，测出来的速度和方向是一个范围值，粒子在这些范围内随机取样，导致状态转移后的粒子不完全重叠
5. 状态转移后再进行权重计算阶段，重复过程234。加权平均或最高权重粒子作为最终位姿。
6. 不断重复上述过程，会在某一时刻完成定位，即大部分粒子都集中到了小车附近，这时小车移动，蒙特卡洛的粒子集中区域就会跟随移动，实现定位效果

由amcl定位的原理可以得知，刚开始时，粒子均匀撒在整个地图上，真正接近真实位姿的粒子可能很少。 小车还没运动或只运动了一点点，激光雷达/深度相机等传感器数据不足以让权重计算准确区分正确粒子。为了解决这个问题，可以在小车启动后先不运动，保持静止几秒钟，让传感器数据（如激光雷达）反复计算权重，淘汰错误粒子，但是如果环境特征不明显（比如空旷走廊），可能仍然不准。也可以在定位未收敛时，限制小车最大速度，让它缓慢运动，避免剧烈碰撞。

# 在move_base或局部规划器中设置
initial_planner:max_vel_x: 0.1  # 初始最大速度（通常0.5m/s，这里降到0.1）convergence_threshold: 0.8  # 粒子置信度>80%才全速运动

也可以启动时使用更多粒子（如5000→10000），等收敛后再减少。

amcl:initial_particles: 10000  # 初始粒子数after_convergence_particles: 2000  # 收敛后减少到2000

 

当机器人定位基本完成时，即这些粒子都集中在一块了，这个时候重采样的粒子就可以自适应的减少一些 。当遭遇劫持绑架(下面会介绍)，即粒子的平均分数(权重)突然降低，在全局重新撒一些粒子来重新找位置。上面所说的自适应调整粒子数量就是自适应解决的问题。

机器人定位分为三大问题：全局定位、位姿跟踪、绑架劫持。

• 全局定位：机器人刚启动，不清楚初始位置，这时靠粒子滤波等方法以及自身的运动来确定自己在地图中的位姿
• 位姿跟踪：已知自身的位姿，持续估计自身在地图中的实时位置和朝向
• 绑架劫持：已知自身的位姿，被人为的放到另一个环境中，这时上一时刻的位姿数据还在，粒子很集中，但是下一时刻计算权重时，发现所有的粒子与测量值不符(所有粒子权重都比较小)。 

官方提供的amcl功能包(包括在navigation中)输入为地图，雷达和小车自身的tf信息，最后以tf转换的形式输出定位信息 

参数 

以下为amcl_omni.launch文件，用于全向移动小车。这些参数取自navigation/amcl/cfg/AMCL.cfg。具体参数含义在最后

<launch><arg name="initial_pose_x" default="$(optenv initial_pose_x 0.0)"/><arg name="initial_pose_y" default="$(optenv initial_pose_y 0.0)"/><arg name="initial_pose_a" default="$(optenv initial_pose_a 0.0)"/>
<node pkg="amcl" type="amcl" name="amcl"><!-- Publish scans from best pose at a max of 10 Hz --><!--<param name="initial_pose_x" value="$(arg initial_pose_x)" />-->  <!-- 根据实际初始位置设置x坐标，单位米 --><!--<param name="initial_pose_y" value="$(arg initial_pose_y)" />-->  <!-- 根据实际初始位置设置y坐标，单位米 --><!--<param name="initial_pose_a" value="$(arg initial_pose_a)" />-->  <!-- 根据实际初始朝向设置角度，单位弧度 --><param name="odom_model_type" value="omni"/><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /> <!--0.1--><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="50"/><param name="use_map_topic" value="false"/>  <!-- //当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。在navigation 1.4.2中新加入的参数。 --><param name="first_map_only" value="true"/>  <!-- //当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图，在navigation 1.4.2中新加入的参数。 --><param name="min_particles" value="1000"/><param name="max_particles" value="10000"/><param name="kld_err" value="0.03"/><param name="kld_z" value="0.99"/><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"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><!-- <param name="laser_model_type" value="likelihood_field"/> --><param name="laser_model_type" value="beam"/><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.15"/> <!--0.2--><param name="update_min_a" value="0.3"/>  <!--0.5--><param name="odom_frame_id" value="odom"/><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><!-- <param name="odom_frame_id" value="odom"/>   --><!-- //里程计默认使用的坐标系 --><!-- <param name="base_frame_id" value="base_link"/>   --><!-- //用作机器人的基坐标系 --><!-- <param name="global_frame_id" value="map"/>   --><!-- //由定位系统发布的坐标系名称 --><!-- <param name="tf_broadcast" value="false"/>   --><!-- //设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换 -->
</node>
</launch>

amcl_diff.launch用于差速小车。 

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="12"/><param name="min_particles" value="1000"/><param name="max_particles" value="10000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><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"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><!-- <param name="laser_model_type" value="likelihood_field"/> --><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.5"/><param name="odom_frame_id"   value="odom"/><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><!--  <param name="odom_frame_id"   value="odom"/><param name="base_frame_id"   value="base_link"/><param name="global_frame_id" value="map"/><param name="scan_frame_id" value="laser_frame"/> --><!-- <param name="scan_topic" value="scan"/>  <remap from="scan"  to="lidar/scan"/> -->
</node>
</launch>

以下是我导航总的启动文件，可以看到对amcl launch文件的调用 

<launch><!-- Run the map server --><include file="$(find ucar_controller)/launch/base_driver.launch" > </include><include file="$(find ydlidar)/launch/ydlidar.launch" > </include><node name="map_server" pkg="map_server" type="map_server" args="$(find ucar_nav)/maps/yf.yaml" output="screen"><param name="frame_id" value="map" /></node> <!-- 启动AMCL重启控制器 --><!--<node name="restart_amcl" pkg="ucar_nav" type="restart_amcl.py" output="screen"/>--><include file="$(find ucar_nav)/launch/config/amcl/amcl_omni.launch" > </include><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/tra_local_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/local_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/laser_filters.yaml" command="load" /><remap from="scan" to="/raw_scan" />  <!-- 输入原始激光话题 --><remap from="scan_filtered" to="/scan" />  <!-- 输出过滤后话题 --></node><node pkg="rviz" type="rviz" name="rviz" args="-d $(find ucar_nav)/launch/config/rviz/rviz.rviz" /></launch>

 

 <--------里程计模型参数-------->
odom_model_type:
含义: 里程计模型类型。对于全向驱动机器人，设置为 omni。
调整建议: 如果机器人是全向驱动的，保持为 omni；如果是差分驱动的，改为 diff。odom_alpha1 到 odom_alpha5:
含义: 这些参数用于调整里程计模型的噪声。具体含义如下：
odom_alpha1: 旋转噪声（由于旋转引起的平移噪声）。
odom_alpha2: 平移噪声（由于平移引起的旋转噪声）。
odom_alpha3: 平移噪声（由于平移引起的平移噪声）。
odom_alpha4: 旋转噪声（由于旋转引起的旋转噪声）。
odom_alpha5: 旋转噪声（由于平移引起的旋转噪声）。
调整建议: 如果机器人的里程计噪声较大，可以适当增加这些值；如果噪声较小，可以减少这些值。<--------粒子滤波器参数-------->
min_particles:
含义: 粒子滤波器的最小粒子数量。
调整建议: 增加粒子数量可以提高定位精度，但会增加计算负载。默认值 1000 通常足够。
max_particles:
含义: 粒子滤波器的最大粒子数量。
调整建议: 增加粒子数量可以提高定位精度，但会增加计算负载。默认值 10000 通常足够。
kld_err:
含义: KLD（Kullback-Leibler Divergence）误差的上限，用于动态调整粒子数量。
调整建议: 减小该值会增加粒子数量，提高定位精度，但会增加计算负载。默认值 0.05 通常足够。
kld_z:
含义: KLD 的置信度参数。
调整建议: 增加该值会增加粒子数量。默认值 0.99 通常足够。<--------激光雷达参数-------->
laser_max_beams:
含义: 用于定位的激光雷达光束的最大数量。
调整建议: 增加该值可以提高定位精度，但会增加计算负载。默认值 50 通常足够。
laser_model_type:
含义: 激光雷达模型类型。beam 表示使用光束模型，likelihood_field 表示使用似然场模型。
调整建议: likelihood_field 模型通常更精确，但计算量更大。如果激光雷达数据质量较高，可以尝试使用 likelihood_field。
laser_z_hit:
含义: 激光雷达命中模型的权重。
调整建议: 增加该值可以提高对激光雷达命中数据的信任度。默认值 0.5 通常足够。
laser_z_short:
含义: 激光雷达短距离模型的权重。
调整建议: 增加该值可以提高对短距离数据的信任度。默认值 0.05 通常足够。
laser_z_max:
含义: 激光雷达最大距离模型的权重。
调整建议: 增加该值可以提高对最大距离数据的信任度。默认值 0.05 通常足够。
laser_z_rand:
含义: 激光雷达随机噪声模型的权重。
调整建议: 增加该值可以提高对随机噪声的容忍度。默认值 0.5 通常足够。
laser_sigma_hit:
含义: 激光雷达命中模型的标准差。
调整建议: 增加该值可以放宽对激光雷达数据的精度要求。默认值 0.2 通常足够。
laser_lambda_short:
含义: 激光雷达短距离模型的衰减系数。
调整建议: 增加该值可以提高对短距离数据的信任度。默认值 0.1 通常足够。
laser_likelihood_max_dist:
含义: 激光雷达似然场模型的最大距离。
调整建议: 增加该值可以扩大似然场的范围。默认值 2.0 通常足够。<--------更新参数-------->
update_min_d:
含义: 触发定位更新的最小平移距离（米）。当机器人移动的距离超过 update_min_d 时，amcl 会触发一次定位更新。
调整建议: 减小该值可以提高定位频率，但会增加计算负载。默认值 0.2 通常足够。
update_min_a:
含义: 触发定位更新的最小旋转角度（弧度）。当机器人旋转的角度超过 update_min_a(弧度) 时，amcl 会触发一次定位更新。
调整建议: 减小该值可以提高定位频率，但会增加计算负载。默认值 0.5 通常足够<--------其他参数-------->
transform_tolerance:
含义: 坐标系变换的容忍时间（秒）。它定义了在查询坐标变换时，允许的最大时间偏差。如果坐标变换的时间戳与当前时间的偏差超过 transform_tolerance，系统会认为该变换无效。
调整建议: 增加该值可以容忍更大的时间偏差。默认值 0.1 通常足够。
resample_interval:
含义: 重采样间隔（更新次数）。用于控制 粒子滤波器的重采样频率。它的作用是决定 AMCL 在多少次更新后才对粒子进行重采样。合理设置 resample_interval 可以提高定位的精度和效率。
调整建议: 增加该值可以减少重采样频率，降低计算负载。resample_interval 设置为 1，则每次更新后都会进行重采样。
recovery_alpha_slow 和 recovery_alpha_fast:
含义: 用于恢复行为的慢速和快速衰减系数。它们的作用是控制 AMCL 在定位失败或丢失时如何调整粒子滤波器的分布，以尝试重新定位机器人。
recovery_alpha_slow控制 AMCL 在定位失败时，缓慢增加粒子分布的随机性。这个过程较慢，适合在定位失败初期使用。取值范围：0.0 到 1.0。
recovery_alpha_fast控制 AMCL 在定位失败时，快速增加粒子分布的随机性。当机器人定位失败较严重时，AMCL 会快速增加粒子的随机性，以尝试快速覆盖更大的区域。这个过程较快，适合在定位失败较严重时使用。
recovery_alpha_slow适用场景：定位失败较轻（例如机器人位置稍有偏差）。环境变化较小（例如机器人移动到一个已知区域但定位稍有偏差）。不会显著改变粒子分布，避免过度随机化导致定位结果不稳定。
recovery_alpha_fast适用场景：定位失败较严重（例如机器人完全丢失定位）。环境变化较大（例如机器人被移动到未知区域）。快速覆盖更大的区域，增加重新定位的可能性。
调整建议: 默认情况下，recovery_alpha_slow 和 recovery_alpha_fast 都设置为 0.0，即禁用恢复行为。这是因为恢复行为可能会引入额外的计算开销，且在某些场景下并不需要。