两轮差速底盘SLAM系列(4)--两轮差速速度解算

前言

前面键盘控制节点在cmd_vel这个主题上发布了Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。本博文主要讲解如何把Twist消息转化为左右轮的期望速度，进而对机器人进行控制。

两轮差速运动解算

1、线速度分解

• 当线速度twist.linear.x=0时，机器人原地旋转：

  // 线速度=角速度∗半径 v = ωr
  Vr = Vθ*(wheel_dist/2.0); 
  Vl = (-1)*Vr;

• 当角速度twist.angular.z=0时，机器人前进/后退：

  Vr = Vl = twist.linear.x;

• 当线速度和角速度都不为零时，表示机器人做弧线运动：

  Vl = Vx-Vθ*(wheelDist/2.0);
  Vr = Vx+Vθ*(wheelDist/2.0);

  其中，Vx是指机器人人整体的线速度；Vθ是指机器人整体的角速度；wheelDist是指两个车轮之间的间距。Vx就是前进速度twist.linear.x，Vθ就是转弯速度twist.angular.z。

推导过程

关于上面的公式如何得来，推导过程如下：

假设两个轮子速度不一样，车子整体围绕半径为r做圆弧运动。

通过上面的推导公式，最后就可以得到通过机器人整体的线速度分解为两轮的线速度了。

2、角速度解算

在后面我们还需要通过左右轮差速转化为机器人的旋转速度，在关于航迹推演(Odometry) 的公式中有一个相关的转换计算公式：

**yaw_rate = (Rwheelspeed - Lwheelspeed) / d **

其中d为两轮间的间距，得到的转速单位rad/s，直接用这个公式就可以计算。