STM32电机PID速度控制

472
0
2020年9月15日 11时20分

2.3 STM32电机PID速度控制

之前的文章,完成了直流减速电机的PWM控制、电机测速。本篇文章,将实现电机的速度闭环控制。
在公众号:小白学移动机器人,发送:速度PID,即可获得本篇文章的STM32工程文件以及相关资料。

 

2.3.1 解决的问题

解决带编码器直流电机的速度闭环问题。

 

2.3.2 PID理论

将偏差的比例、积分、微分,通过线性组合构成控制量,用控制量对被控对象进行控制,这样的控制器称为PID控制器。在连续空间中,我们通常探讨模拟PID的控制原理,如图所示:

image-20200819142421140

 

我们这里用电机速度控制为例,讲解PID控制系统。r(t)为设定电机速度、y(t)为实际电机速度、e(t)=y(t)-r(t)为速度差值作为PID控制器的输入、u(t)为PID控制器的输出,作用到被控对象电机上。根据模拟PID控制器,科学家们也得出了模拟PID控制的公式,如图所示:

 

image-20200819142538096

其中Kp、Ti、Td,分别为控制器的比例系数、积分系数、微分系数。该理论用在控制的例子比比皆是。但是模拟PID控制系统是在连续空间的上描述的,无法在计算机上用代码实现。于是就有数字PID控制理论,将连续空间的PID控制系统在离散空间上描述。积分变成了求和、微分变成了求斜率,于是就出现数字PID控制系统的理论公式,如图所示:

image-20200819142642601

其中Kp、Ti、Td和上面描述的一样,T为采用周期,ek是本次差值,ek-1上一次的差值,直接通过模拟PID转化的数字PID又叫做位置式PID,该方式的PID的输出直接是控制量,非常不适合经常出现异常的系统,另外一种方式是增量式PID,每次只输出一个正向或者反向的调节量,就算出现异常,也不会产生巨大的影响。具体数学公式如下所示:该方法较多的应用于生产生活中,本论文中电机的速度PID控制当然也不例外。

image-20200819142756857

有了上面的理论基础,开始代码实现的介绍。首先就是明确增量式PID系统的输入、输出、控制对象。将速度的设定值和速度的测得值作为PID控制器的输入参数,PID的输出参数为对PWM的调节偏差,控制对象PWM进而驱动电机达到设定速度。以上内容确定之后,就是PID控制器的代码部分了。其实仔细看看增量式PID就只有一个公式,所以使用代码实现并不困难。如下所示核心代码就这一句。

image-20200819142934167

完成上面的代码,只是完成速度PID的一部分,剩下的是尤为重要的PID参数整定。该整定方法丰富多样,最为准确的是模型计算,但是对于我们做机器人多使用试凑法。虽然需要调节一段时间,但是不需要对机器人进行建模。试凑法一般按照P、I、D的顺序进行调节。

 

初始时刻将Ki和Kd都设置成0,按照经验设置Kp的初始值,就这样将系统投入运行,由小到大调节Kp。求得满意的曲线之后,若要引入积分作用,将Kp设置成之前的5/6,然后Ki由小到大开始调节。达到满意效果之后,若要引入微分作用,将Kd按照经验调节即可。经过有规律的试凑,最终达到一个我们满意的就行。

 

2.3.3 代码分享

关于PWM控制、以及电机测速的代码,这里就不在展示了,看之前的文章都可以找到。

 

(1)pid.h

 

#include "pid.h"

/*===================================================================
程序功能:双路电机速度PID
程序编写:公众号:小白学移动机器人
其他   :如果对代码有任何疑问,可以私信小编,一定会回复的。
=====================================================================
------------------关注公众号,获得更多有趣的分享---------------------
===================================================================*/

struct pid_uint pid_Task_Letf;
struct pid_uint pid_Task_Right;

/****************************************************************************
*函数名称:PID_Init(void)
*函数功能:初始化PID结构体参数
****************************************************************************/

void PID_Init(void)
{
//乘以1024原因避免出现浮点数运算,全部是整数运算,这样PID控制器运算速度会更快
/***********************左轮速度pid****************************/
    pid_Task_Letf.Kp = 1024 * 0.5;//0.4
     pid_Task_Letf.Ki = 1024 * 0;    
    pid_Task_Letf.Kd = 1024 * 0.08; 
    pid_Task_Letf.Ur = 1024 * 4000;
    pid_Task_Letf.Adjust   = 0;
    pid_Task_Letf.En       = 1;
    pid_Task_Letf.speedSet = 0;
    pid_Task_Letf.speedNow = 0;
    reset_Uk(&pid_Task_Letf);        
/***********************右轮速度pid****************************/
    pid_Task_Right.Kp = 1024 * 0.35;//0.2
     pid_Task_Right.Ki = 1024 * 0;    //不使用积分
    pid_Task_Right.Kd = 1024 * 0.06; 
    pid_Task_Right.Ur = 1024 * 4000;
    pid_Task_Right.Adjust   = 0;
    pid_Task_Right.En       = 1;
    pid_Task_Right.speedSet = 0;
    pid_Task_Right.speedNow = 0;
    reset_Uk(&pid_Task_Right);
}

/***********************************************************************************************
 函 数 名:void reset_Uk(PID_Uint *p)
 功    能:初始化U_kk,ekk,ekkk
 说    明:在初始化时调用,改变PID参数时有可能需要调用
 入口参数:PID单元的参数结构体 地址
************************************************************************************************/

void reset_Uk(struct pid_uint *p)
{
    p->U_kk=0;
    p->ekk=0;
    p->ekkk=0;
}

/***********************************************************************************************
 函 数 名:s32 PID_commen(int set,int jiance,PID_Uint *p)
 功    能:PID计算函数
 说    明:求任意单个PID的控制量
 入口参数:期望值,实测值,PID单元结构体
 返 回 值:PID控制量
************************************************************************************************/

s32 PID_common(int set,int jiance,struct pid_uint *p)
{
    int ek=0,U_k=0;

    ek=jiance - set;                                                               

    U_k=p->U_kk + p->Kp*(ek - p->ekk) + p->Ki*ek + p->Kd*(ek - 2*p->ekk + p->ekkk);

    p->U_kk=U_k;
    p->ekkk=p->ekk;
    p->ekk=ek;

    if(U_k>(p->Ur))                                            
        U_k=p->Ur;
    if(U_k<-(p->Ur))
        U_k=-(p->Ur);

    return U_k>>10; 
}

/***********************************************************************************
** 函数名称 :void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
** 函数功能 :pid选择函数          
***********************************************************************************/

void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
{
    /**********************左轮速度pid*************************/
    if(pl->En == 1)
    {                                    
        pl->Adjust = -PID_common(pl->speedSet, pl->speedNow, pl);        
    }    
    else
    {
        pl->Adjust = 0;
        reset_Uk(pl);
        pl->En = 2; 
    }
    /***********************右轮速度pid*************************/
    if(pr->En == 1)
    {
        pr->Adjust = -PID_common(pr->speedSet, pr->speedNow, pr);        
    }    
    else
    {
        pr->Adjust = 0;
        reset_Uk(pr);
        pr->En = 2; 
    }
}

/*******************************************************************************
 * 函数名:Pid_Ctrl(int *leftMotor,int  *rightMotor)
 * 描述  :Pid控制
 *******************************************************************************/

void Pid_Ctrl(int *leftMotor,int  *rightMotor)
{
    Pid_Which(&pid_Task_Letf, &pid_Task_Right); 
    *leftMotor  += pid_Task_Letf.Adjust;
    *rightMotor += pid_Task_Right.Adjust;
}

 

(2)main.c

 

#include "sys.h"

//====================自己加入的头文件===============================
#include "delay.h"
#include "led.h"
#include "encoder.h"
#include "usart3.h"
#include "timer.h"
#include "pwm.h"
#include "pid.h"
#include "motor.h"
#include <stdio.h>
/*===================================================================
程序功能:直流减速电机的速度闭环控制测试
程序编写:公众号:小白学移动机器人
其他    :如果对代码有任何疑问,可以私信小编,一定会回复的。
=====================================================================
------------------关注公众号,获得更多有趣的分享---------------------
===================================================================*/
int leftSpeedNow  =0;
int rightSpeedNow =0;

int leftSpeeSet   = -300;//mm/s
int rightSpeedSet = -300;//mm/s

int main(void)
{ 

    GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD

    MY_NVIC_PriorityGroupConfig(2);    //=====设置中断分组

    delay_init();                    //=====延时函数初始化
    LED_Init();                     //=====LED初始化    程序灯    

    usart3_init(9600);              //=====串口3初始化  蓝牙 发送调试信息

    Encoder_Init_TIM2();            //=====初始化编码器1接口
    Encoder_Init_TIM4();            //=====初始化编码器2接口

    Motor_Init(7199,0);             //=====初始化PWM 10KHZ,用于驱动电机 如需初始化驱动器接口

    TIM3_Int_Init(50-1,7200-1);     //=====定时器初始化 5ms一次中断

    PID_Init();                        //=====PID参数初始化

    while(1)
    {
        //给速度设定值和实时值赋值
        pid_Task_Letf.speedSet  = leftSpeeSet;
        pid_Task_Right.speedSet = rightSpeedSet;
        pid_Task_Letf.speedNow  = leftSpeedNow;
        pid_Task_Right.speedNow = rightSpeedNow;

        //执行PID控制函数
        Pid_Ctrl(&motorLeft,&motorRight);

        //根据PID计算的PWM数据进行设置PWM
        Set_Pwm(motorLeft,motorRight);

        //打印速度
        printf("%d,%d\r\n",leftSpeedNow,rightSpeedNow);
    } 
}

//5ms 定时器中断服务函数

void TIM3_IRQHandler(void)                            //TIM3中断
{
    if(TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否
    {
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update);   //清除TIMx的中断待处理位

        Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);//计算电机速度

        Led_Flash(100);                               //程序闪烁灯
    }
}

2.3.4 总结

以上三篇内容,是关于直流减速电机的PWM控制、速度测量以及最后电机速度的闭环控制。现在对于电机的简单控制基本告一段落,对于做一个ROS小车的电机控制,这里基本是足够的。下面我们会介绍使用IIC+DMP获取MPU6050数据。

 

系列文章

搭建ROS小车真的难吗?
ROS小车软件结构以及控制流程
STM32电机PWM控制
STM32电机测速(正交\霍尔编码器)
在这里插入图片描述

发表评论

后才能评论