Odrive0.5.1-FOC电机控制核心代码 Odrive motor.cpp(.hpp)代码实现（四）

本文链接：https://blog.csdn.net/MOS_JBET/article/details/147088886

16 bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase)

这段代码是 FOC（Field-Oriented Control）电压控制的一部分，目的是根据电机当前电角度 pwm_phase，将 d/q 电压向量通过逆 Park 变换（inverse Park transform）转换为 αβ 坐标系下的电压向量，然后交给 SVM 做三相 PWM 输出。

bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
    float c = our_arm_cos_f32(pwm_phase); //pwm_phase 是电机当前的电角度（单位为弧度），用于变换坐标系。
    float s = our_arm_sin_f32(pwm_phase);
    float v_alpha = c*v_d - s*v_q;
    float v_beta = c*v_q + s*v_d;
    return enqueue_voltage_timings(v_alpha, v_beta);
}

17 bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase)

这是一个电流控制器，在 FOC 控制中它属于内环控制，作用是：将期望的 d/q 电流（电机磁通和转矩）转换为 α/β 坐标的调制波形，驱动三相电机输出 PWM。

Park 变换公式

bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) {
    //ictrl 是电流环参数结构体的引用。
    CurrentControl_t& ictrl = current_control_;

    // 设置 Iq_setpoint，用于监控/日志。
    ictrl.Iq_setpoint = Iq_des;

    // 电流采样饱和保护:检查相电流是否超过安全阈值,超过就报错保护，避免损坏驱动或电机。
    if (std::abs(current_meas_.phB) > ictrl.overcurrent_trip_level || std::abs(current_meas_.phC) > ictrl.overcurrent_trip_level) {
        set_error(ERROR_CURRENT_SENSE_SATURATION);
        return false;
    }

    // Clarke 变换（三相 -> 二相 αβ） 将采样的 phB 和 phC 电流转换为 α/β 坐标系，简化控制。
    float Ialpha = -current_meas_.phB - current_meas_.phC;
    float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);

    // Park 变换（静止坐标系 αβ → 转子坐标系 dq）
    float c_I = our_arm_cos_f32(I_phase);
    float s_I = our_arm_sin_f32(I_phase);
    float Id = c_I * Ialpha + s_I * Ibeta;
    float Iq = c_I * Ibeta - s_I * Ialpha;
    
    //一阶滤波:对实际电流做低通滤波，提高显示平滑度。
    ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured);
    ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);

    //电流限幅保护
    float I_trip = effective_current_lim() + config_.current_lim_margin;
    if (SQ(Id) + SQ(Iq) > SQ(I_trip)) {
        set_error(ERROR_CURRENT_LIMIT_VIOLATION);
        return false;
    }

    // PI 控制器输出电压,计算误差后，走 PI 控制器计算目标电压（控制电压）输出。这里只用了比例项 p_gain，积分在后面处理。
    float Ierr_d = Id_des - Id;
    float Ierr_q = Iq_des - Iq;

    float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain;
    float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain;

    //归一化调制（Volt → Mod）:把 Vd/Vq 转为 PWM 调制值 mod_d/mod_q，范围通常是 -1~1。
    float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
    float V_to_mod = 1.0f / mod_to_V;
    float mod_d = V_to_mod * Vd;
    float mod_q = V_to_mod * Vq;

    // 限幅 + 抑制积分超调,防止调制幅度超过 1，导致不可实现。
    float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
    // 饱和时锁住积分器（乘以 <1 的衰减因子），防止超调。
    if (mod_scalefactor < 1.0f) {
        mod_d *= mod_scalefactor;
        mod_q *= mod_scalefactor;
        // TODO make decayfactor configurable
        ictrl.v_current_control_integral_d *= 0.99f;
        ictrl.v_current_control_integral_q *= 0.99f;
    } else {
        未饱和则执行积分更新。
        ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period);
        ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period);
    }

    // 计算母线电流（用于电源电流估计）
    ictrl.Ibus = mod_d * Id + mod_q * Iq;

    // 逆 Park 变换（mod_d/q → mod_alpha/beta）
    float c_p = our_arm_cos_f32(pwm_phase);
    float s_p = our_arm_sin_f32(pwm_phase);
    float mod_alpha = c_p * mod_d - s_p * mod_q;
    float mod_beta = c_p * mod_q + s_p * mod_d;

    //存储 αβ 坐标下的电压，用于无传感器估算
    ictrl.final_v_alpha = mod_to_V * mod_alpha;
    ictrl.final_v_beta = mod_to_V * mod_beta;

    // 进行 SVM 调制（进入 PWM 输出）
    if (!enqueue_modulation_timings(mod_alpha, mod_beta))
        return false; // error set inside enqueue_modulation_timings
    log_timing(TIMING_LOG_FOC_CURRENT);

    //示波器采样触发（调试用）用于调试采样某变量（这里是Ialpha）。有触发条件 trigger_threshold，达到后开始采样。
    if (axis_->axis_num_ == 0) {

        // Edit these to suit your capture needs
        float trigger_data = ictrl.v_current_control_integral_d;
        float trigger_threshold = 0.5f;
        float sample_data = Ialpha;

        static bool ready = false;
        static bool capturing = false;
        if (trigger_data < trigger_threshold) {
            ready = true;
        }
        if (ready && trigger_data >= trigger_threshold) {
            capturing = true;
            ready = false;
        }
        if (capturing) {
            oscilloscope[oscilloscope_pos] = sample_data;
            if (++oscilloscope_pos >= OSCILLOSCOPE_SIZE) {
                oscilloscope_pos = 0;
                capturing = false;
            }
        }
    }

    return true;
}

总结一张图（简略流程）

18 bool Motor::update(float torque_setpoint, float phase, float phase_vel)

这段 Motor::update() 是电机控制主函数中的一环，负责将期望的扭矩转换为电流指令，并调用适当的 FOC（磁场定向控制）函数控制电机。

bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
    float current_setpoint = 0.0f;
    
    //方向与扭矩处理 config_.direction用于统一控制电机正反转方向。
    phase *= config_.direction;
    phase_vel *= config_.direction;
    
    //扭矩转电流,对 BLDC、PMSM 电机而言直接除以力矩常数即可;ACIM 中分母还要乘以rotor_flux（磁链）。
    if (config_.motor_type == MOTOR_TYPE_ACIM) {
        current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
    }
    else {
        current_setpoint = torque_setpoint / config_.torque_constant;
    }
    current_setpoint *= config_.direction;

    // 电流限制:使用 std::clamp 防止指令电流超出电流上限。Id 是直轴电流（影响磁链），Iq 是交轴电流（影响力矩）。
    float ilim = effective_current_lim();
    float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim);
    float iq = std::clamp(current_setpoint, -ilim, ilim);

    if (config_.motor_type == MOTOR_TYPE_ACIM) {
        if (config_.acim_autoflux_enable) {
            float abs_iq = fabsf(iq);
            float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
            id += gain * (abs_iq - id) * current_meas_period;
            id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
            current_control_.Id_setpoint = id;
        }

        // acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
        float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux);
        current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
        float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
        // Check for issues with small denominator. Polarity of check to catch NaN too
        bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz;
        if (!acceptable_vel)
            slip_velocity = 0.0f;
            
        phase_vel += slip_velocity; // 总速度 = 机械速度 + 滑差速度
        current_control_.async_phase_vel = slip_velocity; 

        current_control_.async_phase_offset += slip_velocity * current_meas_period;
        current_control_.async_phase_offset = wrap_pm_pi(current_control_.async_phase_offset);
        phase += current_control_.async_phase_offset;
        
        phase = wrap_pm_pi(phase); // 相位限制在 [-π, π]
    }

    //相位调整:预测当前phase，提前半个 PWM 周期计算控制量。
    float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;

    //  FOC调用:HIGH_CURRENT / ACIM 都是电流环控制；GIMBAL 电机直接使用电压环（开环控制）。
    switch(config_.motor_type){
        case MOTOR_TYPE_HIGH_CURRENT: return FOC_current(id, iq, phase, pwm_phase); break;
        case MOTOR_TYPE_ACIM: return FOC_current(id, iq, phase, pwm_phase); break;
        case MOTOR_TYPE_GIMBAL: return FOC_voltage(id, iq, pwm_phase); break;
        default: set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; break;
    }
    return true;
}