平衡小车制作
本帖最后由 coshi 于 2021-6-8 12:12 编辑第一部分 硬件原理讲解
一、硬件结构
1. 主控板：主控芯片（STM32F103C8T6）+电机驱动芯片（TB6612FNG）,利用AD20画的PCB并进行打板。
2. 小车框架：平衡车底盘、电机、轮子（某宝有售）
3. 电池：狮子航模电池2200mAh11.1V35C，T母头
4. DC-DC降压模块：MP1584EN，3A可调降压稳压电源模块
5. 陀螺仪：MPU6050，检测倾斜角度
6. OLED：4线I2C OLED显示屏
7. 超声波模块：HC-SR04
8. 蓝牙：HC-05
二、硬件原理图
硬件原理图我利用Altium Designer 20画的，并进行了PCB打板。
图1 平衡小车硬件原理图
图2 平衡小车3D视图PCB
三、硬件框架讲解
（1）STM32F103C8T6
主控芯片及周围电路结构如下：
1）STM32F103C8T6（48脚芯片）
2）8M晶振（STM32内部倍频至72MHz，产生供系统正常工作的稳定的脉冲信号），
3）复位电路（RST）
4）启动模式选择电路（BOOT0和BOOT1）
5）去耦电容（3.3V与GND之间加几个104电容）
6）烧写电路（SW）
图3 主控芯片及周围电路
（2）电源电路
电源采用11.1V锂电池供电，通过DC-DC降压模块获取5V电压，在通过AMS1117降至3.3V。图中电解电容主要目的是滤低频纹波，瓷片电容主要目的是抑制高频噪声，需靠近管脚。
图4 DC-DC降压模块
图5 AMS1117转3.3V
（3）编码器
作用：实时检测电机速度
编码器1——PA0/PA1——TIM2
编码器2——PB6/PB7——TIM4
图6 编码器引脚（TIM2,TIM4）
（4）电机驱动芯片TB6612FNG
作用：驱动电机正常运行
PWM1——PA8
PWM2——PA11
电机1——PB12/PB13
电机2——PB14/PB15
图7 TB6612FNG（TIM3）
（5）陀螺仪MPU6050
作用：检测倾斜角
MPU6050中断引脚——PB5
MPU6050所用I2C——PB3/PB4
图8 陀螺仪MPU6050（I2C）
四、实物展示图9 平衡小车实物图图10 平衡小车实物图图11 平衡小车实物图
第二部分电机驱动
一、硬件设计
1.直流减速电机
直流减速电机，即齿轮减速电机，是在普通直流电机的基础上，加上配套齿轮减速箱。齿轮减速箱的作用是，提供较低的转速，较大的力矩。
简单的来说，STM32分配两个IO口给一个直流减速电机，并给予高低电平，来使得电机进行正转或反转。
我用的电机为GM25-370直流减速电机（带霍尔编码器），工作电压：6-24VDC，额定电压12V，额定电流0.65A，空载转速350RPM，额定功率5W，最大精度，1496CPR，配备 CPR霍尔AB两相编码器，减速后输出单圈374个正交脉冲。
图1 GM25-370直流减速电机（带霍尔编码器）
图2 霍尔编码器接口引脚
2.TB6612FNG电机驱动芯片
要实现小车的转向与前进后退控制，我们可以使用STM32实现，但是STM32的IO口带负载能力较弱，而直流电机是大电流感性负载，所以我们需要功率放大器件，在这里，我选择使用TB6612FNG这款电机驱动芯片。
TB6612FNG 是东芝半导体公司生产的一款直流电机驱动器件，它具有大电流MOSFET-H 桥结构，双通道电路输出，可同时驱动 2 个电机。相比于 L298N的热耗性和外围二极管续流电路，TB6612FNG无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，利于减小系统尺寸。对于PWM信号输入频率范围，我采用10KHz的频率，并通过改变占空比调节电机的速度。
特点：
T每通道输出最高1.2A的连续驱动电流，启动峰值电流达2A/3.2A（连续脉冲/单脉冲）；
4种电机控制模式：正转/反转/制动/停止；
PWM支持频率高达100kHz；
待机状态；
片内低压检测电路与热停机保护电路；
工作温度：-20~85°C
封装：SSOP24（画PCB时注意！）
引脚介绍：
VM1：电源输入（电机输入电源），这里我设置为12V。
VCC：驱动电源输入（驱动芯片），这里我设置为5V。
GND：芯片上的地都是共地。
STNDBY：总使能端，相当于EN，高电平有效，这里直接与VCC连接。
PWMA，PWMB：从MCU输入，我设置10Khz的PWM频率
AIN1，AIN2：电机状态控制。
A0，A1，B0，B1：输出到电机。
平衡小车中使用到的引脚：
电机1——PB12/PB13
电机2——PB14/PB15
PWM1——PA8
PWM2——PA11
图3 TB6612FNG电机驱动芯片
图4 AN1和AN2引脚高低电平对应电机状态
3.H桥驱动电路
上面说到TB6612FNG 具有大电流MOSFET-H桥结构，那么很多小伙伴想问：什么是H桥结构呢？我以下面两张图举例，帮助大家简单化理解H桥电路结构。
注：图中的电路Q1,Q2,Q3,Q4为三极管，而TB6612内部集成的是四个MOSFET，我以下图举例，大家不可把下图看做是TB6612内部电路，其内部电路可查看TB6612的参考手册。
这里推荐给大家一个查找芯片的网址：http://www.semiee.com/（半岛小芯）
① 当Q1,Q4导通，Q2,Q3关断时，电流从Q1,从电机正极通过电机负极，再从Q4流出，完成一条回路，电机Motor正转。
图5 H桥电路：电机正转
① 当Q2,Q3导通，Q1,Q4关断时，电流从Q3,从电机负极通过电机正极，再从Q2流出，完成一条回路，电机Motor反转。
图6 H桥电路：电机反转
二、软件编程
1.电机驱动函数——motor.c
1）电机GPIO初始化函数
入口参数：无
初始化GPIO–PB12、PB13、PB14、PB15为推挽输出
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);// 开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;// 初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
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);
}
2）限幅函数
入口参数：电机A脉冲个数，电机B脉冲个数
限制电机的脉冲个数在规定范围内，有个最值，即自动重装载值（我设置的是PWM_MAX = 7200，PWM_MIN = -7200）
void Limit(int *motoA,int *motoB)
{
if(*motoA>PWM_MAX)*motoA=PWM_MAX;
if(*motoA<PWM_MIN)*motoA=PWM_MIN;
if(*motoB>PWM_MAX)*motoB=PWM_MAX;
if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
3）绝对值函数（非常通用，建议保存!!）
入口参数：常规变量
通过与0比较，大于0则返回不变的值，小于0则返回相反的值。
int GFP_abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
4）赋值函数
入口参数：电机A脉冲个数，电机B脉冲个数
入口参数即为PID运算完成后的最终PWM值（后续会讲解PID算法的实现）
void Load(int moto1,int moto2)
{
//1.研究正负号，对应正反转
if(moto1>0)
Ain1=1,Ain2=0;//正转
else
Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,GFP_abs(moto1));
//1.研究正负号，对应正反转
if(moto2>0)
Bin1=1,Bin2=0;
else
Bin1=0,Bin2=1;
//2.研究PWM值
TIM_SetCompare4(TIM1,GFP_abs(moto2));
}
2.电机驱动函数头文件——motor.h
#ifndef_MOTOR_H
#define_MOTOR_H
#include "sys.h"
#define Ain1PBout(14)
#define Ain2PBout(15)
#define Bin1PBout(13)
#define Bin2PBout(12)
void Motor_Init(void);
void Limit(int *motoA,int *motoB);
int GFP_abs(int p);
void Load(int moto1,int moto2);
#endif
3.PWM函数——pwm.c
1. 定时器初始化函数
入口参数：预分频值，自动重装载值
PA8,PA11复用推挽输出
对应定时器1通道1和通道4
开启MOE主输出使能（高级定时器特有!!!）
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP; // 初始化GPIO--PA8、PA11为复用推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 | GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);// 初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=Per;
TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct); // TIM1
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;
TIM_OC1Init(TIM1,&TIM_OCInitStruct);
TIM_OC4Init(TIM1,&TIM_OCInitStruct);
TIM_CtrlPWMOutputs(TIM1,ENABLE);// 高级定时器专属!!!--MOE主输出使能
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);// OC1预装载寄存器使能
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);// OC4预装载寄存器使能
TIM_ARRPreloadConfig(TIM1,ENABLE);// TIM1在ARR上预装载寄存器使能
TIM_Cmd(TIM1,ENABLE); // 开定时器。
}
4.PWM函数头文件——pwm.h
#ifndef_PWM_H
#define_PWM_H
#include "sys.h"
void PWM_Init_TIM1(u16 Psc,u16 Per);
#endif
第三部分 编码器讲解
一、硬件结构
1.什么是编码器？
编码器是一种将角位移或者角速度转换成一串电数字脉冲的旋转式传感器。编码器又分为光电编码器和霍尔编码器。
2.编码器的工作原理
这里以霍尔编码器为主介绍（光电编码器精度比霍尔高38倍）。霍尔编码器是有霍尔马盘和霍尔元件组成。霍尔马盘是在一定直径的圆板上等分的布置有不同的磁极。霍尔马盘与电动机同轴，电动机旋转时，霍尔元件检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。示意图如下：
图1 霍尔编码器
3.编码器接口引脚
我用的电机是GM25-370直流减速电机（带霍尔编码器），其对应编码器引脚图如下：
图2 霍尔编码器接口引脚
4.编码器的测速原理
单位时间内，根据脉冲走过的距离计算电机实际速度。
5.采集数据方式
通常有两种方式，第一种软件技术直接采用外部中断进行采集，根据AB相位差的不同可以判断正负。第二种硬件技术直接使用定时器的编码器模式，这里采用第二种。也是大家常说的四倍频，提高测量精度的方法。其实就是把AB相的上升沿和下降沿都采集而已，所以1变4。自己使用外部中断方式实现就比较占用资源了，这里建议使用定时器的编码器模式来测量脉冲变化值。
二、软件编程
1.编码器函数——Encoder.c
1）编码器1和编码器2初始化函数
入口参数：无
① GPIO初始化
② 定时器基本结构体初始化
③ 编码器配置函数
④ 定时器输入捕获函数
⑤ 清除定时器更新标志位
⑥ 运行更新中断
⑦ 定时数据清零
⑧ 定时器使能
这里具体讲解一下编码器配置函数
// 编码器配置函数: 定时器2，模式3，上升沿
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
① 第一个参数是定时器选择。
② 第二个参数是编码器启动模式；TIM_EncoderMode_TI12：在T1和T2的每个跳变沿均计数。
【模式】：
Tl1模式：在T1的所有边沿计数。
Tl2模式：在T2的所有边沿计数。
Tl1l2模式：在T1和T2的所有边沿计数。
简单举个例子：Tl1处于上升沿时，Tl2处在低电平，即对应Tl1FP1信号向上计数。依次类推，下图我圈起来的就是根据编码器计数操作推导出的计数模式。
图3 T1与T2极性
图4 对应计数模式
③ 第三和第四个参数是极性配置；这里分为上升沿和下降沿，为什么说设置极性变成设置上升沿和下降沿呢？通过进入stm32f10x.tim.c函数中可以查找到，设置极性实际上与TIM_CCER_CC1P这个寄存器是有关联的。我们查找STM32中文参考手册可以了解到。
这里可以看到，我们设置为不反相，对应IC1上升沿。
图5 IC极性查找
编码器函数代码：
#include "encoder.h"
#include "sys.h"
void Encoder_TIM2_Init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 ,ENABLE);// 开启定时器时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO,ENABLE); // 开启GPIO时钟
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING; // 浮空输入
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; // 编码器1:PA0/PA1
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA ,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1; // 不分频
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535; // 重装载值65535
TIM_TimeBaseInitStruct.TIM_Prescaler =0; // 分频系数0
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
// 编码器配置函数: 定时器2，模式3，上升沿
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 10; // 滤波器设置为10
TIM_ICInit(TIM2,&TIM_ICInitStruct);
TIM_ClearFlag(TIM2,TIM_FLAG_Update); // 清除定时器标志位
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);// 定时器2，溢出更新，使能
TIM_SetCounter(TIM2,0); // 定时数据清零
TIM_Cmd(TIM2,ENABLE); // 定时器2使能
}
void Encoder_TIM4_Init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 ,ENABLE);// 开启定时器时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE); // 开启GPIO时钟
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING; // 浮空输入
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; // 编码器2:PB6/PB7
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB ,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1; // 不分频
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;// 向上计数
TIM_TimeBaseInitStruct.TIM_Period = 65535; // 重装载值65535
TIM_TimeBaseInitStruct.TIM_Prescaler = 0; // 分频系数0
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStruct);
// 编码器配置函数：定时器4，模式3，上升沿
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 10; // 滤波器设置为10
TIM_ICInit(TIM4,&TIM_ICInitStruct);
TIM_ClearFlag(TIM4,TIM_FLAG_Update); // 清除定时器溢出更新标志位
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);// 定时器4，溢出更新，使能
TIM_SetCounter(TIM4,0); // 定时数据清零
TIM_Cmd(TIM4,ENABLE); // 定时器2使能
}
2）编码器数据读取函数
入口参数：定时器x
① 选择定时器x
② 采集编码器的计数值并保存
③ 将定时器的计数值清零
④ 返回定时器计数值
注：因为TIM_GetCounter得到的是短整型（short int），所以这里我们要进行一个强制转换。
int Read_Speed(int TIMx)
{
int value_1;
switch(TIMx)
{
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;
}
return value_1;
}
3）定时器中断服务函数
// 定时器2中断服务函数
void TIM2_IRQHandler()
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)// 中断标志位置1
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);// 清楚中断标志位
}
}
// 定时器4中断服务函数
void TIM4_IRQHandler()
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET)// 中断标志位置1
{
TIM_ClearITPendingBit(TIM4,TIM_IT_Update);// 清楚中断标志位
}
}
2.编码器函数头文件——Encoder.h
#ifdef_ENCODER_H
#define _ENCODER_H
void Encoder_TIM2_Init(void); // 编码器1初始化函数
void Encoder_TIM4_Init(void); // 编码器2初始化函数
int Read_Speed(int TIMx); // 编码器速度读取函数
void TIM2_IRQHandler(void); // 定时器2中断服务函数
void TIM4_IRQHandler(void); // 定时器4中断服务函数
#endif
第四部分 陀螺仪MPU6050
一、硬件结构
1.什么是陀螺仪？
陀螺仪是用于测量或维护方位和角速度的设备。它是一个旋转的轮子或圆盘，其中旋转轴可以不受影响的设定在任何方向。当旋转发生时，根据角动量守恒定律，该轴的方向不受支架倾斜或旋转的影响。
2.MPU6050三位角度加速度陀螺仪
MPU6050是一个6轴运动处理传感器。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（Digital Motion Processor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其I2C接口输出一个6轴信号。
MPU6050对陀螺仪和加速度计分别用了三个16位ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的。
陀螺仪的可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。
图1 陀螺仪MPU6050实物图
3.原理图
作用：检测倾斜角
MPU6050中断引脚——PB5
MPU6050所用I2C——PB3/PB4
图2 陀螺仪MPU6050（I2C）
二、软件编程
1.I2C函数——mpuiic.c
GPIO初始化：PB3、PB4，模拟I2C；
通过时序函数模拟I2C通信
2.MPU6050函数——mpu6050.c
MPU6050初始化
3.MPU6050中断函数——exti.c
这里主要是通过MPU6050模块上的中断引脚ANT读取角加速度和角速度的原始数据。
exti.c：
#include "exti.h"
void MPU6050_EXTI_Init(void)
{
EXTI_InitTypeDef EXTI_InitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
// 开启时钟
// 外部中断，需要使能AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU; // PB5配置为上拉输入
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);// 外部中断和GPIO映射起来
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;// MPU6050发生中断时会有一个下降沿
EXTI_Init(&EXTI_InitStruct);
}
exti.h：#ifndef_EXTI_H#define_EXTI_H
#include "sys.h"
void MPU6050_EXTI_Init(void);
#endif
第五部分 位置式PID、直立环与速度环概念
一、PID控制算法
PID控制，即为对偏差进行比例、积分和微分控制。由三个元素构成，分别是比例（P），积分（I），微分（D）。
工程中P必然存在，在P的基础上又有如PI控制（比例积分），PD控制（比例微分），PID控制（比例积分微分）。
比例项：提高响应速度，减小静差。
积分项：消除稳态误差。
微分项：减小震荡以及超调。
二、位置式PID
1.理论分析
位置式PID是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微信进行控制，使偏差趋向于零的过程。
2.公式
PWM_out=Kp×e(k)+Ki×Σe(k)+Kd×
e(k)：本次偏差
e(k-1)：上一次偏差
Σe(k)：e(k)以及以前的偏差的累积和，其中k为1,2,…
PWM_out：输出
3.结构框图
图1 位置式PID算法
4.C语言实现
int Position_PID(int Encoder, int Target)
{
static float Bias, PWM_out, Integral_bias, Last_bias;
Bias = Encoder-Target; // 计算偏差
Integral_bias += Bias; // 计算偏差的积分
// 位置式PID控制器公式
PWM_out = Position_Kp*Bias+Position_Ki* Integral_bias+ Position_KD*(Bias- Last_bias)
Last_bias = Bias; // 保存上一次偏差
return PWM_out; // 输出
}
三、直立环
1.理论
小车往那边倒，车轮就往哪边开，这样就可以保持车子的平衡。
图2 直立环
2.公式
a=b1×θ+b2×θ’;
（比例微分控制【PDout=Kp×Angle+Kd×(Angle- Angle_last)】）
3.结构框图
图3 直立环结构框图
4.比较
① 纯比例控制：
只存在大小与角度偏差成正比的回复力
a=b1×θ;
（纯比例控制【Pout=Kp×Angle】）
② 比例微分控制：
大小与角度成正比，方向与角速度成正比的回复力；
大小与角速度成正比，方向与回复力成反比的阻尼力。
a=b1×θ+b2×θ’;
（比例微分控制【PDout=Kp×Angle+Kd×(Angle- Angle_last)】）
四、速度环、串级PID
1.理论
通过速度反馈使得小车保持平衡。
图4 速度环
2.串级控制系统
图5 速度环、串级PID
速度环输入：① 给定速度；② 速度反馈。
速度环输出：角度值（直立环的期望角度输入）
直立环输入：① 给定角度（速度环输出）；② 角度反馈。
直立环输出：PWM（直接控制小车）
3.公式
① 直立环输出=Kp1×(真实角度-期望角度)+Kd×角度偏差微分
（角度偏差=真实角度-期望角度）
② 速度环输出=Kp2×(反馈编码器值-期望编码器值)+Ki×编码器偏差的积分
（编码器偏差=反馈编码器值-期望编码器值）
【Notes：（1）速度环输出=直立环的期望角度;（2）Kp1：直立环Kp;（3）Kp2：速度环Kp】
③ 串级输出out=Kp1×真实角度+Kd×角度偏差的微分-Kp1×(Kp2×编码器偏差-Ki×编码器偏差的积分)
第六部分位置式PID、直立环与速度环编程
一、直立环（PD控制器）
1.中文公式
直立环输出=Kp1×角度偏差+Kd×角度偏差的微分
// 角度偏差=真实角度-期望角度
2.英文公式
直立环PD控制器：Kp×Ek+Kd×Ek_D
（Ek：角度偏差；Ek_D：角度偏差的微分）
Ek=真实角度-期望角度（Angle-Med，由陀螺仪MPU6050测得）
Ek_D=真实角速度（gyro_Y，由陀螺仪MPU6050测得）
3.软件编程
根据理**式进行软件编程，相信看完上面的讲解后这段代码应该比较清晰易懂。
/*****************
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
出口：直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
return PWM_out;
}
二、速度环（PI控制器）
1.中文公式
速度环输出=Kp2×电机速度偏差+Ki2×电机速度偏差的积分
// 电机速度偏差=真实速度-期望速度
2.英文公式
速度环PI控制器：Kp×Ek+Ki×Ek_S
（Ek：电机速度偏差；Ek_S：电机速度偏差的积分）
Ek=真实速度-期望速度（真实速度：左电机速度+右电机速度；期望速度：0）
Ek_S=速度偏差的累加
3.低通滤波
期间需要低频滤波，我们是以直立环为主，速度环为辅，速度环相对于直立环来说是一个干扰，最终目的是直立。低频滤波作用是使得波形更加平滑，滤除高频干扰，防止速度过大影响直立环正常工作。
4.积分限幅
通过比较限制积分在规定范围内变动，不得超出。
5.软件编程
根据理**式进行软件编程，相信看完上面的讲解后这段代码应该比较清晰易懂。
/*****************
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
// 定义成静态变量，保存在静态存储器，使得变量不丢掉
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
float a=0.7;
// 1.计算速度偏差
//舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
Encoder_Err = ((encoder_left+encoder_right)-Target);
// 2.对速度偏差进行低通滤波
// low_out = (1-a)*Ek+a*low_out_last
EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
EnC_Err_Lowout_last = EnC_Err_Lowout; // 防止速度过大影响直立环的正常工作
// 3.对速度偏差积分出位移
Encoder_S+=EnC_Err_Lowout;
// 4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
// 5.速度环控制输出
PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
return PWM_out;
}
页:
[1]
2