目录
- 导入CMSIS-DSP库
- 使用CMSIS-DSP
点击查看本文开源的完整FOC工程https://gitee.com/best_pureer/stm32_foc
CMSIS-DSP库是ARM开源的、对ARM处理器优化的数学库,
官方对其的介绍是一个针对Cortex-M和Cortex-A内核优化的嵌入式系统计算库
(此处的DSP不是指的硬件,对于没有DSP硬件的CM-3/CM-0 的MCU也可以调用该库)
本文使用了其提供的
- 三角函数、
- 反park变换函数、park变换函数、
- clarke变换函数、
- PID控制器
源码:https://github.com/ARM-software/CMSIS-DSP
教程:https://arm-software.github.io/CMSIS-DSP/latest/dsppp_guidelines.html
导入CMSIS-DSP库
本文使用stm32cube编译好的CMSIS-DSP二进制文件:
- 首先在Middleware and Software Packs中选择X-CUBE-ALGOBUILD。
- 选择安装DSP Library,并且勾选Selection,
- 勾选DSP Library Library:
到此,工程代码里就能够使用CMSIS-DSP库了,只需在要使用DSP库的C源文件加上
#include "arm_math.h"
使用CMSIS-DSP
三角函数:
单独计算sin和cos三角函数,输入弧度制rad:
float32_t arm_sin_f32(float32_t x);float32_t arm_cos_f32(float32_t x);
单个函数完成计算sin和cos三角函数,输入角度制deg:
/*** @brief Floating-point sin_cos function.* @param[in] theta input value in degrees* @param[out] pSinVal points to the processed sine output.* @param[out] pCosVal points to the processed cos output.*/void arm_sin_cos_f32(float32_t theta,float32_t * pSinVal,float32_t * pCosVal);
clarke变换:
/**** @brief Floating-point Clarke transform* @param[in] Ia input three-phase coordinate <code>a</code>* @param[in] Ib input three-phase coordinate <code>b</code>* @param[out] pIalpha points to output two-phase orthogonal vector axis alpha* @param[out] pIbeta points to output two-phase orthogonal vector axis beta* @return none*/__STATIC_FORCEINLINE void arm_clarke_f32(float32_t Ia,float32_t Ib,float32_t * pIalpha,float32_t * pIbeta)
clarke变换是将三个相线电流投影到α \alphaα和β \betaβ轴,由于三个相线电流相加等于0,因此只需要输入两个相线电流Ia
和Ib
到clarke变换函数中,输出到pIalpha
和pIbeta
。
park变换:
/*** @brief Floating-point Park transform* @param[in] Ialpha input two-phase vector coordinate alpha* @param[in] Ibeta input two-phase vector coordinate beta* @param[out] pId points to output rotor reference frame d* @param[out] pIq points to output rotor reference frame q* @param[in] sinVal sine value of rotation angle theta* @param[in] cosVal cosine value of rotation angle theta* @return none** The function implements the forward Park transform.**/__STATIC_FORCEINLINE void arm_park_f32(float32_t Ialpha,float32_t Ibeta,float32_t * pId,float32_t * pIq,float32_t sinVal,float32_t cosVal)
park变换是将α \alphaα和β \betaβ轴投影到dq轴,此处的sinVal
和cosVal
为转子角度的sin和cos值。
反park变换:
/*** @brief Floating-point Inverse Park transform* @param[in] Id input coordinate of rotor reference frame d* @param[in] Iq input coordinate of rotor reference frame q* @param[out] pIalpha points to output two-phase orthogonal vector axis alpha* @param[out] pIbeta points to output two-phase orthogonal vector axis beta* @param[in] sinVal sine value of rotation angle theta* @param[in] cosVal cosine value of rotation angle theta* @return none*/__STATIC_FORCEINLINE void arm_inv_park_f32(float32_t Id,float32_t Iq,float32_t * pIalpha,float32_t * pIbeta,float32_t sinVal,float32_t cosVal)
反park变换是将dq轴投影到α \alphaα和β \betaβ轴,此处的sinVal
和cosVal
为转子角度的sin和cos值。
PID控制器:
CMSIS-DSP库中的PID控制器是增量式PID计算累加到上一次的PID总输出。
位置式PID就是直观地分别将P、I、D控制器的各自输出加起来:
u ( t ) = K p e ( t ) + K i ∑ i = 0 t ( e ( i ) ∗ Δ T ) + K d [ e ( t ) − e ( t − 1 ) ] Δ T u(t) = K_p e(t) + K_i \sum_{i=0}^{t} (e(i)*\Delta T) + K_d \frac{[e(t) - e(t-1)]} {\Delta T}u(t)=Kpe(t)+Kii=0∑t(e(i)∗ΔT)+KdΔT[e(t)−e(t−1)]
为了方便计算,这里将Δ T \Delta TΔT当作为1,在K i K_iKi和K d K_dKd系数上缩放回去就好了。
增量式PID理念是将PID的输出是上次PID输出的基础上加上之前两次的累计误差:
u ( t ) = u ( t − 1 ) + Δ u ( t ) u(t) =u(t-1) +\Delta u(t)u(t)=u(t−1)+Δu(t)
Δ u ( t ) = K p e ( t ) + K i ∑ i = 0 t e ( i ) + K d [ e ( t ) − e ( t − 1 ) ] − ( K p e ( t − 1 ) + K i ∑ i = 0 t − 1 e ( i ) + K d [ e ( t − 1 ) − e ( t − 2 ) ] ) = K p [ e ( t ) − e ( t − 1 ) ] + K i e ( t ) + K d [ e ( t ) − 2 e ( t − 1 ) + e ( t − 2 ) ] = A 0 ⋅ e [ t ] + A 1 ⋅ e [ t − 1 ] + A 2 ⋅ e [ t − 2 ] 其 中 { A 0 = K p + K i + K d A 1 = − K p − 2 K d A 2 = K d \Delta u(t) = K_p e(t) + K_i \sum_{i=0}^{t} e(i) + K_d [e(t) - e(t-1)]\\ -(K_p e(t-1) + K_i \sum_{i=0}^{t-1} e(i) + K_d [e(t-1) - e(t-2)])\\ = K_p [e(t) - e(t-1)] + K_i e(t) + K_d [e(t) - 2e(t-1) + e(t-2)]\\ = A_0⋅e[t]+A_1⋅e[t−1]+A_2⋅e[t−2]\\ 其中 \begin{cases} A_0=K_p+K_i+K_d\\ A_1=-K_p-2K_d\\ A_2=K_d \end{cases}Δu(t)=Kpe(t)+Kii=0∑te(i)+Kd[e(t)−e(t−1)]−(Kpe(t−1)+Kii=0∑t−1e(i)+Kd[e(t−1)−e(t−2)])=Kp[e(t)−e(t−1)]+Kie(t)+Kd[e(t)−2e(t−1)+e(t−2)]=A0⋅e[t]+A1⋅e[t−1]+A2⋅e[t−2]其中⎩⎪⎨⎪⎧A0=Kp+Ki+KdA1=−Kp−2KdA2=Kd
A 0 , A 1 , A 2 , 本 次 误 差 e [ t ] , 上 次 误 差 e [ t − 1 ] , 上 上 次 误 差 e [ t − 2 ] , 上 一 次 的 P I D 输 出 u ( t − 1 ) A_0,A_1,A_2,本次误差e[t],上次误差e[t-1],上上次误差e[t-2],上一次的PID输出u(t-1)A0,A1,A2,本次误差e[t],上次误差e[t−1],上上次误差e[t−2],上一次的PID输出u(t−1)均为已知数,增量式PID的输出u ( t ) u(t)u(t)也就能够计算出来了。
从增量式PID公式Δ u ( t ) = K p [ e ( t ) − e ( t − 1 ) ] + K i e ( t ) + K d [ e ( t ) − 2 e ( t − 1 ) + e ( t − 2 ) ] \Delta u(t) = K_p [e(t) - e(t-1)] + K_i e(t) + K_d [e(t) - 2e(t-1) + e(t-2)]Δu(t)=Kp[e(t)−e(t−1)]+Kie(t)+Kd[e(t)−2e(t−1)+e(t−2)]可以看出,当被控参数突然受到大的扰动时,不像位置式PID的P项K p ∗ e ( t ) K_p*e(t)Kp∗e(t)会产生一个大的输出,增量式PID的K p [ e ( t ) − e ( t − 1 ) ] K_p [e(t) - e(t-1)]Kp[e(t)−e(t−1)]输出是不大的,因此增量式PID对被控参数变化的输出比较平滑,不会突变,当然响应没有位置式PID快。
在代码上,首先需要创建一个PID控制器:
arm_pid_instance_f32 pid_position;
然后设置PID控制器的PID参数:
pid_position.Kp = 1.2;
pid_position.Ki = 0.01;
pid_position.Kd = 2.1;
然后调用函数初始化PID控制器:
/*** @brief Initialization function for the floating-point PID Control.* @param[in,out] *S points to an instance of the PID structure.* @param[in] resetStateFlag flag to reset the state. 0 = no change in state & 1 = reset the state.* @return none.* \par Description:* \par* The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n* The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>* using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)* also sets the state variables to all zeros.*/void arm_pid_init_f32(arm_pid_instance_f32 * S,int32_t resetStateFlag)
其中参数resetStateFlag
为0时,会清空PID控制器内部保存的增量式数据:e [ n ] , e [ n − 1 ] , e [ n − 2 ] e[n],e[n-1],e[n-2]e[n],e[n−1],e[n−2]。
一般初始化的用法是:
arm_pid_init_f32(&pid_position, false);
此时就能够调用函数计算PID输出值了:
/*** @brief Process function for the floating-point PID Control.* @param[in,out] S is an instance of the floating-point PID Control structure* @param[in] in input sample to process* @return processed output sample.*/__STATIC_FORCEINLINE float32_t arm_pid_f32(arm_pid_instance_f32 * S,float32_t in)
其中参数S
是PID控制器,参数in
是被控参数的差值,返回PID控制器的输出值。
参考资料:
1. 转载自:https://blog.csdn.net/qq570437459/article/details/140478449?spm=1001.2014.3001.5502