电机速度获取

下位机驱动电机，单片机可以检测高低电平的变化，得到脉冲数，要将脉冲数转换成轮子实际距离需要测量轮子转一圈所产生的脉冲数。通过多次测试可以得到转一圈的编码脉冲数1930，根据轮子直径65mm，可以计算得到一个脉冲数为多少距离（65*pai/1930=0.105805 mm）

将系数转换成速度（10ms累计法）

电机速度v = 10ms内编码器的脉冲数差值 * 系数0.105805 / 时间10ms

实际会由于电机特性不同，即使设置相同占空比，但是左右电机速度会表现出来不一样，使用PID进行控制。

运动正逆学解--两轮

问题：如何把目标的角速度和线速度转换成机器人两个轮子的速度，这一个过程称之为机器人运动学逆解，反之，通过轮子实际的速度来计算机器人的线速度和角速度的过程为运动学正解

正解

假设机器人在一小段时间t内，它的左右轮子线速度为v_l和v_r，两轮子间d_wb，求机器人的线速度v，角速度w

机器人的线速度方向和轮子转动方向始终保持一致，所以机器人的线速度为左右轮线速度的平均值，即v=(v_l+v_r)/2，我们知道线速度和角速度的关系是v=w*r

可知d_wb=r_r-r_l=v_r/w_r - v_l/w_l，

由于机器人移动时各轮角速度相同，所以w_r=w_l，结合前面的公式可以求出w=(v_r-v_l)/d_wb。至此我们得出了左右轮速度和机器人速度之间的关系如下所示

v_l = v - w/2

v_r = v + w/2

根据上述可以得到机器人的速度为

v = (v_l + v_r)/2

w= (v_r - v_l)/2

角速度w也是有方向的：当vl < vr时，w>0；反之，w<0。

梯形中位线意义：从几何的角度解释公式v = (v_l + v_r)/2，图 中的[vl vr]分别表示梯形的上下底，vc表示该梯形的中位线，所以vc的长度就是梯形上下底之和除以2，也就是公式的形式。

结合公式,点CENTER的转向半径rc可表示为

这里的d_wb就是间距

由此，基于点CENTER沿LR直线方向确定ICR的具体位置。这也印证上述提到的“ICR在直线LR上的具体位置由左右驱动轮速度[vl vr]确定”。

正运动学模型是基于左右驱动轮的速度来计算几何中心点CENTER的速度，综合公式（2-3）可表示为

逆运动学模型是基于几何中心点CENTER的速度分解出左右驱动轮的速度，可表示为

公式同样满足当w=0的情况，vc =0 且 w≠0时，机器人处于自旋状态。感兴趣的读者可以尝试验证。

那问题来了，在实际中这两个模型（5-6）是怎么在实际工程中体现的呢？

正运动学模型应用：计算机器人CENTER的速度。差速机器人的两个驱动轮会安装编码器，可以计算出两个驱动轮的线速度[vl vr]，基于公式（5）就可以计算得到机器人CENTER的速度，用于表示机器人当前的运动速度，并可进一步被用于计算机器人的里程计（里程计计算会在后续文章中更新）。

逆运动学模型应用：用于控制机器人运动。如控制机器人CENTER以设定的速度[vc w]T运动，那就通过公式（6）计算得到两个驱动轮的理论速度，再采用PID算法控制驱动轮精确转动。

ROS官网提供diff_drive_controller软件包[3],已经内置了两轮差速机器人的正逆运动学模型，仅需要根据说明配置好参数后，便可控制仿真器中的TurtleBot或自己设计的两轮差速机器人了

结构体内容包括沿着机器人坐标系x-y-z轴的线速度和角速度，具体为：

但对于两轮差速机器人而言，仅需两个参数：

Vetor3 linear
  float64 x
  float64 y
  float64 z
Vetor3 angular
  float64 x
  float64 y
  float64 z

沿x轴的线速度：linear. x = vc

绕z轴的角速度：angular.z = w

两轮差速驱动机器人虽然仅有两个驱动轮，但是会根据应用场景的需求搭配万向轮作为支撑。

如图 4.1所示，常见的差速底盘有以下4种类型，其中(a)(b)的底盘轮廓是圆形，而(c)(d)的底盘轮廓是矩形。

图 4.1 不同类型的两轮差速平台.圆形表示万向轮，矩形表示驱动轮。

不同的构型在机器人运动稳定性、负载能力等方面有着不同的表现，其应用场景也有区别。需要注意的是：在未对底盘做特殊设计的情况下，万向轮的轮子直径远小于驱动轮的直径，这是万向轮特殊的构造引起的，而轮子的大小极大决定了机器人的越障能力，所以差速机器人一般应用于室内场景中，且没有没有沟壑、门槛等。

图 4.1(a)中的构型是前后采用万向轮，两侧是驱动轮，4个轮子需要使用悬挂，否则容易出现任一轮子悬空打滑的情况；而图 4.1(b)则去掉了1个万向轮，也就无需使用悬挂，为了弥补稳定性，将两驱动轮后移了小段距离。

图 4.1(c)则将两万向轮前置，驱动轮后置，车上的长度可以较长，但也需要悬挂系统。由于旋转中心和重心相差较远，因此机器人旋转运动性能偏弱；而为了弥补这一缺陷，图 4.1(d)使用了4个万向轮，布置于前后，在对称轴处布置驱动轮，这样便保证旋转中心和重心重合，且能做到车身更长。

相较于圆形轮廓，矩形轮廓底盘在狭窄区域的通过性偏差，如图 4.2所示，当矩形轮廓机器人旋转时扫过的区域半径接近机身长度，而圆形轮廓机器人旋转时扫过的区域半径接近机身半径。因此，圆形轮廓的机器人应用场景会更丰富，比如家庭服务、实验室搬运等包含有狭窄过道的场景。而矩形轮廓机器人则多用于仓储物流搬运场景。

图 4.2 矩形轮廓机器人逆时针旋转

经过对两轮差速机器人的对比分析，可知差速机器人应用广泛的原因是：机械结构简单、仅使用2个电机驱动，运动控制模型简洁，实际控制程序简单，成本较低，且作为移动平台，又能满足一般功能需求。

而差速机器人多用于室内场景的原因是：灵活性较强，狭窄区域通过性强，但越障能力偏弱，负载能力有限等。