前言
两年的竞赛生涯一晃而过，不知不觉我也升入了大三，这两天翻出了十六届比赛时写的代码，打算做一下归纳整理，也算是为实验室生活画个句号。
麦轮基本原理
麦克纳姆轮（Mecanum wheel），这种全方位移动方式是基于一个有许多位于机轮周边的轮轴的中心轮的原理上，这些成角度的周边轮轴把一部分的机轮转向力转化到一个机轮法向力上面。
基于麦克纳姆轮技术的全方位运动设备可以实现前行、横移、斜行、旋转及其组合等运动方式。
基本控制算法
速度解算
我们可以将麦轮车的部分分为前进方向（X或Y）和转向方向（Z），在摄像头判断出前方路况后得到X,Y,Z三轴的速度后，需要将车身的X,Y,Z三轴速度分配给四个轮子，以便于后续的控制，这个过程就是速度解算。
速度解算的核心是要得到逆运动学方程矩阵，以便于将导航速度向量映射到四轮角速度向量上。即模型输入为导航向量（车身X,Y,Z三轴速度），输出量为小车四个轮子的转动速度。
（此处推导参考论文《Mecanum轮逆矩阵研究》）
如上图为麦轮运动方程建模，L为车长，W为车宽，车轮顺序为左上，右上，左下，右下。
逆运动学方程如下：
将上式列成表达式为：
由于在程序控制过程中可以根据PID控制系数来改变speed大小，所以我们可以简化上图公式中的系数，并自己合理调整，便于后续参数调节。改动后的程序如下。
/**
* @file 速度解算，由逆矩阵公式解算
* @note 参照论文《Mecanum轮逆矩阵研究》
* @date 2021
*/
void Speed_Calculation(int16 X_speed, int16 Y_speed, int16 Z_speed)
{
if(Z_speed>100) Z_speed = 100;
else if(Z_speed < -100) Z_speed = -100; // 对转向速度限幅
SpeedSet1= X_speed - 0.3*Z_speed- Y_speed;
SpeedSet2= X_speed + 0.3*Z_speed+ Y_speed;
SpeedSet3= X_speed - 0.3*Z_speed+ Y_speed;
SpeedSet4= X_speed + 0.3*Z_speed- Y_speed;
}
经典pid算法
//PID 参数初始化
void PID_Init(PID *SPID)
{
SPID->SumError = 0;
SPID->LastError = 0; //Error[k-1]
SPID->PrevError = 0; //Error[k-2]
SPID->Proportion = 0; //比例常数 Proportional Const
SPID->Integral = 0; //积分常数 Integral Const
SPID->Derivative = 0; //微分常数 Derivative Const
}
int16 PID_Increase(PID *SPID,int16 NextPoint,int16 NowData)
{
int16 iError,iIncpid; //当前误差、累计增量
iError = NextPoint - NowData; //目标速度-当前速度
iIncpid = (int16)( (iError-SPID->LastError)* (SPID->Proportion)
+ iError*(SPID->Integral)
+ (iError-2*SPID->LastError+SPID->PrevError) * (SPID->Derivative));
SPID->PrevError = SPID->LastError; //E(k-2)
SPID->LastError = iError; //E(k-1)
return(iIncpid); //返回增量值
}
int16 PID_Realize(PID *SPID,float iError)
{
int16 place;//dError;
//int16 iError;
//iError = NextPoint - NowData; //偏差
SPID->SumError += iError; //积分
//dError = iError - SPID->LastError; //微分
//SPID->LastError = iError;
place = (int16)(SPID->Proportion * iError //比例项
+ SPID->Integral * SPID->SumError); //积分项
//+ SPID->Derivative * icm_gyro_y); //微分项
return place;
}
控制算法pro
运用陀螺仪的循迹算法
首先要使用陀螺仪控制，这里可能会涉及到陀螺仪初始化过不了的问题，如果用的是逐飞科技的陀螺仪，可以尝试一下以下方法。
这里使用到了两层pid控制，首先用第一层pid算出的turn_R，作为前方道路曲率的倒数，再根据车速及曲率导出应有角速度，与陀螺仪测出的实时角速度比较，代入第二层pid，实现最终控制。代码如下。
void Dir_Speed_Control(float Calculate_Center)
{
turn_R = PID_Realize(&PID1,g_fMid_AD);
Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
w = turn_R * Car_speed;
SpeedSet_Z = PID_Realize(&PID2,icm_gyro_z-w);
if(SpeedSet_Z<-100)
SpeedSet_Z = -100;
}
变速控制
为了最大程度发挥小车性能，更快完成赛道，可引入变速控制，具体方案如下。
SpeedSet_X = K_speed*(84/10 - Error/10) + PWM_default;
PWM_default为设定最低速度，error为摄像头算出的偏差（我的摄像头图像宽度为170，所以最大偏差为84），并通过系数K_speed调节速度上限。另外如果想要控制响应更迅速，也可以考虑使用平方项。
驱动部分的总结到此就告一段落了，相对而言这部分的变换还是相对较少的，主要还是要注意参数的调试，也可以尝试着把电机数据发送到上位机辅助调试。
附录：Motor.c代码
#include "headfile.h"
/*********速度测量相关初始化**********/
int32 speed1_power;
int32 speed2_power;
int32 speed3_power;
int32 speed4_power;
int16 encoder_data[4];
int16 Pulse_Filtered_data1=0;
int16 Pulse_Filtered_data2=0;
int16 Pulse_Filtered_data3=0;
int16 Pulse_Filtered_data4=0;
/*********速度解算相关初始化**********/
float L=0.186,W=0.183; //车尺寸，单位m
//int R=28;
/*********速度控制相关初始化**********/
int16 SpeedSet1;
int16 SpeedSet2;
int16 SpeedSet3;
int16 SpeedSet4;
int16 SpeedSet_X;
int16 SpeedSet_Y;
int16 SpeedSet_Z;
PID Motor1={0,0,0, 16, 3, 3};
PID Motor2={0,0,0, 16, 3, 3};
PID Motor3={0,0,0, 16, 3, 3};
PID Motor4={0,0,0, 16, 3, 3};
PID Dir_Speedr1 ={0,0,0, 11, 0, 0.1}; //右转向
PID Dir_Speedr2 ={0,0,0, 0.002, 0, 0.0002};
PID Dir_Speedl1 ={0,0,0, 10, 0, 0.1}; //左转向
PID Dir_Speedl2 ={0,0,0, 0.002, 0, 0.0002};
PID Roundr1 ={0,0,0, 10, 0, 0}; //右环
PID Roundr2 ={0,0,0, 0.0029, 0, 0.0002 };
PID Roundl1 ={0,0,0, 9, 0, 0}; //左环
PID Roundl2 ={0,0,0, 0.0028, 0, 0.0001};
PID chalul ={0,0,0, 0.37, 0, 0.06};
PID chalur ={0,0,0, 0.37, 0, 0.06};
/**
* @file 相关外设初始化
* @note 电机&&编码器
* @date 2021
*/
void Motor_Init(void)
{
gpio_init(MOTOR1_A, GPO, 0, GPIO_PIN_CONFIG);
pwm_init(MOTOR1_B,MOTOR_HZ,0);
gpio_init(MOTOR2_A, GPO, 0, GPIO_PIN_CONFIG);
pwm_init(MOTOR2_B,MOTOR_HZ,0);
gpio_init(MOTOR3_A, GPO, 0, GPIO_PIN_CONFIG);
pwm_init(MOTOR3_B,MOTOR_HZ,0);
gpio_init(MOTOR4_A, GPO, 0, GPIO_PIN_CONFIG);
pwm_init(MOTOR4_B,MOTOR_HZ,0);
}
void Encoder_Init(void)
{
encoder_init_spi(ABS_ENCODER_SPI_PC1_PIN);
encoder_init_spi(ABS_ENCODER_SPI_PC2_PIN);
encoder_init_spi(ABS_ENCODER_SPI_PC3_PIN);
encoder_init_spi(ABS_ENCODER_SPI_PC4_PIN);
}
/**
* @file 相关测试
* @note 电机&&编码器
* @date 2021
*/
void Test_Motor()
{
ips114_showstr(0, 0, "20191114 mTX-Motortest");
Key_Read();
ips114_showstr(0,1,"Motor-Press B2:");
ips114_showint32(12*12,1,test1,1);
ips114_showstr(0,2,"Speed-Press B4:");
ips114_showint32(12*12,2,test2,1);
ips114_showstr(0,3,"Speed1:"); ips114_showint32(12*8,3,Pulse_Filtered_data1,4);
ips114_showstr(0,4,"Speed2:"); ips114_showint32(12*8,4,Pulse_Filtered_data2,4);
ips114_showstr(0,5,"Speed3:"); ips114_showint32(12*8,5,Pulse_Filtered_data3,4);
ips114_showstr(0,6,"Speed4:"); ips114_showint32(12*8,6,Pulse_Filtered_data4,4);
if(key1_flag==1&&key2_flag==0&&key3_flag==0)
{
test1++;
key1_flag=0;
ips114_showstr(0,1,"Motor-Press B2:");
ips114_showint32(12*12,1,test1,1);
switch(test1)
{
case 1:
Set_Motor(300,0,0,0);
systick_delay_ms(1000);
Motor_Init();
Set_Motor(-300,0,0,0);
systick_delay_ms(1000);
break;
case 2:
Set_Motor(0,300,0,0);
systick_delay_ms(1000);
Motor_Init();
Set_Motor(0,-300,0,0);
systick_delay_ms(1000);
break;
case 3:
Set_Motor(0,0,300,0);
systick_delay_ms(1000);
Motor_Init();
Set_Motor(0,0,-300,0);
systick_delay_ms(1000);
break;
case 4:
Set_Motor(0,0,0,300);
systick_delay_ms(1000);
Motor_Init();
Set_Motor(0,0,0,-300);
systick_delay_ms(1000);
test1=0;
break;
default:
break;
}
}
if(key1_flag==0&&key2_flag==1&&key3_flag==0)
{
test2++;
key2_flag=0;
ips114_showstr(0,2,"Speed-Press B4:");
ips114_showint32(12*12,2,test2,1);
switch(test2)
{
case 1:
Set_Motor(100,100,100,100);
systick_delay_ms(1000);
break;
case 2:
Set_Motor(200,200,200,200);
systick_delay_ms(1000);
break;
case 3:
Set_Motor(300,300,300,300);
systick_delay_ms(1000);
test2=0;
break;
default:
break;
}
}
}
void Test_Encoder(void)
{
ips114_showstr(0, 0, "20191114 mTX-Encodertest");
//Speed_Measure();
//PulseFilter(encoder_data[0],encoder_data[1],encoder_data[2],encoder_data[3]);
Set_Motor(200,200,200,200);
ips114_showstr(0,1,"Speed1:"); ips114_showint32(12*8,1,Pulse_Filtered_data1,4);
ips114_showstr(0,2,"Speed2:"); ips114_showint32(12*8,2,Pulse_Filtered_data2,4);
ips114_showstr(0,3,"Speed3:"); ips114_showint32(12*8,3,Pulse_Filtered_data3,4);
ips114_showstr(0,4,"Speed4:"); ips114_showint32(12*8,4,Pulse_Filtered_data4,4);
}
/**
* @file 电机总控制
* @note
* @date 2021
*/
int16 PWM_default;
float Error;
float K_speed=0.15f;
int Fork_road_flag;
int turn_flag;
int time;
int chuku=1;
int time_chuku;
void intrgral(int16 gr);
int t_cha;
int chalu_finish;
void MotorControl(void)
{
if(chuku)
{
SpeedSet_X=50;
SpeedSet_Y=0;
SpeedSet_Z=100;
intrgral(icm_gyro_z);
if(fabs(Angle)>90)
{
Angle = 0;
chuku = 0;
}
}
if(chuku == 0)
{
Error = fabs(g_fMid_AD);
if(!chalu_flag&&!Startline&&!turn_flag&&!Fork_road_flag)
{
SpeedSet_X = K_speed*(84/10 - Error/10)*(84/10 - Error/10) + PWM_default;
Dir_Speed_Control(g_fMid_AD);
}
if((chalu_flag==3&&turn_flag==0)||(chalu_flag==4&&turn_flag==0)
||(chalu_flag==7&&turn_flag==0)||(chalu_flag==8&&turn_flag==0))//进三岔前停车
{
time++;
SpeedSet_X=0;
SpeedSet_Y=0;
SpeedSet_Z=0;
if(time > 100)
{
Angle = 0;
turn_flag = 1;
time = 0;
}
}
//Stopcar();
if(chalu_flag==3&&turn_flag==1)//左进三岔
{
SpeedSet_X=0;
SpeedSet_Y=0;
SpeedSet_Z=-100;
if(fabs(Angle)>40)
{
Fork_road_flag=1;
turn_flag = 0;
Angle=0;
g_fMid_AD=0;
}
}
if(chalu_flag==4&&turn_flag==1)//右进三岔
{
SpeedSet_X=0;
SpeedSet_Y=0;
SpeedSet_Z=-100;
if(fabs(Angle)>145)
{
Fork_road_flag=1;
turn_flag = 0;
Angle=0;
g_fMid_AD=0;
}
}
if(Fork_road_flag==1)//三岔中
{
SpeedSet_X=0;
SpeedSet_Y=65;
if(SpeedSet_Z<0) //右转
SpeedSet_Z = -PID_Realize(&chalur,g_fMid_AD*fabs(g_fMid_AD));
else
SpeedSet_Z = -PID_Realize(&chalul,g_fMid_AD*fabs(g_fMid_AD));
}
if(chalu_flag==7&&turn_flag==1)//左出三岔
{
SpeedSet_X=0;
SpeedSet_Y=0;
SpeedSet_Z=100;
if(fabs(Angle)>115)
{
chalu_finish = 1;
Fork_road_flag=0;
turn_flag = 0;
chalu_flag = 9;
g_fMid_AD = 0;
Angle=0;
}
}
if(chalu_flag==8&&turn_flag==1)//右出三岔
{
SpeedSet_X=0;
SpeedSet_Y=0;
SpeedSet_Z=-100;
if(fabs(Angle)>155)
{
chalu_finish = 1;
Fork_road_flag=0;
turn_flag = 0;
chalu_flag = 10;
g_fMid_AD=0;
}
}
if(chalu_flag == 9 || chalu_flag ==10)
{
if(t_cha == 0)
t_cha = time_count;
if (time_count - t_cha <= 100)
{
SpeedSet_X = 30;
SpeedSet_Y = 0;
Dir_Speed_Control(g_fMid_AD);
}
else if (time_count - t_cha > 100) {
chalu_flag = 0;
}
}
if(Startline == 1&&round_count == 0)
finall_front();
if(Startline == 1&&round_count == 1)
garage_in();
}
Speed_Calculation(SpeedSet_X, SpeedSet_Y, SpeedSet_Z);
Speed_Measure();
//PulseFilter(encoder_data[0],encoder_data[1],encoder_data[2],encoder_data[3]);
speed1_power += PID_Increase(&Motor1,SpeedSet1,encoder_data[0]);
speed2_power += PID_Increase(&Motor2,SpeedSet2,encoder_data[1]);
speed3_power += PID_Increase(&Motor3,SpeedSet3,encoder_data[2]);
speed4_power += PID_Increase(&Motor4,SpeedSet4,encoder_data[3]);
if(speed1_power > 500) speed1_power = 500;
else if(speed1_power < -500) speed1_power = -500;
if(speed2_power > 500) speed2_power = 500;
else if(speed2_power < -500) speed2_power = -500;
if(speed3_power > 500) speed3_power = 500;
else if(speed3_power < -500) speed3_power = -500;
if(speed4_power > 500) speed4_power = 500;
else if(speed4_power < -500) speed4_power = -500;
Set_Motor(speed1_power, speed2_power, speed3_power, speed4_power);
}
/**
* @file 速度测量
* @note 角速度传感器（绝对式编码器） 范围0-1023
* @date 2021
*/
void Speed_Measure(void)
{
encoder_data[0] = -encoder1_speed_spi(ABS_ENCODER_SPI_PC1_PIN);
encoder_data[1] = encoder2_speed_spi(ABS_ENCODER_SPI_PC2_PIN);
encoder_data[2] = -encoder3_speed_spi(ABS_ENCODER_SPI_PC3_PIN);
encoder_data[3] = encoder4_speed_spi(ABS_ENCODER_SPI_PC4_PIN);
//data_conversion(encoder_data[0], encoder_data[1], encoder_data[2], encoder_data[3], virtual_scope_data);
}
/**
* @file 编码器滤波
* @note
* @date 2021
*/
void PulseFilter(int data1,int data2,int data3,int data4)
{
static int Pre_Pulse_data1[4],Pre_Pulse_data2[4],
Pre_Pulse_data3[4],Pre_Pulse_data4[4];
for(int i=2;i>=0;i--)
{
Pre_Pulse_data1[i+1] = Pre_Pulse_data1[i];
Pre_Pulse_data2[i+1] = Pre_Pulse_data2[i];
Pre_Pulse_data3[i+1] = Pre_Pulse_data3[i];
Pre_Pulse_data4[i+1] = Pre_Pulse_data4[i];
}
Pre_Pulse_data1[0]=data1;
Pre_Pulse_data2[0]=data2;
Pre_Pulse_data3[0]=data3;
Pre_Pulse_data4[0]=data4;
Pulse_Filtered_data1 = (Pre_Pulse_data1[3]+Pre_Pulse_data1[2]+Pre_Pulse_data1[1]+Pre_Pulse_data1[0])/4;
Pulse_Filtered_data2 = (Pre_Pulse_data2[3]+Pre_Pulse_data2[2]+Pre_Pulse_data2[1]+Pre_Pulse_data2[0])/4;
Pulse_Filtered_data3 = (Pre_Pulse_data3[3]+Pre_Pulse_data3[2]+Pre_Pulse_data3[1]+Pre_Pulse_data3[0])/4;
Pulse_Filtered_data4 = (Pre_Pulse_data4[3]+Pre_Pulse_data4[2]+Pre_Pulse_data4[1]+Pre_Pulse_data4[0])/4;
}
/**
* @file 转向速度求解
* @note
* @date 2021
*/
float turn_R,Car_speed,w;
void Dir_Speed_Control(float Calculate_Center)
{
if(Island_flag==3||Island_flag==4)
{
if(Island_flag==3)
{
turn_R = PID_Realize(&Roundl1,g_fMid_AD);
Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
w = turn_R * Car_speed;
SpeedSet_Z = PID_Realize(&Roundl2,icm_gyro_z-w);
if(SpeedSet_Z<-100)
SpeedSet_Z = -100;
}
if(Island_flag==4)
{
turn_R = PID_Realize(&Roundr1,g_fMid_AD);
Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
w = turn_R * Car_speed;
SpeedSet_Z = PID_Realize(&Roundr2,icm_gyro_z-w);
if(SpeedSet_Z<-100)
SpeedSet_Z = -100;
}
}
else
{
if(SpeedSet_Z<0) //右转
{
turn_R = PID_Realize(&Dir_Speedr1,g_fMid_AD);
Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
w = turn_R * Car_speed;
SpeedSet_Z = PID_Realize(&Dir_Speedr2,icm_gyro_z-w);
if(SpeedSet_Z<-100)
SpeedSet_Z = -100;
//SpeedSet_Z = PID_Realize(&Dir_Speedr1,g_fMid_AD);
}
else {
turn_R = PID_Realize(&Dir_Speedl1,g_fMid_AD);
Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
w = turn_R * Car_speed;
SpeedSet_Z = PID_Realize(&Dir_Speedl2,icm_gyro_z-w);
if(SpeedSet_Z>100)
SpeedSet_Z = 100;
//SpeedSet_Z = PID_Realize(&Dir_Speedl1,g_fMid_AD);
}
}
}
/**
* @file 速度解算，由逆矩阵公式解算
* @note 参照论文《Mecanum轮逆矩阵研究》
* @date 2021
*/
void Speed_Calculation(int16 X_speed, int16 Y_speed, int16 Z_speed)
{
if(Z_speed>100) Z_speed = 100;
else if(Z_speed < -100) Z_speed = -100;
SpeedSet1= X_speed - 0.3*Z_speed- Y_speed;
SpeedSet2= X_speed + 0.3*Z_speed+ Y_speed;
SpeedSet3= X_speed - 0.3*Z_speed+ Y_speed;
SpeedSet4= X_speed + 0.3*Z_speed- Y_speed;
}
/**
* @file 电机驱动
* @note
* @date 2021
*/
void Set_Motor(int32 speed1_power, int32 speed2_power, int32 speed3_power, int32 speed4_power)
{
if(0<=speed1_power) //电机1 正转
{
gpio_set(MOTOR1_A, 1);
pwm_duty(MOTOR1_B, speed1_power);
}
else //电机1 反转
{
gpio_set(MOTOR1_A, 0);
pwm_duty(MOTOR1_B, -speed1_power);
}
if(0<=speed2_power) //电机2 正转
{
gpio_set(MOTOR2_A, 1);
pwm_duty(MOTOR2_B, speed2_power);
}
else //电机2 反转
{
gpio_set(MOTOR2_A, 0);
pwm_duty(MOTOR2_B, -speed2_power);
}
if(0<=speed3_power) //电机3 正转
{
gpio_set(MOTOR3_A, 1);
pwm_duty(MOTOR3_B, speed3_power);
}
else //电机3 反转
{
gpio_set(MOTOR3_A, 0);
pwm_duty(MOTOR3_B, -speed3_power);
}
if(0<=speed4_power) //电机4 正转
{
gpio_set(MOTOR4_A, 1);
pwm_duty(MOTOR4_B, speed4_power);
}
else //电机4 反转
{
gpio_set(MOTOR4_A, 0);
pwm_duty(MOTOR4_B, -speed4_power);
}
}
如有疑问或错误，欢迎和我私信交流指正。
W.By Xyq
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
