FOC电机库 定点PID代码分析

代码来源于STM32 FOC电机库中的pid_regulator.c。

PID结构体

typedef struct PID_Handle
{
  int16_t   hDefKpGain;           /**< Default @f$K_{pg}@f$ gain */
  int16_t   hDefKiGain;           /**< Default @f$K_{ig}@f$ gain */
  int16_t   hKpGain;              /**< @f$K_{pg}@f$ gain used by PID component */
  int16_t   hKiGain;              /**< @f$K_{ig}@f$ gain used by PID component */
  int32_t   wIntegralTerm;        /**< integral term */
  int32_t   wUpperIntegralLimit;  /**< Upper limit used to saturate the integral
                                       term given by @f$\frac{K_{ig}}{K_{id}} @f$ * integral of
                                       process variable error */
  int32_t   wLowerIntegralLimit;  /**< Lower limit used to saturate the integral
                                       term given by Ki / Ki divisor * integral of
                                       process variable error */
  int16_t   hUpperOutputLimit;    /**< Upper limit used to saturate the PI output */
  int16_t   hLowerOutputLimit;    /**< Lower limit used to saturate the PI output */
  uint16_t  hKpDivisor;           /**< Kp gain divisor, used in conjuction with
                                       Kp gain allows obtaining fractional values.
                                       If FULL_MISRA_C_COMPLIANCY is not defined
                                       the divisor is implemented through
                                       algebrical right shifts to speed up PI
                                       execution. Only in this case this parameter
                                       specifies the number of right shifts to be
                                       executed */
  uint16_t  hKiDivisor;           /**< Ki gain divisor, used in conjuction with
                                       Ki gain allows obtaining fractional values.
                                       If FULL_MISRA_C_COMPLIANCY is not defined
                                       the divisor is implemented through
                                       algebrical right shifts to speed up PI
                                       execution. Only in this case this parameter
                                       specifies the number of right shifts to be
                                       executed */
  uint16_t  hKpDivisorPOW2;       /**< Kp gain divisor expressed as power of 2.
                                       E.g. if gain divisor is 512 the value
                                       must be 9 as 2^9 = 512 */
  uint16_t  hKiDivisorPOW2;       /**< Ki gain divisor expressed as power of 2.
                                       E.g. if gain divisor is 512 the value
                                       must be 9 as 2^9 = 512 */
  int16_t   hDefKdGain;           /**< Default Kd gain */
  int16_t   hKdGain;              /**< Kd gain used by PID component */
  uint16_t  hKdDivisor;           /**< Kd gain divisor, used in conjuction with
                                       Kd gain allows obtaining fractional values.
                                       If FULL_MISRA_C_COMPLIANCY is not defined
                                       the divisor is implemented through
                                       algebrical right shifts to speed up PI
                                       execution. Only in this case this parameter
                                       specifies the number of right shifts to be
                                       executed */
  uint16_t  hKdDivisorPOW2;       /*!< Kd gain divisor expressed as power of 2.
                                       E.g. if gain divisor is 512 the value
                                       must be 9 as 2^9 = 512 */
  int32_t   wPrevProcessVarError; /*!< previous process variable used by the
                                       derivative part of the PID component */
} PID_Handle_t;

•  变量命名的开头：h表示half word，w表示word。

（1）hKpGain、hKiGain、hKdGain

• 代码中用到的PID增益参数

（2）hDefKpGain、hDefKiGain、hDefKdGain

• 默认PID增益设置，在电机初始化时设置给hKpGain、hKiGain、hKdGain。个人感觉没必要再用这三个变量，老版本的ST电机库也没有这些参数。

（3）wIntegralTerm

• 积分项

（4）wPrevProcessVarError

• 微分项需要记录的每次的误差

（5）wUpperIntegralLimit、wLowerIntegralLimit

• 积分项最大值限制、积分项最小值限制

（6）hUpperOutputLimit、hLowerOutputLimit

• PID输出最大值限制、PID输出最小值限制

（7）hKpDivisor、hKiDivisor、hKdDivisor

• PID的增益参数应该是小数，而定点PID的增益是整数，需要除以一个比例

（8）hKpDivisorPOW2、hKiDivisorPOW2、hKdDivisorPOW2

• CPU算除法效率不太高，所以上面的PID增益的除数一般设置为​​​​​​​\(2^{^{n}}\)，对应​​​​​​​\(hKxDivisorPOW2=\log_2 hKxDivisor\)，这样在代码中算术右移，这样大大提高了运算速率。

PID函数实现

有的控制不需要用到微分项，所以ST将PID函数分为PI控制函数和PID控制函数。代码很简单，具体参考注释。

PI控制函数

__weak int16_t PI_Controller( PID_Handle_t * pHandle, int32_t wProcessVarError )
{
  int32_t wProportional_Term, wIntegral_Term, wOutput_32, wIntegral_sum_temp;
  int32_t wDischarge = 0;
  int16_t hUpperOutputLimit = pHandle->hUpperOutputLimit;
  int16_t hLowerOutputLimit = pHandle->hLowerOutputLimit;

  /* 计算比例项 */
  wProportional_Term = pHandle->hKpGain * wProcessVarError;

  /* 计算积分项 */
  if ( pHandle->hKiGain == 0 )
  {
  	/* 积分增益为0,清空之前的积分项 */
    pHandle->wIntegralTerm = 0;
  }
  else
  {
  	/* 积分增益非0，计算本次误差的积分 */
    wIntegral_Term = pHandle->hKiGain * wProcessVarError;
	/* 加上之前已经累加的积分 */
    wIntegral_sum_temp = pHandle->wIntegralTerm + wIntegral_Term;
	/* 判断溢出 */
    if ( wIntegral_sum_temp < 0 )
    {
      if ( pHandle->wIntegralTerm > 0 )
      {
        if ( wIntegral_Term > 0 )
        {
          /* 如果本次积分与之前的积分都为正，其和小于0，说明超过了INT32_MAX */
          wIntegral_sum_temp = INT32_MAX;
        }
      }
    }
    else
    {
      if ( pHandle->wIntegralTerm < 0 )
      {
        if ( wIntegral_Term < 0 )
        {
          /* 如果本次积分与之前的积分都为负，其和大于0，说明超过了-INT32_MAX */
          wIntegral_sum_temp = -INT32_MAX;
        }
      }
    }
	/* 判断是否超出用户定义的积分限制范围 */
    if ( wIntegral_sum_temp > pHandle->wUpperIntegralLimit )
    {
      pHandle->wIntegralTerm = pHandle->wUpperIntegralLimit;
    }
    else if ( wIntegral_sum_temp < pHandle->wLowerIntegralLimit )
    {
      pHandle->wIntegralTerm = pHandle->wLowerIntegralLimit;
    }
    else
    {
      pHandle->wIntegralTerm = wIntegral_sum_temp;
    }
  }

/* 定点PID，需要除以一个比例 */
#ifdef FULL_MISRA_C_COMPLIANCY
  wOutput_32 = ( wProportional_Term / ( int32_t )pHandle->hKpDivisor ) + ( pHandle->wIntegralTerm /
               ( int32_t )pHandle->hKiDivisor );
#else
  /* 需要保证编译器对这行代码的处理是算术右移，而不是逻辑右移 */
  wOutput_32 = ( wProportional_Term >> pHandle->hKpDivisorPOW2 ) + ( pHandle->wIntegralTerm >> pHandle->hKiDivisorPOW2 );
#endif
  /* 判断PID输出结果是否超过用户定义的范围 */
  if ( wOutput_32 > hUpperOutputLimit )
  {
    /* 超过用户定义的最大范围，将本次超过的值赋给wDischarge */
    wDischarge = hUpperOutputLimit - wOutput_32;
    wOutput_32 = hUpperOutputLimit;
  }
  else if ( wOutput_32 < hLowerOutputLimit )
  {
  	/* 超过用户定义的最小范围，将本次超过的值赋给wDischarge */
    wDischarge = hLowerOutputLimit - wOutput_32;
    wOutput_32 = hLowerOutputLimit;
  }
  else { /* Nothing to do here */ }
  /* 将wDischarge赋值给积分项,下一次计算用 */
  pHandle->wIntegralTerm += wDischarge;

  return ( ( int16_t )( wOutput_32 ) );
}

PID控制函数

__weak int16_t PID_Controller( PID_Handle_t * pHandle, int32_t wProcessVarError )
{
  int32_t wDifferential_Term;
  int32_t wDeltaError;
  int32_t wTemp_output;

  if ( pHandle->hKdGain != 0 ) /* derivative terms not used */
  {
  	/* 积分项计算本次误差和上次的误差的差值 */
    wDeltaError = wProcessVarError - pHandle->wPrevProcessVarError;
	/* 计算微分项 */
    wDifferential_Term = pHandle->hKdGain * wDeltaError;

#ifdef FULL_MISRA_C_COMPLIANCY
    wDifferential_Term /= ( int32_t )pHandle->hKdDivisor;
#else
    /* WARNING: the below instruction is not MISRA compliant, user should verify
    that Cortex-M3 assembly instruction ASR (arithmetic shift right)
    is used by the compiler to perform the shifts (instead of LSR
    logical shift right)*/
    /* 定点PID，需要除以一个比例 */
    wDifferential_Term >>= pHandle->hKdDivisorPOW2;
#endif
	/* 记录本次的微分项 */
    pHandle->wPrevProcessVarError = wProcessVarError;
	/* 计算比例项和积分项，再加上微分项作为本次输出 */
    wTemp_output = PI_Controller( pHandle, wProcessVarError ) + wDifferential_Term;
	/* 用户定义的范围限制 */
    if ( wTemp_output > pHandle->hUpperOutputLimit )
    {
      wTemp_output = pHandle->hUpperOutputLimit;
    }
    else if ( wTemp_output < pHandle->hLowerOutputLimit )
    {
      wTemp_output = pHandle->hLowerOutputLimit;
    }
    else
    {}
  }
  else
  {
  	/* 微分增益为0，只计算比例项和积分项 */
    wTemp_output = PI_Controller( pHandle, wProcessVarError );
  }
  return ( ( int16_t ) wTemp_output );
}

参数调整

应该先调整内环再调整外环。这里定点PID实际应该先调整hKxDivisorPOW2的大小，如果没调好除法项就去调PID的增益，有可能怎么调参数最后PID输出都溢出。