Odrive源碼分析(七) Park逆變換
Odrive中FOC部分代碼分散在各個對象中，并不是集中在某一塊，所以試圖在某一段代碼就能得到FOC全貌是不現實的。
先看下FOC的整個流程：
控制變量到三相電流輸出的關鍵部分分為Park逆變換和SVPWM。本文主要討論逆park變換在odrive中的代碼實現。、
代碼主要在FieldOrientedController::get_alpha_beta_output函數中。
ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(uint32_t output_timestamp, std::optional<float2D>* mod_alpha_beta,std::optional<float>* ibus) {if (!vbus_voltage_measured_.has_value() || !Ialpha_beta_measured_.has_value()) {// FOC didn't receive a current measurement yet.return Motor::ERROR_CONTROLLER_INITIALIZING;} else if (abs((int32_t)(i_timestamp_ - ctrl_timestamp_)) > MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA) {// Data from control loop and current measurement are too far apart.return Motor::ERROR_BAD_TIMING;}// TODO: improve efficiency in case PWM updates are requested at a higher// rate than current sensor updates. In this case we can reuse mod_d and// mod_q from a previous iteration.if (!Vdq_setpoint_.has_value()) {return Motor::ERROR_UNKNOWN_VOLTAGE_COMMAND;} else if (!phase_.has_value() || !phase_vel_.has_value()) {return Motor::ERROR_UNKNOWN_PHASE_ESTIMATE;} else if (!vbus_voltage_measured_.has_value()) {return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;}auto [Vd, Vq] = *Vdq_setpoint_;float phase = *phase_;float phase_vel = *phase_vel_;float vbus_voltage = *vbus_voltage_measured_;std::optional<float2D> Idq;// Park transformif (Ialpha_beta_measured_.has_value()) {auto [Ialpha, Ibeta] = *Ialpha_beta_measured_;float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);float c_I = our_arm_cos_f32(I_phase);float s_I = our_arm_sin_f32(I_phase);Idq = {c_I * Ialpha + s_I * Ibeta,c_I * Ibeta - s_I * Ialpha};Id_measured_ += I_measured_report_filter_k_ * (Idq->first - Id_measured_);Iq_measured_ += I_measured_report_filter_k_ * (Idq->second - Iq_measured_);} else {Id_measured_ = 0.0f;Iq_measured_ = 0.0f;}float mod_to_V = (2.0f / 3.0f) * vbus_voltage;float V_to_mod = 1.0f / mod_to_V;float mod_d;float mod_q;if (enable_current_control_) {// Current control modeif (!pi_gains_.has_value()) {return Motor::ERROR_UNKNOWN_GAINS;} else if (!Idq.has_value()) {return Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT;} else if (!Idq_setpoint_.has_value()) {return Motor::ERROR_UNKNOWN_CURRENT_COMMAND;}auto [p_gain, i_gain] = *pi_gains_;auto [Id, Iq] = *Idq;auto [Id_setpoint, Iq_setpoint] = *Idq_setpoint_;float Ierr_d = Id_setpoint - Id;float Ierr_q = Iq_setpoint - Iq;// Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode)mod_d = V_to_mod * (Vd + v_current_control_integral_d_ + Ierr_d * p_gain);mod_q = V_to_mod * (Vq + v_current_control_integral_q_ + Ierr_q * p_gain);// Vector modulation saturation, lock integrator if saturated// TODO make maximum modulation configurablefloat mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / std::sqrt(mod_d * mod_d + mod_q * mod_q);if (mod_scalefactor < 1.0f) {mod_d *= mod_scalefactor;mod_q *= mod_scalefactor;// TODO make decayfactor configurablev_current_control_integral_d_ *= 0.99f;v_current_control_integral_q_ *= 0.99f;} else {v_current_control_integral_d_ += Ierr_d * (i_gain * current_meas_period);v_current_control_integral_q_ += Ierr_q * (i_gain * current_meas_period);}} else {// Voltage control modemod_d = V_to_mod * Vd;mod_q = V_to_mod * Vq;}// Inverse park transformfloat pwm_phase = phase + phase_vel * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);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;// Report final applied voltage in stationary frame (for sensorless estimator)final_v_alpha_ = mod_to_V * mod_alpha;final_v_beta_ = mod_to_V * mod_beta;*mod_alpha_beta = {mod_alpha, mod_beta};if (Idq.has_value()) {auto [Id, Iq] = *Idq;*ibus = mod_d * Id + mod_q * Iq;power_ = vbus_voltage * (*ibus).value();}return Motor::ERROR_NONE;
}
下面主要分析上述函數的具體實現：
step1 在進入本函數計算之前，必須已經完成線電壓的測量和相電流的測量，同時要保證時效性，如下所示：
//本函數調用前，在ControlLoop_IRQHandler函數中已經完成了測量。所以本條件必然滿足。
if (!vbus_voltage_measured_.has_value() || !Ialpha_beta_measured_.has_value()) {// FOC didn't receive a current measurement yet.return Motor::ERROR_CONTROLLER_INITIALIZING;
}
//i_timestamp_和ctrl_timestamp_實際上是同一個值，因為都是外部傳進來的
else if (abs((int32_t)(i_timestamp_ - ctrl_timestamp_)) > MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA) {// Data from control loop and current measurement are too far apart.return Motor::ERROR_BAD_TIMING;
}
step2 相關控制數據檢查，如下所示：
//判斷是否已經計算出Vdq_setpoint（motor.cpp已通過PID計算得出）
if (!Vdq_setpoint_.has_value()) {return Motor::ERROR_UNKNOWN_VOLTAGE_COMMAND;
}
//判斷是否已經得到電角度和電角速度（已經通過encode.cpp計算得出）
else if (!phase_.has_value() || !phase_vel_.has_value()) {return Motor::ERROR_UNKNOWN_PHASE_ESTIMATE;
}
//這里其實重復判斷了...
else if (!vbus_voltage_measured_.has_value()) {return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
}//變量提取
auto [Vd, Vq] = *Vdq_setpoint_;
float phase = *phase_;
float phase_vel = *phase_vel_;
float vbus_voltage = *vbus_voltage_measured_;
step3 通過測量到的電流進行Park變換轉換到觀察到的DQ，如下所示：
// 檢查AB空間數據有效性，Ialpha_beta_measured_是通過FieldOrientedController父類 AlphaBetaFrameController::on_measurement計算得出。
if (Ialpha_beta_measured_.has_value()) {auto [Ialpha, Ibeta] = *Ialpha_beta_measured_;//這里得到電流測量時刻的電角度，這里很嚴謹，雖然i_timestamp_和 ctrl_timestamp_其實是相等的值，但是軟件保留了拓展能力。float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);//計算Park變換得到測量的DQ值用于后面的電流環閉環float c_I = our_arm_cos_f32(I_phase);float s_I = our_arm_sin_f32(I_phase);Idq = {c_I * Ialpha + s_I * Ibeta,c_I * Ibeta - s_I * Ialpha};Id_measured_ += I_measured_report_filter_k_ * (Idq->first - Id_measured_);Iq_measured_ += I_measured_report_filter_k_ * (Idq->second - Iq_measured_);
} else {Id_measured_ = 0.0f;Iq_measured_ = 0.0f;
}//相關中間變量計算
//這里mod_to_V是線電壓的2/3.實際上這是相電壓能達到的最大值，這里可以理解為SVPWM扇區的內接圓半徑。
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
float V_to_mod = 1.0f / mod_to_V;
float mod_d;
float mod_q;
step4 電流環控制，如下所示：
//只有在開啟了電流閉環控制時以下代碼才會生效
//當motor_.config_.motor_type 為 Motor::MOTOR_TYPE_GIMBA 云臺電機時，不使用電流控制模式而采用電壓控制模式。
if (enable_current_control_) {//檢查是否配置了PI參數if (!pi_gains_.has_value()) {return Motor::ERROR_UNKNOWN_GAINS;} //電流環控制必須已經得到了Idq（上面計算已經得出）else if (!Idq.has_value()) {return Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT;} //是否存在控制量Idq_setpoint_（該變量已經由motor.cpp中計算得出）else if (!Idq_setpoint_.has_value()) {return Motor::ERROR_UNKNOWN_CURRENT_COMMAND;}//語法糖auto [p_gain, i_gain] = *pi_gains_;auto [Id, Iq] = *Idq;auto [Id_setpoint, Iq_setpoint] = *Idq_setpoint_;//使用PI控制計算得出mod_q,mod_d,這里mod_q和mod_d可以理解為mod_to_V的倍數。float Ierr_d = Id_setpoint - Id;float Ierr_q = Iq_setpoint - Iq;//Vd/Vq作為前饋量加入到電流環PI控制中mod_d = V_to_mod * (Vd + v_current_control_integral_d_ + Ierr_d * p_gain);mod_q = V_to_mod * (Vq + v_current_control_integral_q_ + Ierr_q * p_gain);// Vector modulation saturation, lock integrator if saturated// TODO make maximum modulation configurablefloat mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / std::sqrt(mod_d * mod_d + mod_q * mod_q);if (mod_scalefactor < 1.0f) {mod_d *= mod_scalefactor;mod_q *= mod_scalefactor;// TODO make decayfactor configurablev_current_control_integral_d_ *= 0.99f;v_current_control_integral_q_ *= 0.99f;} else {v_current_control_integral_d_ += Ierr_d * (i_gain * current_meas_period);v_current_control_integral_q_ += Ierr_q * (i_gain * current_meas_period);}} else {//直接使用Vd/Vq 電壓控制,得到控制量為mod_to_V的倍數。mod_d = V_to_mod * Vd;mod_q = V_to_mod * Vq;
}step5 正式完成park逆變換，如下所示：
//這里得到電角度，這里output_timestamp的時間會大于ctrl_timestamp_（理論上真好會大于一個控制周期），此時的電角度比測量電流時的電角度是不同的。
float pwm_phase = phase + phase_vel * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
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;// Report final applied voltage in stationary frame (for sensorless estimator)
final_v_alpha_ = mod_to_V * mod_alpha;
final_v_beta_ = mod_to_V * mod_beta;//得到最終計算出的AB控制量（后面會輸入到SVPWM模塊逆變出三相電流）
*mod_alpha_beta = {mod_alpha, mod_beta};//計算出相關數據
if (Idq.has_value()) {auto [Id, Iq] = *Idq;//計算出線電流*ibus = mod_d * Id + mod_q * Iq;//計算功率（電壓和電流乘積）power_ = vbus_voltage * (*ibus).value();
}
完成Park逆變換后，下一步就直接送入了SVPWM模塊了。
` 方法移動端 `兼容 ios`，ios環境new Date()返回NaN)
