AMCL包接收地图，建立运动模型使得粒子可以跟着机器人一起运动，建立观测模型对模拟机器人运动的粒子进行打分，并且建立相关的tf变换信息，最后输出位姿。接收地图部分在上一讲已讲解完毕，这一讲讲述运动模型和观测模型对AMCL位姿输出的影响机制。
图1. AMCL包sensor文件夹展开
sensor文件夹主要表达了两个模型，一个是运动模型，根据机器人运动类型，选择相应的运动模型，用来将粒子一起运动(平移，旋转)，得到粒子样本的位姿；另一个是观测模型，根据选定的观测更新模型，对给定的位姿计算权重。
在amcl_sensor.h定义基类AMCLSensor 和基类AMCLSensorData,为两个派生类amcl_laser和amcl_odom提供统一接口。
1.传感器模型介绍
先来看AMCLSensor类，主要功能是根据运动模型更新粒子滤波器，根据观测模型初始化滤波器，根据观测模型更新粒子滤波器，仅定义运动模型里的位姿。从这里可以看到，运动模型和观测模型与粒子滤波器联系紧密。
// Base class for all AMCL sensors
class AMCLSensor
{
// Default constructor
public: AMCLSensor();
// Default destructor
public: virtual ~AMCLSensor();
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Initialize the filter based on the sensor model. Returns true if the
// filter has been initialized.
public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
// Update the filter based on the sensor model. Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
// Flag is true if this is the action sensor
public: bool is_action;
// Action pose (action sensors only)
public: pf_vector_t pose;
};再来看AMCLSensorData类，主要功能是把AMCLSensor类实例化的对象放在AMCLSensorData类里：
// Base class for all AMCL sensor measurements
class AMCLSensorData
{
// Pointer to sensor that generated the data
public: AMCLSensor *sensor;
virtual ~AMCLSensorData() {}
// Data timestamp
public: double time;
};2. 运动模型
接下来浓墨重彩地介绍运动模型，我们就amcl_odom.h 和amcl_odom.cpp展开讲述。
值得一提的是，amcl_odom.h的头文件引用使用了pf文件夹下的pf_pdf.h。不过这也没什么可紧张的，下一讲我们会介绍pf文件夹。
#include "amcl_sensor.h"
#include "../pf/pf_pdf.h" 选择运动模型，这一般是根据机器人的运动类型来决定，比如我的机器人是差分驱动，那么我一般是选择差分运动模型odom_model_diff，如果我的机器人是全向驱动，那么就选择全向运动模型odom_model_omni。
//里程计运动模型
typedef enum
{
ODOM_MODEL_DIFF,
ODOM_MODEL_OMNI,
ODOM_MODEL_DIFF_CORRECTED,
ODOM_MODEL_OMNI_CORRECTED
} odom_model_t; 使用继承了AMCLSensorData基类的AMCLOdomData派生类定义运动模型的数据结构。这里定义里程计的位姿pose以及里程计位姿pose的微小改变。
//里程计数据结构
// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
// Odometric pose 里程计位姿
public: pf_vector_t pose;
//里程计位姿改变
// Change in odometric pose
public: pf_vector_t delta;
};接着定义里程计运动模型，使用AMCLOdom类来表示，它继承了AMCLSensor基类。主要的功能是定义不同运动模型的输入参数，以及通过运动来更新粒子滤波器。
// Odometric sensor model
class AMCLOdom : public AMCLSensor
{
// Default constructor
public: AMCLOdom();
public: void SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4);
public: void SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5);
public: void SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 = 0 );
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Current data timestamp
private: double time;
// Model type
private: odom_model_t model_type;
// Drift parameters
private: double alpha1, alpha2, alpha3, alpha4, alpha5;
};进入amcl_odom.cpp里，几个小函数用来处理角度：normalize函数和angle_diff函数。
//normalize函数
static double
normalize(double z)
{
return atan2(sin(z),cos(z));//atan2 返回以弧度表示的y/x的反正切；返回方向角
}
//angle_diff函数
static double
angle_diff(double a, double b)
{
double d1, d2;
a = normalize(a);
b = normalize(b);
d1 = a-b;
d2 = 2*M_PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);//注意这里要么返回d1要么d2;只是为了返回一个约束在0-2pi之间的角度
}总览amcl_odom.cpp，这里需要设置运动模型，以及将运动模型实例化。关于运动模型的设置，使用UpdateAction函数，它的输入是粒子滤波器pf以及AMCLSensorData类型的data。
//通过运动更新粒子滤波器，其输入为粒子滤波器pf和AMCLSensorData提供的data
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
//在此使用AMCLOdomData类 创建一个里程计数据模型 ndata ：只有pose ,delta
AMCLOdomData *ndata;
//实例化,来源于传入的AMCLSensorData的AMCLOdomData
ndata = (AMCLOdomData*) data;
//创建新的粒子sample集
pf_sample_set_t *set;
//这里设定粒子sample集由传入的粒子滤波器的粒子sample集和它的当前粒子sample集
set = pf->sets + pf->current_set;
//这里定义旧的位姿，即X(t-1)，使用简单的vector加减运算
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
//选择运动模型的类型，这里主要介绍差分运动模型
switch( this->model_type )
{
//差分运动模型
case ODOM_MODEL_DIFF:
{
//设置运动采样模型，见《概率机器人》
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
//避免两个位姿之间靠得太近
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);//使用angle_diff函数
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
//设置里程计的运动噪声
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
//遍历粒子sample集，对粒子sample集里的每个sample对象的pose进行更新
for (int i = 0; i < set->sample_count; i++)
{
//新建一个粒子sample,用于接收粒子sample集的sample对象
pf_sample_t* sample = set->samples + i;
//使用pf_ran_gaussian函数采样生成位姿的微小变化
// Sample pose differences 使用pf_ran_gaussian()产生delta_*_hat
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
//对粒子sample里的pose进行更新;得到新的sample
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;//先产生 sample->pose.v[2]再计算得到上面两个
}
}
break;
}
};要问这个粒子的更新算法原理是什么，答案就在《概率机器人》第5章 机器人运动。
图2.基于里程计信息的采样算法
总的来说，运动模型的作用就是更新粒子sample的位姿。
3.观测模型
好的，下面开始介绍观测模型，该观测模型具体在amcl_laser.h和amcl_laser.cpp实现。
值得一提的是，amcl_laser.h的头文件引用使用了map文件夹下的map.h。这是为什么呢？稍后解释。
#include "amcl_sensor.h"
#include "../map/map.h"观测模型对应的测距仪是激光传感器，我们需要设置激光模型的相关参数：
// Laser model params
//
// Mixture params for the components of the model; must sum to 1
private: double z_hit;//具有局部测量噪声的正常范围，对应高斯分布模型，满足正态分布
private: double z_short;//意外对象引起，对应指数分布
private: double z_max;//检测失败时，会返回最大值，满足点群分布
private: double z_rand;//随机测量，即产生了无法解释的测量，满足分布在完整传感器测量范围的均匀分布
//
// Stddev of Gaussian model for laser hits. 高斯模型
private: double sigma_hit;
// Decay rate of exponential model for short readings. 指数模型
private: double lambda_short;
// Threshold for outlier rejection (unused)
private: double chi_outlier;关于测距仪的传感器模型的组成，见图3所示，参考《概率机器人》第6章 机器人感知。
图3 测距仪传感器模型的组成：注意这里c)为点群分布，不是均匀分布！
选择观测模型，观测模型有好几种，有波束模型，似然域模型， 极大似然域模型。
typedef enum
{
LASER_MODEL_BEAM,
LASER_MODEL_LIKELIHOOD_FIELD,
LASER_MODEL_LIKELIHOOD_FIELD_PROB
} laser_model_t;使用AMCLLaserData类定义观测模型的数据结构，其中包含的基本成员是激光的测量值，最大测量距离，激光发出的射线数目。
// Laser sensor data
class AMCLLaserData : public AMCLSensorData
{
public:
AMCLLaserData () {ranges=NULL;};
virtual ~AMCLLaserData() {delete [] ranges;};
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
public: double (*ranges)[2];
};接下来使用AMCLLaser类定义观测模型，先是定义了AMCLLaser的默认构造函数，它的输入为最大的激光射线发出的数目以及map_t格式的地图。
// Default constructor
AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(),
max_samples(0), max_obs(0),
temp_obs(NULL)
{
this->time = 0.0;
this->max_beams = max_beams;
this->map = map;
return;
}值的注意的是 AMCLLaser类里包含了map_t格式的地图以及用于与里程计运动模型产生的位姿做转换的laser_pose。
private: laser_model_t model_type;
// Current data timestamp
private: double time;
// The laser map 注意 AMCLLaser类包含map,且是私有成员变量
private: map_t *map;
//注意 AMCLLaser类包含laser_pose,且是私有成员变量；用于与里程计模型产生的位姿做转换
// Laser offset relative to robot
private: pf_vector_t laser_pose;
// Max beams to consider
private: int max_beams;在AMCLLaser类里，我们可以设置不同的观测模型的参数，这里选择似然域模型并设置它的相关参数。注意这里还使用了map_update_cspace函数，更新cspace_distance.
void
AMCLLaser::SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
map_update_cspace(this->map, max_occ_dist);//这里导入地图，更新cspace distance
}我们需要根据观测模型的数据来更新滤波器，因此定义UpdateSensor函数设置激光观测模型。
// Apply the laser sensor model设置激光观测模型
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) //输入为粒子滤波器pf，AMCLSensorData类型的data
{
if (this->max_beams < 2)
return false;
// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
return true;
}使用SetLaserPose函数设置激光的位姿，使用选定的观测模型（这里选用LikelihoodFieldModel）来计算给定位姿的权重。
//似然域模型计算给定位姿的权重，其输入为粒子sample集set以及AMCLLaserData类型的的data
// Determine the probability for the given pose
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
//创建粒子sample
pf_sample_t *sample;
//创建粒子sample的位姿
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
//遍历传入的粒子sample集
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
//这里接收到粒子sample的每个sample对象
sample = set->samples + j;
//存储接收的每个sample对象的位姿
pose = sample->pose;
//考虑激光的位姿与机器人的相对关系
// Take account of the laser pose relative to the robot
//从局部转换到全局;laser_pose(a)与里程计运动模型得出的位姿(b)的对应转换关系,最后得到位姿(c)
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
//预先计算一些东西
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
step = (data->range_count - 1) / (self->max_beams - 1);
// Step size must be at least 1
if(step < 1)
step = 1;
//遍历激光ranges范围内存储的所有距离值
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
//计算激光射线末端点的坐标
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.转到map坐标系下的i,j
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
//将map坐标系下的i,j转换成map坐标系下的index
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
// Gaussian model高斯模型
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
//计算得到粒子sample的权重
sample->weight *= p;
total_weight += sample->weight;
}
//返回权重
return(total_weight);
}关于似然域模型，参考《概率机器人》第6章 机器人感知，算法如图4所示。
图4.利用与最近物体的欧式距离计算测距仪似然的算法
另外比较重要的rellocTempData函数，其根据粒子sample数目的最大值和max_obs进行重新分配。
//新的样本，新的max_obs
void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
max_obs = new_max_obs;
max_samples = fmax(max_samples, new_max_samples);
temp_obs = new double*[max_samples]();
for(int k=0; k < max_samples; k++){
temp_obs[k] = new double[max_obs]();
}4.总结
从上面的介绍来看，里程计的运动模型对传入的粒子sample集的每个sample的位姿进行更新，激光传感器的观测模型对传入的粒子sample集的每个sample的位姿计算出权重。
因为是观测模型，所以需要知道机器人所在的位置，精确到栅格地图（栅格单元），才能计算出距离障碍物的距离。因此AMCLLaser类里定义了map_t格式的map，调用了map_update_cspace函数来更新cspace distance以及定义了激光测距仪的末端的位姿laser_pose。
使用pf_vector_coord_add 函数 将激光的位姿laser_pose转换到里程计运动模型产生的粒子sample的位姿所在的坐标系。这里就有运动模型与观测模型的关联了。
/*来自amcl_laser.cpp
*/
// Take account of the laser pose relative to the robot
//从局部转换到全局;laser_pose(a)与这是里程计位姿(b)的对应转换关系,最后得到位姿(c)
pose = pf_vector_coord_add(self->laser_pose, pose);关于pf_vector_coord_add函数：
/*来自pf_vector.cpp*/
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
c.v[2] = b.v[2] + a.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}还有，从里程计运动模型的采样算法来看，存在一个旧的位姿，这个旧的位姿如何产生呢？
// Apply the action model 设置运动模型
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) //更新运动，其输入为粒子滤波器pf和AMCLSensorData提供的data
{
AMCLOdomData *ndata;//在此使用AMCLOdomData类 创建一个里程计数据模型 ndata ：只有pose ,delta
ndata = (AMCLOdomData*) data;//实例化？来源于传入的AMCLSensorData的AMCLOdomData
// Compute the new sample poses 创建新的sample集，其实是用于接收传入的pf里的粒子sample集
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
//这个是旧的位姿，使用简单的vector加减
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
转自：4.AMCL包源码分析 | 传感器模型与sensor文件夹 - 知乎 (zhihu.com)