运动学模型（二）----轮速计 & 后轮速差模型

轮速计相关模型最常用，最易入门的便是后轮速差模型。因此本文将针对后轮速差模型进行详细的原理解释以及代码模型。

后轮差速轮速计定位的关键求解目标有两组，一个是位置pos，一个是姿态altitude
由于轮速计只能反映车辆在平面上的运动状态，所以三维的pos，减少为pos.x和pos.y，同理，姿态altitude也只能反应yaw的状态。
也就是求解值只有3个：x，y，yaw
如上图所示，车辆被简化为只有后轮的模型，车辆宽度（轮距）为l(BASE_length)，左轮车速为vl，右轮车速vr，车辆转弯半径为r，车辆转弯半径对应的转弯角度theta1,车辆左轮（转向轮）的转弯角度theta2，车辆自身的转弯角度theta3，由于车辆每次轮速计计算之间的转弯角度比较小，可以近似为theta1=theta2=theta3。
此时通过几何计算可以得到：

车辆前进距离：forward_dis=(vl+vr)t/2
车辆左右轮前进距离差：delta_dis=(-vl+vr)t
车辆转弯角度：delta_theta=delta_dis/BASE_length

经过坐标系分解可得（x超前，y朝左，右手系）

delta_x=forward_dis* cos(delta_theta)
delta_y=forward_dis* sin(delta_theta)

至此，单帧的Δ变化量就计算完毕了。但是我们是要计算车辆的定位。所以还需要计算车辆相对原点的绝对位置。
累积的x，y，yaw计算方法如下：

  accumulate_x_ = accumulate_x_ - delta_y * sin(accumulate_th_ + delta_theta / 2.0f) +
                  delta_x * cos(accumulate_th_ + delta_theta / 2.0f);
  accumulate_y_ = accumulate_y_ + delta_y * cos(accumulate_th_ + delta_theta / 2.0f) +
                  delta_x * sin(accumulate_th_ + delta_theta / 2.0f);
  accumulate_th_ = accumulate_th_ + delta_theta;

想来计算到此，便可结束了。但根据实车经验和轮速计精度限制，在车辆低速的情况下，轮速是非常不稳定且不准确的。因此，需要用轮速的计数来估计车轮移动距离。
首先可以记录下每帧的车轮的计数和前一帧的车轮的计数。然后用帧间差作为轮速的替代。左轮计数Δ为cl，右轮计数Δ为cr；左车轮长度为wl，右车轮长度为wr；车轮走一周的计数增量为resolution；

车辆前进距离：forward_dis=(cl*wl+cr*wr)*pi/resolution/2
车辆左右轮前进距离差：delta_dis=(-cl*wl+cr*wr)*pi/resolution
车辆转弯角度：delta_theta=delta_dis/BASE_length

这种计算方法在低速时相对轮速来说是靠谱的，但是对于车辆拐弯时左右轮速计数差受轮速计线数影响，会有一定程度出现车辆恰好同时计数加一，导致漏掉了宝贵的车辆拐弯增量。会导致yaw角变化不稳定不准确。从而对车辆的定位产生影响。

此外，此种计算方式还需要额外标定数据来保证定位精度：
标定数据有上文提到的：
左车轮长度为wl；
右车轮长度为wr；
车轮走一周的计数增量为resolution；

如何确定轮速计精度呢？上文提到了轮速计线数很关键；轮速线数表示的是编码器计数的最大值。这个值对精度起到关键性作用，数值越大精度越高。需要注意的是这个值只是关系到轮速向上计数何时归零的情况。不代表一周的轮速计数变化，与上述的resolution无关。

参考文章：

移动机器人差速轮运动学模型
两轮差速运动模型推导过程
运动模型分析：运动控制与里程计解算