Skip to content

Dev #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
llLeo306 merged 2 commits into from
May 11, 2026
Merged

Dev #21

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
57a235e
功率控制适配履带
Rui-Xiaoyu May 11, 2026
1174692
Merge branch 'feat/track-priority-power' into dev
Rui-Xiaoyu May 11, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
169 changes: 142 additions & 27 deletions PowerControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand All @@ -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_) {
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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_; /* 底盘静态功耗 */

Expand All @@ -387,15 +501,16 @@ 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电机数目 */

float kt_3508_ = 1.99688994e-6f;
float k1_3508_ = 0;
float k2_3508_ = 0;
Eigen::Matrix<float, 2, 1> samples_3508_;
Eigen::Matrix<float, 2, 1> params_3508_;
Eigen::Matrix<float, 2, 1> samples_3508_; /* RLS 输入: [i^2_sum, rpm^2_sum] */
Eigen::Matrix<float, 2, 1> params_3508_; /* RLS 输出: [k1, k2] */

float output_current_3508_[MAX_MOTOR_COUNT] = {};
float rotorspeed_rpm_3508_[MAX_MOTOR_COUNT] = {};
Expand Down
Loading