01 前言

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

控制板上的电机接口：

机器人汽车电机驱动开发-测量汽车速度

上一篇文章《STM32机器人控制开发教程No.驱动电机(基于HAL关于如何使用库)NANO汽车上的机器人控制板控制减速电机完成前进、后退、转向等基本功能。如果你需要知道如何测量汽车的实时行驶速度？本章将向您介绍如何使用它NANO驱动板进行小车速度的测量！

02STM32与编码器测速

准确获取汽车的速度信息对控制系统有至关重要的影响。编码器是一种旋转传感器，将角速或角转换为数字脉冲，用户可以通过编码器测量电机的速度信息。本例主要介绍STM32与编码器电机的搭配使用方法仅简要介绍编码器的具体原理。

本例程中使用的电机为带霍尔编码器的减速电机，由减速器、电机和霍尔编码器三部分组成。

霍尔编码器的工作原理：霍尔编码器通过电磁转换将机械位移转换为脉冲信号并输出A、B两相方波信号，A、B两相脉冲信号相差900°，检测规定时间内的脉冲数和脉冲数A、B编码器的值及其运动方向可以在两相脉冲信号的相对位置获得。

那么如何在规定时间内测量脉冲数和两个信号之间的相位关系呢？

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

根据本文使用的减速电机参数，电机为11线霍尔编码器减速电机，减速比为1:30，即转动一圈可输出11 * 30=330脉冲，通过STM324倍频后，电机旋转一圈得到的脉冲数为330 * 4=1320通过定时器中断速度采样周期为50ms，即每50ms读取定时器中的计数器值。根据轮胎的大小，需要考虑最终速度NANO配套车的轮胎直径为65mm，因此周长C为3.14*6.5=20.41cm。参考上述参数，然后计算汽车的运动速度。根据测速原理：采样周期内输入的脉冲数为N，电机旋转一圈得到的脉冲数为1320，轮子旋转一圈的运动距离为20.41cm。那么得到脉冲数N时运动的距离S=(0.2041 * N完整的计算公式如下：

03 STM32CubeMX配置和测速驱动代码

如何创建项目和系统配置可以参考上一篇文章《STM32机器人控制开发教程No.1驱动电机（基于HAL本文主要介绍编码器相关定时器的配置！

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

在STM32CubeMX找到定时器2和定时器3，进行模式配置。以定时器2为例，定时器3只需要相同的配置。选择定时器作为编码器模式，设置为不分频，最大值为65535，可自动重新装载并选择TI1和TI二、二路输入，实现四倍频效果。

配置完定时器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]);   } }

(电机驱动代码可参考上一篇文章) 

下载程序STM在32台机器人控制板上，通过串口与上位机通信，可以观察到不同的串口调试助手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   */ nt 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!