FOC算法笔记

起源：使用ST WorkBench配置的HALL BLDC控制算法抖动严重。

ST的电机控制算法，代码非常高端大气，值得学习。

HAL库与LL库混用，效率很高。很多中断回调都直接重写了，没有使用HAL库那一套。

只是好多地方不知道干嘛的，改动也无从下手。

为了加深理解，这里选择自己敲一遍。谨此记录。

1、硬件配置

stm32G474RET6 + X-NUCLEO-IHM07M1 ;

一对极BLDC；

12V直流电源；

逻辑分析仪/示波器；

红外测速仪器；

2、软件配置

这里使用keil5+cubeMX; 电机控制板和驱动板的原理图必须搞到；

• 配置定时器1，四路PWM输出。1-3路为IHM07M1的mos管开关输入，该驱动不需要互补输出，第四路PWM用做触发ADC采样，不需要输出。具体配置如下图：

stm32g474主频为170M，定时器1主频也为170M，这里配置PWM中心对齐加6分频，实际的PWM频率为：
$$CL{K}_{PWM}=\frac{定时器主频}{(Prescaler+1)\ast 2}=12.14K$$
计数值这里暂定1K吧。

Trigger Output（TRGO）配置为 OC4REF，即为第四通道的比较输出功能。作为下文ADC采集的硬件触发功能。这样可以根据实际情况通过改变PWM通道的占空比灵活配置采样时刻。

• 生成代码后，还需要手动开启PWM输出：

  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
  

• 还需要开启UART功能，通过上位机Vofa+ 可以观察数据波形。开启功能简单叙述：

  
  

​

由于需要借助VOFA+软件观察波形，所以还需要简单的通信协议；
定义发送缓冲区：char buff[3*4+4] = {0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00,0x00, 0x00, 0x80, 0x7f};
填充数据：*(float*)buff = MotorM1.CurVelocity;*((float*)buff+1) = MotorM1.CurPosition;*((float*)buff+2) = 0;
发送数据：HAL_UART_Transmit_DMA(&huart2,(uint8_t *)&buff,sizeof(buff));

2、开环运行

开环运行原理图如上；只需要左侧三个参数即可让电机动起来；其中 V_d = 0; θ 为自增变量；V_q为一个较小的任意值；下面贴出关键函数的实现：

SVPWM：
void Motor::SvpwmTransform(Ualpha_beta_t vol, Dabc_t* duty)
{uint32_t timelimit = sqrt3 * 1000 / 2;uint32_t t0 = 0, t1 = 0, t2 = 0;uint8_t sectiontable[] = { 0, 2, 6, 1, 4, 3, 5, 0 };int32_t U1 = vol.beta;int32_t U2 = (sqrt3 * vol.alpha - vol.beta) / 2;int32_t U3 = -(sqrt3 * vol.alpha + vol.beta) / 2;int16_t ta = 0, tb = 0, tc = 0;uint8_t a = U1 > 0 ? 1 : 0;uint8_t b = U2 > 0 ? 1 : 0;uint8_t c = U3 > 0 ? 1 : 0;uint8_t n = 4 * c + 2 * b + a;//CurSector = sectiontable[n];switch (sectiontable[n]) {case 1:t1 = U2;//4t2 = U1;//6break;case 2:t1 = -U3;//6t2 = -U2;//2break;case 3:t1 = U1;//2t2 = U3;//3break;case 4:t1 = -U2;//3t2 = -U1;//1break;case 5:t1 = U3;//1t2 = U2;//5break;case 6:t1 = -U1;//5t2 = -U3;//4break;default:break;}//最大到六边形的内切圆，if (t1 + t2 > timelimit) {uint32_t temp = t1 + t2;t1 = t1 * (timelimit) / temp;t2 = t2 * (timelimit) / temp;}t0 = (timelimit - t1 - t2) >> 1;switch (sectiontable[n]) {case 1:ta = t0 >> 1;tb = (t0 + t1) >> 1;tc = (t0 + t1 + t2) >> 1;break;case 2:ta = (t0 + t2) >> 1;tb = t0 >> 1;tc = (t0 + t1 + t2) >> 1;break;case 3:ta = (t0 + t1 + t2) >> 1;tb = t0 >> 1;tc = (t0 + t1) >> 1;break;case 4:ta = (t0 + t1 + t2) >> 1;tb = (t0 + t2) >> 1;tc = t0 >> 1;break;case 5:ta = (t0 + t1) >> 1;tb = (t0 + t1 + t2) >> 1;tc = t0 >> 1;break;case 6:ta = t0 >> 1;tb = (t0 + t1 + t2) >> 1;tc = (t0 + t2) >> 1;break;default:break;}duty->a = ta;duty->b = tb;duty->c = tc;
}

更新占空比：
Dabc_t Motor::UpdateDuty(void)
{int ccr_limit = 1000;Dabc_t duty = { 0,0,0 };duty.a = MotorM1.Duty.a / 1000;duty.b = MotorM1.Duty.b / 1000;duty.c = MotorM1.Duty.c / 1000;if(MotorM1.Enable){__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,duty.a * ccr_limit);__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,duty.b * ccr_limit);__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,duty.c * ccr_limit);}else{__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,duty.a * 0);__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,duty.b * 0);__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,duty.c * 0);}return duty;
}

连接好驱动板、控制板、电机，如果硬件没有问题应该就可以缓慢的转动了。可以适当调节Vq和θ的值。同时配合示波器观察实际输出。逐步排查问题，常见的问题如：驱动没有使能，没有供电，相关硬件保护被触发，跳线帽不正确，导线连接问题等。

实际占空比的波形如下：（vd = 200, θ += 0.001）

开环转动过程中可能会伴随着电机的噪声、抖动，驱动的发热等，均为正常现象，该步骤只是暂时的过渡阶段，仅用于验证代码和硬件。下面着手实现闭环：

3、电流检测

硬件原理

IHM07M1驱动采样电路简图：

其中 Ra、Rb、Rc为采样电阻。由原理图知：只有对应相的下桥臂打开时才能测量该相的电流。否则没有电流经过测量无意义。同时该驱动板支持三电阻采样。

每个采样电阻阻值为0.33欧姆。

三路电流采样电路相同，放大倍数已经标注为1.53倍。电流采样非常简单，即使用采样电阻串联在电机某相回路中，通过测量单片机ADC采样电阻两端电压，由于采样电阻阻值已知，可以通过欧姆定律反推实际电流：
$$I_{相电流}=\frac{U_{采样电阻两端电压}}{R_{采样电阻阻值}\ast 放大倍数}$$

软件原理

​ 由于ADC采样在每个PWM周期，速度很快（12.14K），为节约CPU资源一般是使用DMA的，在DMA完成中断回调中进行计算和处理。

其中ADC配置如下图示，配置仅做参考。

通道1和通道7对应PA0和PC1，这两个GPIO连接了采样电流运放的输出。所以只能配置这两个通道。

其中外部触发源一定选择 Timer 1 Trigger Out Event，该触发源对应前文叙述的PWM通道4。

DMA功能也非常重要，12位ADC只能开启半字传输，也就是16位传输，同时开启循环模式，这样会自动更新索引并覆盖之前的数据。

配置完成后还需要手动开启：

uint16_t ADC_result[3] ; // ADC缓冲区
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)ADC_result, 2); //开启传输

同时设置定时器1第四通道的比较值：

__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,950);

在DMA中断回调中计算电机相电流：

void  HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{if(hadc->Instance==ADC1){		MotorM1.PhaseCurr.a = (ADC_result[0] - 1910) * 3.3 / 4096 / 1.528 / 0.33;MotorM1.PhaseCurr.b = (ADC_result[1] - 1910) * 3.3 / 4096 / 1.528 / 0.33;MotorM1.PhaseCurr.c = -MotorM1.PhaseCurr.a - MotorM1.PhaseCurr.b;HAL_GPIO_TogglePin(USER_LED_GPIO_Port, USER_LED_Pin); //翻转LED观察测量时刻是否正常}
}

采样电阻两端的电压计算公式为：
$$U_{采样电阻}=(采样值-校零值)\frac{3.3}{4096}$$
将采样电阻的电压带入上文的相电流计算公式即得到电机相电流，计算前两相电流后，可以通过基尔霍夫定律得到第三相电流。

通过逻辑分析仪观察实际电流采样时刻：

可以看到；周期结束和开始时进行LED翻转，而LED翻转在DMA中断回调中进行，该配置与预期相符。

通过Vofa上位机观察实际的三相电流：

可以看到电流三相对称，与预期相符。

实际的电机电流可能有抖动、震荡，如下图：

这样需要根据实际情况修改采样时刻，在电流稳定的时刻进行采样。

4、电流闭环运行

将采样电流与目标电流进行误差控制，实现电流环控制，将电流环控制程序放在定时器1的溢出中断。每个PWM周期进行一次电流采样，采样后进行PI控制，同时调整下一周期的PWM占空比。θ依然为自增变量，目标电流与前文V_d、V_q相同。部分代码如下：

motor_t theta = 0；
void FOC_ControllerM1()
{Ialpha_beta_t cur_two;Ialpha_beta_t vol_two;Idq_t cur_rotate;Idq_t vol_rotate;theta += 0.001;if (theta > 2 * Pi){theta -= 2 * Pi;}if(theta < - 2 * Pi){theta += 2 * Pi;}cur_two = Clark_Transform(MotorM1.PhaseCurr);cur_rotate = Park_Transform(cur_two, theta);vol_rotate.d = PIDCtrlBlock(MotorM1.TargetCurr.d, cur_rotate.d*1000, &pid_Id);vol_rotate.q = PIDCtrlBlock(MotorM1.TargetCurr.q, cur_rotate.q*1000, &pid_Iq);vol_two = Inverse_Park_Transform(vol_rotate, theta);MotorM1.SvpwmTransform(vol_two, &MotorM1.Duty);MotorM1.UpdateDuty();
}

PI控制器的参数需要调整到比较稳定的数值，因为接下来是速度环，需要先将电流环稳定才行。

5、速度闭环运行

霍尔信号检测速度和位置，其实就是根据三个霍尔信号来判断的。根据三个信号的高低电平，霍尔信号分为 5->1->3->2->6->4 六个状态，并按照这样的顺序依次变化，反转就是逆序的变化。如图，120度摆放的霍尔传感器，每60度就会变化一次状态。

HALL捕获配置

将定时器配置为HALL Sensor Mode 模式。三个通道就会连接在一起，当三个通道状态变化时，就会发生中断。

软件中有两个点需要注意：

• HALL Sensor Ratio 2分频后，只有上升沿触发中断。不分频则是上升沿下降沿都会中断。

• HALL开启溢出中断，可以设置中断源，设置为只有计数器上/下溢才中断，否则会在每次HALL中断都触发溢出中断。
  

  HAL_TIMEx_HallSensor_Start_IT(&htim2);  //开启HALL中断
  __HAL_TIM_ENABLE_IT(&htim2, TIM_IT_UPDATE);//开启更新中断
  __HAL_TIM_URS_ENABLE(&htim2);				//设置中断源为溢出中断重写输入捕获中断回调；
  void  HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
  {
  // 中断中，读取HALL的三个信号的状态；
  // 通过判断HALL状态的顺序，判断正反转。
  // HALL状态对应了转子的位置；因为每次中断电机转动60度，所以实际位置 = 上一次位置 +（反转-） 60度；
  // 每次中断都需要读取通道1的捕获值；中断会中断清空计数值；
  // 捕获值记录了该次中断与上次中断的时间间隔；由此可以计算速度；因为每次中断都代表电机转动了60度，可以计算出瞬时速度；}
  

速度闭环

上面已经得到了离散的六个霍尔位置，并且计算得到了速度；

此时，开环运行的θ就不需要自加了。该位置信息为 六个离散的位置加对瞬时速度的积分，积分时间为FOC速度环的控制周期；

inter = ((float)(MotorM1.CurVelocity)* 2.0f * Pi ) / 60000.0f ;
//速度单位为转每分，转化为弧度，除60000后变为 1ms 经过的弧度。
theta = (float)MotorM1.CurPosition/(65536*2π) + inter;
//65535为霍尔定时器的计算器装载值；

速度环的执行频率暂定为1K。
至此，只需要调节PID的参数，即可实现正常运行。

实测轻松达到万转；

但是PID参数还是需要细调，速度不是一直很平稳。