AMCL代码详解（四）根据运动更新粒子位姿

AMCL中粒子的运动更新函数在AmclNode::laserReceived函数中进行调用，当接收到正常的激光点云后调用里程计更新函数对每个粒子的位姿进行运动更新，具体实现是在amcl_odom.cpp函数中的AMCLOdom::UpdateAction：

// Apply the action model
// pf是需要更新的滤波器对象，data则是指示了更新的运动数据。
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
  //通过强制类型转换，从输入参数data中获取运动数据对象。
  //AMCLOdomData包含了一个三维的pose以及三维的delta
  //pose里面存放的应该是上一时刻的位姿xy以及角度，delta里面存放的是上一时刻到当前时刻的xy位移以及角度变化量
  AMCLOdomData *ndata;
  ndata = (AMCLOdomData*) data;

  // Compute the new sample poses
  // 通过pf模块的函数pf_vector_sub，从运动数据中推算出上一时刻的里程计位姿。 此外，还通过临时变量set从里程计对象中获取新的粒子集合。
  pf_sample_set_t *set;

  set = pf->sets + pf->current_set;
  pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
  // 根据模型类型更新粒子集合。
  switch( this->model_type )
  {
  //里程计运动模型可以参看这个https://gaoyichao.com/Xiaotu/?book=probabilistic_robotics&title=pr_chapter5#%E7%AE%97%E6%B3%955.6
  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).
    // 里程计模型中两个旋转和一个平移运动的计算
    // 运动分解中将A到B的过程分解为三部分：
    // 从当前机器人朝向旋转到AB点连线delta_rot1
    // 从A点直线行驶到B点delta_trans
    // 在B点旋转到达最终位姿delta_rot2
    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);

    // 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)));
    //对粒子集合中的每个样本进行更新
    for (int i = 0; i < set->sample_count; i++)
    {
      pf_sample_t* sample = set->samples + i;

      // Sample pose differences
      //计算三次运动的旋转以及平移量，pf_ran_gaussian产生一个随机高斯分布的参数应用到运动过程
      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));

      // Apply sampled update to particle pose
      // 将位姿赋值给原来的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;
    }
  }
  break;
  }
  return true;
}

这个函数的传参给进来的是两个参数：需要更新的滤波器对象(pf)以及更新的运动数据(data)。这两个数据都在amcl_node.cc中处理。

其中data包含了上一时刻的位姿(pose)以及上一时刻到当前时刻的xy方向的位移分量和z方向的旋转角度

在UpdateAction这个函数中其实差不多就干了一件事：将pf的点的位姿根据里程计信息进行一次迭代，当然这个里程计信息在迭代的过程中是加了一定噪声的。主要步骤包含如下：

首先对机器人的运动进行分解，在这里机器人的运动(从A到B)可以分解为三个部分：

// 从当前机器人朝向旋转到AB点连线delta_rot1
// 从A点直线行驶到B点delta_trans
// 在B点旋转到达最终位姿delta_rot2

    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)));

最后根据位姿以及偏移量还有两个噪声值更新每个pf粒子的位姿：

    for (int i = 0; i < set->sample_count; i++)
    {
      pf_sample_t* sample = set->samples + i;

      // Sample pose differences
      //计算三次运动的旋转以及平移量，pf_ran_gaussian产生一个随机高斯分布的参数应用到运动过程
      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));

      // Apply sampled update to particle pose
      // 将位姿赋值给原来的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;
    }

大致的步骤就是分为以上三步，这里之所以不是简单的直接对每个粒子直接加上一个位姿的偏移量应该是考虑了里程计数据也不一定是完全准确的，所以需要增加一个高斯噪声，而高斯噪声的协方差应该是随时间或者速度变化的，所以先进行运动分解，根据分解的结果每次产生一个不完全相同的高斯噪声函数，最后再将含有噪声的位移变化传递给pf。

看到这里可以发现AMCL的噪声存在于两个地方，一个在于随机生成粒子时，需要添加一个噪声项以利于随机生成一系列粒子；另一个地方则是这里运动更新时，对里程计数据添加噪声以生成一系列不同情况的位姿。