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

feat(FOC): 添加电流卡尔曼滤波器并优化MIT控制模式

Browse Source 
- 优化MIT控制模式的角度和速度计算
- 调整多个PID控制器的参数配置
- 增强卡尔曼滤波器的数值稳定性
This commit is contained in:
cbb
2026-02-08 18:53:55 +08:00
parent 173fadadd2
commit 9689841148
10 changed files with 237 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+22 -16
View File
@@ -16,6 +16,7 @@ extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
// 初始变量及函数定义
#define _constrain(amt, low, high) \
@@ -29,8 +30,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
// 计算电气角度
float angle_el = fmod(shaft_angle * PP, 360.0f);
float I_alpha, I_beta;
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -41,8 +42,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float I_q, I_d;
I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q * 1000.0f -
KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
// 返回Iq值，乘以1000以转换为mA
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
}
/**
@@ -52,12 +53,15 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
*/
u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
vel_loop_Ki,
vel_loop_Kd); // 设置电机速度控制参数
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
100.0f,
0.2f); // 设置电机角度控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
7e-8); // 设置电机电流控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
current_loop_Ki,
current_loop_Kd); // 设置电机电流控制参数
vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
vel_angloop_Kd); // 设置电机速度控制参数
@@ -65,7 +69,9 @@ u8 FOC_Controller::init() {
ang_angloop_Ki,
ang_angloop_Kd); // 设置电机角度控制参数
CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
vel_MIT_Ki,
vel_MIT_Kd); // 设置电机角度控制参数
return 0;
}
@@ -262,7 +268,7 @@ void FOC_Controller::update_power(float voltage) {
voltage_error = false;
voltage_power_supply = voltage;
v_half = voltage * 0.5f;
v_safe = voltage * 1.05f;
v_safe = voltage * 1.1f;
} else {
voltage_error = true;
}
@@ -347,7 +353,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
}
/**
* @brief 带速度限制的角度闭环控制
* @brief MIT控制
*
* @param target_angle: 目标角度
* @param max_velocity: 最大速度限制 (单位: rad/s)
@@ -356,17 +362,17 @@ float FOC_Controller::angle_control_with_velocity_limit() {
float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = rt_angle - target_angle;
float angle_error = target_angle - rt_angle;
float target_velocity =
ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
CAN0_transmiter.testfloat_union.f = target_velocity;
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = target_velocity - rt_velocity;
float target_current =
vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
float velocity_error = rt_velocity - target_velocity;
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
CAN0_transmiter.testfloat_union.f = target_current;
// 第三层：电流环（内环）
// 计算当前实际电流
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
@@ -410,8 +416,8 @@ float FOC_Controller::current_control() {
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
current_loop.set_P(current_loop_p);
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压