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

开始
输出方程介绍
定义
底盘布局
快速计算函数
主函数
最后

开始

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

输出方程介绍

电机输出方程是什么？
顾名思义就是通过方程计算得到电机运动参数（转速，角度）的方程。确切的来说，作为一个函数，他的入口参数是（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);elsesine = rad * (1.27323954f - 0.405284735f * rad);if (sine < 0)sine = sine*(-0.225f * (sine + 1) + 1);elsesine = 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步兵小陀螺，一边自旋一边运动。

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