From 99cc3979315b0499b02429a99ab462c04cb01b90 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 8 Nov 2025 02:15:14 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E4=BC=98=E5=8C=96SVPWM=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E5=B9=B6=E9=87=8D=E6=9E=84=E8=A7=92=E5=BA=A6=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 引入新的三值约束宏 `_constrain3` 用于SVPWM计算优化 - 修改 `setPhaseVoltage` 函数签名，移除冗余的角度参数，内部自动计算电气角度 - 调整PWM占空比计算方式，提高电压利用率和控制精度 - 移除旧的DEG2RAD和RAD2DEG常量定义，改由initial.h统一管理 - 在initial.h中新增减速比常量REDUCTION_RATIO，并修正拼写错误 - 为角度传感器类更新速度滤波器尺寸，提升速度估算稳定性 - 更新PID控制器时间片常量，简化dt计算逻辑，提高执行效率 - 切换角度闭环控制至新NPID版本，增强系统响应性能 - 修正CAN通信中的目标角度解析逻辑，确保指令正确应用 --- Robot_V2/Algorithm/DengFOC.cpp | 75 +++++++++++++++++++++------------- Robot_V2/Algorithm/DengFOC.h | 7 ++-- Robot_V2/Algorithm/pid.cpp | 23 ++++------- Robot_V2/device/Can.cpp | 2 +- Robot_V2/device/Timer.cpp | 4 +- Robot_V2/device/ang_sensor.cpp | 6 +-- Robot_V2/device/ang_sensor.h | 2 +- Robot_V2/sys/initial.h | 4 ++ 8 files changed, 70 insertions(+), 53 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 3d60a01..3602114 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -5,6 +5,7 @@ #include "adc.h" #include "ang_sensor.h" #include "gd32c10x_dbg.h" +#include "initial.h" #include "pid.h" #include "systick.h" #include @@ -13,8 +14,6 @@ #include constexpr double PI = 3.1415926535897932384626433832795f; -constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f; -constexpr float RAD2DEG = 360.0f / 6.28318530718f; constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f; constexpr float _1_SQRT3 = 0.57735026919f; constexpr float _2_SQRT3 = 1.15470053838f; @@ -34,6 +33,11 @@ CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); // 初始变量及函数定义 #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) + +// 三值约束比较函数，用于SVPWM算法 +#define _constrain3(a, b, c) \ + ((a) < (b) ? ((a) < (c) ? (a) : (c)) : ((b) < (c) ? (b) : (c))) + #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) @@ -59,14 +63,15 @@ inline float _electricalAngle() { FOC_Controlroller::FOC_Controlroller() { ; } /** - * @brief 设置电机输出 + * @brief 设置电机输出，使用优化的SVPWM算法 * * @param Uq: q轴电压 * @param Ud: d轴电压 * @param angle_el: 电机轴角度 */ -inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, - float angle_el) { +inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { + // 计算电气角度 + float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle); // 帕克逆变换 (直接计算，避免函数调用开销) const float sin_el = sin(angle_el); const float cos_el = cos(angle_el); @@ -79,15 +84,17 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half; // 计算占空比 - Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage); - Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage); - Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage); float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f); float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f); float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f); + // 设置PWM输出 + dc_a = dc_a * limit_voltage / voltage_power_supply; + dc_b = dc_b * limit_voltage / voltage_power_supply; + dc_c = dc_c * limit_voltage / voltage_power_supply; Set_3Pwm(dc_a, dc_b, dc_c); // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); } + /** * @brief 电机关机 * @@ -128,13 +135,10 @@ float FOC_Controlroller::velocity_open_loop() { shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f); - // 直接计算电气角度，避免函数调用开销 - float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle); - // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; - setPhaseVoltage(Uq, 0.0f, angle_el); + setPhaseVoltage(Uq, 0.0f); return limit_voltage; } @@ -157,19 +161,35 @@ float FOC_Controlroller::velocity_open_loop() { * @brief 角度闭环控制 * * @param target_angle: 目标角度 - * @return float: q轴电压 + * @return float: 当前角度 */ float FOC_Controlroller::angle_control(float target_angle) { shaft_angle = KTH7823.getRawAngle() * DEG2RAD; - static float Kp = 2.0f; - float Uq = - 1.0f - _constrain(Kp * (target_angle - DIR * shaft_angle) * RAD2DEG, - -voltage_power_supply, voltage_power_supply); - static float Ud = 0.0f; - // 直接计算电气角度，避免函数调用开销 - float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle); + // PID 控制计算 + float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR); + CAN0_transmiter.testfloat_union.f = + angle_loop(target_angle * ANGLE_FACTOR - shaft_angle); // 设置相电压 - setPhaseVoltage(Uq, 0.0f, angle_el); + setPhaseVoltage(Uq, 0.0f); + return shaft_angle; +} + +/** + * @brief 角度闭环控制 + * + * @param target_angle: 目标角度 + * @return float: 当前角度 + */ +float FOC_Controlroller::angle_control_NPID(float target_angle) { + shaft_angle = KTH7823.getRawAngle() * DEG2RAD; + float Kp = 20.0f; + // 控制力矩计算 + float Uq = + 1.0f - + _constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_angle), + -voltage_power_supply, voltage_power_supply); + // 设置相电压 + setPhaseVoltage(Uq, 0.0f); return shaft_angle; } @@ -177,16 +197,15 @@ float FOC_Controlroller::angle_control(float target_angle) { * @brief 速度闭环控制 * * @param target_velocity: 目标速度 - * @return float: q轴电压 + * @return float: 当前速度 */ float FOC_Controlroller::velocity_control(float target_velocity) { - // PID 控制计算 - float Uq = v_half - vel_loop(target_velocity - rt_velocity); - shaft_angle = KTH7823.getRawAngle() * DEG2RAD; - // 直接计算电气角度，避免函数调用开销 - float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle); + // PID 控制计算 + float Uq = vel_loop(rt_velocity - target_velocity); + + CAN0_transmiter.testfloat_union.f = Uq; // 设置相电压 - setPhaseVoltage(Uq, 0.0f, angle_el); + setPhaseVoltage(Uq, 0.0f); return rt_velocity; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index dcb059e..c1e2aa5 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -22,6 +22,7 @@ class FOC_Controlroller { float velocity_open_loop(); float angle_control(float target_angle); float velocity_control(float target_velocity); + float angle_control_NPID(float target_angle); void update_power(void); void DFOC_alignSensor(int _PP); @@ -38,10 +39,10 @@ class FOC_Controlroller { float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 PIDController vel_loop = - PIDController{0.8f, 3.0f, 0, 100000, voltage_power_supply}; - PIDController angle_loop = PIDController{2, 0, 0, 100000, 100}; + PIDController{1.5f, 1.0f, 0.0005, 100000, voltage_power_supply}; + PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 100000, voltage_power_supply}; - void setPhaseVoltage(float Uq, float Ud, float angle_el); + void setPhaseVoltage(float Uq, float Ud); }; float _normalizeAngle(float angle); diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 02e4ca5..ef0d02c 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -5,48 +5,41 @@ #include "Can.h" extern CAN_transmiter CAN0_transmiter; +constexpr float TIME_SLICE = 1e-4; PIDController::PIDController(float P, float I, float D, float ramp, float limit) : P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅 , limit(limit) // PID控制器输出限幅 , - error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) { - timestamp_prev = get_system_runtime_us(); -} + error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {} // PID 控制器函数 float PIDController::operator()(float error) { - // 计算两次循环中间的间隔时间 - unsigned long timestamp_now = get_system_runtime_us(); - float Ts = (timestamp_now - timestamp_prev) * 1e-6f; - if (Ts <= 0 || Ts > 0.5f) - Ts = 1e-3f; - // P环 float proportional = P * error; // Tustin 散点积分（I环） - float integral = integral_prev + I * Ts * 0.5f * (error + error_prev); + float integral = + integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); integral = _constrain(integral, -limit, limit); // D环（微分环节） - float derivative = D * (error - error_prev) / Ts; + float derivative = D * (error - error_prev) / TIME_SLICE; // 将P,I,D三环的计算值加起来 float output = proportional + integral + derivative; output = _constrain(output, -limit, limit); if (output_ramp > 0) { // 对PID的变化速率进行限制 - float output_rate = (output - output_prev) / Ts; + float output_rate = (output - output_prev) / TIME_SLICE; if (output_rate > output_ramp) - output = output_prev + output_ramp * Ts; + output = output_prev + output_ramp * TIME_SLICE; else if (output_rate < -output_ramp) - output = output_prev - output_ramp * Ts; + output = output_prev - output_ramp * TIME_SLICE; } // 保存值（为了下一次循环） integral_prev = integral; output_prev = output; error_prev = error; - timestamp_prev = timestamp_now; return output; } diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 8a51e0d..07bed66 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -678,7 +678,7 @@ void CAN_transmiter::communication_check() { } target_angle_union.f = unpack_bytes_to_float(receive_message.rx_data, 7).f; - FOC.target_angle = target_angle_union.f * ANGLE_FACTOR; + FOC.target_angle = target_angle_union.f; target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index a98e4a9..02189aa 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -167,7 +167,7 @@ void TIMER2_IRQHandler(void) { // 增加总计数器 (使用更高效的位运算代替取模运算) total_us100_counter++; - system_runtime_us += 100; + // system_runtime_us += 100; // 每100us执行一次的操作 sampler.update_current(); // 更新电流采样值 @@ -175,7 +175,7 @@ void TIMER2_IRQHandler(void) { FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); if (FOC.control_mode == 1) { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 + FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 } else if (FOC.control_mode == 2) { FOC.velocity_control(FOC.target_velocity);// 速度闭环控制 } else if (FOC.control_mode == 4) { diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index df63e62..487f443 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -5,6 +5,7 @@ #include #include #include +#include "initial.h" #define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) @@ -13,7 +14,6 @@ constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f; constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f; constexpr u16 DEFAULT_COMMAND = 0x0000; constexpr u16 FAULT_RECEIVE = 0xFFFF; -constexpr u16 REDDUCTION_RATIO = 25; constexpr float VELOCITY_SCALE = 1e4f; constexpr float ANGLE_WRAP_THRESHOLD = 270.0f; constexpr float ANGLE_WRAP_VALUE = 360.0f; @@ -259,7 +259,7 @@ float angle_sensor::update(void) { * @return 累积角度值 */ float angle_sensor::getAngle(void) const { - return m_cumulativeAngle / REDDUCTION_RATIO; + return m_cumulativeAngle / REDUCTION_RATIO; } /** @@ -273,7 +273,7 @@ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; } * @return 速度值(输出级) */ float angle_sensor::getVelocity(void) const { - return getRawVelocity() / REDDUCTION_RATIO; + return getRawVelocity() / REDUCTION_RATIO; } /** diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index f255ce3..f416bce 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -11,7 +11,7 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord); int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data, uint16_t *rx_data); -constexpr u16 VEL_FILTER_SIZE = 3; +constexpr u16 VEL_FILTER_SIZE = 4; class angle_sensor { public: diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 33a2536..5f7b390 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -8,6 +8,10 @@ extern "C" { #include #include "gd32c10x_eval.h" +constexpr u16 REDUCTION_RATIO = 25; +constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f; +constexpr float RAD2DEG = 360.0f / 6.28318530718f; + void rcu_config(void); static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现