Skip to main content

底盘运动学 差速轮系

双轮差速轮系

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

   https://yltzdhbc.top/IcarExample

轮系结构

轮系结构图如下:

相关物理量的定义如下:

  • pp点 : 机器人旋转中心
  • oo点 : 机器人几何中心
  • rr : 机器人旋转中心与几何中心连线的距离(m)
  • DD : 机器人左右轮之间的距离(m)
  • dd : 机器人几何中心到一个轮子的距离(m),d=D/2d=D/2

相关变量的定义如下:

  • vxv_x : 机器人局部坐标系x方向线速度(m/s)
  • vyv_y : 机器人局部坐标系y方向线速度(m/s)
  • ω\omega : 机器人局部坐标系绕z轴旋转角速度(rad/s)
  • vv : 双轮差速轮系中等效于vxv_x,机器人局部坐标系x方向线速度(m/s)
  • vlv_l : 左轮线速度(m/s)
  • vrv_r : 右轮线速度(m/s)

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

ω=ωl=ωr(1)\omega = \omega_l = \omega_r \tag{1}

逆运动学

逆解已知量为 [vω]\left[\begin{array}{l} v \\ \omega \end{array}\right], 逆解待解量为 [vlvr]\left[\begin{array}{l} v_l \\ v_r \end{array}\right]

由几何关系不难得到

vl=vωd(2)v_l = v - \omega d \tag{2}
vl=v+ωd(3)v_l = v + \omega d \tag{3}

写为矩阵形式:

[vlvr]=[1d1d][vω](4)\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;
}

正运动学

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

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

vl=ωl(rd)(5)v_l = \omega_l ( r-d ) \tag{5}
vr=ωr(r+d)(6)v_r = \omega_r ( r+d ) \tag{6}

通过式(1)(1)已知ωl=ωr\omega_l = \omega_r

联立式(1)(1)(5)(5)(6)(6)可得旋转半径rr

vlrd=vrr+dvl(r+d)=vr(rd)r=(vr+vl)dvrvl(7)\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+dr+d可表示为:

r+d=(vr+vl)dvrvl+(vrvl)dvrvl=2vrdvrvl(8)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)(8)带入式(6)(6)中可得求出角速度ω\omega

ω=vrr+d=vr(vrvl)2vrd=vrvl2d(9)\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}

线速度vv

v=w×r=vrvl2d×(vr+vl)dvrvl=vr+vl2(10)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}

写为矩阵形式为:

[vw]=[1/21/21/2d1/2d][vlvr](11)\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);
}

里程计

里程计关系图:

里程计积分

由几何关系图可得下式:

[xyθ]=[xyθ]+[cosθsinθ0sinθcosθ0001][dxdydθ]\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,y,θ)( x^{\prime}, y^{\prime}, \theta^{\prime} ) 为当前时刻在全局坐标系下的位姿
  • (x,y,θ)(x, y, \theta) 为上一时刻在全局坐标系下的位姿
  • (dx,dy,dθ)(d x, d y, d \theta) 在机器人局部坐标系下的运动增量

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

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

速度求微分量(双轮差速)

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

dx=v×Tdy=0dθ=ω×T\begin{aligned} & dx = v \times T \\ & dy = 0 \\ & d\theta = \omega \times T \end{aligned}

写成矩阵形式为:

[dxdydθ]=[T0000T][vω]\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]

里程求微分量(双轮差速)

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

设在在单位时间TT内,左轮转过的角度(Rad)为drldr_l,右轮转过的角度(Rad)为drrdr_r,则有以下关系:

dsl=drl×Tdsr=drr×T\begin{aligned} & ds_l = dr_l \times T \\ & ds_r = dr_r \times T \\ \end{aligned}

上式中,dslds_l为左轮走过的距离(m),dsrds_r为右轮走过的距离(m),则单位时间内机器人局部坐标系的位移量可以由此计算得:

dx=(dsl+dsr)2=(drr+drl)T2dy=0dθ=(drrdrl)TD\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}

上式中DD为左右轮间距,写为矩阵形式:

[dxdydθ]=[T/2T/200T/DT/D][drldrr]\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,为该函数两次调用之间的时间间隔;顺便计算出了实时的速度vv和角速度ww,速度和位姿分别填入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;
}

双履带式轮系

未完待续

待续...

四轮差速轮系

未完待续

待续...