amcl的传感器模型
和Gmapping一样，amcl使用里程计和激光雷达作为传感器进行定位。 根据《Probabilistic Robotics》的说法，里程计实际上是一种运动模型， 用于完成滤波器的运动更新，而激光雷达才是对于环境的感知器，完成滤波器的测量更新。把里程计模型看作是运动模型有点奇怪，毕竟它仍然是一种传感器。 在编码过程中，程序猿们都喜欢将它们归结为传感器。
既然都是传感器，amcl为里程计和激光雷达定义了一个共同的基类AMCLSensor，用于描述一些传感器所共有的特性。 此外还未传感器的数据定义了基类AMCLSensorData。(按：好在amcl所用的传感器统共就两种，这种继承的方式不会带来太多的维护成本。 很多时候，继承并不是面向对象的一个好的选择。以后会挖个新坑，介绍一些常用的设计模式。)
本文首先分析这两个基类，在结合《Probabilistic Robotics》分别介绍amcl中实现的各种里程计模型和激光传感器模型。
1. 传感器基类
下面是定义在amcl_sensor.h中的基类AMCLSensor， 我们省略了其中关于GUI的成员函数。这里看到的各个函数基本上都是空的，没有任何操作。其中构造函数、析构函数和InitSensor用于传感器对象的构建销毁和初始化。
成员变量is_action用于标记传感器是否用于运动模型计算。 根据《P.R.》的第五章和第六章的内容， 我们可以认为，用于运动模型的传感器的更新目标是机器人的位姿，而测量模型的传感器大多是用于更新环境的。 这里的成员变量pose用于运动模型的位姿，用户通过函数UpdateAction完成运动更新过程，而测量更新过程则通过UpdateSensor完成。
class AMCLSensor {
public: AMCLSensor();
public: virtual ~AMCLSensor();
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
public: bool is_action;
public: pf_vector_t pose;
};
而传感器数据的基类则更加简单，如下代码所示。它只定义了两个成员变量sensor和time分别用于指示与之关联的传感器对象和数据产生的时间。唯一的析构函数还是空的。
class AMCLSensorData {
public: AMCLSensor *sensor;
virtual ~AMCLSensorData() {}
public: double time;
};
2. 里程计模型
typedef enum {
ODOM_MODEL_DIFF,
ODOM_MODEL_OMNI,
ODOM_MODEL_DIFF_CORRECTED,
ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;
amcl一共提供了四种里程计模型，我们可以通过运行参数odom_model_type来配置，右面的代码是对应四种模型的枚举定义。
其中diff是按照《P.R.》中的sample_motion_model_odometry算法来实现的， 使用odom_alpha1到odom_alpha4来描述噪声。omni则是在diff基础上增加了一个参数odom_alpha5来描述垂直与运动方向的平移趋势。而diff-corrected和omni-corrected则是为了修复diff和omni模型中的bug而设计的， 它们在本质上没有大的区别。
下面是里程计数据的定义，它在基类的基础上增加了pose和delta两个字段，分别用于记录里程计的位姿和运动量。它们将用作运动更新的控制量输入\(u_t\)。
class AMCLOdomData : public AMCLSensorData {
public: pf_vector_t pose;
public: pf_vector_t delta;
};
下面是里程计模型对应的结构定义。其中构造函数和三个SetModel*函数的实现都是为了构造和初始化里程计模型的。而函数UpdateAction则是根据里程计数据@data来更新粒子滤波器@pf， 如果成功更新则返回true。
此外，AMCLOdom中还定义了一些私有的成员变量，其中alpha1~alpha5描述了模型的噪声参数，而model_type记录了实际使用的模型类型。
class AMCLOdom : public AMCLSensor {
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 );
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
private: double time;
private: odom_model_t model_type;
private: double alpha1, alpha2, alpha3, alpha4, alpha5;
};
下面我们详细解析一下运动更新函数UpdateAction。如下面的代码所示，它有两个参数，其中pf是需要更新的滤波器对象，data则是指示了更新的运动数据。
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) {
在函数的一开始，我们通过强制类型转换，从输入参数data中获取运动数据对象。并通过pf模块的函数pf_vector_sub，从运动数据中推算出上一时刻的里程计位姿。 此外，还通过临时变量set从里程计对象中获取新的粒子集合。
AMCLOdomData *ndata;
ndata = (AMCLOdomData*) data;
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
pf_sample_set_t *set = pf->sets + pf->current_set;
然后，根据模型类型更新粒子集合。这里我们以ODOM_MODEL_DIFF为例介绍， 它是《P.R.》中sample_motion_model_odometry算法的实现。 在对应的分支中，首先定义了一堆临时变量，它们都是更新过程中需要用到的中间变量。
switch(this->model_type) {
case ODOM_MODEL_DIFF:
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
下面的这几行代码完成了里程计模型中两个旋转和一个平移运动的计算。 对应算法的第2到4行。
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]);
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);
接下来计算两个旋转噪声。
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)));
然后就对粒子集合中的每个样本进行更新，它们都是对算法的5到10行的忠实实现。
for (int i = 0; i < set->sample_count; i++) {
pf_sample_t* sample = set->samples + i;
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.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;
}
最后break并返回。
break;
}
return true;
}
3. 激光雷达模型
amcl提供了三种激光传感器模型，其中beam实现的是波束模型， likelihood_field则是似然场模型，likelihood_field_prob也是似然场模型， 只是增加了波束过滤的策略。下面的左右两边的代码分别是激光传感器模型的枚举定义，以及激光传感器数据的类型定义:
|
|
下面的代码片段是AMCLLaser类的定义，它只有一个构造函数，我们需要指定最大的波束数量，并提供一个地图对象。析构函数只负责释放成员temp_obs所指的内存。
在构建了AMCLLaser的对象之后，我们还需要根据模型类型，完成传感器模型的初始化工作。下面的三个SetModel*的函数，分别完成了波束模型、 似然场模型和增加了波束过滤策略的似然场模型。它们的参数列表都很长，这些参数都是各自模型所需的各种配置。
class AMCLLaser : public AMCLSensor {
public: AMCLLaser(size_t max_beams, map_t* map);
public: virtual ~AMCLLaser();
public: void SetModelBeam(double z_hit, double z_short, double z_max, double z_rand, double sigma_hit, double labda_short, double chi_outlier);
public: void SetModelLikelihoodField(double z_hit, double z_rand, double sigma_hit, double max_occ_dist);
public: void SetModelLikelihoodFieldProb(double z_hit, double z_rand, double sigma_hit, double max_occ_dist, bool do_beamskip, double beam_skip_distance, double beam_skip_threshold, double beam_skip_error_threshold);
函数UpdateSensor用于根据传感器模型和数据来更新粒子滤波器。函数SetLaserPose则用于更新传感器的位姿，传感器的位姿是滤波器的测量更新的基准。
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
public: void SetLaserPose(pf_vector_t& laser_pose) { this->laser_pose = laser_pose; }
下面这三个静态的私有函数用于具体实现测量模型，它们将在函数UpdateSensor运行的过程中，根据模型类型得到调用。
private: static double BeamModel(AMCLLaserData *data, pf_sample_set_t* set);
private: static double LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set);
private: static double LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set);
而函数用于申请辅助数据的内存，用于进行波束过滤时缓存测量数据。
private: void reallocTempData(int max_samples, int max_obs);
下面我们只列举了部分成员变量，其它的成员变量都是各种激光模型的配置参数，这里省略之。
private: laser_model_t model_type;
private: double time; // 当前传感器数据的时间戳
private: map_t *map; // 地图对象
private: pf_vector_t laser_pose; // 雷达传感器位姿
private: int max_beams; // 最大波束数量
// 省略了其它配置参数
};
下面是函数UpdateSensor的具体实现。我们可以看到在第4到11行的代码中，各个模型类型都调用了pf_update_sensor完成更新。 只是传递的第二个参数不同，这第二个参数是一个函数指针，它们就是AMCLLaser中那三个私有的静态函数，实现了具体的测量模型。
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) {
if (this->max_beams < 2)
return false;
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;
}
我们已经提到beam实现的是波束模型， likelihood_field则是似然场模型，likelihood_field_prob也是似然场模型， 只是增加了波束过滤的策略。具体的函数无非是对算法中各个公式的实现而已，这里不再赘述。
4. 完
本文中，我们介绍了amcl的传感器模型。它为传感器及其数据定义了AMCLSensor和AMCLSensorData两个基类。定位过程中所需的里程计模型和激光模型都继承自这两个基类。 其中diff里程计模型是《P.R.》中sample_motion_model_odometry算法的实现。 而激光雷达的beam实现的是波束模型， likelihood_field则是似然场模型的实现。