feat(FOC): 添加角度同步Uq前馈补偿和速度环优化
- 实现update_angle_sync_uq_feedforward函数，支持mode2和mode3的角度同步 Uq前馈补偿，按物理360°查表学习负载/阻力差异 - 实现get_mode3_velocity_p函数，mode3低速时降低速度环P参数减少抖动 - 在velocity_control和angle_control_with_velocity_limit中集成Uq前馈补偿 - 优化normalizeAngle360函数，大角度时先扣整圈数避免while循环过多 - 调整配置参数，降低DEFAULT_vel_loop_p并添加详细的Uq前馈补偿配置 - 更新电源电压参数使用本周期采样值，避免电压滞后影响
This commit is contained in:
+129
-25
@@ -334,6 +334,107 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
// return I_q * 1000 - KE_FACTOR * rt_velocity;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 角度同步 Uq 前馈更新/输出。
|
||||
* @note 只在稳定同向巡航时输出和学习；停止、低速、反向、明显刹车/超速时立即退出。
|
||||
*/
|
||||
float FOC_Controller::update_angle_sync_uq_feedforward(
|
||||
u8 mode, float table[], u16 table_size, float mech_angle,
|
||||
float target_vel, float actual_vel, float velocity_error) {
|
||||
if (table_size == 0U) return 0.0f;
|
||||
|
||||
bool enable = false;
|
||||
bool learn_enable = false;
|
||||
float min_target = 0.0f;
|
||||
float stop_speed = 0.0f;
|
||||
float brake_err = 0.0f;
|
||||
float learn_alpha = 0.0f;
|
||||
float gain = 0.0f;
|
||||
float max_ff = 0.0f;
|
||||
float dir_sign = 1.0f;
|
||||
|
||||
switch (mode) {
|
||||
case 2:
|
||||
enable = MODE2_UQ_FF_ENABLE;
|
||||
learn_enable = MODE2_UQ_FF_LEARN_ENABLE;
|
||||
min_target = MODE2_UQ_FF_MIN_TARGET;
|
||||
stop_speed = MODE2_UQ_FF_STOP_SPEED;
|
||||
brake_err = MODE2_UQ_FF_BRAKE_ERR;
|
||||
learn_alpha = MODE2_UQ_FF_LEARN_ALPHA;
|
||||
gain = MODE2_UQ_FF_GAIN;
|
||||
max_ff = MODE2_UQ_FF_MAX;
|
||||
dir_sign = 1.0f;
|
||||
break;
|
||||
case 3:
|
||||
enable = MODE3_UQ_FF_ENABLE;
|
||||
learn_enable = MODE3_UQ_FF_LEARN_ENABLE;
|
||||
min_target = MODE3_UQ_FF_MIN_TARGET;
|
||||
stop_speed = MODE3_UQ_FF_STOP_SPEED;
|
||||
brake_err = MODE3_UQ_FF_BRAKE_ERR;
|
||||
learn_alpha = MODE3_UQ_FF_LEARN_ALPHA;
|
||||
gain = MODE3_UQ_FF_GAIN;
|
||||
max_ff = MODE3_UQ_FF_MAX;
|
||||
dir_sign = DIR;
|
||||
break;
|
||||
default:
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
const bool active = (fabsf(target_vel) > stop_speed) &&
|
||||
(fabsf(actual_vel) > stop_speed) &&
|
||||
((target_vel * actual_vel) > 0.0f) &&
|
||||
((velocity_error * dir_sign) > -brake_err);
|
||||
|
||||
float angle = normalizeAngle360(mech_angle);
|
||||
float index_f = angle / 360.0f * (float)table_size;
|
||||
u16 idx0 = (u16)index_f;
|
||||
if (idx0 >= table_size) idx0 = table_size - 1U;
|
||||
u16 idx1 = (idx0 + 1U) % table_size;
|
||||
float frac = index_f - (float)idx0;
|
||||
|
||||
float ff = table[idx0] * (1.0f - frac) + table[idx1] * frac;
|
||||
if (ff > max_ff) ff = max_ff;
|
||||
else if (ff < -max_ff) ff = -max_ff;
|
||||
|
||||
if (learn_enable && active && fabsf(target_vel) > min_target) {
|
||||
float ff_target = velocity_error * dir_sign * gain;
|
||||
if (ff_target > max_ff) ff_target = max_ff;
|
||||
else if (ff_target < -max_ff) ff_target = -max_ff;
|
||||
table[idx0] += learn_alpha * (ff_target - table[idx0]);
|
||||
}
|
||||
|
||||
return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
|
||||
}
|
||||
/**
|
||||
* @brief mode3 低速/静止附近降低速度内环 P，运行速度升高后平滑恢复。
|
||||
*/
|
||||
float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
|
||||
float vel_p = vel_angloop_p;
|
||||
if (target_vel < -120.0f) {
|
||||
vel_p = vel_angloop_p * 2.0f;
|
||||
} else if (target_vel < -30.0f) {
|
||||
vel_p = vel_angloop_p * 1.5f;
|
||||
}
|
||||
|
||||
if (MODE3_LOW_SPEED_P_SCALE_ENABLE) {
|
||||
float abs_target = fabsf(target_vel);
|
||||
float abs_speed = fabsf(actual_vel);
|
||||
float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
|
||||
float p_scale = 1.0f;
|
||||
if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
|
||||
p_scale = MODE3_LOW_SPEED_P_MIN_SCALE;
|
||||
} else if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
|
||||
p_scale = MODE3_LOW_SPEED_P_MIN_SCALE +
|
||||
(1.0f - MODE3_LOW_SPEED_P_MIN_SCALE) *
|
||||
(speed_ref - MODE3_LOW_SPEED_P_LOW) /
|
||||
(MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
|
||||
}
|
||||
vel_p *= p_scale;
|
||||
}
|
||||
return vel_p;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机控制类初始化函数
|
||||
*
|
||||
@@ -711,23 +812,27 @@ float FOC_Controller::angle_control() {
|
||||
* @return float: 当前速度
|
||||
*/
|
||||
float FOC_Controller::velocity_control() {
|
||||
// 速度误差计算
|
||||
// 获取电机轴角度：mode2 Uq 前馈补偿需要用物理 360° 角度查表。
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
// mode2 速度环：真实速度波动保留在反馈里，PID 继续看真实 rt_velocity。
|
||||
float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;
|
||||
|
||||
// 目标速度突变检测：target 大幅变化时清积分
|
||||
static float last_target_vel = 0.0f;
|
||||
const float target_delta = target_velocity - last_target_vel;
|
||||
if (fabsf(target_delta) > 5.0f) {
|
||||
vel_loop.reset_integral();
|
||||
}
|
||||
last_target_vel = target_velocity;
|
||||
// mode2 使用独立速度环参数。
|
||||
vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
|
||||
vel_loop.set_limit(limit_voltage);
|
||||
float Uq_pid = vel_loop(velocity_error);
|
||||
|
||||
// PID 控制计算
|
||||
float Uq = vel_loop(velocity_error);
|
||||
static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f};
|
||||
float Uq_ff = update_angle_sync_uq_feedforward(
|
||||
2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle,
|
||||
target_velocity, rt_velocity, velocity_error);
|
||||
|
||||
float Uq = Uq_pid + Uq_ff;
|
||||
if (Uq > limit_voltage) Uq = limit_voltage;
|
||||
else if (Uq < -limit_voltage) Uq = -limit_voltage;
|
||||
CAN0_transmiter.testfloat_union.f = Uq;
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
// 设置相电压
|
||||
@@ -779,14 +884,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
|
||||
float target_velocity = ang_inVelAngloop(angle_error);
|
||||
|
||||
// 使用速度PID控制器计算最终输出电压
|
||||
// 根据目标速度大小，选择不同的PID参数
|
||||
if (target_velocity < -120.0f) {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
|
||||
} else if (target_velocity < -30.0f) {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
|
||||
} else {
|
||||
vel_inVelAngloop.set_P(vel_angloop_p);
|
||||
}
|
||||
vel_inVelAngloop.set_P(get_mode3_velocity_p(target_velocity, rt_velocity));
|
||||
float velocity_error = target_velocity - rt_velocity;
|
||||
|
||||
// 目标速度突变检测：target 大幅变化时清积分
|
||||
@@ -796,14 +894,20 @@ float FOC_Controller::angle_control_with_velocity_limit() {
|
||||
}
|
||||
last_vang_target_vel = target_velocity;
|
||||
|
||||
static float last_velocity_error = 0.0f;
|
||||
last_velocity_error = velocity_error;
|
||||
float Uq_pid = vel_inVelAngloop(velocity_error) * DIR;
|
||||
|
||||
float Uq = vel_inVelAngloop(velocity_error)* DIR;
|
||||
|
||||
// 获取电机轴角度
|
||||
// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
static float mode3_uq_ff_table[MODE3_UQ_FF_TABLE_SIZE] = {0.0f};
|
||||
float Uq_ff = update_angle_sync_uq_feedforward(
|
||||
3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle,
|
||||
target_velocity, rt_velocity, velocity_error);
|
||||
|
||||
float Uq = Uq_pid + Uq_ff;
|
||||
if (Uq > limit_voltage) Uq = limit_voltage;
|
||||
else if (Uq < -limit_voltage) Uq = -limit_voltage;
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
|
||||
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
|
||||
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
|
||||
// 获取当前实际电流值
|
||||
|
||||
@@ -310,6 +310,11 @@ class FOC_Controller {
|
||||
* @return float 返回值
|
||||
*/
|
||||
float cal_Iq_Id(float current_a, float current_b, float current_c, float angle_el);
|
||||
float update_angle_sync_uq_feedforward(u8 mode, float table[],
|
||||
u16 table_size, float mech_angle,
|
||||
float target_vel, float actual_vel,
|
||||
float velocity_error);
|
||||
float get_mode3_velocity_p(float target_vel, float actual_vel);
|
||||
};
|
||||
|
||||
// 定义函数指针类型
|
||||
|
||||
Reference in New Issue
Block a user