全方位移动全向轮、麦克纳姆轮底盘运动学逆解详解(内含电机输出方程)

全方位移动全向轮、麦克纳姆轮底盘运动学逆解详解(内含电机输出方程)

开始

    智能小车全方位移动底盘常用包括:全向轮底盘,麦克纳姆轮底盘(简称麦轮底盘)。以下内容只包括该两种底盘。
    由于特殊的结构,全向轮按轮数分为三轮全向轮,四轮全向轮。

输出方程介绍

  1. 电机输出方程是什么?
        顾名思义就是通过方程计算得到电机运动参数(转速,角度)的方程。确切的来说,作为一个函数,他的入口参数是(vx,vy,vz),通过方程实时计算,实时返回多个电机的转速或角度。
    2.电机输出方程有什么作用?
        首先,我们将小车全方位移动分解为Vx,Vy,Vz,方向上的速度。若想控制小车,只需输入相应方向速度,根据输出方程合成,计算结果输出给相应序号电机。

定义

1.定义一个xyx场地坐标系,一个XYZ机体坐标系.初始情况两坐标系中心重合,夹角α为0.

2.电机逆时针旋转为正值。

3.陀螺仪角度范围为-180-0-180.

4.不管哪种底盘,默认取左上角第一个轮子对应的电机为Motor1,顺时针旋转依次为Motor1,Motor2,Motor3,Motor4

底盘布局

三轮全向轮布局

图1 三轮全向轮布局
在这里插入图片描述

图2 四轮全向轮布局

在这里插入图片描述

图3 四轮麦轮布局

快速计算函数

#define ONE_PI   (3.14159265)

float angle_to_radian = 0.01745f;//角度转弧度

double mx_sin(double rad)
{   
	double sine;
	if (rad < 0)
		sine = rad * (1.27323954f + 0.405284735f * rad);
	else
		sine = rad * (1.27323954f - 0.405284735f * rad);
	if (sine < 0)
		sine = sine*(-0.225f * (sine + 1) + 1);
	else
		sine = sine * (0.225f *( sine - 1) + 1);
	return sine;
}

double my_sin(double rad)
{
	s8 flag = 1;
	
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		rad -= ONE_PI;
		flag = -1;
	}

	return mx_sin(rad) * flag;
}

double my_cos(double rad)
{
	s8 flag = 1;
	rad += ONE_PI/2.0;
   
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		flag = -1;
		rad -= ONE_PI;
	}
	return my_sin(rad)*flag;
}

主函数

void Task_Auto_Run(unsigned short int motor_mode)
{
	float a,b,c,ang_motion=imu[z].Relative_Dist;
	unsigned short int moto_num,motor_num,axis_num;
		
	switch (motor_mode)
	{
		//三轮全向轮
		case 1: a = (60 + ang_motion)*angle_to_radian; 
				b = (60 - ang_motion)*angle_to_radian; 
				c = (a - b)*0.5f;//c = (0 + ang_motion)*angle_to_radian;
				motor_num = 3;
				break;
		//四轮全向轮
		case 2: a = (60 + ang_motion)*angle_to_radian; 
				b = (60 - ang_motion)*angle_to_radian; 
				c = (a - b)*0.5f;//c = (0 + ang_motion)*angle_to_radian;
				motor_num = 4;
				break;
		//四轮麦轮
		case 3: a = (45 - ang_motion)*angle_to_radian;
				b = (45 + ang_motion)*angle_to_radian;
				c = a;
				motor_num = 4;
				break;
	}
	
	//合成公式
	speed[MOTOR1] = +my_cos(a)*motion[x].v + my_sin(a)*motion[y].v - motion[z].v;//1******2	 
	speed[MOTOR2] = +my_cos(b)*motion[x].v - my_sin(b)*motion[y].v - motion[z].v;//********
	speed[MOTOR3] = -my_cos(c)*motion[x].v - my_sin(c)*motion[y].v - motion[z].v;//********
	speed[MOTOR4] = -my_cos(b)*motion[x].v + my_sin(b)*motion[y].v - motion[z].v;//4******3	 	
}
//详细公式
//	/**************************************************三轮全向轮*****************************************************/
//	speed[MOTOR1] = (+my_cos((60+ang_motion)*angle_to_radian)*motion[x].v + my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//1********2	 /---------\
//	speed[MOTOR2] = (+my_cos((60-ang_motion)*angle_to_radian)*motion[x].v - my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//**********
//	speed[MOTOR3] = (-my_cos((0 -ang_motion)*angle_to_radian)*motion[x].v - my_sin((0 -ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//****3****       ——

//	/**************************************************四轮全向轮*****************************************************/
//	speed[MOTOR1] = (+my_cos((60+ang_motion)*angle_to_radian)*motion[x].v + my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//1******2	 /---------\
//	speed[MOTOR2] = (+my_cos((60-ang_motion)*angle_to_radian)*motion[x].v - my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//********
//	speed[MOTOR3] = (-my_cos((60+ang_motion)*angle_to_radian)*motion[x].v - my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//********
//	speed[MOTOR4] = (-my_cos((60-ang_motion)*angle_to_radian)*motion[x].v + my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//4******3	 \---------/

//	/***************************************************四轮麦轮**********************************************************/
//	speed[MOTOR1] = (+my_cos((45-ang_motion)*angle_to_radian)*motion[x].v + my_sin((45-ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//1******2	 \---------/
//	speed[MOTOR2] = (+my_cos((45+ang_motion)*angle_to_radian)*motion[x].v - my_sin((45+ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//********
//	speed[MOTOR3] = (-my_cos((45-ang_motion)*angle_to_radian)*motion[x].v - my_sin((45-ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//********
//	speed[MOTOR4] = (-my_cos((45+ang_motion)*angle_to_radian)*motion[x].v + my_sin((45+ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//4******3	 /---------\	
	

/* end of motor_ctrl.c */

最后

1.ang_motion=imu[z].Relative_Dist; //将陀螺仪z轴角度赋值参与运动方程解算。(一般可不用陀螺仪,只需将ang_motion赋值为0即可)
2.当将陀螺仪z轴数据赋值参与运算,需注意z轴数据方向,方程默认顺时针为正。使用陀螺仪即可解锁无头模式。机体坐标系的xy方向只与上电时初始方向有关,运动过程中不管如何旋转,方向不发生改变。参考RM步兵小陀螺,一边自旋一边运动。

    写得简陋,有时间补图,大体都在,推导过程繁琐且网上有大量资源本文不做阐述。这只是和学长交流中无意发现加以总结。

  • 46
    点赞
  • 361
    收藏
    觉得还不错? 一键收藏
  • 23
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 23
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值