沉默杀手HomeArchivesTagsCategories
amcl的话题和参数
|激光SLAMamcl和粒子滤波|
Word count: 3k|Reading time: 9 min

AMCL定位偶尔会出现定位不准的情况，推测是由于工控机性能不足(粒子滤波很耗时)或地图上大特征较少/小特征较多(其总体结果有随机分布倾向)，造成AMCL无法及时定位

AMCL中，粒子很分散，定位置信度就很低。如果置信度太低，定位不准，可以停掉导航模块，先通知上层进行重定位。

amcl用到的话题和服务

话题

机器人稍微运动后，amcl节点开始读取scan,map,tf话题上的数据， 在重采样之后发布话题 amcl_pose， 平时小车不发布位姿坐标的，而重采样的触发条件是里程计显示小车参数update_min_d或者旋转参数update_min_a。我平时获取小车位姿用的是话题robot_pose，它来自robot_pose_publisher包，发布频率是10Hz

rviz中的particlecloud话题:

消息类型是geometry_msgs/PoseArray，由/amcl发布，被rviz订阅。作用是在rviz中显示机器人的可能位置，就是这样的一坨:


amcl还订阅了一个话题initialpose，但是我发现它没有发布者，看来是供用户发布的。

amcl_pose 带协方差的位姿.png

rviz中的amcl_pose.png

服务

服务global_localization，类型为std_srvs/Empty，初始化全局定位，使所有粒子在地图的自由空间上随机分散，也就是像这样：

call这个服务其实就相当于重新初始化了粒子位置，用于amcl的重新定位，但是我尝试移动机器人，定位经常是错误的。


服务static_map，类型nav_msgs/GetMap，它是由map_server发布的，用于获得地图数据进行定位。所以说amcl离不开map_server，其消息成员：

1
2
---
nav_msgs/OccupancyGrid map

很简单，请求为空，响应就是地图数据

request_nomotion_update服务，该服务是手动的来更新粒子并发布新的粒子，可以使用如下命令来调用服务执行更新粒子的操作，该服务一般需要多次调用才能逐渐看到粒子收敛的效果：

1
rosservice call request_nomotion_update "{}"

本质上是让m_force_update为true，进行强制粒子滤波

机器人准确定位

机器人定位需要的信息:

  • 雷达扫描数据
  • 里程计数据
  • 扫描地图数据(不是静态地图)
  • odom->base_link->laser的tf变换


初始点位姿参数有6个，均在handleMapMessage函数中使用，最终是在 pf_pdf_gaussian_sample函数

每次电脑重启后，在同一个点的坐标是不同的，每次都要手动定位。可以在amcl.launch中设置机器人初始化数据。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
<!--初始位姿均值（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)"/>

粒子收敛会使得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中的参数了，显然是参数服务器在更新这个参数

amcl参数

蓝色的是需要自己修改的

滤波器参数

handleMapMessage函数中的pf_alloc所用：

  • min_particles和max_particles 是允许的粒子数量的最小值和最大值，也就是在rviz里看到的箭头数量。分别对应程序里的变量min_samples和max_samples。前者默认100，后者默认5000。原则上越大，定位精度越好，但会影响占用的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

重采样所用：

  • resample_interval : 在重采样前需要等待的滤波次数，只能是大于1的整数，默认2。越大，重采样频率越低

源码里对应：

1
2
3
4
5
if(  !(++resample_count_ % resample_interval_)  )
{
  pf_update_resample(pf_);
  resampled = true;
}

这段在laserReceived回调里，resample_count_初始化为0，每次回调一次就增加一次，所以resample_interval越大，重采样频率越低。

如何接受地图数据：

  • 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

里程计模型参数，都在运动模型部分使用

  • odom_model_type: 模型使用，默认diff，还可以是omni(全向轮), diff-corrected, omni-corrected，后面两个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小。一般用diff

  • odom_alpha1 : 指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2。增大该值，机器人发生有旋转运动时，就会出现扇形噪声粒子云
    odom_alpha1
  • odom_alpha2: 指定由机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2
    odom_alpha2
  • odom_alpha3: 指定由机器人运动部分的平移分量估计的里程计平移的期望噪声，默认0.2。纵向分量运动噪声，沿着机器人前进方向分布
    odom_alpha3
  • odom_alpha4: 指定由机器人运动部分的旋转分量估计的里程计平移的期望噪声，默认0.2。斜角方向上的运动噪声
    odom_alpha4

  • odom_alpha5: 平移相关的噪声参数（仅用于模型是omni的情况），默认0.1

  • odom_frame_id: 里程计默认使用的坐标系，默认odom

  • base_frame_id: 机器人的基坐标系，默认base_link
  • global_frame_id: 由定位系统发布的坐标系名称，默认map
  • tf_broadcast: 设置为false会阻止amcl发布全局坐标系和里程计坐标系之间的tf变换，一般为true

参数：geometry_msgs/PoseWithCovariance