里程计计算旋转角度_运动模型--速度和里程计 (更新2019-11-7)

在自动驾驶，轮子机器人的使用中，一般我们都是用平面世界中的运动模型来进行移动计算（预测）。在平面环境中运动的移动机器人， 它的运动学状态，或者说是位姿pose，可以用三个变量来表示

。在平面中我们有两个模型，一个是速度模型一个是里程计模型，里程计模型一般比速度模型更精确，因此大多数商业机器人并不使用速度模型。但是里程计模型具有一定的滞后， 因此不能用于运动规划。蔽障的规划算法不得不预测运动的结果。因此，里程计模型通常用于估计，而速度模型用于概率的运动规划。在之后的介绍中会详细解释这么说的原因。

概率运动学模型，或者说是运动模型motion model在移动机器人的状态转移模型中扮演着重要的角色。 这个模型实际上就是一个我们熟悉的条件概率密度函数：

两种模型都有两种算法，分别是概率算法和采样算法：

速度模型

代表的是机器人的方向角度，x,y是平面中所在位置。

代表发送给差分轮，转机的速度命令，

。

这里不做过多的推到，根据

，可以推出

真实运动中实际速度与给定速度是有误差的，并将其表示为均值为0，方差有限的随机变量：

因此

后的机器人的真实状态，这里用叠加噪声后的运动

代替了给定速度

：

之后我们可以通过公式计算出在得知

的情况下，带噪声的真实的

概率算法：（获取到移动后估计位置的概率）

该算法输入估计的后验位置

,控制

和前一时刻的位置

Prob的实现可以选择正太分布或是三角分布

在噪声范围内计算出真实的速度数据之后，就可以计算出prob,这个函数就是计算真实速度（在指定方差范围内）的后验概率。里面最大的的概率就是最有可能的最终

。如下例子：

显示了投影在x-y平面上的速度运动模型。在三个子图中，机器人都具有相同的线速度和角速度，只是误差参数α1,⋯,α6不同。 其中左图描述的是误差参数α1到α6都比较小的现象， 中间的图中角速度误差(α3,α4)较小，而平移误差(α1,α2)较大。右图则相反，角速度误差大平移误差小

这个算法适用于卡尔曼滤波等。

采样算法，适用于粒子滤波。公式如下：

原理都类似，只是该算法由

依概率给出一个

sample算法如下：

与前图类似，下图为在运动模型中采样的结果：

从速度运动模型采了500个样本

里程计

到目前为止，我们讨论的运动模型都是根据机器人的速度计算位姿的后验概率的模型。有时我们可能想用里程计的测量值作为机器人运动的计算基础。 里程计通常通过对轮子编码器数据的积分获得，大多数商业机器人以一定的周期积分，进而估计位姿数据。这就是本章要讨论的第二种运动模型——里程计运动模型odometry motion model。 这种模型用里程计的测量值来代替控制量。

实际经验告诉我们尽管里程计也存在误差，但它也比速度模型要准确很多。这两种方法都要受到打滑、数据漂移的影响，此外速度模型还受限于实际运动控制器与数学模型之间的差异。 但是里程计具有一定的滞后性，只有在机器人运动之后才能够获得。这对于滤波器算法而言不是什么问题，但是对于准确的运动规划和控制就不合适了。

我们先来看看里程计的算法，再来解释里程计适合估算的原因。

根据上图我们可以看出里程计是从

到

通过旋转--平移--旋转

里程计公式如下：

同理，根据如下公式计算出

，只是现在的值是trans和rot1，rot2.

同样有采样的算法：

最终的目的和速度模型类似。

在路径规划的时候，对于预测是很重要的，我们需要在不知道下一步具体是怎么走的情况下，规划设定出下一步的动作行为，但是如何评估这个预测的动作的准确度是在哪里呢？

可以由速度Ut来估算，odom不行，因为一定要具体的移动之后，encoder出来的数据进行估算。这样在实际运动过程中，就delay了，无法及时获取到最可靠，概率最高的路径。而Ut则一直都在。