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

feat(FOC): 添加齿槽转矩补偿功能

Browse Source 
- 实现齿槽转矩补偿算法，通过查表+线性插值方式计算补偿电压值
- 添加齿槽转矩校准功能，支持正转3次+反转3次的非阻塞校准状态机
- 在所有控制模式中集成齿槽转矩补偿，包括开环、角度、速度、电流和MIT模式
- 硬编码预校准的正反转补偿表数据
- 通过CAN命令支持运行时触发校准功能
- 添加相关配置参数和调试开关
This commit is contained in:
cbb
2026-05-19 01:26:42 +08:00
parent 9955132e0e
commit bec7c11b87
7 changed files with 322 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+265 -66
View File
@@ -7,8 +7,11 @@
#include "config.h"
#include "motor.h"
#include "pid.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <gd32c10x.h>
FOC_Controller FOC; // FOC控制初始化
@@ -19,6 +22,207 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
extern u16 adc_value[3]; // regular DMA缓冲区：MCU温度 / DRV温度 / VBUS
// 齿槽转矩补偿：查表 + 线性插值
// angle_el: 电角度（度）
// 返回: 补偿电压值 (V)，叠加到 Uq 上
inline float FOC_Controller::get_cogging_compensation(float angle_el) {
if (!cogging_comp_enabled) return 0.0f;
// 归一化到 [0, 360)
float angle = fmodf(angle_el, 360.0f);
if (angle < 0.0f) angle += 360.0f;
// 线性插值查表
float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE;
u16 idx0 = (u16)index_f;
u16 idx1 = (idx0 + 1) % COGGING_TABLE_SIZE;
float frac = index_f - (float)idx0;
// 根据转向选择补偿表（带滞回，避免零速附近跳变）
static float *last_table = cogging_table;
if (rt_velocity < -0.5f) last_table = cogging_table_rev;
else if (rt_velocity > 0.5f) last_table = cogging_table;
float *table = last_table;
float comp = table[idx0] * (1.0f - frac) + table[idx1] * frac;
return -comp * cogging_comp_scale; // mA→V: 补偿电流(mA) × 换算系数(V/mA)
}
// === 齿槽转矩校准（DEBUG，平时不启用）===
// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
#if FOC_DEBUG
// 齿槽转矩校准静态数据
float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
/**
* @brief 齿槽转矩校准 - 非阻塞状态机
* 正转3次 + 反转3次，分开存储补偿表。
* 只打印最终平均数据和 C Array。
*/
void FOC_Controller::calibrate_cogging_tick(void) {
constexpr u32 SETTLE_TICKS = 16000; // 800ms / 50us
constexpr u32 STOP_TICKS = 10000; // 500ms / 50us
constexpr u32 TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
constexpr float SLOW_VEL = 1.0f; // 开环速度 rad/s
constexpr u32 SAMPLE_DIV = 20; // 1ms / 50us
constexpr u8 TOTAL_RUNS = 6; // 3正+3反
switch (cog_calib_state) {
case CogCalibState::IDLE:
if (!cogging_calib_request) return;
cogging_calib_request = false;
cog_calib_run = 0;
cog_calib_saved_mode = control_mode;
cog_calib_saved_vel = target_velocity;
cogging_comp_enabled = false;
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
control_mode = 6;
target_velocity = SLOW_VEL;
cog_calib_settle_tick = 0;
cog_calib_state = CogCalibState::SETTLING;
return;
case CogCalibState::SETTLING:
cog_calib_settle_tick++;
if (cog_calib_settle_tick >= SETTLE_TICKS) {
cog_calib_state = CogCalibState::SAMPLING;
cog_calib_sample_count = 0;
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
memset(cog_calib_count, 0, sizeof(cog_calib_count));
}
return;
case CogCalibState::SAMPLING:
// 采样分频
static u32 sample_div = 0;
sample_div++;
if (sample_div < SAMPLE_DIV) return;
sample_div = 0;
{
float real_angle = KTH7823.getcurrent_sensorangle();
float angle_el = fmodf(real_angle * PP + KTH7823.calibration_angle, 360.0f);
if (angle_el < 0.0f) angle_el += 360.0f;
float iq = rt_current;
if (fabsf(iq) < 5000.0f) {
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
cog_calib_accum[idx] += iq;
cog_calib_count[idx]++;
}
cog_calib_sample_count++;
}
if (cog_calib_sample_count >= TARGET_SAMPLES) {
target_velocity = 0;
cog_calib_state = CogCalibState::STOPPING;
cog_calib_stop_tick = 0;
}
return;
case CogCalibState::STOPPING:
cog_calib_stop_tick++;
if (cog_calib_stop_tick >= STOP_TICKS) {
// 处理本轮数据：去直流
float dc = 0;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] =
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
dc += cog_calib_run_data[cog_calib_run][i];
}
dc /= (float)COGGING_TABLE_SIZE;
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cog_calib_run_data[cog_calib_run][i] -= dc;
}
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
cog_calib_run + 1, TOTAL_RUNS, dc);
cog_calib_run++;
if (cog_calib_run < TOTAL_RUNS) {
// run 0~2 正转，run 3~5 反转
control_mode = 6;
target_velocity = (cog_calib_run < 3) ? SLOW_VEL : -SLOW_VEL;
cog_calib_state = CogCalibState::SETTLING;
cog_calib_settle_tick = 0;
} else {
cog_calib_state = CogCalibState::DONE;
}
}
return;
case CogCalibState::DONE:
// 正转平均 (run 0~2)
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cogging_table[i] = (cog_calib_run_data[0][i] +
cog_calib_run_data[1][i] +
cog_calib_run_data[2][i]) / 3.0f;
}
// 反转平均 (run 3~5)
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
cog_calib_run_data[4][i] +
cog_calib_run_data[5][i]) / 3.0f;
}
cogging_comp_enabled = true;
control_mode = cog_calib_saved_mode;
target_velocity = cog_calib_saved_vel;
// 打印正转平均
printf("\r\n[COG] === FWD Average ===\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table[i]);
fwdgt_counter_reload();
}
// 打印反转平均
printf("\r\n[COG] === REV Average ===\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table_rev[i]);
fwdgt_counter_reload();
}
// 打印正转 C Array
printf("\r\n[COG] --- FWD C Array ---\r\n");
printf("static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
if (i % 8 == 0) printf(" ");
printf("%.2ff", cogging_table[i]);
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
fwdgt_counter_reload();
}
printf("};\r\n");
// 打印反转 C Array
printf("\r\n[COG] --- REV C Array ---\r\n");
printf("static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {\r\n");
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
if (i % 8 == 0) printf(" ");
printf("%.2ff", cogging_table_rev[i]);
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
fwdgt_counter_reload();
}
printf("};\r\n");
printf("\r\n[COG] === Done. ===\r\n");
cog_calib_state = CogCalibState::IDLE;
return;
}
}
#endif // FOC_DEBUG
// 初始变量及函数定义
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
@@ -92,6 +296,47 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化
// 齿槽转矩补偿表（硬编码，3正+3反平均，用真实角度算Iq）
static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {
29.28f, 28.84f, 28.26f, 28.83f, 29.79f, 30.26f, 29.52f, 28.77f,
28.23f, 27.68f, 26.65f, 25.19f, 23.92f, 22.59f, 19.61f, 16.77f,
13.32f, 10.40f, 7.64f, 3.51f, -0.83f, -4.96f, -8.19f, -11.11f,
-13.40f, -14.39f, -15.69f, -15.99f, -16.39f, -15.50f, -14.76f, -14.21f,
-13.76f, -13.88f, -13.22f, -11.18f, -10.65f, -10.54f, -10.62f, -10.23f,
-9.52f, -8.19f, -6.31f, -4.14f, -1.94f, -0.25f, 2.06f, 4.08f,
6.05f, 8.30f, 9.74f, 11.38f, 13.32f, 15.65f, 17.55f, 20.22f,
21.72f, 22.28f, 23.30f, 24.78f, 23.53f, 23.35f, 23.00f, 21.42f,
20.35f, 20.57f, 20.07f, 18.18f, 17.04f, 16.03f, 14.81f, 13.71f,
12.47f, 10.93f, 9.38f, 6.53f, 4.63f, 3.37f, 1.29f, -1.28f,
-5.08f, -11.11f, -16.35f, -21.00f, -24.61f, -28.30f, -31.11f, -33.48f,
-35.25f, -36.54f, -38.01f, -38.69f, -39.27f, -39.52f, -38.18f, -36.55f,
-34.96f, -33.14f, -31.92f, -31.45f, -31.32f, -30.25f, -29.60f, -28.72f,
-27.51f, -25.30f, -22.66f, -20.87f, -18.19f, -16.10f, -13.25f, -10.81f,
-8.00f, -4.67f, -2.28f, 0.87f, 3.83f, 7.25f, 10.56f, 13.70f,
17.70f, 19.90f, 22.14f, 23.62f, 24.53f, 26.03f, 26.96f, 27.95f,
};
static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {
22.45f, 24.95f, 26.05f, 26.01f, 26.11f, 25.87f, 26.20f, 26.23f,
26.19f, 25.87f, 25.97f, 26.09f, 24.77f, 24.22f, 23.71f, 22.10f,
19.84f, 17.60f, 15.36f, 12.99f, 9.54f, 5.35f, 1.71f, -1.94f,
-4.15f, -6.16f, -7.93f, -9.10f, -10.01f, -10.25f, -10.85f, -11.74f,
-11.98f, -10.78f, -9.65f, -8.89f, -8.14f, -6.85f, -5.94f, -5.37f,
-4.28f, -3.38f, -3.36f, -3.40f, -2.81f, -2.36f, -1.04f, -0.18f,
1.08f, 2.15f, 4.19f, 5.51f, 7.33f, 8.64f, 10.46f, 11.89f,
13.42f, 14.83f, 17.11f, 19.12f, 20.76f, 21.88f, 22.35f, 22.86f,
22.73f, 22.13f, 21.10f, 20.53f, 18.65f, 17.09f, 15.47f, 14.76f,
14.02f, 12.94f, 11.98f, 10.29f, 8.11f, 5.83f, 4.23f, 2.42f,
1.03f, -1.80f, -4.01f, -7.93f, -11.79f, -15.90f, -20.22f, -23.04f,
-25.66f, -28.80f, -30.45f, -32.11f, -33.17f, -33.97f, -34.68f, -34.26f,
-34.18f, -33.78f, -32.47f, -31.11f, -29.40f, -27.89f, -27.05f, -26.72f,
-25.59f, -24.86f, -23.61f, -22.97f, -22.89f, -21.81f, -20.78f, -19.23f,
-17.74f, -15.09f, -13.18f, -10.51f, -8.18f, -5.89f, -3.35f, -0.02f,
2.02f, 3.87f, 6.37f, 9.11f, 12.33f, 14.63f, 17.12f, 19.12f,
};
memcpy(cogging_table, COGGING_TABLE_FWD, sizeof(COGGING_TABLE_FWD));
memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
cogging_comp_enabled = true;
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
@@ -109,50 +354,6 @@ u8 FOC_Controller::init() {
*/
FOC_Controller::FOC_Controller() { ; }
// /**
// * @brief 设置电机输出
// *
// * @param Uq: q轴电压
// * @param Ud: d轴电压
// * @param angle_el: 电机轴角度
// */
// inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
// // 计算电气角度
// float angle_el = fmod(shaft_angle * PP, 360.0f);
// // 帕克逆变换 (直接计算，避免函数调用开销)
// float sin_el, cos_el;
// arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
// float Ualpha = Ud * cos_el - Uq * sin_el;
// float Ubeta = Ud * sin_el + Uq * cos_el;
// // 克拉克逆变换 (使用预计算常量优化)
// Ua = Ualpha + v_half;
// Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
// Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
// if (Ua > v_safe)
// Ua = v_safe;
// else if (Ua < -v_safe)
// Ua = -v_safe;
// if (Ub > v_safe)
// Ub = v_safe;
// else if (Ub < -v_safe)
// Ub = -v_safe;
// if (Uc > v_safe)
// Uc = v_safe;
// else if (Uc < -v_safe)
// Uc = -v_safe;
// // 设置PWM输出
// float voltage_exchange = limit_voltage / v_safe;
// duty_a = Ua / voltage_power_supply * voltage_exchange;
// duty_b = Ub / voltage_power_supply * voltage_exchange;
// duty_c = Uc / voltage_power_supply * voltage_exchange;
// Set_3Pwm(duty_a, duty_b, duty_c);
// }
/**
* @brief 使用SVPWM算法设置电机输出
*
@@ -418,7 +619,9 @@ float FOC_Controller::velocity_open_loop() {
// 因为在开环控制中，Ud 始终为 0，只控制 Uq
const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
2.0f, voltage_power_supply * 0.5f);
setSVPWM_PhaseVoltage(Uq, 0.0f);
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
return limit_voltage;
}
@@ -436,8 +639,10 @@ float FOC_Controller::angle_control() {
shaft_angle = KTH7823.getcurrent_sensorangle();
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
return shaft_angle;
}
@@ -465,8 +670,10 @@ float FOC_Controller::velocity_control() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
@@ -487,8 +694,10 @@ float FOC_Controller::current_control() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
return 1;
}
@@ -529,25 +738,17 @@ float FOC_Controller::angle_control_with_velocity_limit() {
last_velocity_error = velocity_error;
float Uq = vel_inVelAngloop(velocity_error)* DIR;
CAN0_transmiter.testfloat_union.f = Uq;
// // 实现力度控制：如果电流超过目标电流，则限制输出
// if (abs(rt_current) > abs(target_current)) {
// // 当实际电流超过目标电流时，减小输出以限制电流
// // 可以采用比例限制或直接限制输出电压的方式
// float current_ratio = abs(target_current) / abs(rt_current);
// if (current_ratio < 1.0f) {
// Uq *= current_ratio; // 按比例缩小输出电压
// }
// }
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 获取当前实际电流值
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
return shaft_angle;
}
@@ -561,14 +762,12 @@ float FOC_Controller::angle_control_with_velocity_limit() {
*/
float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = target_angle - rt_angle;
ang_inMITloop.set_P(ang_MIT_p);
float target_velocity =
ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射
ang_inMITloop(angle_error);
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = (target_velocity - rt_velocity)* DIR;
// 目标速度变化检测：target 大幅变化时清积分
@@ -582,11 +781,9 @@ float FOC_Controller::MIT_Mode() {
float target_current = vel_inMITloop(velocity_error);
// 第三层：电流环（内环）
// 计算当前实际电流
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 通过电流环PID控制器计算输出电压
float current_error = target_current - rt_current;
current_inMITloop.set_P(cur_MIT_p);
float Uq = current_inMITloop(current_error);
@@ -594,8 +791,10 @@ float FOC_Controller::MIT_Mode() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
return shaft_angle;
}