0、关于
利用蒙特卡罗定位（MCL）算法完成机器人定位，此方法是一种在2D环境下移动机器人的概率统计方法,这种方法在ROS系统中的具体实现是通过在已知地图的基础上使用粒子滤波算法跟踪机器人的位姿,包括程序解读以及算法说明两部分.
- 加载地图模型
- 加载里程计与激光雷达数据
- 使用粒子滤波算法-跟踪机器人的位姿
- 蒙特卡洛定位算法
1、加载地图模型 load_map.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| class LoadMap
{
public:
LoadMap(const char* map_str);
~LoadMap();
int ReadFromData(const char* map_name);//从wean.data中读取数据
void PublishMap(ros::NodeHandle node_);//将地图转换格式,并发布topic在rviz中显示
map_type* GetMap(); //将地图传递到类外
private:
map_type* map;
ros::Publisher map_pub;
ros::Publisher map_meta_pub;
};
|
函数说明
1
2
| int ReadFromData(const char* map_name); //读取地图数据并给地图上所有有效点概率赋值,框出有效点范围
void PublishMap(ros::NodeHandle node_); //发布地图数据 给nav_msgs/msg/OccupancyGrid 节点参数赋值
|
2、加载里程计与激光雷达数据 load_log.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
| class LoadLog
{
public:
LoadLog(const char* log_str);
~LoadLog();
int ReadFromData(const char* logfile, vector<log_data>& logfile_data);
vector<log_data> GetLog();
void ShowLogData();
private:
vector<log_data> log;
};
|
函数说明
1
2
| int ReadFromData(const char* logfile, vector<log_data>& logfile_data); //读取传感器数据
void ShowLogData(); //终端显示
|
3、粒子滤波算法**
1-粒子集初始化
在前面(1)所框出的有效地图:map->prob[x][y] 内随机生成坐标点(x,y),同时生成粒子的角度theta和权重weight,
1
2
3
4
5
6
7
8
9
10
11
12
13
14
| particle_temp.x = rand() / (float)RAND_MAX * (map_->max_x - map_->min_x) + map_->min_x; //初始化粒子X坐标
particle_temp.y = rand() / (float)RAND_MAX * (map_->max_y - map_->min_y) + map_->min_y; //初始化粒子Y坐标
if (map_->prob[(int) particle_temp.x][(int) particle_temp.y] <=map_threshold_) //若随机出的粒子不在地图有效范围内,则重新生成粒子
continue;
count++;
//.h
vector<particle_state> particles_;//存放 particle_state 结构的粒子集
//.cpp
particles_.push_back(particle_temp);//由先验概率随机生成的点(particle_temp.x,particle_temp.y)存入粒子集
|
2-基于里程计运动模型的采样算法
概率机器人P103
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
| particle_state PFLocalization::SampleMotionModelOdometry(particle_state particle)
{
float deltarot1 = atan2(motion_.y_rear - motion_.y_front,motion_.x_rear - motion_.x_front) - motion_.theta_rear;
float deltatrans1 = sqrt(pow((motion_.x_rear - motion_.x_front),2) + pow((motion_.y_rear - motion_.y_front),2));
float deltarot2 = motion_.theta_rear - motion_.theta_front - deltarot1;
// float deltarot1_hat = deltarot1 - SampleStandardNormalDistribution(alpha1_*deltarot1 + alpha2_*deltatrans1);
// float deltatrans1_hat = deltatrans1 - SampleStandardNormalDistribution(alpha3_*deltatrans1 + alpha4_*(deltarot1 + deltarot2));
// float deltarot2_hat = deltarot2 - SampleStandardNormalDistribution(alpha1_*deltarot2 + alpha2_*deltatrans1);
float deltarot1_hat = deltarot1 - SampleStandardNormalDistribution(alpha1_*deltarot1*deltarot1 + alpha2_*deltatrans1*deltatrans1);
float deltatrans1_hat = deltatrans1 - SampleStandardNormalDistribution(alpha3_*deltatrans1*deltatrans1 + alpha4_*(deltarot1*deltarot1 + deltarot2*deltarot2));
float deltarot2_hat = deltarot2 - SampleStandardNormalDistribution(alpha1_*deltarot2*deltarot2 + alpha2_*deltatrans1*deltatrans1);
particle_state particle_temp;
//地图是以dm为单位,初始化的粒子位置是基于地图生成的,所以也是dm单位,而里程计数据单位是cm,需在这里进行单位转换
particle_temp.x = particle.x + deltatrans1_hat * cos(particle.theta + deltarot1_hat);
particle_temp.y = particle.y + deltatrans1_hat * sin(particle.theta + deltarot1_hat);
particle_temp.theta = particle.theta + deltarot1_hat + deltarot2_hat;
particle_temp.weight = particle.weight;
return particle_temp;
}
//从标准正态分布中采样
float PFLocalization::SampleStandardNormalDistribution(float var)
{
float sum = 0;
for (int i = 0;i < 12; i++)
//LO + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(HI-LO)))
sum += (rand() - RAND_MAX / 2) / (float)RAND_MAX * 2;
return (var / 6.0) * sum;
}
|
4、蒙特卡洛定位算法
n、MOOC-AMCL-demo.launch注释
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
| <launch>
<arg name="use_map_topic" default="false"/> <!-- 当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。 -->
<arg name="scan_topic" default="scan"/> <!-- laser data-->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="odom"/> <!-- odom -->
<arg name="base_frame_id" default="base_footprint"/> <!-- base -->
<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"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/> <!-- 扫描和路径发布到可视化软件的最大频率，设置参数为-1.0意为失能此功能，默认-1.0 -->
<param name="laser_max_beams" value="810"/> <!--30 更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准） -->
<param name="laser_max_range" value="-1"/> <!-- 被考虑的最大扫描范围；参数设置为-1.0时，将会使用激光上报的最大扫描范围 -->
<param name="min_particles" value="500"/> <!-- 最小粒子数 -->
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/> <!-- 使得真实的后验与基于采样的近似之间的误差小于ε（ε就是kld_err配置参数） -->
<param name="kld_z" value="0.99"/> <!-- KLD_Sampling_MCL随时间改变粒子数，改良了度过初期后的蒙特卡洛大样本集合的资源浪费,KLD采样以概率1-δ确定样本数（1-δ就是kld_z配置参数） -->
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<!--
这4个laser_z参数，在动态环境下的定位时用于异常值去除技术（还有一种状态增广技术-将隐藏状态包含进状态估计，缺点是计算复杂，acml定位未使用这种定位）
这种定位思想是环境中的动态物体总是会获得比静态地图障碍物更短的读数（人在障碍物后面是扫描不到的-假如不考虑体积，比如单个激光光束不用考虑体积），利用这样的不对称性去除异常值
缺点是：在其他可改变环境的其他类型情景（如去除障碍物）时，这样的非对称性可能不存在，但相同概率分析通常是可适用的。因为每一个异常值都被舍弃了，缺少对称性的缺点可能是从全局定位失效中恢复变得不可能。这种情况下，×××强加额外约束（如限制部分可能已被破坏的测量值）是有意义的（×××怎么约束）。（这里说的舍弃与likelihood_field模型的舍弃有区别，这里定位是先计算测量值对应非预期物体的概率（意外对象概率/混合概率）大于用户设定的阀值（amcl配置参数里貌似没有这个？）舍弃，而似然域概率是舍弃的超出最大测量范围的值，不计算概率。）
（针对这个缺点不想改代码的粗暴又好用省心的处理方式可能是构图的时候将可移动的障碍物搬走，还有更直接的办法就是PS）
最后，概率由这4个权重乘他们对应的概率然后相加，算法中4个权重相加等于1(这里默认值不等于1，估计做了归一化)。
这6个laser_参数可以用learn_intrinsic_parameters算法计算，该算法是期望值极大化算法，是估计极大似然参数的迭代过程。（×××好吧，amcl好像并没有做这个工作）-->
<param name="laser_z_hit" value="0.5"/> <!-- //模型的z_hit部分的混合权值，默认0.95(混合权重1.具有局部测量噪声的正确范围以测量距离近似真实距离为均值，其后laser_sigma_hit为标准偏差的高斯分布的权重) -->
<param name="laser_z_short" value="0.05"/> <!-- //模型的z_short部分的混合权值，默认0.1（混合权重2.意外对象权重（类似于一元指数关于y轴对称0～测量距离（非最大距离）的部分：ηλe^(-λz)，其余部分为0，其中η为归一化参数，λ为laser_lambda_short,z为t时刻的一个独立测量值（一个测距值，测距传感器一次测量通常产生一系列的测量值）），动态的环境，如人或移动物体） -->
<param name="laser_z_max" value="0.05"/> <!-- //模型的z_max部分的混合权值，默认0.05（混合权重3.测量失败权重（最大距离时为1，其余为0），如声呐镜面反射，激光黑色吸光对象或强光下的测量，最典型的是超出最大距离） -->
<param name="laser_z_rand" value="0.5"/> <!-- //模型的z_rand部分的混合权值，默认0.05（混合权重4.随机测量权重均匀分布（1平均分布到0～最大测量范围），完全无法解释的测量，如声呐的多次反射，传感器串扰） -->
<param name="laser_sigma_hit" value="0.2"/> <!-- //被用在模型的z_hit部分的高斯模型的标准差，默认0.2m -->
<param name="laser_lambda_short" value="0.1"/> <!-- //模型z_short部分的指数衰减参数，默认0.1（根据ηλe^(-λz)，λ越大随距离增大意外对象概率衰减越快） -->
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_model_type" value="beam"/> <!-- //模型使用，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征官网的注释），默认是“likehood_field” -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.1"/> <!-- 在执行滤波更新前平移运动的距离 -->
<param name="update_min_a" value="0.2"/> <!-- 执行滤波更新前旋转的角度 rad -->
<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="1"/> <!-- 在重采样前需要的滤波更新的次数,默认2 -->
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/> <!-- tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的 默认0.1 -->
<param name="recovery_alpha_slow" value="0.0"/> <!-- 慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值 -->
<param name="recovery_alpha_fast" value="0.0"/> <!-- 快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值 -->
<param name="initial_pose_x" value="$(arg initial_pose_x)"/> <!-- base_link init -->
<param name="initial_pose_y" value="$(arg initial_pose_y)"/> <!-- base_link init -->
<param name="initial_pose_a" value="$(arg initial_pose_a)"/> <!-- base_link init -->
<remap from="scan" to="$(arg scan_topic)"/>
<!--
//里程计模型参数 ×××里程计模型并没有涉及机器人漂移或打滑的情况，一旦出现这样的情况，后续定位基本废了，虽然Augmented_MCL有失效恢复，但是实际运行中耗时太长且结果不太理想（位置居然跳，这很不合理，可能参数配置不太好）
<param name="odom_model_type" value="diff"/> //模型使用，可以是"diff", "omni", "diff-corrected", "omni-corrected",后面两 个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小
<param name="odom_alpha1" value="0.2"/> //指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2（旋转存在旋转噪声）
<param name="odom_alpha2" value="0.2"/> //制定由机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2（旋转中可能出现平移噪声）
<param name="odom_alpha3" value="0.8"/> //指定由机器人运动部分的平移分量估计的里程计平移的期望噪声，默认0.2（类似上）
<param name="odom_alpha4" value="0.2"/> //指定由机器人运动部分的旋转分量估计的里程计平移的期望噪声，默认0.2（类似上）
<param name="odom_alpha5" value="0.1"/> //平移相关的噪声参数（仅用于模型是“omni”的情况wiki官网的注释）
<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="true"/> //设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换
//机器人初始化数据设置
<param name="initial_pose_x" value="0.0"/> //初始位姿均值（x），用于初始化高斯分布滤波器。（initial_pose_参数决定撒出去的初始位姿粒子集范围中心）
<param name="initial_pose_y" value="0.0"/> //初始位姿均值（y），用于初始化高斯分布滤波器。（同上）
<param name="initial_pose_a" value="0.0"/> //初始位姿均值（yaw），用于初始化高斯分布滤波器。（粒子朝向）
<param name="initial_cov_xx" value="0.5*0.5"/> //初始位姿协方差（x*x），用于初始化高斯分布滤波器。（initial_cov_参数决定初始粒子集的范围）
<param name="initial_cov_yy" value="0.5*0.5"/> //初始位姿协方差（y*y），用于初始化高斯分布滤波器。（同上）
<param name="initial_cov_aa" value="(π/12)*(π/12)"/> //初始位姿协方差（yaw*yaw），用于初始化高斯分布滤波器。（粒子朝向的偏差） -->
|