底盘运动学 差速轮系

双轮差速轮系

双轮差速轮系的移动机器人列表：

BalanceXE

   https://yltzdhbc.top/Icar

Icar

     https://yltzdhbc.top/BBOT

mazebot

     https://yltzdhbc.top/MazeBot

miniagv

     https://yltzdhbc.top/miniAGV

minicup

     https://yltzdhbc.top/Icar

轮系结构

轮系结构图如下：

相关物理量的定义如下：

• $p$点 ： 机器人旋转中心
• $o$点 ： 机器人几何中心
• $r$ ： 机器人旋转中心与几何中心连线的距离（m）
• $D$ ： 机器人左右轮之间的距离（m）
• $d$ ： 机器人几何中心到一个轮子的距离（m），$d=D/2$

相关变量的定义如下：

• $v_x$ ： 机器人局部坐标系x方向线速度（m/s）
• $v_y$ ： 机器人局部坐标系y方向线速度（m/s）
• $\omega$ ： 机器人局部坐标系绕z轴旋转角速度（rad/s）
• $v$ ： 双轮差速轮系中等效于$v_x$，机器人局部坐标系x方向线速度（m/s）
• $v_l$ ： 左轮线速度（m/s）
• $v_r$ ： 右轮线速度（m/s）

在刚体中，每一点的角速度相等，因此有

$$\omega = \omega_l = \omega_r \tag{1}$$

逆运动学

逆解已知量为 $\left[\begin{array}{l} v \\ \omega \end{array}\right]$， 逆解待解量为 $\left[\begin{array}{l} v_l \\ v_r \end{array}\right]$ 。

由几何关系不难得到

$$v_l = v - \omega d \tag{2}$$

$$v_l = v + \omega d \tag{3}$$

写为矩阵形式：

$$\left[\begin{array}{l} v_l \\ v_r \end{array}\right]=\left[\begin{array}{ccc} 1 & -d \\ 1 & d \end{array}\right] \cdot \left[\begin{array}{l} v \\ \omega \end{array}\right] \tag{4}$$

示例程序：

#define WHEEL_SEPARATION 0.032 // 车体中心到轮子中心距离
//*****************      m/s          m/s         rad/s
void ikine_diff2(float vel_x, float vel_y, float w_z)
{
  float vel_l, vel_r;

  vel_l = vel_x - w_z * WHEEL_SEPARATION;
  vel_r = vel_x + w_z * WHEEL_SEPARATION;
}

正运动学

正解已知量为 $\left[\begin{array}{l} v_l \\ v_r \end{array}\right]$ 待解量为 $\left[\begin{array}{l} v \\ \omega \end{array}\right]$ 。

由角速度与线速度的关系可知

$$v_l = \omega_l ( r-d ) \tag{5}$$

$$v_r = \omega_r ( r+d ) \tag{6}$$

通过式$(1)$已知$\omega_l = \omega_r$

联立式$(1)$、$(5)$、$(6)$可得旋转半径$r$：

$$\begin{aligned} &\frac{v_{l}}{r-d}=\frac{v_{r}}{r+d} \\ \Rightarrow &v_{l}(r+d)=v_{r}(r-d) \\ \Rightarrow &r=\frac{\left(v_{r}+v_{l}\right) d}{v_{r}-v_{l}} \end{aligned} \tag{7}$$

所以$r+d$可表示为：

$$r+d = \frac{\left(v_{r}+v_{l}\right) d}{v_{r}-v_{l}} + \frac{\left(v_{r}-v_{l}\right) d}{v_{r}-v_{l}} = \frac{2 v_r d}{v_{r}-v_{l}} \tag{8}$$

将式$(8)$带入式$(6)$中可得求出角速度$\omega$：

$$\omega = \frac{ v_r }{ r+d } = \frac{ v_r (v_r-v_l)}{2 v_r d} = \frac{ v_r-v_l }{2 d} \tag{9}$$

线速度$v$：

$$v=w \times r=\frac{v_{r}-v_{l}}{2 d} \times \frac{\left(v_{r}+v_{l}\right) d}{v_{r}-v_{l}}=\frac{v_{r}+v_{l}}{2} \tag{10}$$

写为矩阵形式为：

$$\left[\begin{array}{c} v \\ w \end{array}\right]=\left[\begin{array}{cc} 1/2 & 1/2 \\ -1/2d & 1/2d \end{array}\right] \cdot \left[\begin{array}{l} v_{l} \\ v_{r} \end{array}\right] \tag{11}$$

示例程序：

#define WHEEL_SEPARATION 0.032 // 车体中心到轮子中心距离

void fkine_diff2(float vel_l, float vel_r)
{
  float vel_x, float vel_y, float w_z;

  vel_x = (vel_r + vel_l) / 2;
  w_z = (vel_r - vel_l) / (2 * WHEEL_SEPARATION);
}

里程计

里程计关系图：

里程计积分

由几何关系图可得下式：

$$\left[\begin{array}{l} x^{\prime} \\ y^{\prime} \\ \theta^{\prime} \end{array}\right] = \left[\begin{array}{l} x \\ y \\ \theta \end{array}\right] + \left[\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] \cdot \left[\begin{array}{l} d x \\ d y \\ d \theta \end{array}\right]$$

上式中：

• $( x^{\prime}, y^{\prime}, \theta^{\prime} )$ 为当前时刻在全局坐标系下的位姿
• $(x, y, \theta)$ 为上一时刻在全局坐标系下的位姿
• $(d x, d y, d \theta)$ 在机器人局部坐标系下的运动增量

到此为止我们已得到里程计积分公式，里程计的计算过程就是不断的将固定采样间隔$T$时间内全局的$x$、$y$、$\theta$变化量进行累加。

在有了全局积分公式之后，我们需要求得每个周期内的位姿变化量，一般有两种方法，一是通过车体质点速度，二是直接通过编码器反馈角度，对双轮差速轮系来说，两种方法都适用，接下来分别进行推导。

速度求微分量（双轮差速）

将车体看做一个质点，通过正解解出的车体状态量$\left[\begin{array}{l} v \\ \omega \end{array}\right]$，在采样间隔时间&T&内使用微分的思想将机器人的运动看做匀速直线运动，则$(d x, d y, d \theta)$可以使用下式计算得出：

$$\begin{aligned} & dx = v \times T \\ & dy = 0 \\ & d\theta = \omega \times T \end{aligned}$$

写成矩阵形式为：

$$\left[\begin{array}{l} dx \\ dy \\ d\theta \end{array}\right] = \left[\begin{array}{cc} T & 0 \\ 0 & 0 \\ 0 & T \end{array}\right] \cdot \left[\begin{array}{l} v \\ \omega \end{array}\right]$$

里程求微分量（双轮差速）

轮子里程求微分量的意思是：我们可以直接获取每个轮子在单位时间$T$中的位移量，通过运动学关系计算出车体姿态变化量，相比通过编码器旋转角度微分计算速度，再通过速度积分计算车体位移，省去了微分计算速度的过程，计算量较小，且能够减少计算误差。

设在在单位时间$T$内，左轮转过的角度（Rad）为$dr_l$，右轮转过的角度（Rad）为$dr_r$，则有以下关系：

$$\begin{aligned} & ds_l = dr_l \times T \\ & ds_r = dr_r \times T \\ \end{aligned}$$

上式中，$ds_l$为左轮走过的距离（m），$ds_r$为右轮走过的距离（m），则单位时间内机器人局部坐标系的位移量可以由此计算得：

$$\begin{aligned} & dx = \frac{ ( ds_l + ds_r ) }{ 2 } = \frac{ ( dr_r + dr_l ) T }{ 2 } \\ & dy = 0 \\ & d\theta = \frac{ ( dr_r - dr_l ) T }{ D } \end{aligned}$$

上式中$D$为左右轮间距，写为矩阵形式：

$$\left[\begin{array}{l} dx \\ dy \\ d\theta \end{array}\right] = \left[\begin{array}{cc} T/2 & T/2 \\ 0 & 0 \\ -T/D & T/D \end{array}\right] \cdot \left[\begin{array}{l} dr_l \\ dr_r \end{array}\right]$$

综上，所有流程已打通，接下来是示例代码。

里程计示例代码

该函数中使用左右轮的角度来计算里程计，原始数据为last_diff_tick，含义为左右轮的编码器脉冲数；传入参数为diff_time，为该函数两次调用之间的时间间隔；顺便计算出了实时的速度$v$和角速度$w$，速度和位姿分别填入odom_pose数组和odom_vel数组中。

bool calcOdometry(double diff_time)
{
  float *orientation;
  double wheel_l, wheel_r; // rotation value of wheel [rad]
  double delta_s, theta, delta_theta;
  static double last_theta = 0.0;
  double v, w; // v = translational velocity [m/s], w = rotational velocity [rad/s]
  double step_time;

  wheel_l = wheel_r = 0.0;
  delta_s = delta_theta = theta = 0.0;
  v = w = 0.0;
  step_time = 0.0;

  step_time = diff_time;

  if (step_time == 0)
    return false;

  wheel_l = TICK2RAD * (double)last_diff_tick[LEFT];
  wheel_r = TICK2RAD * (double)last_diff_tick[RIGHT];

  if (isnan(wheel_l))
    wheel_l = 0.0;

  if (isnan(wheel_r))
    wheel_r = 0.0;

  delta_s = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;
  delta_theta = WHEEL_RADIUS * (wheel_r - wheel_l) / WHEEL_SEPARATION;

  // compute odometric pose
  odom_pose[2] += delta_theta;
  odom_pose[0] += delta_s * cos(odom_pose[2]);
  odom_pose[1] += delta_s * sin(odom_pose[2]);

  // compute odometric instantaneouse velocity
  v = delta_s / step_time;
  w = delta_theta / step_time;

  odom_vel[0] = v;
  odom_vel[1] = 0.0;
  odom_vel[2] = w;

  last_velocity[LEFT] = wheel_l / step_time;
  last_velocity[RIGHT] = wheel_r / step_time;

  return true;
}

双履带式轮系

未完待续

待续...

四轮差速轮系

未完待续

待续...