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/sys/config.h
+44 -14
View File
@@ -12,9 +12,9 @@
// 版本号定义 - 可根据实际项目进行调整
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 1;
constexpr u16 SOFTWARE_VERSION_PATCH = 24;
constexpr u16 SOFTWARE_VERSION_BUILD = 1;
constexpr u16 SOFTWARE_VERSION_MINOR = 2;
constexpr u16 SOFTWARE_VERSION_PATCH = 8;
constexpr u16 SOFTWARE_VERSION_BUILD = 5;
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -57,9 +57,11 @@ constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.95f;
constexpr u16 ANGLESENS_OFFSET = +0;
constexpr float KE_FACTOR = 14.0f;
constexpr float KE_FACTOR = 0.0f;
// PID配置
constexpr float DEFAULT_current_loop_p = 6e-2f / 100 * 12; // 电流P
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-4; // 电流P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
@@ -67,21 +69,49 @@ 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;
constexpr float DEFAULT_vel_angloop_p = 0.22f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 12.0f; // 角度 速度-角度I
constexpr float vel_angloop_Kd = 6e-04f; // 角度 速度-角度D
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 8.0f; // 速度 速度-角度I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D
constexpr float DEFAULT_ang_angloop_p = 120.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04; // 角度 速度-角度D
constexpr float DEFAULT_ang_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04; // 角度 速度-角度D
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
constexpr float DEFAULT_vel_MIT_p = 25.0f / 100 * 12; // 速度 速度-角度P
constexpr float DEFAULT_vel_MIT_p = 9.0f / 100; // 速度 MIT P
constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
constexpr float vel_MIT_Ki = 3e-2f;
constexpr float vel_MIT_Kd = 5.355000e-05f;
constexpr float vel_MIT_Ki = 0.1f;
constexpr float vel_MIT_Kd = 0.01f;
// 角度卡尔曼滤波器参数
// 参数调节建议：
// - 增大Q值：增加对测量的信任，提高响应速度
// - 减小Q值：增加对预测的信任，提高平滑度
// - 增大R值：增加平滑度，减少噪声，但会降低响应速度
// - 减小R值：提高响应速度，但会增加噪声
constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q =
1e-3f; // 过程噪声协方差 - 调节此参数控制平滑度
constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R =
0.0062f; // 测量噪声协方差 - 调节此参数控制响应速度
// 速度卡尔曼滤波器参数
constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
1e-6f; // 过程噪声协方差 - 通常保持较小
constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
1.0f; // 测量噪声协方差 - 主要调节参数
// 电流卡尔曼滤波器参数
constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 1e-3f; // 增大测量噪声
// 常量定义
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 SIZE_U32 = sizeof(uint32_t);