当前位置：网站首页>ROS机器人学习——麦克纳姆轮运动学解算

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

2022-04-22 13:01:00 【范子琦】

麦克纳姆轮运动学解算

一、麦克纳姆轮介绍

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

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

ML-1

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

ML-2

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

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

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

1. 基础知识

1.1 坐标系统

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

ML-3

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

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

如图所示：

ML-4

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

ML-5

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

1.2 测量单位

ROS使用公制 ：

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

1.3 轮子序号定义

左前1 右前2

左后3 右后4

2. 逆运动学解析

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

2.1 底盘运动的分解

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

如下图所示:

ML-6

  • 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 的方向（即切线方向）的速度分量；
ML-7

可以计算出：

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​+ω⋅ry​vy​=vty​+ω⋅rx​​

其他三个轮子同理

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

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

ML-8

其中 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 ​1​⋅i^+2 ​1​⋅j^​

则沿轴线的速度为 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​

ML-9

由于辊子与轮轴呈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​+ω⋅ry​vy​=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​=vtx​−vty​−ω(rx​+ry​)vw2​=vtx​+vty​+ω(rx​+ry​)vw3​=vtx​+vty​−ω(rx​+ry​)vw4​=vtx​−vty​+ω(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​+v3​​vty​=2v3​−v1​​ω=2(rx​+ry​)v2​−v3​​​

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

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

版权声明
本文为[范子琦]所创，转载请带上原文链接，感谢
https://blog.csdn.net/oXiaoLingTong/article/details/120198677

边栏推荐

猜你喜欢

随机推荐