AMCL中odom的数据处理

最新推荐文章于 2025-06-12 08:34:31 发布

原创

最新推荐文章于 2025-06-12 08:34:31 发布 · 6.1k 阅读

64 ·

CC 4.0 BY-SA版权

文章标签：

#amcl

最近在看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] +