STM32F407 CubeMX HAL 库三环串级 PID FOC 电控算法实现总结

void Current_Sense_init() { 
    printf("ADC 开始初始化\r\n"); 
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,2000); // 通道四触发 adc 采样 
    HAL_ADCEx_InjectedStart_IT(&hadc1); 
    Voltage_to_Current = 1.0f / (sampling_resistor * Current_gain); 
    Gain_a = Voltage_to_Current * -1; // 正负号代表电流的流向，实际需要根据采集电路做判断！！！ 
    Gain_b = Voltage_to_Current; 
} 

// ADC 注入组转换完成回调函数 
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) { 
    if (hadc->Instance == ADC1) { 
        if(callback_counter <= 100) { 
            bias_voltage[0] += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); 
            bias_voltage[1] += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2); 
            if(callback_counter == 100) { 
                My_adcData[0] = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); 
                My_adcData[1] = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2); 
                bias_voltage_ia = (bias_voltage[0] / 100.0f); 
                bias_voltage_ib = (bias_voltage[1] / 100.0f); 
                differ_Ia = My_adcData[0] - bias_voltage_ia; 
                differ_Ib = My_adcData[1] - bias_voltage_ib; 
                adc_conversion_flag = 1; 
                My_adcData[0] = 0; 
                My_adcData[1] = 0; 
                HAL_ADCEx_InjectedStop_IT(&hadc1); 
            } 
            callback_counter++; 
        } else { 
            My_adcData[0] = (float)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1); 
            My_adcData[1] = (float)HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2); 
            current.I_a = (My_adcData[0] - bias_voltage_ia ) * ADC_CONVERSION * Gain_a ; 
            current.I_b = (My_adcData[1] - bias_voltage_ib ) * ADC_CONVERSION * Gain_a ; 
            float I_alpha = 0; 
            float I_beta = 0; 
            arm_clarke_f32(current.I_a,current.I_b,&I_alpha,&I_beta); 
            rotor_angle_encoder = Get_Position_EncoderMode(); 
            float sin_value = arm_sin_f32(rotor_angle_encoder); 
            float cos_value = arm_cos_f32(rotor_angle_encoder); 
            float _motor_Iq = 0; 
            float _motor_Id = 0; 
            arm_park_f32(I_alpha,I_beta,&_motor_Id,&_motor_Iq,sin_value,cos_value); 
            float filter_alpha_Id = 0.2; 
            float filter_alpha_Iq = 0.2; 
            motor_Id = low_pass_filter(_motor_Id,motor_Id,filter_alpha_Id); 
            motor_Iq = low_pass_filter(_motor_Iq,motor_Iq,filter_alpha_Iq); 
        } 
    } 
}

其中注入组的中断回调函数中，上电开始前的 bias_voltage_ib 即为偏置电压。一般电路会将其配置为 1.65V，但实际操作过程中会有波动，必须在上电前进行校准。用到的 Park 和 Clark 变换均为 ARM DSP 库函数。

此时可以先同开环旋转电机，打印电流信息是否符合相位差的情况。

D 轴强拖转子零位与电流闭环

确认电流采集正常后，开始电流闭环控制。此处涉及 PID 及 D 轴强拖转子零位。

D 轴转子零位的公式推导如图：

void find_zeropoint() { 
    set_pwm_duty(0.4, 0, 0); // d 轴强拖，形成 SVPWM 模型中的基础矢量 1，即对应转子零度位置 
    HAL_Delay(800); 
    set_pwm_duty(0, 0, 0); 
    HAL_Delay(800); 
} 

void set_pwm_duty(float d_u, float d_v, float d_w) { 
    d_u = min(d_u, 0.9f); 
    d_v = min(d_v, 0.9f); 
    d_w = min(d_w, 0.9f); 
    compare_1 = d_u * htim1.Instance->ARR; 
    compare_2 = d_v * htim1.Instance->ARR; 
    compare_3 = d_w * htim1.Instance->ARR; // 修正原逻辑错误 
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, d_u * htim1.Instance->ARR); 
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, d_v * htim1.Instance->ARR); 
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, d_w * htim1.Instance->ARR); 
}

注意此处将占空比强行设置为 90%，不跑满。因为在旋转过程中会有下半桥完全不导通的情况，为了保证下半桥的电流采样，需要通过控制上半桥的导通时间，给下半桥的电流采样留足时间。

确定转子零位后，即可开始电机控制。此处涉及的电流环控制参考了相关工程实践代码并进行微调。在实际使用过程中发现 ARM 的 PID 函数一直会有积分过饱和的情况，因此自行编写了位置式 PID。

float PID_Calc_Current(PID_t *pid,float target,float actual) {
    float error = target - actual / MAX_CURRENT; 
    pid->e = error; 
    pid->I += pid->Ki * pid->e; 
    if (pid->I > pid->I_max) pid->I = pid->I_max; 
    else if (pid->I < pid->I_min) pid->I = pid->I_min; 
    float u = pid->Kp * pid->e + pid->I + pid->Kd * (pid->e - pid->e_last); 
    pid->e_last = pid->e; 
    if (u > pid->out_max) u = pid->out_max; 
    else if (u < pid->out_min) u = pid->out_min; 
    return u; 
}

typedef struct {
    float Kp;
    float Ki;
    float Kd;
    float e; // 当前误差
    float e_last; // 上一次误差
    float I; // 积分累计项
    float out_min; // 输出下限
    float out_max; // 输出上限
    float I_min;
    float I_max;
} PID_t;

void Torque_control(float torque_d , float torque_q) { 
    float d = PID_Calc_Current(&pid_torque_d,torque_d,motor_Id); 
    float q = PID_Calc_Current(&pid_torque_q,torque_q,motor_Iq); 
    foc_forward(d, q, rotor_angle_encoder); 
}

void foc_forward(float d, float q, float rotor_rad) { 
    float d_u = 0; 
    float d_v = 0; 
    float d_w = 0; 
    svpwm(rotor_rad, d, q, &d_u, &d_v, &d_w); 
    set_pwm_duty(d_u, d_v, d_w); 
}

static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w) { 
    d = min(d, 1); 
    d = max(d, -1); 
    q = min(q, 1); 
    q = max(q, -1); 
    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}}; 
    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2}; 
    float sin_phi = arm_sin_f32(phi); 
    float cos_phi = arm_cos_f32(phi); 
    float alpha = 0; 
    float beta = 0; 
    arm_inv_park_f32(d, q, &alpha, &beta, sin_phi, cos_phi); 
    bool A = beta > 0; 
    bool B = fabs(beta) > SQRT3 * fabs(alpha); 
    bool C = alpha > 0; 
    int K = 4 * A + 2 * B + C; 
    int sector = K_to_sector[K]; 
    float t_m = arm_sin_f32(sector * rad60) * alpha - arm_cos_f32(sector * rad60) * beta; 
    float t_n = beta * arm_cos_f32(sector * rad60 - rad60) - alpha * arm_sin_f32(sector * rad60 - rad60); 
    float t_0 = 1 - t_m - t_n; 
    *d_u = t_m * v[sector - 1][0] + t_n * v[sector % 6][0] + t_0 / 2; 
    *d_v = t_m * v[sector - 1][1] + t_n * v[sector % 6][1] + t_0 / 2; 
    *d_w = t_m * v[sector - 1][2] + t_n * v[sector % 6][2] + t_0 / 2; 
}

由于各个电机的 PID 参数不同，PID 的初始化函数未在此列出。上文中提到的 rotor_angle_encoder 是通过编码器得到的电角度信息。

SVPWM 部分参考了工程实践代码，并增加了均值分量的版本进行测试，均能正常工作。

至此电流环闭环基本实现。内环电流环实现后，后续的串级 PID 较为简单，因为速度的获取就是编码器的位置信息进行积分。