ROS机器人学习——麦克纳姆轮运动学解算

麦克纳姆轮运动学解算

一、麦克纳姆轮介绍

了解过Robomaster的同学都知道,RM战车所用的轮子均为麦克纳姆轮,这种轮子安装方式与普通轮子无异,可安装于平行轴上,但是麦克纳姆轮可以实现全向移动,即前后运动水平移动绕中心自转。正因为以上优点,许多工业上的全向移动平台都会应用这种轮子。缺点也有,就是不耐磨,需要定期更换。

麦克纳姆轮由两部分组成:轮毂辊子,轮毂为轮子的主体,辊子为轮毂周围的类似椭球体的小轮子,轮毂和辊子都有自己的轴,且轮毂轴与辊子轴夹角为45°(可以为其他角度但45°角最为常见)

麦轮的安装方式也有讲究,虽然都是同轴安装,但与普通轮子不同,麦轮分为左旋和右旋两种,在一个四轮底盘上需要用两个左旋和两个右旋。安装方式为O型,如图所示:

ROS机器人学习——麦克纳姆轮运动学解算_第1张图片

左图为安装后你看到的样子,右图为四个轮子与地面接触的辊子围成的形状,也就是“O形”

这里的O形指的是与地面接触的辊子围成的形状噢,不要再问为什么左图看起来是个X了

二、麦克纳姆轮运动学模型

1. 基础知识

1.1 坐标系统

在ROS机器人中,坐标系统使用右手定义

ROS机器人学习——麦克纳姆轮运动学解算_第2张图片

对于ROS机器人,如果以它为坐标系的原点,那么

  • x轴:前方
  • y轴:左方
  • z轴:上方

如图所示:

ROS机器人学习——麦克纳姆轮运动学解算_第3张图片

除此之外,对于旋转运动,也使用右手定义:

ROS机器人学习——麦克纳姆轮运动学解算_第4张图片

根据右手定义,围绕 z轴正旋转逆时针旋转

1.2 测量单位

ROS使用公制 :

  • 线速度:m/s
  • 角速度:rad/s

1.3 轮子序号定义

左前1 右前2

左后3 右后4

2. 逆运动学解析

逆运动学模型(inverse kinematic model)得到的公式可以根据底盘的运动状态解算出四个轮子的速度。

2.1 底盘运动的分解

刚体在平面内的运动可以分解为三个独立分量:X轴平动、Y轴平动、yaw 轴自转。底盘的运动也可以分解为三个量:

如下图所示:

ROS机器人学习——麦克纳姆轮运动学解算_第5张图片

  • v t x v_{tx} vtx 表示 X 轴运动的速度,即前后方向,定义向前为正;
  • v t y v_{ty} vty 表示 Y 轴运动的速度,即左右方向,定义向左为正;
  • ω → \overrightarrow{\omega} ω 表示 yaw 轴自转的角速度,定义逆时针为正。

2.2 计算轮子轴心位置的速度

如下图所示,以右前轮为例,蓝色的方框代表轮子,定义以下变量:

  • r → \overrightarrow{r} r 为从底盘中心指向轮子轴心的矢量;
  • v → \overrightarrow{v} v 为轮子轴心的速度矢量;
  • v r → \overrightarrow{v_r} vr 为轮子轴心沿垂直于 r → \overrightarrow{r} r 的方向(即切线方向)的速度分量;
ROS机器人学习——麦克纳姆轮运动学解算_第6张图片

可以计算出:

KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \overrightarro…

r → \overrightarrow{r} r 分解为 r x r_x rx r y r_y ry,分别计算轮子轴心在X、Y轴的速度分量:

{ v x = v t x + ω ⋅ r y v y = v t y + ω ⋅ r x \left\{\begin{matrix} v_x=v_{tx}+\omega\cdot{r_y} \\ v_y=v_{ty}+\omega\cdot{r_x} \end{matrix}\right. {vx=vtx+ωryvy=vty+ωrx

其他三个轮子同理

2.3计算与地面接触的辊子速度

由2.2算得的轮子轴心速度,可以分解为沿辊子轴方向的 v ∥ → \overrightarrow{v_\parallel} v 和垂直辊子轴方向的 v ⊥ → \overrightarrow{v_\perp} v ,如图所示

ROS机器人学习——麦克纳姆轮运动学解算_第7张图片

其中 v ⊥ → \overrightarrow{v_\perp} v 用于让辊子空转,可以忽略

定义一个沿辊子方向的单位矢量 e ^ \hat{e} e^,对于右前轮来说, e ^ = 1 2 ⋅ i ^ + 1 2 ⋅ j ^ \hat{e}=\frac{1}{\sqrt{2}}\cdot\hat{i}+\frac{1}{\sqrt{2}}\cdot\hat{j} e^=2 1i^+2 1j^

则沿轴线的速度为 v → \overrightarrow{v} v e ^ \hat{e} e^方向的投影:

KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \overrightarr…

2.4 计算轮子的转速(和地面接触点的线速度)

如图所示,轮子转速为 v w v_w vw

ROS机器人学习——麦克纳姆轮运动学解算_第8张图片

由于辊子与轮轴呈45°角,则 v ω v_\omega vω可求得:

KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ v_w&=\frac{v_…

将2.2求出的 { v x = v t x + ω ⋅ r y v y = v t y + ω ⋅ r x \left\{\begin{matrix} v_x=v_{tx}+\omega\cdot{r_y} \\ v_y=v_{ty}+\omega\cdot{r_x} \end{matrix}\right. {vx=vtx+ωryvy=vty+ωrx带入上式,可求出此轮的转速:
v w = v t x + v t y + ω ( r x + r y ) v_w=v_{tx}+v_{ty}+\omega(r_x+r_y) vw=vtx+vty+ω(rx+ry)

结合以上四个步骤,可以根据底盘运动状态解算出四个轮子的转速:

{ v w 1 = v t x − v t y − ω ( r x + r y ) v w 2 = v t x + v t y + ω ( r x + r y ) v w 3 = v t x + v t y − ω ( r x + r y ) v w 4 = v t x − v t y + ω ( r x + r y ) \left\{\begin{matrix} v_{w1}=v_{tx}-v_{ty}-\omega(r_x+r_y)\\ v_{w2}=v_{tx}+v_{ty}+\omega(r_x+r_y)\\ v_{w3}=v_{tx}+v_{ty}-\omega(r_x+r_y)\\ v_{w4}=v_{tx}-v_{ty}+\omega(r_x+r_y) \end{matrix}\right. vw1=vtxvtyω(rx+ry)vw2=vtx+vty+ω(rx+ry)vw3=vtx+vtyω(rx+ry)vw4=vtxvty+ω(rx+ry)

以上方程组就是O形麦轮底盘的逆运动学模型。

2.5 代码实现

//参数宏定义
#define ENCODER_RESOLUTION      1440.0   //编码器分辨率, 轮子转一圈,编码器产生的脉冲数
#define WHEEL_DIAMETER          0.058    //轮子直径,单位:米
#define D_X                     0.18     //底盘Y轴上两轮中心的间距
#define D_Y                     0.25     //底盘X轴上两轮中心的间距
#define PID_RATE                50       //PID调节PWM值的频率

double pulse_per_meter = 0;
float rx_plus_ry_cali = 0.3;
double angular_correction_factor = 1.0;
double linear_correction_factor = 1.0;
double angular_correction_factor = 1.0;

/**
  * @函数作用:运动学解析参数初始化
  */
void Kinematics_Init(void)
{
	//轮子转动一圈,移动的距离为轮子的周长WHEEL_DIAMETER*3.1415926,编码器产生的脉冲信号为ENCODER_RESOLUTION。则电机编码器转一圈产生的脉冲信号除以轮子周长可得轮子前进1m的距离所对应编码器计数的变化
    pulse_per_meter = (float)(ENCODER_RESOLUTION/(WHEEL_DIAMETER*3.1415926))/linear_correction_factor;
    
    float r_x = D_X/2;
    float r_y = D_Y/2;
    rx_plus_ry_cali = (r_x + r_y)/angular_correction_factor;
}

/**
  * @函数作用:逆向运动学解析,底盘三轴速度-->轮子速度
  * @输入:机器人三轴速度 m/s
  * @输出:电机应达到的目标速度(一个PID控制周期内,电机编码器计数值的变化)
  */
void Kinematics_Inverse(int16_t* input, int16_t* output)
{
	float v_tx   = (float)input[0];
	float v_ty   = (float)input[1];
	float omega = (float)input[2];
	static float v_w[4] = {0};
	
	v_w[0] = v_tx - v_ty - (r_x + r_y)*omega;
	v_w[1] = v_tx + v_ty + (r_x + r_y)*omega;
	v_w[2] = v_tx + v_ty - (r_x + r_y)*omega;
	v_w[3] = v_tx - v_ty + (r_x + r_y)*omega;

    //计算一个PID控制周期内,电机编码器计数值的变化
	output[0] = (int16_t)(v_w[0] * pulse_per_meter/PID_RATE);
	output[1] = (int16_t)(v_w[1] * pulse_per_meter/PID_RATE);
	output[2] = (int16_t)(v_w[2] * pulse_per_meter/PID_RATE);
	output[3] = (int16_t)(v_w[3] * pulse_per_meter/PID_RATE);
}

3. 正运动学解析

3.1 正运动学模型

正运动学模型(forward kinematic model)让我们可以通过四个轮子的速度,计算出底盘的运动状态。可以直接根据逆运动学模型中的三个方程解出来,比如:

{ v t x = v 4 + v 3 2 v t y = v 3 − v 1 2 ω = v 2 − v 3 2 ( r x + r y ) \left\{\begin{matrix} v_{tx}=\frac{v_4+v_3}{2}\\ v_{ty}=\frac{v_3-v_1}{2}\\ \omega=\frac{v_2-v_3}{2(r_x+r_y)} \end{matrix}\right. vtx=2v4+v3vty=2v3v1ω=2(rx+ry)v2v3

转换为底盘坐标系下对时间求积分即为里程计变化量

3.2 代码实现

//参数宏定义
#define ENCODER_MAX 32767        
#define ENCODER_MIN -32768 
#define ENCODER_LOW_WRAP  ((ENCODER_MAX - ENCODER_MIN)*0.3+ENCODER_MIN)
#define ENCODER_HIGH_WRAP ((ENCODER_MAX - ENCODER_MIN)*0.7+ENCODER_MIN)
#define PI 3.1415926

//变量定义
int32_t  wheel_turns[4] = {0};
int32_t  encoder_sum_current[4] = {0};

/**
  * @函数功能:正向运动学解析,轮子编码值->底盘三轴里程计坐标
  * @输入:编码器累加值
  * @输出:三轴里程计 x y yaw
  */
void Kinematics_Forward(int16_t* input, int16_t* output)
{
	static double dv_w_times_dt[4]; //轮子瞬时变化量dxw=dvw*dt
	static double dv_t_times_dt[3]; //底盘瞬时变化量dxt=dvt*dt
	static int16_t encoder_sum[4];
	
    //将左面轮子编码器累加值乘以-1,以计算前进的距离
	encoder_sum[0] = -input[0];
	encoder_sum[1] = input[1];
	encoder_sum[2] = -input[2];
	encoder_sum[3] = input[3];
	
	//编码器计数溢出处理
	for(int i=0;i<4;i++)
	{
		if(encoder_sum[i] < ENCODER_LOW_WRAP && encoder_sum_current[i] > ENCODER_HIGH_WRAP)
			wheel_turns[i]++;
		else if(encoder_sum[i] > ENCODER_HIGH_WRAP && encoder_sum_current[i] < ENCODER_LOW_WRAP)
			wheel_turns[i]--;
		else
			wheel_turns[i]=0;
	}

	//将编码器数值转化为前进的距离,单位m
	for(int i=0;i<4;i++)
	{	
		dv_w_times_dt[i] = 1.0*(encoder_sum[i] + wheel_turns[i]*(ENCODER_MAX-ENCODER_MIN)-encoder_sum_current[i])/pulse_per_meter;
		encoder_sum_current[i] = encoder_sum[i];
	}
	
    //要计算坐标所以变回来
	dv_w_times_dt[0] = -dv_w_times_dt[0];
	dv_w_times_dt[1] =  dv_w_times_dt[1];
	dv_w_times_dt[2] = -dv_w_times_dt[2];
	dv_w_times_dt[3] =  dv_w_times_dt[3];
	
	//计算底盘坐标系(base_link)下x轴、y轴变化距离m与Yaw轴朝向变化rad 一段时间内的变化量
	dv_t_times_dt[0] = ( dv_w_times_dt[3] + dv_w_times_dt[2])/2.0;
	dv_t_times_dt[1] = ( dv_w_times_dt[2] - dv_w_times_dt[0])/2.0;
	dv_t_times_dt[2] = ( dv_w_times_dt[1] - dv_w_times_dt[2])/(2*wheel_track_cali);
	
	//积分计算里程计坐标系(odom_frame)下的机器人X,Y,Yaw轴坐标
	//dx = ( vx*cos(theta) - vy*sin(theta) )*dt
	//dy = ( vx*sin(theta) + vy*cos(theta) )*dt
	output[0] += (int16_t)(cos((double)output[2])*dv_t_times_dt[0] - sin((double)output[2])*dv_t_times_dt[1]);
	output[1] += (int16_t)(sin((double)output[2])*dv_t_times_dt[0] + cos((double)output[2])*dv_t_times_dt[1]);
	output[2] += (int16_t)(dv_t_times_dt[2]*1000);
		
    //Yaw轴坐标变化范围控制-2Π -> 2Π
	if(output[2] > PI)
		output[2] -= 2*PI;
	else if(output[2] < -PI)
		output[2] += 2*PI;
		
	//发送机器人X轴y轴Yaw轴瞬时变化量,在ROS端除以时间
	output[3] = (int16_t)(dv_t_times_dt[0]);
	output[4] = (int16_t)(dv_t_times_dt[1]);
	output[5] = (int16_t)(dv_t_times_dt[2]);
}

参考文献:

【1】https://zhuanlan.zhihu.com/p/20282234

【2】https://blog.csdn.net/shixiaolu63/article/details/78496457

你可能感兴趣的:(自动驾驶,机器学习,人工智能)