开始
智能小车全方位移动底盘常用包括:全向轮底盘,麦克纳姆轮底盘(简称麦轮底盘)。以下内容只包括该两种底盘。
由于特殊的结构,全向轮按轮数分为三轮全向轮,四轮全向轮。
输出方程介绍
- 电机输出方程是什么?
顾名思义就是通过方程计算得到电机运动参数(转速,角度)的方程。确切的来说,作为一个函数,他的入口参数是(vx,vy,vz),通过方程实时计算,实时返回多个电机的转速或角度。
2.电机输出方程有什么作用?
首先,我们将小车全方位移动分解为Vx,Vy,Vz,方向上的速度。若想控制小车,只需输入相应方向速度,根据输出方程合成,计算结果输出给相应序号电机。
定义
1.定义一个xyx场地坐标系,一个XYZ机体坐标系.初始情况两坐标系中心重合,夹角α为0.
2.电机逆时针旋转为正值。
3.陀螺仪角度范围为-180-0-180.
4.不管哪种底盘,默认取左上角第一个轮子对应的电机为Motor1,顺时针旋转依次为Motor1,Motor2,Motor3,Motor4
底盘布局
快速计算函数
#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步兵小陀螺,一边自旋一边运动。
写得简陋,有时间补图,大体都在,推导过程繁琐且网上有大量资源本文不做阐述。这只是和学长交流中无意发现加以总结。