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!