cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(FOC): 添加角度同步Uq前馈补偿和速度环优化

Browse Source 
- 实现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:
cbb
2026-05-24 04:56:33 +08:00
parent e646544f7f
commit b71fba2016
9 changed files with 172 additions and 568 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+129 -25
View File
@@ -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);
// 获取当前实际电流值
Robot_V2/Algorithm/FOC.h
+5
View File
@@ -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);
};
// 定义函数指针类型
Robot_V2/device/ang_sensor.h
+6 -2
View File
@@ -96,10 +96,14 @@ class angle_sensor {
};
/**
* @brief 快速角度归一化到 [0, 360)
* 用 while 减法替代 fmodf，在角度变化量很小时（电机控制场景）
* 通常只需 0~1 次减法，远快于 fmodf 的 ~30-50 周期开销。
* 小角度保留 while 快路径；大角度先一次性扣掉整圈数，避免累计角很大时 while 循环过多。
* 不依赖 fmodf，减少嵌入式数学库开销和链接风险。
*/
inline float normalizeAngle360(float angle) {
if (angle >= 720.0f || angle <= -360.0f) {
int turns = (int)(angle / 360.0f);
angle -= (float)turns * 360.0f;
}
while (angle >= 360.0f) angle -= 360.0f;
while (angle < 0.0f) angle += 360.0f;
return angle;
Robot_V2/sys/config.h
+30 -1
View File
@@ -126,16 +126,45 @@ constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流 P
constexpr float current_loop_Ki = 0.55f; // 电流 P
constexpr float current_loop_Kd = 3e-4f; // 电流 P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度 P
constexpr float DEFAULT_vel_loop_p = 0.005f / 100 * 12; // 速度 P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
// mode2 角度同步 Uq 前馈补偿：固定 Uq 下的真实速度波动不应从反馈中滤掉，
// 而应按物理 360° 提前补偿负载/阻力差异，降低速度 PID 的周期性纠偏量。
constexpr bool MODE2_UQ_FF_ENABLE = true;
constexpr bool MODE2_UQ_FF_LEARN_ENABLE = true;
constexpr u16 MODE2_UQ_FF_TABLE_SIZE = 180;
constexpr float MODE2_UQ_FF_LEARN_ALPHA = 0.01f; // 越小越慢，避免误学瞬态
constexpr float MODE2_UQ_FF_MIN_TARGET = 1.5f; // 目标速度过低时不学习
constexpr float MODE2_UQ_FF_GAIN = 0.003f; // 速度误差(deg/s)到前馈电压(V)的学习比例
constexpr float MODE2_UQ_FF_MAX = 3.5f; // 前馈补偿电压限幅(V)
constexpr float MODE2_UQ_FF_STOP_SPEED = 2.0f; // 目标/实际速度低于该值时不输出前馈
constexpr float MODE2_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
// mode3 角度同步 Uq 前馈补偿：单独建表，避免和 mode2 互相污染。
constexpr bool MODE3_UQ_FF_ENABLE = true;
constexpr bool MODE3_UQ_FF_LEARN_ENABLE = true;
constexpr u16 MODE3_UQ_FF_TABLE_SIZE = 180;
constexpr float MODE3_UQ_FF_LEARN_ALPHA = 0.01f;
constexpr float MODE3_UQ_FF_MIN_TARGET = 2.5f; // mode3 内部目标速度过低时不学习
constexpr float MODE3_UQ_FF_GAIN = 0.3f; // mode3 速度误差未乘减速比，故增益独立于 mode2
constexpr float MODE3_UQ_FF_MAX = 3.5f;
constexpr float MODE3_UQ_FF_STOP_SPEED = 2.0f; // mode3 内部目标/实际速度低于该值时不输出前馈
constexpr float MODE3_UQ_FF_BRAKE_ERR = 3.0f; // 明显超速/刹车时不输出前馈，单位同 velocity_error
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度 - 角度 P
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度 - 角度 I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度 - 角度 D
// mode3 低速/静止附近速度 P 降低，减少速度反馈噪声、齿槽和摩擦导致的来回拉扯。
constexpr bool MODE3_LOW_SPEED_P_SCALE_ENABLE = true;
constexpr float MODE3_LOW_SPEED_P_MIN_SCALE = 0.2f;
constexpr float MODE3_LOW_SPEED_P_LOW = 1.0f;
constexpr float MODE3_LOW_SPEED_P_FULL = 5.0f;
constexpr float DEFAULT_angle_angloop_p =
150.0f / 100 * 12; // 角度 速度 - 角度 P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度 - 角度 I
Robot_V2/sys/health.cpp
+2 -2
View File
@@ -115,8 +115,8 @@ void power_manage(void) {
sampler.update_vbus_voltage();
float vbus = sampler.get_vbus_voltage();
// 更新FOC的电源电压参数
FOC.update_power(last_vbus_slow);
// 更新FOC的电源电压参数：使用本周期刚采到的 vbus，避免上一拍电压滞后放大速度/电流耦合。
FOC.update_power(vbus);
last_vbus_slow = vbus;
// 如果快速检测已触发紧急关机，执行保存