目录
一.所需材料
1.陀螺仪MPU6050--读取三轴的加速度和角速度,然而我们平衡车需要的不是加速度和角速度,而是需要实时的角度,即我们还需要进行姿态角转换,可以直接移植正点原子的DMP函数
2.TB6612电机驱动,l298N也可,我用的是TB6612,点击查看接线
3.OLED,用来显示姿态角,可以直接移植
4.12v锂电池和DCDC降压模块给电机和单片机供电
5.编码电机,点击查看接线
6.c8t6单片机
二.PID算法(简单说明)
P(比例)算法:Kp*误差。
可以看出,误差越大,P的输出就越大,所以P的作用是减小误差,但是只有P算法会发生超调现象,即在不受外力下可以看作简谐运动,一直震荡
I(积分)算法:Ki*误差的积分。
可以看出,只要存在误差(一般叫做稳态误差,比如小车在平衡位置偏左或右一点点,但是又不会倒下等等的情况下产生的角度误差)就会一直累加,累加累加的总误差就变的很大,滚雪球一样,越来越大,所以I可以消除稳态误差
D(微分):Kd*误差的微分。
可以看出,两次误差之差反映的是系统的反应速度,响应越快,误差之差越大,D输出很大,反之很小,所以D算法会抑制过冲,消弱系统的反应速度。例如简谐运动,以最下点为期望点,小球从空中开始速度很小,后面越来越快,即可以理解为误差微分越来越大,且为负的,所以D输出就越来越大,力的方向与小球运动方向相反,若没有D算法,小球就会继续简谐运动,若此时有了D算法,小球的运动就会收到阻碍而令小球到达的高度降低,长此以往,小球就会静止在最低点
也可以这样看,在水中挥拳,挥的速度越大,受到的阻尼越大
直立环
PWM_OUT=Kp*误差+Kd*误差的微分
误差=真实角度-期望角度
误差的微分=角速度,姿态角转换得到
作用:让小车保持短时间的平衡,但是推一下就可能倒下
cs
/*
直立环
PWM_OUT = Kp*角度偏差+Kd*角度偏差的微分
角度偏差求微分就是对角度偏差求导,即为角速度
gyro_Y:俯仰角的角速度
*/
int Vertical(float Expect_Angle,float Angle,float gyro_Y)
{
int PWM_out1=0;
PWM_out1 = Vertical_Kp*(Expect_Angle-Angle)+Vertical_Kd*(gyro_Y-0);
return PWM_out1;
}
/*
速度环
PWM_OUT=Kp*误差+Ki*误差的积分
误差:期望速度-真实速度
误差的积分=误差累加
作用:消除稳态误差,加强系统的反应速度
极性问题:
速度环在平衡一类的基本上都是用正反馈,即积分误差,放大误差,先注释掉直立环,则极性正确的现象就是转动一下轮子,立刻就会加速到最大,因为我们速度理论值是0,我们转了一下,产生了误差,速度环不断放大误差,使速度环输出越大,就会让轮子不断加速到最大速度,若是负反馈,我们很难转动轮子
cs
int Velocity(int Target,int left,int right)
{
static int Encoder_S,EnC_Err_Lowout_last,PWM_out3,Encoder_Err,EnC_Err_Lowout;
float a = 0.7;
Encoder_Err = left+right-Target;//速度偏差
//对速度偏差进行一阶低通滤波
EnC_Err_Lowout = a*EnC_Err_Lowout_last+(1-a)*Encoder_Err;
EnC_Err_Lowout_last=EnC_Err_Lowout;
//积分
Encoder_S+=EnC_Err_Lowout;
//限幅
Encoder_S=I_Limit(Encoder_S);
PWM_out3 = Velocity_Kp*EnC_Err_Lowout + Velocity_Ki*Encoder_S;
return PWM_out3;
}
cs
int I_Limit(int Encoder_S)
{
if(Encoder_S>10000)Encoder_S=10000;
else if(Encoder_S<-10000)Encoder_S=-10000;
else
Encoder_S=Encoder_S;
return Encoder_S;
}
速度环必须滤波,因为速度会突变,突变的速度会对系统产生很大影响
滤波:编码器测到的数据(速度)是存在一些突变的,这些突变会导致系统的不稳定,所以我们要滤除这些突变,这就是滤波,类似于模电里面的"滤除电路中的高频信号"
一阶低通滤波:把权重给到上次滤波后的速度,另一个这次的速度为(1-a)倍,0.5<a<1
积分限幅:误差的积分在一直累加,若没有限幅,积分会越来越大,举个例子:小车倒下了,我们为了保护电机而关掉了电机,但是单片机的电源并没有关掉,误差一直在累加,很大很大了,当我们把小车放好并打开电机后,由于I的作用根本无法平衡。所以要限幅。
串级PID
串级,顾名思义,就是一个环的输出作为另一个环的输入,例如速度环的输出作为直立环的输入,或者直立环的输出作为速度环的输入
速度环输入:1.给定速度。2.速度反馈。
输出:角度值(直立环的期望速度输入)
直立环输入:1.给定角度(速度环输出)。2.角度反馈
输出:PWM(直接控制小车)
Vertical_out=Kp1*(Angle-Expect_Angle)+Kd* gyro_y直立环输出=Kp1*(真实角度-期望角度+机械中值)+Kd*角度偏差的微分
Velocity_out =Kp2*(Encoder_ real- Encoder_ expect)+Ki*(Encoder_ real- Encoder_ expect)的积分
速度环输出=Kp2*(反馈编码器值-期望编码器值)+Ki*编码器偏差的积分
合并推导:Expect_Angle = Velocity_outVertical_out = Kp1*{ Angle-[ Kp2*(Encoder_ real- Encoder_ expect)+Ki*Σ(Encoder_ real- Encoder_ expect) ]}+Kd*gyro_y
=Kp1*真实角度+ Kd*角度偏差的微分-Kp1* [Kp2*编码器偏差- Ki *编码器偏差的积分]
三.使用到的外设
1.定时器输出比较-PWM
cs
void PWM_Init_TIM1(uint16_t Psc,uint16_t Per)
{
//开时钟,AFIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;//复用推挽
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置,所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//不分频----滤波器的采样频率,可以由内部时钟直接提供,
//也可以由内部时钟加一个时钟分频而来,
//分频系数就是由TIM_ClockDivision决定
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = Per;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = Psc;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器,这里不需要
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
TIM_OCInitTypeDef TIM_OCInitStruct;
TIM_OCStructInit(&TIM_OCInitStruct);//初始化输出比较结构体
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;//输出比较模式
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //极性,有效电平为高电平
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;//输出使能
TIM_OCInitStruct.TIM_Pulse = 0;//CCR
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器特有的,必须使能
//使能ARR影子寄存器
TIM_ARRPreloadConfig(TIM1,ENABLE);
//使能输出比较预装载值寄存器
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);
TIM_Cmd(TIM1,ENABLE);
}
2.定时器编码器模式
cs
//配置编码器
//编码器1-PA0/PA1-TIM2
//编码器2-PB6/PB7-TIM4
void Encoder_TIM2_Init()
{
//开启GPIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//开启定时器时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
//初始化GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
//初始化定时器
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置,所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//滤波分频系数
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器,这里不需要
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
//编码器特有的配置函数,配置模式
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
//初始化输入捕获
TIM_ICInitTypeDef TIM_ICInitStruct;
TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置,所以这里先初始化一下
TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器,范围是 0x0 and 0xF
TIM_ICInit(TIM2,&TIM_ICInitStruct);
//清除定时器溢出中断标志位
TIM_ClearFlag(TIM2,TIM_FLAG_Update);
//配置溢出中断
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
//设置计数器初始值为0
TIM_SetCounter(TIM2,0);
//开启定时器
TIM_Cmd(TIM2,ENABLE);
}
void Encoder_TIM4_Init()
{
//开启GPIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
//开启定时器时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
//初始化GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_Init(GPIOB,&GPIO_InitStruct);
//初始化定时器
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//因为没有将所有的结构体内容全部配置,所以这里先初始化一下
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//外部时钟滤波分频系数
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535;//ARR的值
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;//PSC的值
//TIM_TimeBaseInitStruct.TIM_RepetitionCounter 重复计数器,这里不需要
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
//编码器特有的配置函数
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
//配置IC输入
TIM_ICInitTypeDef TIM_ICInitStruct;
TIM_ICStructInit(&TIM_ICInitStruct);//因为没有将所有的结构体内容全部配置,所以这里先初始化一下
TIM_ICInitStruct.TIM_ICFilter = 10;// 滤波器
TIM_ICInit(TIM4,&TIM_ICInitStruct);
//清除定时器溢出中断标志位
TIM_ClearFlag(TIM4,TIM_FLAG_Update);
//配置溢出中断
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
//设置计数器初始值为0
TIM_SetCounter(TIM4,0);
//开启定时器
TIM_Cmd(TIM4,ENABLE);
}
3.编码器读取速度
cs
/*******************
编码器速度读取函数
*入口参数:定时器2/4
*******************/
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
//int ? short
case 2:value_1=(short)TIM_GetCounter(TIM2);TIM_SetCounter(TIM2,0);break; //先读取编码器的计数值,然后清零计数值,目的是直接获得速度,而不用计数值相减来算速度
case 4:value_1=(short)TIM_GetCounter(TIM4);TIM_SetCounter(TIM4,0);break;
default :value_1 = 0;
}
//这里TIM_GetCounter()函数的返回值是uint16_t ,而我们的返回值是有符号的int类型,所以这里强制类型转化一下
return value_1;
}
4.电机驱动函数
cs
//电机初始化函数
void Motor_Init()
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;//复用推挽
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15 ;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
//限幅函数
void Limit(int *motorA ,int *motorB)
{
if(*motorA > PWM_MAX)*motorA=PWM_MAX;
if(*motorA < PWM_MIN)*motorA=PWM_MIN;
if(*motorB > PWM_MAX)*motorB=PWM_MAX;
if(*motorB < PWM_MIN)*motorB=PWM_MIN;
}
//绝对值函数
int abs(int p)
{
int q;
q = p>0?p:(-p);
return q;
}
//电机驱动函数
//入口参数:PID运算完成后最终PWM值
void Load(int moto1,int moto2)
{
//判断正反转
if(moto1>0)Ain1=1,Ain2=0;
else Ain1=0,Ain2=1;
//输入PWM的绝对值
TIM_SetCompare1(TIM1,abs(moto1));
if(moto2>0)Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,abs(moto2));
}
cs
//电机为10KHZ
//72000000/7200 = 10 000 HZ
#define PWM_MAX 7200
#define PWM_MIN -7200
extern int MOTO1,MOTO2;
#define Ain1 PBout(14)
#define Ain2 PBout(15)
#define Bin1 PBout(13)
#define Bin2 PBout(12)
5.外部中断
dmp读取是用的中断读取,10ms读取一次,所以我们要配置MPU6050的外部中断
cs
void MPU6050_EXTI_Init(void)
{
//开启时钟
//因为是复用功能,所以开启AFIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);
//配置GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU;//GPIO_Mode_AF_PP;复用推挽
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
//映射GPIO和外部中断
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);
//配置EXTI结构体
EXTI_InitTypeDef EXTI_InitStruct;
EXTI_InitStruct.EXTI_Line = EXTI_Line5;//中断线
EXTI_InitStruct.EXTI_LineCmd = ENABLE;//使能中断线
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;//中断模式
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿触发
EXTI_Init(&EXTI_InitStruct);
}
void NVIC_Config()
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
//外部中断
NVIC_InitTypeDef NVIC_InitStruct;
NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_Init(&NVIC_InitStruct);
//串口
NVIC_InitStruct.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority =0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;
NVIC_Init(&NVIC_InitStruct);
}
void EXTI9_5_IRQHandler(void)
写中断服务函数,里面做的是PID算法
void EXTI9_5_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line5) == SET)//MPU6050外部中断是PB5
{
if(PBin(5) == 0)//引脚定义为外部上拉,而外部中断触发模式是下降沿,这里就是再进行下降沿的检测
{
EXTI_ClearITPendingBit(EXTI_Line5);
//1.采集编码器数据和MPU6050的角度信息
Encoder_Left = -Read_Speed(2);//因为电机是相对安装的,即左右电机相差180度,为了让编码器输出极性一致,
Encoder_Right = Read_Speed(4);//所以需要取反,或者调转一下极性也可以
mpu_dmp_get_data(&Pitch,&Roll,&Yaw);
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
//2.将数据压入闭环控制中,计算输出控制量
Velocity_out = Velocity(Target_Speed,Encoder_Left,Encoder_Right);
Vertical_out = Vertical(Velocity_out+Med_Angle,Pitch,gyroy);
Turn_out = Turn(gyroz);
PWM_out = Vertical_out;
//3.把控制量加载到电机上
MOTO1 = PWM_out-Turn_out;//这里一加一减是因为两个电机是反向安装的,实际上就是同向加减了
MOTO2 = PWM_out+Turn_out;
Limit(&MOTO1,&MOTO2);
Load(MOTO1,MOTO2);
}
}
}
四、小车
本人不会打板,所以选择了做一回杜邦线战神!
调了直立环和速度环,但是不理想,不能很好的回位
调试
串级的PID调试时应该断掉两个环的连接
机械中值 **:**把平衡小车放在地面上,从前向后以及从后向前绕电机轴旋转平衡小车,两次的向另一边倒下的角度的中值,就是机械中值。
直立环Kp 极性:
极性错误:小车往哪边倒,车轮就往反方向开,会使得小车加速倒下。
极性正确:小车往哪边倒,车轮就往哪边开,以保证小车有直立的趋势。
Kp 大小:
Kp一直增加,直到出现大幅低频震荡。
Kd 极性:
极性错误:拿起小车绕电机轴旋转,车轮反向转动,无跟随。
极性正确:拿起小车绕电机轴旋转,车轮同向转动,有跟随。
Kd 大小:
Kd一直增加,直到出现高频震荡。
直立环调试完毕后,对所有确立的参数乘以0.6作为最终参数。
原因:之前得到的参数都是Kp、Kd最大值,根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。
结果:乘以0.6后,小车的抖动消失,但同时直立效果也变差。待下面加入速度环就能得到更好的性能。
在调试速度环参数极性时:需要去掉(注释掉)直立环运算
在调试速度环参数大小时:再次引入(取消注释)直立环运算
Kp&Ki :
线性关系、Ki=(1/200)*Kp、仅调Kp即可。
Kp&Ki 极性:
极性错误:手动转动其中一个车轮,另一车轮会以同样速度反向旋转------典型负反馈。
极性正确:手动转动其中一个车轮,两个车伦会同向加速,直至电机最大速度------典型正反馈。
Kp&Ki 大小:
增加Kp&Ki,直至:小车保持平衡的同时,速度接近于零。且回位效果较好。
转向环Kp 极性:
极性错误:拿起小车,并将小车绕Z轴旋转,两车轮旋转的趋势与小车旋转趋势一致------典型正反馈。
极性正确:拿起小车,并将小车绕Z轴旋转,两车轮旋转的趋势与小车旋转趋势相反------典型负反馈。
Kp 大小:
加大Kp,直至走直线效果较好,且无剧烈抖动。
参考于b站up主天下行走的平衡车教学