里程计运动模型

  学习激光slam过程中，涉及到一些关于运动模型的知识，gmapping代码中使用里程计运动模型，直接看代码有些不知所以然，所以在概率机器人这本书中看了下理论知识，可能对这个知识点理解的不是很到位，如有错误的地方还望指出。
  里程计虽然也有测量误差，但是相比于速度里程计来讲更加精确。我们利用里程计获取相对于机器人内部坐标的相对运动关系，即机器人在[t-1，t]时刻段内的运动(这里的相对运动关系指的是里程计位姿间的相对运动关系，而不是世界坐标系下位姿间的相对运动)。假设机器人在时间间隔[t-1，t]时间内位姿由${x_{t - 1}}$xt−1​变为${x_{t}}$xt​（这里位姿都为世界坐标系下坐标）。

  里程计采集数据反映了${\overline x _{t - 1}} = {(\overline x ,\overline y ,\overline \theta )^T}$xt−1​=(x,y​,θ)T到${\overline x _t} = {(\overline x ,\overline y ,\overline \theta )^T}$xt​=(x,y​,θ)T的相对运动，这里" - "代表其是嵌在机器入内部坐标的，该坐标系与全局世界坐标的关系是未知的。其实就是利用里程计获得的两个时刻的里程计位姿相对运动关系来估计世界坐标系下位姿的相对运动，从而获得新的位姿。一般我们将这个相对运动变换分为三部分：初始旋转 ${\delta _{rot1}}$δrot1​、平移${\delta _{trans}}$δtrans​ 和第二次旋转${\delta _{rot2}}$δrot2​.

1 、闭式计算

  我们最终想要计算的是t时刻机器人的位姿${x_{t}}$xt​，通常这个位姿是有噪声的、有偏差的，因此我们通常求得是关于${x_{t}}$xt​的一个分布概率$p({x_t}|{u_t},{x_{t - 1}})$p(xt​∣ut​,xt−1​)。下面伪码表明了闭式计算后验概率的算法流程：

  算法输入为初始位姿${x_{t - 1}}$xt−1​、由里程计得到的一对位姿$({\overline x _{t - 1}},{\overline x _t})$(xt−1​,xt​)和假定的最终位姿${x_{t}}$xt​，输出为数值概率$p({x_t}|{u_t},{x_{t - 1}})$p(xt​∣ut​,xt−1​)。算法程序的第2 -4 行从里程计读数获得了相对运动参数$({\delta _{rot1}},{\delta _{trans}},{\delta _{rot2}})$(δrot1​,δtrans​,δrot2​)。对给定位姿x,-1 和x, 相关的相对运动参数$({\hat \delta _{rot1}},{\hat \delta _{trans}},{\hat \delta _{rot2}})$(δ^rot1​,δ^trans​,δ^rot2​)由算法的第5 -7 行计算。第8 -10 行计算各自运动参数的误差概率。${\hat \delta _{roti}} - {\delta _{roti}}$δ^roti​−δroti​即表示里程计误差。其中prob(a,${b^2}$b2)表示自变量为a，均值为0，方差为${b^2}$b2的高斯噪声。第11 行返回由各自误差概率$P_1$P1​ 、$P_2$P2​ 和$P_3$P3​相乘得到的组合误差概率。${\alpha _1}$α1​~${\alpha _4}$α4​是指定机器人运动噪声的机器人特定参数。

2、采样算法

  上面我们假定输入含有最终的位姿${x_{t - 1}}$xt−1​，但是实际上最终的位姿我们是不知道的，因此我们实际中更多的利用里程计获得的相对运动关系${u_t} = ({\overline x _{t - 1}},{\overline x _t})$ut​=(xt−1​,xt​)以及上一时刻的位姿${x_{t-1}}$xt−1​来获得当前时刻的位姿${x_{t}}$xt​。利用粒子滤波定位，也就是对状态进行采样，利用粒子来表示位姿后验分布。采样算法的输入为初始位姿${x_{t - 1}}$xt−1​、由里程计得到的一对位姿$({\overline x _{t - 1}},{\overline x _t})$(xt−1​,xt​)，随机推测出一个状态${x_{t}}$xt​，而不是计算${x_{t}}$xt​的概率。

  算法流程就是利用里程计位姿求解相对运动关系，然后加入噪声，利用上一时刻世界坐标系下位姿求解此时的位姿(在gmapping中对于采样的每一个粒子都要进行这个操作，也就是里程计更新)。这里适用于已知两个点变换距离情况，其中运动模型为:

  如果已知两时刻里程计位姿相对变换关系，可以使用下面的式子进行更新，其实这个运动模型与下面这个式子是等价的，只需将上面式子的cos和sin函数展开即可，gmapping实现中使用的就是下面这个方程：

3、gmapping里程计更新实现

在gmapping代码中openslam_gmapping文件夹下的motionmodel.cpp中实现了这个过程，实现并没有一步步求出$({\delta _{rot1}},{\delta _{trans}},{\delta _{rot2}})$(δrot1​,δtrans​,δrot2​),然后加入噪声得到相对于世界坐标系的相对位姿变换，而是直接利用两个点的求出$(dx,dy,d\theta )$(dx,dy,dθ)，然后加入噪声求解世界坐标系下相对位姿变换，其实两个的方法的思想是完全一致的：

参考资料：《概率机器人》、激光slam理论与实践课程