Lleno de información útil, simplemente lea este artículo para aprender sobre la medición de la velocidad del motor del robot y el control PID.

​

01 Prefacio

¡Tutorial sobre diferencial de dos ruedas STM32/robot de dirección Ackerman/motor de automóvil! , presentamos cómo usar el tablero de control del robot NANO para controlar el motor reductor para completar funciones básicas como avance, retroceso y dirección. Si necesita saber la velocidad de desplazamiento del automóvil en tiempo real, ¿cómo medirla? En este capítulo, le presentaré cómo utilizar la placa de conducción NANO para medir la velocidad del automóvil.

La placa de control del robot utilizada en este tutorial tiene 4 interfaces de motor con codificadores, 4 interfaces de servo, interfaz de comunicación en serie, interfaz de depuración de descarga SWD, interfaz de control remoto de modelo de avión, interfaz de salida USB5V y una interfaz de 40 pines para conexión directa con Raspberry Pi, etc. ., ¡los recursos integrados son ricos y convenientes para la depuración! Puede controlar robots/coches de dirección de dos ruedas, cuatro ruedas, Ackerman y Mecanum.

Interfaz del motor en el tablero de control:

​

02 STM32 y medición de velocidad del codificador

La adquisición precisa de la información de velocidad del vehículo tiene un impacto crucial en el sistema de control. Un codificador es un sensor giratorio que convierte la velocidad angular o la posición angular en pulsos digitales. El usuario puede medir la información de velocidad del motor a través del codificador. En esta rutina, presentamos principalmente el método de uso entre STM32 y el motor del codificador, y solo damos una breve introducción al principio específico del codificador.

El motor utilizado en esta rutina es un motor reductor con codificador Hall.El motor consta de tres partes: reductor, motor y codificador Hall .

El principio de funcionamiento del codificador Hall: el codificador Hall convierte el desplazamiento mecánico en una señal de pulso mediante conversión electromagnética y emite señales de onda cuadrada bifásica A y B. La diferencia de fase entre las señales de pulso bifásica A y B es 90 °.A través de la detección El número de pulsos dentro del tiempo especificado y las posiciones relativas de las señales de pulso de fase A y B se pueden utilizar para obtener el valor del codificador y su dirección de movimiento.

​

Entonces, ¿cómo medir el número de pulsos dentro de un tiempo específico y la relación de fase entre las dos señales?

Los temporizadores de STM32 son potentes y algunos de ellos tienen modos de codificador que pueden resolver los problemas anteriores. Las señales de fase de entrada A y B se pueden procesar a través del modo codificador del temporizador STM32. Y el modo codificador de STM32 se puede configurar para contar pulsos en señales TI1 y TI2 al mismo tiempo, es decir, ¡cuatro veces la frecuencia! La precisión de la detección se puede mejorar. Y al detectar la relación de fase entre TI1 y TI2, el contador puede contar hacia arriba o hacia abajo para detectar la dirección de su movimiento.

​

Según los parámetros del motor reductor utilizado en este artículo, el motor es un motor reductor con codificador Hall de 11 cables con una relación de reducción de 1:30, es decir, una rotación puede generar 11 * 30 = 330 pulsos. frecuencia a través de STM32, el motor gira El número de pulsos obtenidos en un círculo es 330 * 4 = 1320. El período de muestreo de velocidad se establece en 50 ms a través de la interrupción del temporizador, es decir, el valor del contador en el temporizador se lee cada 50 ms. Para obtener la velocidad final, también es necesario considerar el tamaño del neumático. Según el diámetro del neumático combinado con el coche NANO, el diámetro es de 65 mm, por lo que la circunferencia C es 3,14*6,5 = 20,41 cm. Con referencia a los parámetros anteriores, puede calcular la velocidad del automóvil. Según el principio de medición de velocidad: suponga que el número de pulsos entrantes durante el período de muestreo es N, el número de pulsos obtenidos por el motor que gira una vez es 1320 y la distancia de movimiento de la rueda que gira una vez es 20,41 cm. Luego, la distancia S de movimiento cuando se obtiene el número de pulso N es (0.2041 * N/1320), y luego se divide por el período de muestreo especificado para obtener la velocidad de movimiento. La fórmula de cálculo completa es la siguiente:

              车轮周长            规定时间内得到的脉冲数        0.2041        N
小车速度(cm/s)= ----------------- * --------------------- = -------- * ----------- = 0.3092424 * N(cm/s)
              电机转动一圈的脉冲数       定时器规定时间           1320    50*0.001

03 Código del controlador de medición de velocidad y configuración STM32CubeMX

Para obtener información sobre cómo crear un proyecto y una configuración del sistema, consulte el artículo anterior Tutorial sobre diferencial de dos ruedas STM32/robot de dirección Ackerman/control de motor de automóvil. ¡Este artículo presenta principalmente la configuración de temporizadores relacionados con el codificador!

De acuerdo con el diagrama esquemático de la placa del controlador NANO, busque la interfaz del codificador de cuatro canales correspondiente al motor de dos canales y el puerto IO en el STM32, y busque su temporizador correspondiente para la configuración del modo. Como se muestra en la figura siguiente, los codificadores de los dos motores utilizados corresponden al temporizador 2 y al temporizador 3 respectivamente.

​

Busque el temporizador 2 y el temporizador 3 en la interfaz STM32CubeMX en STM32CubeIDE y configure el modo. A continuación se toma como ejemplo el temporizador 2. El temporizador 3 solo necesita configurarse de la misma manera. Seleccione el temporizador para que esté en modo codificador, configúrelo sin división de frecuencia, el valor máximo de conteo es 65535, habilite la recarga automática y seleccione dos entradas, TI1 y TI2, para lograr un efecto de frecuencia cuádruple.

​

Entre ellos, es necesario volver a crear una imagen completa del temporizador 2. Haga clic en PB3 para seleccionar TIM2_CH2, y PA15 se seleccionará automáticamente como TIM2_CH1.

​

Después de configurar el temporizador 2 y el temporizador 3, necesita usar otro temporizador para generar una interrupción de 50 ms para leer el valor de velocidad actual del automóvil. En esta rutina, el temporizador 6 se usa para generar una interrupción.

El período es de 50 ms y el método de cálculo es: T=(arr+1)*(psc+1)/Tclk

​

​

Además, configure el puerto serie 1 para la impresión, configúrelo como puerto serie asíncrono, no abra el flujo de hardware, configure la velocidad en baudios en 115200, longitud de palabra de datos de 8 bits, sin paridad, 1 bit de parada, modo transceptor, 16 veces sobremuestreo.

​

Por supuesto, no olvide la configuración del variador del motor. Después de todo, aún debe moverlo primero para probar mejor su función de medición de velocidad. Para la configuración y el variador relevantes del motor, consulte el artículo anterior.

Después de completar la configuración, mantenga presionada Ctrl+s para generar el código de inicialización. A continuación se explican principalmente los códigos relevantes para la medición de la velocidad del codificador. Para otros códigos específicos, consulte el archivo de rutina.

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]);
  }
}

(Para conocer el código de accionamiento del motor, consulte el artículo anterior)

Los números de punto flotante se utilizan en el resultado anterior y se debe realizar la siguiente configuración en las propiedades del proyecto.

​

Descargue el programa en el tablero de control del robot STM32 y comuníquese con la computadora host a través del asistente de depuración del robot cooneo. Puede observar la velocidad de movimiento del automóvil bajo diferentes valores de PWM.

Como se muestra en la figura siguiente, cuando se emite la señal PWM con un ciclo de trabajo del 50%, la velocidad del automóvil es la siguiente:

​

Cuando se emite la señal PWM con un ciclo de trabajo del 90%, la velocidad del automóvil es la siguiente:

​

A través del modo codificador utilizando el temporizador presentado anteriormente, se lee el valor del pulso del codificador y finalmente se obtiene la velocidad de marcha real del automóvil. Se puede ver que durante el funcionamiento real, incluso si el ciclo de trabajo de la señal PWM controlada es consistente, todavía hay una cierta desviación en la velocidad de los motores izquierdo y derecho. Para hacer que la velocidad sea más estable y la respuesta del efecto de control de velocidad sea más rápida y suave, se puede agregar un algoritmo PID para controlar la velocidad del motor.

04 Control PID incremental

El PID se puede dividir en PID posicional y PID incremental. El conocimiento del principio de control específico de PID no se presentará en detalle aquí. La atención se centra en el PID incremental utilizado en esta rutina.

• El PID incremental controla la estabilidad de la cantidad controlada cambiando el tamaño de la cantidad de salida. El PID incremental es diferente del PID posicional. El valor devuelto por el tipo incremental es la diferencia entre la cantidad de control en el momento actual y la cantidad de control en el momento actual. valor del momento anterior y utilice esta diferencia como nueva variable de control para la retroalimentación.

• Por ejemplo: establezca la velocidad del automóvil en 0,2 m/s, mida la velocidad a través del codificador para obtener retroalimentación de velocidad, y hay una desviación ek del valor establecido. El sistema guarda la última desviación e(k-1) y lo anterior La última desviación e ( k-2), estos tres valores se utilizan como cantidades de entrada para obtener Δu (k) a través de la fórmula de cálculo incremental de PID, agregue u (k-1) después del último cálculo de PID a esto veces del incremento Δu(k), se obtiene la salida PID u(k) de este ciclo de control. Después de calcular y convertir el valor de salida dos veces, se obtiene el ciclo de trabajo PWM que puede controlar la velocidad del motor y luego se controla la velocidad de movimiento del automóvil.

• La fórmula del PID incremental: coeficiente proporcional Kp, coeficiente integral Ki, coeficiente diferencial Kd, desviación e(k)

  Δu(k)=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]

Después de comprender algunos conocimientos básicos de PID incremental, ¡echemos un vistazo a cómo implementar el proceso del algoritmo en el código!

/*
*  定义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;
}

Luego llame a la función PID en la función de devolución de llamada de interrupción del temporizador 6 escrita en el artículo anterior para ajustar la velocidad actual con un período de 50 ms.

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);
  }
}

Una vez escrito el código, descargue el programa en el tablero de control del robot STM32 y podrá comenzar a ajustar los parámetros felizmente.

Con respecto al ajuste de parámetros PID, generalmente siga los siguientes principios:

1. Primero ajuste el coeficiente proporcional Kp, luego ajuste el coeficiente integral Ki y finalmente ajuste el coeficiente diferencial Kd.

2. Al ajustar Kp, ajuste de pequeño a grande y seleccione un coeficiente de proporción más pequeño para que la curva de salida se ajuste básicamente a la línea recta objetivo.

3. Ajuste Ki y use Ki para eliminar errores estáticos.

4. Ajuste Kd para mejorar la velocidad de respuesta de la regulación de velocidad.

Ejemplo de ajuste de parámetros PID: (La imagen solo muestra la curva de velocidad de la rueda izquierda, la curva de velocidad de la rueda derecha es aproximadamente la misma)

• El primer grupo de parámetros PID: Kp=5,0, Ki=0,0, Kd=0,0

  Se puede ver que existe una gran brecha entre la velocidad objetivo de 20 cm/s en la velocidad de las ruedas izquierda y derecha, por lo que es necesario continuar aumentando el valor de Kp.

​

• El segundo conjunto de parámetros PID: Kp=15,0, Ki=0,0, Kd=0,0

  Se puede ver que la velocidad actual ha mejorado y está más cerca de la velocidad objetivo, en este momento se agrega Ki para participar en el ajuste.

​

• El tercer grupo de parámetros PID: Kp=15,0, Ki=0,5, Kd=0,0

  Se puede ver que el valor de velocidad actual está cerca del valor objetivo de 20, y la velocidad de las ruedas izquierda y derecha es casi la misma. En este momento, continuar aumentando Ki puede reducir el error estático, haciendo que el valor de velocidad final se acerque a 20.

​

• El cuarto conjunto de parámetros PID: Kp=15,0, Ki=1,0, Kd=0,0

  Bajo la regulación de velocidad de este conjunto de parámetros PID, se puede ver que la desviación entre el valor de velocidad actual y el valor objetivo es solo 0,1 ~ 0,3, y la velocidad ya es relativamente estable.

​

Pero al mismo tiempo, se puede observar que la velocidad tarda aproximadamente 1 segundo en aumentar de 0 cm/s a 20 cm/s (como se muestra a continuación). El tiempo de ajuste de velocidad es demasiado largo, por lo que la función de Kd necesita Se agregará para aumentar la velocidad de respuesta del sistema de ajuste de velocidad y alcanzar los valores objetivo más rápido.

​

• El quinto conjunto de parámetros PID: Kp=15,0, Ki=1,0, Kd=1,0

  Se puede ver en la curva que el valor de velocidad final es relativamente estable y el tiempo de respuesta es más rápido que en los grupos de depuración anteriores. Pero el efecto real no es muy obvio, porque el efecto diferencial generalmente se usa en sistemas con gran inercia, por lo que el efecto en el control de velocidad del automóvil no es tan obvio, y cuando Kd se establece demasiado grande, hará que el sistema oscilar, lo que dificulta la estabilización, por lo que en la aplicación del control de velocidad del automóvil, se puede considerar eliminar el efecto diferencial, es decir, Kd = 0.

​

En este artículo, presentamos principalmente la función del tablero de control del robot STM32 para usar el codificador para leer la velocidad del automóvil e implementar la regulación de velocidad PID. ¡Este es un enlace clave para que el automóvil funcione sin problemas y ajuste la velocidad! Para códigos de controlador específicos, consulte los archivos de rutina de soporte. En el próximo artículo, continuaré mostrándoles cómo usar la interfaz de control remoto en el tablero de control para controlar motores y servos, y cómo usar STM32 para procesar señales PPM, etc., y llevarlo a desbloquear más funciones de este tablero de control. !

¿Código? Aquí: GitHub - COONEO/NEOR-nano: robot móvil portátil de código abierto para desarrolladores.