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

开始

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

输出方程介绍

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

定义

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

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

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

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

底盘布局

图1 三轮全向轮布局

快速计算函数

#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步兵小陀螺，一边自旋一边运动。

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