AMCL中odom的数据处理

101 阅读 0 评论 67 点赞

我是靠谱客的博主内向牛排，最近开发中收集的这篇文章主要介绍AMCL中odom的数据处理，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

最近在看AMCL的程序，想知道launch文件中关于odom的那些配置参数是怎么运行的，看完后做点笔记。

1.AMCL的launch文件关于odom的参数配置列表（以下讨论的都是差速模型）：

odom参数列表
参数	默认值	描述
odom_model_type	diff	里程计运动模型，种类有diff、ommi等
odom_alpha1	0.2	机器人旋转分量中的旋转噪音
odom_alpha2	0.2	机器人平移分量中的旋转噪音
odom_alpha3	0.2	机器人平移分量中的平移噪音
odom_alpha4	0.2	机器人旋转分量中的平移噪音
odom_frame_id	odom	里程计用于哪个坐标系
base_frame_id	base_link	机器人底盘坐标系
global_frame_id	map	定位系统的参考坐标系
tf_broadcast	true	是否发布map和odom坐标系之间的转换

2.里程计模型

假如对AMCL算法不清楚第一眼看过去大概就会有点懵。然后就会去翻AMCL的算法原理，一看发现原来在《概率机器人》中文版一书上讲过，但书上没有特别直接，所以还是得看程序具体怎么写的。但程序虽然是最直接的，为了理解程序最好还是要从算法原理上有一点基础的认识，所以这一小节简单概括一下里程计模型以及其采样算法，算法伪代码可以参考《概率机器人》书上103页。

里程计diff模型，也就是最常见的差速模型，两平行轮作驱动，另外附加一个万向轮作支撑。在AMCL算法中将机器人的每一步的广义运动进行了拆解，拆解成包含三个动作的序列，即：a.在起始点旋转，转向终止点的方向；b.沿着该方向做直线运动到终止点；c.在终止点进行旋转，转到目标方向，其运动示意图如下所示。学过机器人学大的会问，平面上的刚体运动可以拆分为一个旋转和一个平移，为什么这个要做3次运动。这个忽略了一个大前提，就是平面上的刚体有3个自由度：x、y、yaw，但是差速模型机器人虽然可以抵达平面上的任意位置和姿态，但是并不具备3个自由度的概念。想一下，差速模型的机器人只能直行和转弯，并不能沿着驱动轮的轴线进行运动吧？而且这用做的一大好处是可以提取相对距离便于定位校核的。

其中， $delta _{rot1}$ 代表在起始点的旋转， $delta _{trans}$ 代表着第二段平移过程， $delta _{rot2}$ 代表着在终止点的旋转。

3.里程计噪音

要讨论里程计噪音，首先来讨论一下怎么样里程记没有噪声，也就是说，里程计读轮子的转圈和机器人的位移是精确对应的。当然理论上和仿真中可以做到：首先假设轮子和地面之间不打滑、地面绝对平整且光滑、轮子绝对圆润且不变形、里程计和机器人轮轴以及机器人轮子之间没有摩擦和迟滞可以做到完全时间同步...等等。当然现实中做不到，所以在差速模型的里程记计算中需要对机器人进行噪音估计，因为我们读到的直很可能不是真值。

在AMCL中，里程计是作为状态预测器存在的，通过接受当前的控制信号，从上一帧机器人状态对这一帧机器人状态进行预测，并与当前观测所得结果对当前预测进行加权打分。所以通过输入和输出我们知道，里程计在AMCL中的作用就是根据当前控制信号更新上一帧的能表征机器人状态的粒子集合。在AMCL中关于odom的伪代码如下所示：

4.AMCL代码分析

在AMCL中，关于里程计在算法中做机器人位姿估计更新的代码，主要在amcl_odom.cpp中。主要执行函数是：bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)，这个函数输入是机器人上一帧位姿粒子状态pf和当前传感器信息data，程序运行完成就返回bool类型的true。

首先它利用switch...case...方法辨别里程计的模型，

然后，当model_type的值是 ODOM_MODEL_DIFF类型时，进行相应的机器人位姿更新：

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]);
    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
      //double pf_ran_gaussian(double sigma)sigma。
      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
      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;
    }

首先一开始是定义一些变量，注意这些变量选用的都是double类型的。

    double delta_rot1, delta_trans, delta_rot2;
    double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
    double delta_rot1_noise, delta_rot2_noise;

然后就开始计算 $delta _{rot1}$ 、 $delta _{rot2}$ 和 $delta _{trans}$ 。

    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}$ 时，多了个判定条件：

当 sqrt(ndata->delta.v[1]*ndata->delta.v[1] + ndata->delta.v[0]*ndata->delta.v[0]) < 0.01 时，也就是 $sqrt{delta x^{2}+delta y^{2}}<0.01$ ,注意，这里的单位是米，也就是说，以位移是否小于1cm作为判断条件，如果里程计量得机器人位移小于1cm， $delta _{rot1}$ 就置为0，其它的就和《概率机器人》一书中p103页一样。

$begin{cases} delta_{rot1}=0 ,& text{ if } sqrt{delta x^{2}+delta y^{2}}<0.01 \ delta_{rot1}=atan2(delta x,delta y)-theta_{old}, & text{ if } sqrt{delta x^{2}+delta y^{2}}>=0.01 end{cases}$

$delta_{trans}=sqrt{delta x^{2}+delta y^{2}}$

$delta_{rot2}=delta theta-delta_{rot1}$

其中：

$delta x$ 表示机器人两帧间x方向上的位移， $delta y$ 表示机器人两帧间y方向上的位移， $delta theta$ 表示机器人两帧间yaw(偏航)方向上的位移。 $theta_{old}$ 表示机器人上一帧的yaw偏航角度。

这是根据里程计的测量结果得到机器人的三段动作的估计值。之前也讲过，由于机器人运动过程中存在着噪音，所以测量值很可能不是机器人的真实值。这样就需要对机器人运动过程中的里程计的噪声进行估计，来得到能涵盖机器人位姿真实值的区域，可以用一定数量的粒子来表征这个区域，然后对这些粒子进行采样来尽可能多的逼近机器人的真实值。简而言之，上面用机器人呢差速运动模型对机器人位姿的估计我不全信，我要获取这个位姿邻近的值看是否能够更好地满足观测。

这个采样采多大？怎么采？这就涉及到我们前面的AMCL的launch文件中的odom_alpha设计参数了。

为了前后对称地估计旋转过程中的噪音，我们看到代码里面首先是取了两次旋转角的锐角 $delta _{rot1-noise}$ 和 $delta _{rot2-noise}$ 。

    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_ran_gaussian()进行高斯采样，也就是书103页的sample()函数。书中没有具体写sample()函数的实现，但是pf_pdf.c代码中对这函数进行了解释：从零平均高斯分布中随机抽取，带有标准差sigma。

// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
//   http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma)
{
  double x1, x2, w, r;

  do
  {
    do { r = drand48(); } while (r==0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r==0.0);
    x2 = 2.0 * r - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);

  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

这里主要是用到了一个叫Box-Muller变换的东西。

Box-Muller变换

Box-Muller变换是通过服从均匀分布的随机变量，来构建服从高斯分布的随机变量的一种方法。也就是说，选取两个服从[0,1]上均匀分布的x1，x2，则要使y1、y2满足均值为0,方差为1的高斯分布，需使得：

$left{begin{matrix} y_{1}=cosleft ( 2pi x_{2} right )sqrt{-2ln(x_{1})}\ y_{2}=sinleft ( 2pi x_{2} right )sqrt{-2ln(x_{1})} end{matrix}right.$

具体证明过程有兴趣可以去看帅帅Go的博客：https://blog.csdn.net/weixin_41793877/article/details/84700875

这里讲一下它的应用。很明显，这个高斯随机数产生的方式比较繁琐，需要调用三角函数、sqrt()函数和ln()函数，这使得它的运行速度非常慢。其次，可以看得到，当x1接近0的时候，ln()函数将会给映射带来数值不稳定，而x1接近于0是可以接受的。最关键的是，y1、y2位于(0,1]区间，属于伪随机数。

那么，怎么样才能避免这些呢？数学直觉告诉我们，当有三角函数存在时，普通坐标系下搞不定可以放到极坐标形式进行计算。同样假设两个服从[-1,1]上均匀分布的x1，x2，，但是令：

$w=R^{2}=x_{1}^{2}+x_{2}^{2}$ ，

那么，根据标准式，Box-Muller的极坐标式可以表示为：

$left{begin{matrix} y_{1}=frac{x_{1}}{sqrt{w}}sqrt{-2lnw}\ y_{2}=frac{x_{2}}{sqrt{w}}sqrt{-2lnw}end{matrix}right.$

其中，w在(0,1]范围内。y1和y2服从N(0,1)随机数。代码里面取的是y2。如果前面乘sigma，则表示N(0,sigma)分布的随机数。

根据Box-Muller高斯随机采样方法，其位姿估计代码如下所示：

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

转换成算法公式就是：

$hat{delta}_{rot1}=delta _{rot1}-rand[N(0,alpha_{1}delta_{rot1-noise}^{2}+alpha_{2}delta_{trans}^{2})]$

$hat{delta}_{trans}=delta _{trans}-rand[N(0,alpha_{3}delta_{trans}^{2}+alpha_{4}delta_{rot1-noise}^{2}+alpha_{4}delta_{rot1-noise}^{2})]$

$hat{delta}_{rot2}=delta _{rot2}-rand[N(0,alpha_{1}delta_{rot2-noise}^{2}+alpha_{2}delta_{trans}^{2})]$

注意到，这里的 $alpha_{1},alpha_{2},alpha_{3},alpha_{4}$ 都是绝对量，分别和odom中两次旋转的弧度和一次平移的米的值进行相乘得到方差。公式中的单位主要根据代码中变量的赋值最后在AmclNode::getOdomPose()函数中看到数据单位初始的由来：

  x = odom_pose.pose.position.x;
  y = odom_pose.pose.position.y;
  yaw = tf2::getYaw(odom_pose.pose.orientation);

再次声明：平移的单位是米、偏航的单位是弧度。

在amcl的example的launch文件中，odom_alpha3也就是 $alpha_{3}$ 设置的比较大，并未设置成0.2反而设成0.8，这个得根据实际硬件和工况进行设置。这里用一张经典老图来表示一下不同方差下的正态分布，可以看到：方差越大，分布越分散，方差越小，分布越集中。

那么，真实情况是怎么样呢？就拿 $delta _{trans}$ 的采样估算来说，假设机器人两帧之间移动了1cm，转动了0.01弧度（也就是0.5度），这个假设既便于计算也比较符合实际情况。根据公式：

$hat{delta}_{trans}=delta _{trans}-rand[N(0,alpha_{3}delta_{trans}^{2}+alpha_{4}delta_{rot1-noise}^{2}+alpha_{4}delta_{rot1-noise}^{2})]$

对于正态分布： $N(0,alpha_{3}delta_{trans}^{2}+alpha_{4}delta_{rot1-noise}^{2}+alpha_{4}delta_{rot1-noise}^{2})$ ，我们取默认值： $alpha_{3}=0.2,alpha_{4}=0.2$ ，则方差 $sigma=0.2 times 0.01^{2}+0.2 times 0.01^{2}+0.2 times 0.01^{2}=0.0006$ ，我们可以绘制 $N(0,0.0006)$ 的正态分布图像。注意正态分布y轴代表概率，x轴代表当前取值。

可以看到，可能取值基本都分布于（-0.002，0.002）区间之间，这个偏差相较于0.01而言有20%的偏差。

同样可以改变 $alpha_{3},alpha_{4}$ 的值，这次假设 $alpha_{3}=0.1,alpha_{4}=0.1$ ，则其分布图像如下所示：

肯定这样采样范围狭窄了不少，偏差大约只有10%。

假设 $alpha_{3}=0.8,alpha_{4}=0.2$ ，则：

采样范围相较于alpha都为0.2拓宽了不少。具体噪音估计参数效果需要根据实际情况调整，需要让机器人的真值时刻被包裹在粒子集合中，这样机器人位姿估计才能稳定收敛于真值。

最后一步当然就是对初始预测的修正，代码如下：

      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;

这一步和书上完全一样，转换成公式就是：

$x_{t}=x_{t-1}+hat{delta}_{trans}cosleft ( theta_{t-1}+hat{delta}_{rot1} right )$

$y_{t}=y_{t-1}+hat{delta}_{trans}sinleft ( theta_{t-1}+hat{delta}_{rot1} right )$

$theta_{t}=theta_{t-1}+hat{delta}_{rot1}+hat{delta}_{rot2}$

注意，这里只是一次采样，也只是产生粒子群中的1个粒子，AMCL需要维持一定的粒子数目来表征机器人的位姿，所以需要像开头里的循环那样反复采集机器人位姿，并根据当前观测给该粒子加权重，添加进粒子群中进行后续优化。

至此，AMCL对里程计数据的处理就完成了。总的来说就是先进行基于当前数据的预测更新，然后进行高斯随机采样得到当前以当前数据为中心的位姿粒子群，最后进行观测赋予权值。