0


STM32机器人控制开发教程No.2 霍尔编码器电机测速以及增量式PID控制(基于HAL库)

01 前言

本教程使用的机器人控制板拥有4个带编码器的电机接口,4个舵机接口,串口通信接口、SWD下载调试接口、航模遥控接口、USB 5V输出接口以及方便与树莓派直接连接的40PIN接口等,板载资源丰富,方便调试!可以控制两轮、四轮差速及阿克曼转向机器人/小车。

控制板上的电机接口:

机器人小车电机驱动开发——测量小车速度

在上一篇文章《STM32机器人控制开发教程No.1驱动电机(基于HAL库)》中介绍了关于如何使用NANO小车上的机器人控制板控制减速电机完成前进、后退和转向等基础功能,如果需要知道小车的实时行进速度该如何进行测量呢?在本章节给你介绍如何使用NANO驱动板进行小车速度的测量!

02 STM32与编码器测速

小车的速度信息的准确获取对于控制系统有着至关重要的影响。编码器是一种将角速度或角位转换成数字脉冲的旋转式传感器,使用者可以通过编码器测量电机的速度信息。在本例程中主要介绍STM32与编码器电机之间的搭配使用方法,关于具体的编码器原理只做简略介绍。

本例程中使用的电机为带霍尔编码器的减速电机,电机由三部分组成:减速器,电机以及霍尔编码器。

霍尔编码器工作原理:霍尔编码器通过电磁转换,将机械的位移转化为脉冲信号,并且输出A、B两相的方波信号,A、B两相脉冲信号相位相差90°,通过检测规定时间内的脉冲数,以及A、B两相脉冲信号的相对位置,便能得到编码器的值与其运动方向。

那么如何测量规定时间内的脉冲数以及两个信号的相位关系呢?

STM32的定时器功能强大,其中部分定时器具有的编码器模式,便可以解决上述问题!通过STM32定时器的编码器模式可以对输入的A、B相信号进行处理。并且STM32的编码器模式可以设置为对TI1、TI2两路信号同时进行脉冲计数,即四倍频!可以提高检测的精度。并且通过检测TI1与TI2的相位关系,计数器可以实现向上计数或向下计数,以此检测其运动方向。

根据本文使用的减速电机参数,电机为11线的霍尔编码器减速电机,减速比为1:30,即转动一圈可以输出11 * 30=330个脉冲,通过STM32四倍频后,电机转动一圈得到的脉冲数为330 * 4=1320,通过定时器中断设置速度采样周期为50ms,即每50ms读取一次定时器中的计数器值。要得到最终的速度,还需要考虑轮胎的大小,根据NANO小车配套的轮胎直径尺寸为65mm,所以周长C为3.14*6.5=20.41cm。参考以上参数,然后便可以计算小车的运动速度。根据测速原理:设采样周期内传入的脉冲数为N,电机转动一圈得到的脉冲数为1320,而轮子转动一圈的运动距离为20.41cm。那么得到脉冲数N时运动的距离S=(0.2041 * N/1320),再除以规定的采样周期时间便可以得到运动的速度,完整的计算公式如下:

03 STM32CubeMX配置及测速驱动代码

关于如何创建工程以及系统配置可以参考上一篇文章《STM32机器人控制开发教程No.1驱动电机(基于HAL库)》,本篇文章主要介绍编码器相关定时器的配置!

根据NANO驱动板的原理图,找到两路电机对应的四路编码器接口对应STM32上的IO口,并找到其对应的定时器进行模式配置。如下图所示,使用的两路电机的编码器分别对应到定时器2和定时器3。

在STM32CubeMX中找到定时器2与定时器3,进行模式配置。以下以定时器2为例,定时器3只需进行相同配置即可。选择定时器为编码器模式,设置为不分频,最大计数值为65535,使能自动重装载,并选择TI1和TI2两路输入,实现四倍频效果。

配置完定时器2和定时器3后,需要再使用一个定时器,利用其产生50ms中断来读取当前的小车速度值,本次例程中采用定时器6产生中断。

周期为50ms,计算方法为 :T=(arr+1)*(psc+1)/Tclk

当然,不要忘记了电机驱动的配置,毕竟还是得先动起来才能更好的检验其测速功能,电机的相关配置和驱动具体可参考上一篇文章。

完成配置后,由STM32CubeMX自动生成初始化代码,选择合适的编译器打开当前功能,进行驱动功能的程序开发。以下主要对编码器测速的相关代码进行解释,其余具体代码可参考例程文件。


short encoderPulse[2]={0};

/**
  * @brief  读取定时器2和定时器3的计数值(编码器脉冲值)
  * @retval None
  */
void GetEncoderPulse()
{    
      encoderPulse[0] = -((short)__HAL_TIM_GET_COUNTER(&htim2)); //配合小车轮子运动方向,进行取反操作
    encoderPulse[1] = -((short)__HAL_TIM_GET_COUNTER(&htim3));

    __HAL_TIM_GET_COUNTER(&htim2) = 0;   //计数值重新清零
    __HAL_TIM_GET_COUNTER(&htim3) = 0;  
}

/**
  * @brief  根据得到的编码器脉冲值计算速度 单位:m/s
  * @retval 速度值
  */
float CalActualSpeed(int pulse)
{
    return (float)(0.003092424 * pulse);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)  //定时器6中断回调函数,每50ms调用一次
{
  float c_leftSpeed,c_rightSpeed; 
  if(htim==(&htim6))
  {
    GetEncoderPulse(); 
    c_leftSpeed = CalActualSpeed(encoderPulse[0]);   //获得当前的速度值
    c_rightSpeed = CalActualSpeed(encoderPulse[1]);
  }
}
(电机驱动的代码可参考上篇文章)

将程序下载到STM32机器人控制板上,通过串口与上位机进行通信,使用串口调试助手可观察到在不同PWM值下,小车的运动速度。

如下图输出占空比为10%的PWM信号时,小车速度为速度如下图:

当输出占空比为90%的PWM信号时,小车的速度如下图:

通过上述介绍的使用定时器的编码器模式读取编码器的脉冲值,最终得到实际的小车运行速度。可以看到,在实际运行时,即使控制的PWM信号占空比一致,左右电机的转速仍存在一定的偏差。为了使速度更稳定以及调速效果响应更迅速和顺滑,可以加入PID算法,对电机的转速进行控制。

04 增量式PID控制

PID可以分为位置式PID与增量式PID,关于PID的具体控制原理知识不在此进行详细介绍,重点介绍的为在本例程中采用的增量式PID。

  • 增量式PID是通过改变输出量的大小来控制被控量的稳定,增量式PID与位置式PID不同,增量式返回的数值为当前时刻的控制量与上一时刻的控制量的差值,以此差值作为新的控制量进行反馈。
  • 举个例子:设定小车的速度为0.2m/s,通过编码器进行测速得到速度反馈,与设定值产生了偏差ek,系统中保存了上一次的偏差e(*k-1)还有上上次的的偏差e(*k-2),这三个值作为输入量通过增量式PID的计算公式得到Δu(k),将上一次经过PID计算后的u(k-1)加上本次的增量Δu(k),便得到本次控制周期的PID输出u(k)。将输出值经过二次的计算转换后,得到可以对电机转速进行控制的PWM占空比,进而对小车的运动速度进行控制。
  • 增量式PID的公式:Kp比例系数、Ki积分系数、Kd微分系数、e(k)偏差Δu(k)=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]

了解增量式PID的一些基础知识后,让我们来看看如何用代码实现算法过程吧!


/*
*  定义PID的结构体,结构体内存储PID参数、误差、限幅值以及输出值
*/
typedef struct
{
    float Kp; 
    float Ki;
    float Kd;

    float last_error;  //上一次偏差
    float prev_error;  //上上次偏差

    int limit;   //限制输出幅值
    int pwm_add; //输出的PWM值  
}PID;
/**
  * @brief  PID相关参数的初始化 
  * @param  PID的结构体指针
  */
void PID_Init(PID *p)
{
    p->Kp = Velocity_Kp;
    p->Ki = Velocity_Ki;
    p->Kd = Velocity_Kd;
    p->last_error = 0;
    p->prev_error = 0;
    p->limit = limit_value;
    p->pwm_add = 0;
}
/**
  * @brief  PID相关参数的初始化 
  * @param  targetSpeed目标速度值,PID的结构体指针p
  */
int PID_Cal(int targetSpeed,int currentSpeed,PID *p)
{
   int error = targetSpeed - currentSpeed;  //得到目标速度与当前速度的误差
    p->pwm_add += p->Kp*(error - p->last_error) + p->Ki*error + p->Kd*(error - 2*p->last_error+p->prev_error);   //根据增量PID公式计算得到输出的增量

    p->prev_error = p->last_error;  //记录上次误差
    p->last_error = error;    //记录本次误差

    if(p->pwm_add>p->limit) p->pwm_add=p->limit;  //限制最大输出值
    if(p->pwm_add<-p->limit) p->pwm_add=-p->limit;
}

​​​​​​然后在前文中写好的定时器6的中断回调函数中调用PID函数,以50ms为周期对当前速度进行调整。

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)  //定时器6中断回调函数,每50ms调用一次
{
  float c_leftSpeed,c_rightSpeed;  
  if(htim==(&htim6))
  {
    GetEncoderPulse(); 
    c_leftSpeed = CalActualSpeed(encoderPulse[0]);   //获得当前的速度值
    c_rightSpeed = CalActualSpeed(encoderPulse[1]);

      PID_Cal(leftTargetSpeed,c_leftSpeed,&LeftMotor_PID);//左边电机的PID计算
      PID_Cal(rightTargetSpeed,c_rightSpeed,&RightMotor_PID);//右边电机的PID计算

      MotorControl(leftMotor_PID.pwm_add,RightMotor_PID.pwm_add);

      //通过上位机查看速度曲线以及PID调速效果
      printf("{currentLeftSpeed is:%.2f}\r\n",c_leftSpeed); 
    printf("{currentrightSpeed is:%.2f}\r\n",c_rightSpeed);
  }
}

代码编写完成后,将程序下载到STM32机器人控制板上,就可以愉快的开始调参啦!

关于PID的参数调节,一般遵循以下原则:

  1. 先调整比例系数Kp,然后再调整积分系数Ki,最后再调整微分系数Kd。
  2. 调整Kp时,从小到大进行调整,选取偏小的比例系数使得输出曲线基本贴合目标直线。
  3. 调整Ki,用Ki消除静态误差。
  4. 调整Kd,提高调速的响应速度。

PID调参示例:(图片仅展示左轮的速度曲线,右轮的速度曲线大致相同)

  • 第一组PID参数:Kp=5.0,Ki=0.0 , Kd=0.0可以看到左右轮子的速度里目标速度20cm/s差距较大,因此需要继续增加Kp的值。

  • 第二组PID参数:Kp=15.0,Ki=0.0 , Kd=0.0可以看到当前的速度有所提高,离目标速度更接近,此时加入Ki参与调节。

  • 第三组PID参数:Kp=15.0,Ki=0.5 , Kd=0.0可以看到当前的速度值接近于目标值20,并且左右两轮的速度接近于一致。此时再继续增大Ki,便可减小静态误差,使得最终速度值更接近于20。

  • 第四组PID参数:Kp=15.0,Ki=1.0 , Kd=0.0在这组PID参数的调速下可以看到当前的速度值与目标值的偏差仅有0.1~0.3,速度已经较为稳定。

但是同时可以观察到,速度从0cm/s增加到20cm/s的时间需要大约1s的时间(如下图),调速的时间过长,因此需要加入Kd的作用,增加调速系统的响应速度,更快的达到目标值。

  • 第五组PID参数:Kp=15.0,Ki=1.0 , Kd=1.0通过曲线可以看到最终的速度值较为稳定,并且响应时间较前几组的调试有所加快。但实际效果不是很明显,因为通常微分作用应用在惯性较大的系统中,所以在小车的速度控制中效果不那么的明显,而且当Kd设置过大,会引起系统出现震荡,难以稳定,因此在小车速度控制的应用中可以考虑去除微分作用,即Kd=0。

在本篇文章中主要介绍了STM32机器人控制板利用编码器读取小车速度并实现PID调速的功能,这是使得小车运行平稳并进行速度调节的关键一环!关于具体的驱动代码可以参考配套的例程文件。在下一篇文章中,继续为你演示如何使用控制板上的遥控接口对电机、舵机进行控制,以及如何利用STM32对PPM信号进行处理等操作,带你一起解锁这块控制板的更多功能!

欢迎加入我们的交流群,该群面向热爱机器人研发的朋友们,方便大家一起学习、分享、交流智能机器人创造,结识更多志同道合的小伙伴。更有不定期的社区专属福利哦!关注公众号(COONEO)即可获取入群方式。

创作不易,如果喜欢这篇内容,请您也转发给您的朋友,一起分享和交流创造的乐趣,也激励我们为大家创作更多的机器人研发攻略,让我们一起learning by doing!


本文转载自: https://blog.csdn.net/COONEO/article/details/125909782
版权归原作者 COONEO 所有, 如有侵权,请联系我们删除。

“STM32机器人控制开发教程No.2 霍尔编码器电机测速以及增量式PID控制(基于HAL库)”的评论:

还没有评论