文章已获作者授权转载，版权归原作者所有，如有侵权，与本账号无关，可联系删除。 原文作者:敲代码敲到头发茂密
文章目录
使用stm32实现电机的PID控制
PID控制应该算是非常古老而且应用非常广泛的控制算法了，小到热水壶温度控制，大到控制无人机的飞行姿态和飞行速度等等。在电机控制中，PID算法用的尤为常见，本文将由浅入深介绍使用stm32实现电机的PID控制，希望能帮助到有需要的人
直接上代码仓库链接：gitee-基于stm32的PID电机控制源码
1 电机基本控制
直流电机的内部结构和工作原理其实在高中的物理教材上就已经讲过，这里主要讨论用单片机和电机驱动模块驱动一个直流电机。
1.1 器件准备和接线
本文介绍使用的单片机型号是stm32f103rct6，也可以用c8t6。
电机驱动选择TB6612
直流电机选择这种带霍尔传感器的编码电机，12V，减速比1/30，速度330rpm。
为了方便观察和操作，使用了一块0.96寸的OLED
接线
| 模块引脚 | 单片机引脚 |
|---|---|
| OLED_SCL | PB8 |
| OLED_SDA | PB9 |
| 按键K1 | PC9 |
| 按键K2 | PC8 |
| TB6612_AIN1 | PB12 |
| TB6612_AIN2 | PB13 |
| 编码器A相 | PB6 |
| 编码器B相 | PB7 |
1.2 代码展示
TB6612的驱动非常简单，使用到两个普通的GPIO输出高低电平控制电机正反转，再使用一个复用定时器的IO生成一个PWM控制电机转速即可。
motor.c部分代码如下：
#include "motor.h"
/**
* @brief 电机方向控制引脚设置
* @param None
* @retval None
*/
static void motor_gpio_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50M
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB
}
/**
* @brief 定时器初始化
* @param arr：自动重装值,设为一个时钟频率的最大值
* @param psc： 预分频值
* @retval None
*/
void Motor_PWM_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
motor_gpio_Init();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);//
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE); //使能GPIO外设时钟使能
//设置该引脚为复用输出功能,输出TIM1 CH1 CH4的PWM脉冲波形
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //TIM_CH1 //TIM_CH4
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值 不分频
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OC4Init(TIM1, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主输出使能
TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable); //CH4预装载使能
TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器
TIM_Cmd(TIM1, ENABLE); //使能TIM1
}
使用一个函数即可，输入的是带符号的整型变量，正负号代表选择方向，绝对值代表占空比。
/**
* @brief 电机输出控制
* @param motor_pwm: 占空比 0-7200
* @retval None
*/
void Set_Pwm(int motor_pwm)
{
if(motor_pwm>0)
{
BIN1=1;
BIN2=0; //前进
}
else if(motor_pwm<0)
{
BIN1=0;
BIN2=1; //后退
}
else //停止
{
BIN1=0;
BIN2=0;
}
PWMB=myabs(motor_pwm);
TIM_SetCompare4(TIM1, PWMB);
}
1.3 效果展示
OLED的第一行显示运行状态，ON或者OFF，第二行显示电机PWM数值，+代表正转，-代表反转，按下K1切换电机旋转方向。按下K2,电机启动/关闭。
gitee-基于stm32的PID电机控制源码
2 电机速度读取
所用的编码器是一个霍尔传感器，两个霍尔元件相差90度放置。编码电机速度的读取方式从单片机读取方式上分有定时器输入捕获法和外部中断法，从编码器原理上又分为二倍频和四倍频，从速度计算方式上又分为M法测速和T法测速。
下面就简单了解一下M法测速和T法测速
- M法测速：在一个固定的计时周期内，统计这段时间的编码器脉冲数，从而计算速度值。
转速n的计算公式为：
n = M 0 / ( C ∗ T 0 ) \ n = M0/(CT0) \ n=M0/(C∗T*0)
C：编码器单圈总脉冲数
T0：计数周期，单位：秒（s）
M0：在计数周期内，统计到的编码器脉冲数
假设已知编码器转过一圈需要100个脉冲，在100ms内测得产生了20个脉冲。通过公式计算n = 20/(100*0.1)=2 (圈/秒)
- T法测速：这种方法是建立一个已知频率的高频脉冲并对其计数。
转速n的计算公式为：
n = F 0 / ( C ∗ M 1 ) \ n = F0/(CM1) \ n=F0/(C∗M*1)
C：编码器单圈总脉冲数
F0：高频脉冲的频率
M1：一个脉冲转的圈数
T法是利用一个已知脉冲来测量编码器两个脉冲之间的时间来计算出速度的。
假设编码器转过一圈需要100个脉冲（C=100），则1个脉冲转了1/100圈，用时为20ms，即50hz，转速为0.5圈/秒。
M法测速和T法测速是解决速度计算层面的问题，下面就以定时器输入捕获法和外部中断法展开讨论。
2.1 定时器输入捕获法
2.1.1 定时器输入捕获内部结构
首先是两个正交信号从GPIO输入到滤波器，然后再进行边沿检测和极性选择。
下面是计数方向与编码信号的关系：
2.1.2 代码展示
encoder.c 部分代码如下：
/**
* @brief 编码器初始化,使用定时器输入捕获法
* @param None
* @retval None
*/
void Encoder_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //ARR
TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICFilter = 0xF;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
/*TI1和TI2都计数，上升沿计数*/
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_Cmd(TIM4, ENABLE);
}
/**
* @brief 获取定时器的计数值
* @param None
* @retval None
*/
int16_t Encoder_Get(void)
{
int16_t Temp;
Temp = TIM_GetCounter(TIM4);
TIM_SetCounter(TIM4, 0);
return Temp;
}
此外还要再使用一个定时器定时采集速度：
/**
* @brief 定时器中断，每100ms更新一次速度
* @param None
* @retval None
*/
void TIM2_IRQHandler(void)
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
{
Speed = Encoder_Get();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}
}
关于电机轮子实际转速的计算：
1.首先根据速度的计算公式本质就是：
v = s / t v =s/tv=s/t
t：已知的编码器计数周期（100ms），s:轮子在这个计数周期内走过的路程。
2.下面就是需要计算s。
我们知道，编码器采集的是单位时间内的脉冲个数，而且编码器的码盘安装在电机轴的末端，测的是电机输出轴的速度，电机轴还需要经过减速器才能和轮子相连。
由此，我们可以先算出单位时间内，一个脉冲 轮子走过的距离
l = 2 ∗ P I ∗ R / ( 4 ∗ n ∗ i ) l =2PIR/(4ni)l=2∗P**I∗R/(4∗n∗i)
2 * PI R 就是轮子周长，R是轮子半径（34mm），使用4倍频计数，n就是编码器的码盘转一圈的脉冲数（11），i就是电机的减速比（30）。
3.一个脉冲轮子走过的距离是可以直接手算出来的，那么我们再乘以编码器测量的脉冲个数就知道轮子在这个计数周期内走过的路程s了。
因此，
s = m ∗ l s=mls=m∗l
m就是编码器测量的脉冲个数。
上述计算过程使用C代码表示如下：
/**
* @brief 编码器读数转换为轮子速度（mm/s）
* @param encoder：编码器计数
* @retval : Velocity 轮子速度
*/
int Get_Velocity_Form_Encoder(int encoder)
{
float Distance,Velocity;
Distance= 2*3.14159*34/(4*11*30);//单位是mm
Velocity= encoder*Distance/0.1; //单位是mm/s。0.1就是编码器计数周期100ms，0.1s
return Velocity;
}
2.2.外部中断法
2.2.1 外部中断法简介
使用单片机引脚的跳变信号触发中断，然后在中断里判断两个编码器引脚的电平，让计数值增加或者减少。我们规定，正转计数值++，反转正转计数值—。
采用4倍频计数，就是A相的上升沿、下降沿和B相的上升沿、下降沿都触发中断。
| A相边沿 | B相电平 | 对应区域 | 计数值 |
|---|---|---|---|
| 上升沿 | 0 | 2 | Encoder_EXTI++ |
| 下降沿 | 1 | 4 | Encoder_EXTI++ |
| B相边沿 | A相电平 | 对应区域 | 计数值 |
| 上升沿 | 1 | 3 | Encoder_EXTI++ |
| 下降沿 | 0 | 1 | Encoder_EXTI++ |
当电机反转时，A相、B相的信号与正转时的相位相差90度，换句话说就是，正转时A相先触发上升沿，反转时B相先触发上升沿。
| A相边沿 | B相电平 | 对应区域 | 计数值 |
|---|---|---|---|
| 上升沿 | 1 | 3 | Encoder_EXTI– |
| 下降沿 | 0 | 1 | Encoder_EXTI– |
| B相边沿 | A相电平 | 对应区域 | 计数值 |
| 上升沿 | 0 | 2 | Encoder_EXTI– |
| 下降沿 | 1 | 4 | Encoder_EXTI– |
2.2.2 代码展示
部分代码如下：
/**
* @brief 编码器初始化,使用外部中断法
* @param None
* @retval None
*/
void Encoder_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
EXTI_InitTypeDef EXTI_InitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_Init(GPIOB,&GPIO_InitStruct);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
//设置IO口与中断线的映射关系
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource6);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource7);
//4.初始化线上中断
EXTI_InitStruct.EXTI_Line = EXTI_Line6 | EXTI_Line7;
EXTI_InitStruct.EXTI_LineCmd = ENABLE;
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//跳变沿触发
EXTI_Init(&EXTI_InitStruct);
//5.配置中断分组
NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_Init(&NVIC_InitStruct);
}
/**
* @brief 中断服务函数，采用4倍频测速
* @param None
* @retval None
*/
int Encoder_EXTI=0;
void EXTI9_5_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line6) != RESET)//右轮A相 PB6
{
EXTI_ClearITPendingBit(EXTI_Line6); //清除LINE上的中断标志位
if(PBin(6)==0) //这里判断检测到的是否是下降沿
{
if(PBin(7)==0) Encoder_EXTI++;//B相的电平如果是低，电机就是正转加1
else Encoder_EXTI--;//否则就是反转减1
}
else //上升沿
{
if(PBin(7)==1) Encoder_EXTI++; //B相电平如果为高，电机就是正转加1
else Encoder_EXTI--;//否则就是反转减1
}
}
if(EXTI_GetITStatus(EXTI_Line7) != RESET)//右轮B相 PB7
{
EXTI_ClearITPendingBit(EXTI_Line7); //清除LINE上的中断标志位
if(PBin(7)==0) //这里判断检测到的是否是下降沿
{
if(PBin(6)==1) Encoder_EXTI++;//A相的电平如果是高，电机就是正转加1
else Encoder_EXTI--;//否则就是反转减1
}
else //上升沿
{
if(PBin(6)==0) Encoder_EXTI++; //A相电平如果为低，电机就是正转加1
else Encoder_EXTI--;//否则就是反转减1
}
}
}
/**
* @brief 获取中断的计数值
* @param None
* @retval None
*/
int16_t Encoder_Get(void)
{
int16_t Temp;
Temp = Encoder_EXTI;
Encoder_EXTI =0;
return Temp;
}
/**
* @brief 定时器中断，每100ms更新一次速度
* @param None
* @retval None
*/
void TIM2_IRQHandler(void)
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
{
Speed = Encoder_Get();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}
}
2.2.3 效果展示
3 位置式PID
3.1 计算公式
在电机控制中，我们给电机输出的是一个PWM占空比的数值。
话不多说，直接上位置式PID基本公式：
控制流程图如下：
上图中的目标位置一般我们可以通过按键或者开关等方式编程实现改变目标值，测量位置就是通过stm32去采集编码器的数据。目标位置和测量位置之间做差这个就是目前系统的偏差。送入PID控制器进行计算输出，然后再经过电机驱动的功率放大控制电机的转动去减小偏差， 最终达到目标位置的过程。
3.2 C语言实现
位置式 PID 具体通过 C 语言实现的代码如下：
先定义一个PID参数的结构体：
typedef struct
{
float target_val; //目标值
float Error; /*第 k 次偏差 */
float LastError; /* Error[-1],第 k-1 次偏差 */
float PrevError; /* Error[-2],第 k-2 次偏差 */
float Kp,Ki,Kd; //比例、积分、微分系数
float integral; //积分值
float output_val; //输出值
}PID;
然后弄一个PID参数初始化的函数：
/**
* @brief PID参数初始化
* @note 无
* @retval 无
*/
void PID_param_init()
{
PosionPID.target_val=3600;
PosionPID.output_val=0.0;
PosionPID.Error=0.0;
PosionPID.LastError=0.0;
PosionPID.integral=0.0;
PosionPID.Kp = 10;
PosionPID.Ki = 0.5;
PosionPID.Kd = 0.8;
}
最后再根据公式写出位置式PID的实现函数：
/**
* @brief 位置PID算法实现
* @param actual_val:实际测量值
* @note 无
* @retval 通过PID计算后的输出
*/
float PosionPID_realize(PID *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->Error = pid->target_val - actual_val;
/*积分项*/
pid->integral += pid->Error;
/*PID算法实现*/
pid->output_val = pid->Kp * pid->Error +
pid->Ki * pid->integral +
pid->Kd *(pid->Error -pid->LastError);
/*误差传递*/
pid-> LastError = pid->Error;
/*返回当前实际值*/
return pid->output_val;
}
函数入口参数为编码器的速度测量值和PID参数的结构体，返回值为电机控制 PWM。
4 增量式PID
4.1 计算公式
增量式PID也称速度环PID，速度闭环控制就是根据单位时间获取的脉冲数（这里使用了 M 法测速）测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
在我们的速度控制闭环系统里面只使用 PI 控制，因此对 PID 控制器可简化为以下公式：
控制框图和位置式的一样的。
上图中的目标速度一般我们可以通过按键或者开关等方式编程实现改变目标值，测量速度前面在编码器的章节已经有说到就是通过单片机定时去采集编码器的数据并清零。目标速度和测量速度之间做差这个就是目前系统的偏差。送入 PID 控制器进行计算输出，然后再经过电机驱动的功率放大控制电机的转动去减小偏差， 最终达到目标速度的过程。
4.2 C语言实现
增量式 PID 的结构体定义和成员初始化和位置式一样的，通过 C 语言实现的代码如下：
/**
* @brief 速度PID算法实现
* @param actual_val:实际值
* @note 无
* @retval 通过PID计算后的输出
*/
float addPID_realize(PID *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->Error = pid->target_val - actual_val;
/*PID算法实现，照搬公式*/
pid->output_val += pid->Kp * (pid->Error - pid-> LastError) +
pid->Ki * pid->Error +
pid->Kd *(pid->Error -2*pid->LastError+pid->PrevError);
/*误差传递*/
pid-> PrevError = pid->LastError;
pid-> LastError = pid->Error;
/*返回当前实际值*/
return pid->output_val;
}
函数入口参数为编码器的速度测量值和PID参数的结构体，返回值为电机控制 PWM，可以看出增量式PID只与最近三次的测量值有关。
5 串级PID
串级PID就是先输入位置PID再经过速度PID,最后再输出。
6 P、I、D各个参数的作用
自动控制系统的性能指标主要有三个方面：稳定性、快速性、准确性。
稳定性：系统在受到外作用后，若控制系统使其被控变量随时间的增长而最终与给定期望值一致，则称系统是稳定的，我们一般称为系统收敛。如果被控量随时间的增长，越来越偏离给定值，则称系统是不稳定的，我们一般称为系统发散。稳定的系统才能完成自动控制的任务，所以，系统稳定是保证控制系统正常工作的必要条件。一个稳定的控制系统其被控量偏离给定值的初始偏差应随时间的增长逐渐减小并趋于零。
快速性：快速性是指系统的动态过程进行的时间长短。过程时间越短，说明系统快速性越好，过程时间持续越长，说明系统响应迟钝，难以实现快速变化的指令信号。稳定性和快速性反映了系统在控制过程中的性能。系统在跟踪过程中，被控量偏离给定值越小，偏离的时间越短，说明系统的动态精度偏高。
准确性：是指系统在动态过程结束后，其被控变量（或反馈量）对给定值的偏差而言，这一偏差即为稳态误差，它是衡量系统稳态精度的指标，反映了动态过程后期的性能。
在实践生产工程中，不同的控制系统对控制器效果的要求不一样。比如平衡车、倒立摆对系统的快速性要求很高，响应太慢会导致系统失控。智能家居里面的门窗自动开合系统，对快速性要求就不高，但是对稳定性和准确性的要求就很高，所以需要严格控制系统的超调量和静差。
总结
本文主要介绍了在电机的PID控制中常用的位置式PID和增量式PID。
上述代码见仓库链接：gitee-基于stm32的PID电机控制源码
原文链接：https://blog.csdn.net/weixin_43811044/article/details/127956227
1142
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
