代码收藏家 毕业分享 stm32智能平衡小车设计与实现
文章目录
* 4.1 核心PID控制
0 前言
🔥
这两年开始毕业设计和毕业答辩的要求和难度不断提升，传统的毕设题目缺少创新和亮点，往往达不到毕业答辩的要求，这两年不断有学弟学妹告诉学长自己做的项目系统达不到老师的要求。
为了大家能够顺利以及最少的精力通过毕设，学长分享优质毕业设计项目，今天要分享的是
🚩 基于stm32的智能平衡小车
🥇学长这里给一个题目综合评分(每项满分5分)
🧿 项目分享：
https://gitee.com/sinonfin/sharing
1 项目背景
随着机器人研究的进一步深入，在工业生产、安防系统、智能家居、物流网等领域的应用更加广泛，在实际应用中，可能遇到复杂的任务环境。相比多轮的轮式机器人，两轮自平衡机器人体积小，运动灵活，能够在比较狭窄、需要大转角的场合中运动[1]。这种机器人两轮共轴，可以通过运动保持自身平衡，能实现前进、后退、转向、原地静止等基本运动功能，由于其结构特殊，能适应不同的地形环境，研究两轮自平衡机器人，具有重要的意义。
其次，两轮自平衡车还可以作为代步交通工具。相比传统的代步工具如自行车、电动车等，两轮自平衡车体积大幅减小，重力大幅减。运动灵活，绿色环保。
2 设计思路
从控制的角度来看，电机是系统唯一的控制对象。车模运动控制任务可以分解成以下三个基本控制任务：
（1） 直立控制任务：车模的倾角作为控制的输入量，使用PD算法，控制车模稳定在平衡位置。
（2）
速度控制任务：直立车模的速度控制与普通的车模速度控制不同，在直立系统中，速度控制是通过改变车模倾角来完成的。具体实施思路是，对电机转速加入干扰，使车身偏离平衡位置，以此刺激直立控制任务，从而达到控制速度的目的，速度控制使用PI算法。
（3） 方向控制任务：通过控制两个电机的差速来达到转向的目的，方向控制使用PD算法，使用X轴的角速度作为微分项的因子，可以极大改善转向的动态性能，避免振荡。
3 硬件设计
该小车学长采用一个主控电路板+一个电机驱动电路板的结构，将两者分开设计，一是可以避免电机驱动对主控的影响及电磁干扰，二是出于经济的考虑。
主控电路板主要包括以下部分：微控制器电路、电源管理电路、微控制器接口、按键电路、蜂鸣器电路。其中，电源管理电路分为3.3V电源管理电路和5V电源管理电路，5V管理电路使用LM2940三端线性稳压器，输入7.2V电池电压，输出5V电压。3.3V管理电路使用LM1117三端线性稳压器，输入接LM2940的5V电压，输出3.3V电压。考虑到本系统中器件、传感器较多，因此5V管理电路和3.3V管理电路均使用两个。微控制器接口主要包括：OLED接口、蓝牙接口、MMA7361传感器接口、L3G4200D传感器接口、编码器正交解码接口2个、四通道PWM接口、遥控器解码接口，以及预留IO，方便调试使用。
原理图
4 软件设计
4.1 核心PID控制
在自动控制中，按照偏差（目标值与反馈值之差）的比例§、积分（I）、微分（D）的组合进行控制的方法称为PID控制算法，由于其原理简单、稳定性好、可靠性高、易于调整等优点，在自动控制领域应用最为广泛，已有近70年历史，现有的很多控制方法都是基于PID控制算法发展演变而来。当我们对目标系统和控制对象的模型不够了解，或者不能得到控制系统的参数时，PID控制算法尤为适用。
1）比例控制部分：成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，调节器立即产生控制作用以减小偏差，其控制作用最为明显。
2）积分控制部分：积分控制作用的强弱取决于积分时间常数Ti，Ti越大，积分作用越弱，反之则越强。积分控制主要用于消除静差，提高系统的无差度。
3）微分控制部分：微分控制的计算因子是偏差的变化率，能够抑制偏差的变化，并偏差的值变得太大之前，引入一个早期修正量，达到加快系统响应调节速度的目的。积分控制可以提高系统的动态性能，提高反应速度，克服振荡。
PID控制算法的计算结果是偏差信号的比例、积分、微分三部分的线性叠加，偏差值e(t)即目标值r(t)与反馈值c(t)的差，属于闭环控制系统，因此需要使用传感器构成反馈回路。
4.2 直立控制程序设计
车模平衡控制需要负反馈，就需要能够测量车体的倾角，构成反馈回路。在能够测量倾角的情况下，设定目标平衡角度，使用PID算法进行闭环控制。因为车体只会在一个的方向上运动，只存在一个维度的倾斜，因此只需要测量一个维度的倾角，然后控制轮子转动，抵消车体在该维度上的倾斜，就可以完成平衡控制
算法流程
关键代码
void Angle_Calculate()
{
int16_t AngleControlOut_P,AngleControlOut_D;
angle_Accel = (A_Z-AZ_ZERO);
angle_Accel*=0.0039f;
angle_Accel = asin(angle_Accel);
angle_Accel*=500;
Gyro_Now = (T_Y-TY_OFFSET) * TY_Ratio;
Erjie_Lvbo(angle_Accel,Gyro_Now);
if(FilterSwitch==0) myfilter=QingHua_AngleCalaulate;
else if(FilterSwitch==1) myfilter=Yijie_Lvbo;
else if(FilterSwitch==2) myfilter=Erjie_Lvbo;
else if(FilterSwitch==3) myfilter=Kalman_Filter;
if(FilterSwitch==3)
{
myfilter(angle_Accel,Gyro_Now);
ZL.error=angle_now_kal-(Balance_Point);
}
else
{
myfilter(angle_Accel,Gyro_Now);
ZL.error=angle_now-(Balance_Point);
}
AngleControlOut_P=-(int16_t)(ZL.P*ZL.error);
AngleControlOut_D=-(int16_t)(ZL.D*Gyro_Now);
if(AngleControlOut_D>500) AngleControlOut_D=500;//对微分项限幅
else if(AngleControlOut_D<-500) AngleControlOut_D=-500;
AngleControlOut=AngleControlOut_P+AngleControlOut_D;
if(AngleControlOut>800) AngleControlOut=800;
else if(AngleControlOut<-800) AngleControlOut=-800;
}
4.3 速度控制程序设计
直立车模的速度控制方法与普通车模的速度控制方法不同，因为普通车模的电机只需要控制转动，而直立车模的电机输出是平衡控制、速度控制、方向控制三部分之间的叠加。
因为直立系统的首要任务是保持直立，当车模倾角发生改变，车模就会在直立控制的作用下向倾斜方向运动。速度控制就是利用了直立系统的这种特性
void Speed_Calculate()
{
SP.goal=(t1-t1_mid)/10;
if(SP.goal>50) SP.goal=50;
else if(SP.goal<-30) SP.goal=-30;
SpeedControlOut_Old=SpeedControlOut;
SP.error=(SP.goal)-(Speed_L+Speed_R)/2;
SP.error_sum+=SP.error;
if(SP.error_sum>200) SP.error_sum=200;//积分限幅
else if(SP.error_sum<-200) SP.error_sum=-200;
SpeedControlOut=-(int16_t)((SP.P)*SP.error+SP.I*SP.error_sum);
if(SpeedControlOut>300) SpeedControlOut=300;//速度环输出限幅
else if(SpeedControlOut<-300) SpeedControlOut=-300;
}
4.4 方向控制程序设计
直立车模的转向是通过两轮的差速来完成的，转向控制使用PD算法，本系统不具备自主识别路径的功能，因此PD算法的输入参数使用遥控器完成。为了改善转向的动态特性，避免振荡，对PD算法做了修改，微分项的因子不再使用传统的e(t)-e(t-1)，而是使用陀螺仪的X轴，因为陀螺仪输出灵敏度更高
void Direction_Calculate(int16_t bias)
{
DIR.last_error=DIR.error;
DIR.error=bias/6-dmp_yaw;
DIR.error_sum+=DIR.error;
if(DIR.error_sum>2000) DIR.error_sum=2000;
else if(DIR.error_sum<-2000) DIR.error_sum=-2000;
DirectionControlOut_Old=DirectionControlOut;
DirectionControlOut = DIR.P*DIR.error
+ DIR.I*DIR.error_sum
+ DIR.D*(T_X-TX_OFFSET);
DirectionControlOut_Old=DirectionControlOut;
DirectionControlOut=bias*0.15f;
if(DirectionControlOut>300) DirectionControlOut=300;
else if(DirectionControlOut<-300) DirectionControlOut=-300;
}
互补滤波法调试结果
4.5 关键代码
int main()
{
uint8_t dirswitchtemp,spswitchtemp;
SmartCar_Init();
while(1)
{
VisualScope_Out();
while(DMA_IsMajorLoopComplete(HW_DMA_CH2));
if(StandUp_Flag==1&&IS_RUNNING==0)
{
dirswitchtemp=DirectionControlSwitch;//保存之前的开关
spswitchtemp =SpeedControlSwitch;
DirectionControlSwitch=0;
SpeedControlSwitch=0;
ZL.P*=1.5f;
ZL.D*=1.5f;
DelayMs(500);
Motor_Enable();
IS_RUNNING=1;//将小车运行标志置位
DelayMs(500);
StandUp_Flag=0;
DelayMs(1000);
ZL.P/=1.5f;
ZL.D/=1.5f;
SpeedControlSwitch=dirswitchtemp;
DirectionControlSwitch=dirswitchtemp;
}
}
}
void PIT0_ISR()
{
static uint16_t FindZeroIndex=0;
systime_speed++;//速度控制节拍+1
systime_direction++;//方向控制节拍+1
ADC_GetDataAndFilter();
Angle_Calculate();
//Yaw_Calculate();
if(systime_direction==5)
{
//HMC_angle=Get_Angle();
systime_direction=0;
Dr_Smooth=0.2;
Direction_Calculate(t2-t2_mid);
}
if(systime_speed==20)
{
systime_speed=0 ;
GPIO_ToggleBit(HW_GPIOE,26);//闪烁
Sp_Smooth=0.05;//重置平滑系数
Get_Speed();
Speed_Calculate();
}
if(FindZeroFlag)
{
FindZeroIndex++;
if(FindZeroIndex>=400)//说白了按下键之后两秒才开始记录数据
{
GYROY_SUM+=T_Y;
GYROX_SUM+=T_X;
if(FindZeroIndex>=499)
{
FindZeroFlag=0;//次数够了，清标志位
FindZeroIndex=0;
TY_OFFSET=GYROY_SUM*0.01f;
TX_OFFSET=GYROX_SUM*0.01f;
GYROX_SUM=0;
GYROY_SUM=0;
}
}
}
Motor_Output();
}
void SmartCar_Init()
{
DelayInit();
/******Debug_初始化******/
/*******主要是DMP用到了printf*********/
UART_QuickInit(UART3_RX_PE05_TX_PE04,115200);
UART_SelectDebugInstance(HW_UART3);
//**********LED初始化，用作系统运行指示***********//
GPIO_QuickInit(HW_GPIOE,26,kGPIO_Mode_OPP);
GPIO_SetBit(HW_GPIOE,26);
/********OLED初始化**************/
OLED_Init();
//**************模拟加速度计陀螺仪初始化*****************/
GPIO_QuickInit(MMA7361_EN,kGPIO_Mode_OPP);
GPIO_SetBit(MMA7361_EN); //使能MMA7361
ADC_QuickInit(ADC_ACCEL_Z,kADC_SingleDiff10or11);//单端12位输入
//**************IIC及L3G4200D\HMC5883初始化******************//
I2C_QuickInit(I2C0_SCL_PD08_SDA_PD09,I2C_SPEED);
L3G4200D_Init();
CT_IIC_Init();
while(mpu_dmp_init())
{
OLED_P8x16Str(0,0,"DMP Error");
OLED_P8x16Num(0,0,mpu_dmp_init());
DelayMs(200);
}
OLED_P8x16Str(0,0,"DMP OK!");
/****DMP数据输出中断*/
GPIO_QuickInit(HW_GPIOE,4,kGPIO_Mode_IFT); //DMP输出输出中断
GPIO_CallbackInstall(HW_GPIOE,GPIOE_ISR);
GPIO_ITDMAConfig(HW_GPIOE,4,kGPIO_IT_FallingEdge,true);
/****************PWM初始化*****************/
FTM_PWM_QuickInit(FTM0_CH0_PC01,kPWM_EdgeAligned,10000);
FTM_PWM_QuickInit(FTM0_CH1_PC02,kPWM_EdgeAligned,10000);
FTM_PWM_QuickInit(FTM0_CH2_PC03,kPWM_EdgeAligned,10000);
FTM_PWM_QuickInit(FTM0_CH3_PC04,kPWM_EdgeAligned,10000);
FTM_PWM_ChangeDuty(FTM_PWM_LEFT,0);
FTM_PWM_ChangeDuty(FTM_PWM_LEFT_,0);
FTM_PWM_ChangeDuty(FTM_PWM_RIGHT,0);
FTM_PWM_ChangeDuty(FTM_PWM_RIGHT_,0);
/**************FTM正交解码初始化**************/
/***********初始化位 脉冲-方向型编码器**********/
FTM_QD_QuickInit(FTM1_QD_PHA_PB00_PHB_PB01,kFTM_QD_NormalPolarity,kQD_CountDirectionEncoding);
FTM_QD_QuickInit(FTM2_QD_PHA_PB18_PHB_PB19,kFTM_QD_NormalPolarity,kQD_CountDirectionEncoding);
GPIO_QuickInit(DIR_LEFT,kGPIO_Mode_IFT);//左边编码器方向角设置为悬空输入
GPIO_QuickInit(DIR_RIGHT,kGPIO_Mode_IFT);//右边编码器方向角设置为悬空输入
//**********************串口初始化********/
UART_QuickInit(UART4_RX_PE25_TX_PE24,115200);
UART_ITDMAConfig(HW_UART4,kUART_DMA_Tx,true);
UART_DMASendConfig(HW_UART4,HW_DMA_CH2);
//**********************按键中端配置************/
GPIO_QuickInit(KEY_GPIO,KEY_OK,kGPIO_Mode_IPU);
GPIO_QuickInit(KEY_GPIO,KEY_UP,kGPIO_Mode_IPU);
GPIO_QuickInit(KEY_GPIO,KEY_DOWN,kGPIO_Mode_IPU);
GPIO_QuickInit(KEY_GPIO,KEY_LEFT,kGPIO_Mode_IPU);
GPIO_QuickInit(KEY_GPIO,KEY_RIGHT,kGPIO_Mode_IPU);
GPIO_CallbackInstall(KEY_GPIO,GPIOA_ISR);//按键中断回调函数
GPIO_ITDMAConfig(KEY_GPIO,KEY_OK,kGPIO_IT_FallingEdge,true);
GPIO_ITDMAConfig(KEY_GPIO,KEY_UP,kGPIO_IT_FallingEdge,true);
GPIO_ITDMAConfig(KEY_GPIO,KEY_DOWN,kGPIO_IT_FallingEdge,true);
GPIO_ITDMAConfig(KEY_GPIO,KEY_LEFT,kGPIO_IT_RisingEdge,true);
GPIO_ITDMAConfig(KEY_GPIO,KEY_RIGHT,kGPIO_IT_FallingEdge,true);
//*************解码通道配置****************/
GPIO_QuickInit(HW_GPIOD,12,kGPIO_Mode_IFT);
GPIO_QuickInit(HW_GPIOD,13,kGPIO_Mode_IFT);
GPIO_QuickInit(HW_GPIOD,14,kGPIO_Mode_IFT);
GPIO_CallbackInstall(HW_GPIOD,GPIOD_ISR);
GPIO_ITDMAConfig(HW_GPIOD,12,kGPIO_IT_RisingFallingEdge,true);
GPIO_ITDMAConfig(HW_GPIOD,13,kGPIO_IT_RisingFallingEdge,true);
GPIO_ITDMAConfig(HW_GPIOD,14,kGPIO_IT_RisingFallingEdge,true);
//*****************PIT定时中断初始化*****************/
PIT_QuickInit(HW_PIT_CH0,3000);
PIT_ITDMAConfig(HW_PIT_CH0,kPIT_IT_TOF,true);
PIT_CallbackInstall(HW_PIT_CH0,PIT0_ISR);
/*******************NVIC配置****************/
NVIC_SetPriorityGrouping(NVIC_PriorityGroup_2); //中断优先级分成2组
NVIC_SetPriority(PORTD_IRQn, NVIC_EncodePriority(NVIC_PriorityGroup_2, 0, 0));//遥控器
NVIC_SetPriority(PIT0_IRQn, NVIC_EncodePriority(NVIC_PriorityGroup_2, 1, 0));//周期性中断优先级
NVIC_SetPriority(PORTE_IRQn, NVIC_EncodePriority(NVIC_PriorityGroup_2, 2, 0));//DMP
NVIC_SetPriority(PORTA_IRQn, NVIC_EncodePriority(NVIC_PriorityGroup_2, 3, 0));//按键中断
OLED_P8x16Str(0,2,"Hello World!");
}
5 最后
🧿 项目分享：
