FOC算法程序分析
固件版本fw-0.5.1（0积分下载）
代码分析
下列分析函数类型均为bool类型，在各步均设检测，当不满足正常运行检测时，设置错误原因set_error返回false程序停止。
1-4为FOC_current函数内容，函数输入为DQ轴电流期望值，电角度I_phase以及下一时刻电角度pwm_phase
5为函数enqueue_modulation_timings，输入为mod_alpha和mod_beta
1.采样电流校验
首先语法糖给current_control重新命了个名，然后将输入的Iq_des赋给Iq_setpoint，其中Iq_des通过输入期望的位置速度电流等与实际数据的偏差经过PD控制器计算的出；
之后检查采样电流phB与phC的绝对值与设定的过流限制overcurrent_trip_level对比，存在过流时set_error设置错误原因ERROR_CURRENT_SENSE_SATURATION，返回false，程序停止。
bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase)
CurrentControl_t& ictrl = current_control_;
// For Reporting
ictrl.Iq_setpoint = Iq_des;
// Check for current sense saturation
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;
}
2.Clark与Park变换
首先根据Clark变换公式，利用采集的两相电流phB phC算出Ialpha Ibeta
之后是park变换，c_I,s_I为电角度的正弦余弦值（由于arm_math库有bug，在角度接近-π/2会出现峰值，详情，ODrive里用修正后的our_arm_cos/sin_f32）然后计算DQ轴电流Id、Iq,程序里Iq_measured和Id_measured为了给上位机做显示用，其中filter_k为1，意为每次计算Iq/d_measured与Iq/d的差，然后补到Iq/d_measured进行数值更新。
再计算Id和Iq 的平方和与限制电流I_trip的平方进行比较，超过的话set_error设置错误原因ERROR_CURRENT_LIMIT_VIOLATION，返回false，程序停止。
// Clarke transform
float Ialpha = -current_meas_.phB - current_meas_.phC;
float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
// Park transform
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);
// Check for violation of current limit
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;
}
3.PI控制器
首先根据输入期望D轴和Q轴电流Id_des和Iq_des和上步得到的DQ轴电流测量值Id/q计算DQ轴电流偏差Ierr_d/q，之后利用PI控制器，公式U=Kp Ierr+Ki Δt Ierr 来求出DQ轴电压。 integral_d和integral_q是积分项，相当于公式中的Ki Δt Ierr，初始值为0，在下面程序中计算，根据偏差Ierr_d/q乘以KI系数i_gain再乘以采样周期（也即两次采样间隔时间）current_meas_period得出；积分项再加上Kp Ierr 也就是程序中的 Ierr_d/q * p_gain就算出了Vd/q。
经过等幅值变换，电压幅值为原来的2/3也即 mod_to_V，为方便SVPWM计算以及积分相防饱和运算，将计算的DQ轴电压除以变换后的幅值mod_d和mod_q
根据积分项公式可知，偏差存在积分项就会不断累加，为了防止积分项过大饱和，需要进行积分项抗饱和运算利用系数mod_scalefactor,经过换算相当于sqrt(modd2+modq2)>0.6928也即mod_scalefactor<1,则认定为饱和，将mod_d/q计算为mod_d/q * mod_scalefactor，积分项integral_d/q保持原值不再累加；否则认为积分项未饱和,继续进行积分项运算。
根据比例乘以计算得出的Iq Id算出母线电流。
// Current error
float Ierr_d = Id_des - Id;
float Ierr_q = Iq_des - Iq;
// TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
// Apply PI control
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;
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;
// Vector modulation saturation, lock integrator if saturated
// TODO make maximum modulation configurable
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
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);
}
// Compute estimated bus current
ictrl.Ibus = mod_d * Id + mod_q * Iq;
4.反Park
先计算pwm_phase的正余弦值，值得注意的是该角度不是Park变换时的电角度I_phase,而是在up_data函数里更新的估算的更新角度，根据整体控制框架，在进行反Park时相位需更新1.5个周期，公式为pwm_phase = I_phase + 1.5f * current_meas_period * phase_vel,phase_vel为电角速度。
根据正余弦值以及mod_d/q，反Park变换算出mod_alpha,mod_beta,再乘以mod_to_V也即乘以2Udc/3,转化为实际的最终alpha,beta相电压。
mod_alpha,mod_beta作为输入进入函数enqueue_modulation_timings，进行SVPWM运算，CCR值计算，设置位信号，将CCR值更新。
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 sensorles estimator)
ictrl.final_v_alpha = mod_to_V * mod_alpha;
ictrl.final_v_beta = mod_to_V * mod_beta;
// Apply SVM
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false; // error set inside enqueue_modulation_timings
log_timing(TIMING_LOG_FOC_CURRENT);
return true;
5.CCR值计算
输入的mod_alpha和mod_beta进入SVM函数进行运算，输出三相的上mos管的开启时刻tA,tB,tC，乘以定时器周期ARR值也即TIM_1_8_PERIOD_CLOCKS，得出CCR值**next_timings_数组，然后将next_timings_valid_ **置为true，传入数值更新函数，更新CCR值（可参考我的文章#1 电机校准程序，里面有详细说明）。
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
float tA, tB, tC;
if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
return set_error(ERROR_MODULATION_MAGNITUDE), false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_valid_ = true;
return true;
}
6.SVPWM运算
以第一扇区的扇区判断以及矢量作用时间和开启时间的计算为代表说明ODrive与正常SVPWM运算的区别，其他同理。
函数输入alpha,beta即我们计算的mod_alpha,mod_beta,与正常公式推导中Valpha,Vbeta的区别为都除以了2Udc/3,因此alpha,beta的比例和正负不变因此扇区判断与正常推导一致，可参考附图判断条件前三列。
计算矢量作用时间时，因最终为计算比例，故将周期时间ts设为1，以第一扇区为例，常规推导为：t1=(3Ualpha - Ubeta√3 ) / 2Udc , t2=Ubeta√3 / Udc,ODrive里由于输入的Ubeta和Ualpha都已除以2Udc/3,故将常规推导结果除以2Udc/3即可，为t1=Ualpha - Ubeta/√3, t2=2Ubeta/√3与程序计算结果一致。常规推导矢量作用时间结果见附录2；
开启时间与常规推导一致，计算出tA,tB,tC传出。
各开启时间需>=0且<=1，否则返回false,在上步分析的函数enqueue_modulation_timings会set_error调制错误ERROR_MODULATION_MAGNITUDE，返回false，程序停止。
int SVM(float alpha, float beta, float* tA, float* tB, float* tC){
int Sextant;
if (beta >= 0.0f) {
if (alpha >= 0.0f) {
//quadrant I
if (one_by_sqrt3 * beta > alpha)
Sextant = 2; //sextant v2-v3
else
Sextant = 1; //sextant v1-v2
} else {
//quadrant II
if (-one_by_sqrt3 * beta > alpha)
Sextant = 3; //sextant v3-v4
else
Sextant = 2; //sextant v2-v3
}
} else {
if (alpha >= 0.0f) {
//quadrant IV
if (-one_by_sqrt3 * beta > alpha)
Sextant = 5; //sextant v5-v6
else
Sextant = 6; //sextant v6-v1
} else {
//quadrant III
if (one_by_sqrt3 * beta > alpha)
Sextant = 4; //sextant v4-v5
else
Sextant = 5; //sextant v5-v6
}
}
switch (Sextant) {
// sextant v1-v2
case 1: {
// Vector on-times
float t1 = alpha - one_by_sqrt3 * beta;
float t2 = two_by_sqrt3 * beta;
// PWM timings
*tA = (1.0f - t1 - t2) * 0.5f;
*tB = *tA + t1;
*tC = *tB + t2;
} break;
// sextant v2-v3
case 2: {
// Vector on-times
float t2 = alpha + one_by_sqrt3 * beta;
float t3 = -alpha + one_by_sqrt3 * beta;
// PWM timings
*tB = (1.0f - t2 - t3) * 0.5f;
*tA = *tB + t3;
*tC = *tA + t2;
} break;
// sextant v3-v4
case 3: {
// Vector on-times
float t3 = two_by_sqrt3 * beta;
float t4 = -alpha - one_by_sqrt3 * beta;
// PWM timings
*tB = (1.0f - t3 - t4) * 0.5f;
*tC = *tB + t3;
*tA = *tC + t4;
} break;
// sextant v4-v5
case 4: {
// Vector on-times
float t4 = -alpha + one_by_sqrt3 * beta;
float t5 = -two_by_sqrt3 * beta;
// PWM timings
*tC = (1.0f - t4 - t5) * 0.5f;
*tB = *tC + t5;
*tA = *tB + t4;
} break;
// sextant v5-v6
case 5: {
// Vector on-times
float t5 = -alpha - one_by_sqrt3 * beta;
float t6 = alpha - one_by_sqrt3 * beta;
// PWM timings
*tC = (1.0f - t5 - t6) * 0.5f;
*tA = *tC + t5;
*tB = *tA + t6;
} break;
// sextant v6-v1
case 6: {
// Vector on-times
float t6 = -two_by_sqrt3 * beta;
float t1 = alpha + one_by_sqrt3 * beta;
// PWM timings
*tA = (1.0f - t6 - t1) * 0.5f;
*tC = *tA + t1;
*tB = *tC + t6;
} break;
}
// if any of the results becomes NaN, result_valid will evaluate to false
int result_valid =
*tA >= 0.0f && *tA <= 1.0f
&& *tB >= 0.0f && *tB <= 1.0f
&& *tC >= 0.0f && *tC <= 1.0f;
return result_valid ? 0 : -1;
}
附表1：扇区判断
附表2：矢量作用时间
775
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
