From 57a235e06dc24fc9302ea3a72618c1054391d4e3 Mon Sep 17 00:00:00 2001 From: Lu-Xiaoyu <728171597@qq.com> Date: Mon, 11 May 2026 23:31:46 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=9F=E7=8E=87=E6=8E=A7=E5=88=B6=E9=80=82?= =?UTF-8?q?=E9=85=8D=E5=B1=A5=E5=B8=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PowerControl.hpp | 169 +++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 142 insertions(+), 27 deletions(-) diff --git a/PowerControl.hpp b/PowerControl.hpp index f7a6a2b..eaeda4a 100644 --- a/PowerControl.hpp +++ b/PowerControl.hpp @@ -30,8 +30,8 @@ depends: [] #define POP_POWERDISTRIBUTION 15 #define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f /* 底盘限功率余量 */ -/** - * @brief 计算单个电机模型预测功率 (不含静态损耗) +/** + * @brief 计算单个电机模型预测功率 (不含静态损耗) */ inline float calculate_motor_model_power(float current, float rpm, float kt, float k1, float k2) { @@ -81,6 +81,13 @@ struct PowerControlData { class PowerControl : public LibXR::Application { public: static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT; + /* 3508 组分配偏置: 先保底, 再分剩余功率 */ + struct AllocationBias3508 { + bool enabled = false; + float reserve_fraction = 0.0f; + float reserve_weight[MAX_MOTOR_COUNT] = {}; + float allocation_weight_scale[MAX_MOTOR_COUNT] = {}; + }; PowerControl(LibXR::HardwareContainer& hw, LibXR::ApplicationManager& app, SuperPower* super_power, bool is_helm = false, @@ -128,9 +135,24 @@ class PowerControl : public LibXR::Application { } } + void SetAllocationBias3508(const AllocationBias3508& bias) { + LibXR::Mutex::LockGuard lock(mutex_); + allocation_bias_3508_.enabled = bias.enabled; + allocation_bias_3508_.reserve_fraction = + std::clamp(bias.reserve_fraction, 0.0f, 1.0f); + for (int i = 0; i < MAX_MOTOR_COUNT; i++) { + allocation_bias_3508_.reserve_weight[i] = + std::max(0.0f, bias.reserve_weight[i]); + const float WEIGHT_SCALE = bias.allocation_weight_scale[i]; + /* 未配置时保持 1.0 */ + allocation_bias_3508_.allocation_weight_scale[i] = + WEIGHT_SCALE > 0.0f ? WEIGHT_SCALE : 1.0f; + } + } + void CalculatePowerControlParam() { LibXR::Mutex::LockGuard lock(mutex_); - /*从超电得到底盘的真实功率*/ + /* 用实测功率修正 3508 功率模型参数 */ measured_power_ = superpower_->GetChassisPower(); samples_3508_(0, 0) = 0; samples_3508_(1, 0) = 0; @@ -145,7 +167,6 @@ class PowerControl : public LibXR::Application { kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i]; } - /*计算残差*/ float residual = measured_power_ - mechanical_power - k3_chassis_; if (is_helm_) { @@ -193,7 +214,7 @@ class PowerControl : public LibXR::Application { float available_power = max_power - k3_chassis_ - CHASSIS_POWER_LIMIT_MARGIN_W; - /* 计算每个电机功率, 并累计正功电机的误差之和 */ + /* 正功率参与分配, 负功率等效增加可用功率 */ sum_error_ = 0.0f; for (int i = 0; i < motor_count_3508_; i++) { motor_power_3508_[i] = calculate_motor_model_power( @@ -224,26 +245,119 @@ class PowerControl : public LibXR::Application { error_confidence_ = 0.0f; } - for (int i = 0; i < motor_count_3508_; i++) { - if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { - /* 误差权重: 按速度跟踪误差大小分配 */ - float power_weight_error = - (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) : 0.0f; - /* 比例权重: 按功率需求比例分配 */ - float power_weight_prop = - motor_power_3508_[i] / required_power_3508_sum; - /* 混合权重 */ - float power_weight = error_confidence_ * power_weight_error + - (1.0f - error_confidence_) * power_weight_prop; - float power_quota = available_power * power_weight; + if (!allocation_bias_3508_.enabled) { + /* 默认按误差/需求混合权重分配 */ + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f) { + /* 误差权重: 按速度跟踪误差大小分配 */ + float power_weight_error = (sum_error_ > 1e-6f) + ? (speed_error_3508_[i] / sum_error_) + : 0.0f; + /* 比例权重: 按功率需求比例分配 */ + float power_weight_prop = + motor_power_3508_[i] / required_power_3508_sum; + /* 混合权重 */ + float power_weight = error_confidence_ * power_weight_error + + (1.0f - error_confidence_) * power_weight_prop; + float power_quota = available_power * power_weight; + + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } + } + } else { + /* 偏置路径: 先保底, 再分剩余功率 */ + float reserve_power_3508[MAX_MOTOR_COUNT] = {}; + float residual_power_3508[MAX_MOTOR_COUNT] = {}; + float weighted_residual_3508[MAX_MOTOR_COUNT] = {}; + const float RESERVE_POWER_POOL = std::max(0.0f, available_power) * + allocation_bias_3508_.reserve_fraction; + float reserve_weight_sum = 0.0f; + float reserved_power_sum = 0.0f; + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0.0f) { + reserve_weight_sum += allocation_bias_3508_.reserve_weight[i]; + } + } - powercontrol_data_.new_output_current_3508[i] = - solve_current_for_power(power_quota, rotorspeed_rpm_3508_[i], - kt_3508_, k1_3508_, k2_3508_, - output_current_3508_[i]); - } else { - powercontrol_data_.new_output_current_3508[i] = - output_current_3508_[i]; + if (RESERVE_POWER_POOL > 1e-6f && reserve_weight_sum > 1e-6f) { + /* 保底池只分给正功率电机 */ + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] <= 0.0f) { + continue; + } + const float RESERVE_QUOTA = + RESERVE_POWER_POOL * allocation_bias_3508_.reserve_weight[i] / + reserve_weight_sum; + reserve_power_3508[i] = + std::min(motor_power_3508_[i], RESERVE_QUOTA); + reserved_power_sum += reserve_power_3508[i]; + } + } + + /* 剩余功率再走普通分配 */ + const float REMAINING_AVAILABLE_POWER = + std::max(0.0f, available_power - reserved_power_sum); + float residual_required_power_sum = 0.0f; + for (int i = 0; i < motor_count_3508_; i++) { + residual_power_3508[i] = + std::max(0.0f, motor_power_3508_[i] - reserve_power_3508[i]); + residual_required_power_sum += residual_power_3508[i]; + } + + /* 剩余功率权重仍基于误差/需求, 再叠加偏置缩放 */ + float weighted_residual_sum = 0.0f; + if (residual_required_power_sum > 1e-6f) { + for (int i = 0; i < motor_count_3508_; i++) { + if (residual_power_3508[i] <= 0.0f) { + continue; + } + const float POWER_WEIGHT_ERROR = + (sum_error_ > 1e-6f) ? (speed_error_3508_[i] / sum_error_) + : 0.0f; + const float POWER_WEIGHT_PROP = + residual_power_3508[i] / residual_required_power_sum; + const float POWER_WEIGHT = + error_confidence_ * POWER_WEIGHT_ERROR + + (1.0f - error_confidence_) * POWER_WEIGHT_PROP; + weighted_residual_3508[i] = + POWER_WEIGHT * allocation_bias_3508_.allocation_weight_scale[i]; + weighted_residual_sum += weighted_residual_3508[i]; + } + } + + for (int i = 0; i < motor_count_3508_; i++) { + if (motor_power_3508_[i] > 0.0f && required_power_3508_sum > 1e-6f) { + float residual_quota = 0.0f; + if (residual_power_3508[i] > 0.0f && + REMAINING_AVAILABLE_POWER > 1e-6f) { + if (weighted_residual_sum > 1e-6f) { + residual_quota = REMAINING_AVAILABLE_POWER * + weighted_residual_3508[i] / + weighted_residual_sum; + } else if (residual_required_power_sum > 1e-6f) { + residual_quota = REMAINING_AVAILABLE_POWER * + residual_power_3508[i] / + residual_required_power_sum; + } + } + /* 单路电机的最终配额 = 保底配额 + 剩余池配额, 但不超过自身请求 */ + const float POWER_QUOTA = std::min( + motor_power_3508_[i], reserve_power_3508[i] + residual_quota); + powercontrol_data_.new_output_current_3508[i] = + solve_current_for_power(POWER_QUOTA, rotorspeed_rpm_3508_[i], + kt_3508_, k1_3508_, k2_3508_, + output_current_3508_[i]); + } else { + powercontrol_data_.new_output_current_3508[i] = + output_current_3508_[i]; + } } } } else { @@ -378,7 +492,7 @@ class PowerControl : public LibXR::Application { LibXR::Mutex mutex_; SuperPower* superpower_; bool is_helm_; - RLS<2> rls_; + RLS<2> rls_; /* 在线辨识 3508 二次损耗模型参数 */ PowerControlData powercontrol_data_; float k3_chassis_; /* 底盘静态功耗 */ @@ -387,6 +501,7 @@ class PowerControl : public LibXR::Application { float speed_error_3508_[MAX_MOTOR_COUNT] = {}; /* 3508速度跟踪误差 */ float speed_error_6020_[MAX_MOTOR_COUNT] = {}; /* 6020速度跟踪误差 */ + AllocationBias3508 allocation_bias_3508_{}; /* 3508 组动态分配偏置 */ int motor_count_3508_; /* 3508电机数目 */ int motor_count_6020_; /* 6020电机数目 */ @@ -394,8 +509,8 @@ class PowerControl : public LibXR::Application { float kt_3508_ = 1.99688994e-6f; float k1_3508_ = 0; float k2_3508_ = 0; - Eigen::Matrix samples_3508_; - Eigen::Matrix params_3508_; + Eigen::Matrix samples_3508_; /* RLS 输入: [i^2_sum, rpm^2_sum] */ + Eigen::Matrix params_3508_; /* RLS 输出: [k1, k2] */ float output_current_3508_[MAX_MOTOR_COUNT] = {}; float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {};