Repository navigation
Expand file tree
/
Copy pathPowerControl.hpp
More file actions
737 lines (671 loc) · 26.2 KB
/
Copy pathPowerControl.hpp
File metadata and controls
737 lines (671 loc) · 26.2 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
#pragma once
// clang-format off
/* === MODULE MANIFEST V2 ===
module_description: 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮底盘 / Chassis power control Module that redistributes the motor output currents under a power limit, for omni-wheel, mecanum and steering-wheel chassis
depends:
- id: QDU-Robomaster/SuperPower
ref: same-or-dev
=== END MANIFEST === */
// clang-format on
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include "RLS.hpp"
#include "SuperPower.hpp"
#include "message.hpp"
#include "thread.hpp"
/// 速度误差总和高于该值时,功率全部按误差分配
/// Above this speed error sum, power is allocated entirely by error
#define ERROR_POWERDISTRIBUTION_SET 20
/// 速度误差总和低于该值时,功率全部按需求比例分配
/// Below this speed error sum, power is allocated entirely by demand ratio
#define POP_POWERDISTRIBUTION 15
/// 底盘限功率余量 (W)
/// Chassis power limit margin (W)
#define CHASSIS_POWER_LIMIT_MARGIN_W 4.0f
/**
* @brief 计算单个电机的模型预测功率,不含静态损耗。
* Compute the model-predicted power of one motor, excluding the static loss.
*
* @param current 电机电流。
* Motor current.
* @param rpm 转子转速 (rpm)。
* Rotor speed (rpm).
* @param kt 机械功率系数。
* Mechanical power coefficient.
* @param k1 电流平方项系数。
* Coefficient of the squared current term.
* @param k2 转速平方项系数。
* Coefficient of the squared speed term.
* @return 预测功率 (W)。
* Predicted power (W).
*/
inline float calculate_motor_model_power(float current, float rpm, float kt, float k1,
float k2)
{
return (kt * current * rpm) + (k1 * current * current) + (k2 * rpm * rpm);
}
/**
* @brief 由目标功率反解电流,结果限幅到 ±16384。
* Solve the current from a target power, clamped to ±16384.
*
* @param target_power 目标功率 (W)。
* Target power (W).
* @param rpm 转子转速 (rpm)。
* Rotor speed (rpm).
* @param kt 机械功率系数。
* Mechanical power coefficient.
* @param k1 电流平方项系数。
* Coefficient of the squared current term.
* @param k2 转速平方项系数。
* Coefficient of the squared speed term.
* @param original_current 原始期望电流,其符号决定取二次方程的哪个根。
* Original desired current, whose sign selects the root of the
* quadratic equation.
* @return 反解得到的电流。
* Solved current.
*/
inline float solve_current_for_power(float target_power, float rpm, float kt, float k1,
float k2, float original_current)
{
float a = k1;
float b = kt * rpm;
float c = k2 * rpm * rpm - target_power;
float delta = b * b - 4.0f * a * c;
float x3 = -b / (2.0f * a);
float final_current = 0;
if (delta < 1e-9f)
{
final_current = x3;
}
else
{
float sqrt_delta = sqrtf(delta);
float x1 = (-b + sqrt_delta) / (2.0f * a);
float x2 = (-b - sqrt_delta) / (2.0f * a);
if (original_current >= 0)
{
final_current = x1;
}
else
{
final_current = x2;
}
}
return std::clamp(final_current, -16384.0f, 16384.0f);
}
/// 最大电机数量
/// Maximum number of motors
static constexpr int POWER_CONTROL_MAX_MOTOR_COUNT = 6;
/**
* @brief 功率限制后的输出。
* Output after the power limit.
*/
struct PowerControlData
{
/// 3508 功率限制后的电流
/// Currents of the 3508 motors after the power limit
float new_output_current_3508[POWER_CONTROL_MAX_MOTOR_COUNT] = {};
/// 6020 功率限制后的电流
/// Currents of the 6020 motors after the power limit
float new_output_current_6020[POWER_CONTROL_MAX_MOTOR_COUNT] = {};
/// 是否触发了功率限制
/// Whether the power limit was applied
bool is_power_limited = false;
};
/**
* @brief 底盘功率控制模块:按功率上限重新分配各电机的输出电流,支持全向轮、麦轮与舵轮
* 底盘。
* Chassis power control Module that redistributes the motor output currents
* under a power limit, for omni-wheel, mecanum and steering-wheel chassis.
*/
class PowerControl
{
public:
/// 最大电机数量
/// Maximum number of motors
static constexpr int MAX_MOTOR_COUNT = POWER_CONTROL_MAX_MOTOR_COUNT;
/**
* @brief 3508 组的分配偏置:先按保底池分配,再分剩余功率。
* Allocation bias of the 3508 group: distribute a reserve pool first, then
* the remaining power.
*/
struct AllocationBias3508
{
/// 是否启用
/// Whether enabled
bool enabled = false;
/// 可用功率中作为保底池的比例,范围 [0, 1]
/// Fraction of the available power used as the reserve pool, in [0, 1]
float reserve_fraction = 0.0f;
/// 保底池分配权重
/// Reserve pool allocation weights
float reserve_weight[MAX_MOTOR_COUNT] = {};
/// 剩余功率权重的缩放,未配置或不大于 0 时为 1
/// Scale of the remaining-power weights, 1 when unset or not greater than 0
float allocation_weight_scale[MAX_MOTOR_COUNT] = {};
};
/**
* @brief 构造 PowerControl。
* Construct PowerControl.
*
* @param super_power SuperPower 实例,提供实测底盘功率与超级电容在线状态。
* SuperPower instance that provides the measured chassis power
* and the supercapacitor online state.
* @param is_helm true 为舵轮底盘,同时限制 6020;false 为全向轮或麦轮底盘。
* True for a steering-wheel chassis, which also limits the 6020; false
* for an omni-wheel or mecanum chassis.
* @param chassis_static_power_loss 底盘静态功耗 (W)。
* Chassis static power loss (W).
* @param motor_count_3508 3508 电机数量,最大 `MAX_MOTOR_COUNT`。
* Number of 3508 motors, at most `MAX_MOTOR_COUNT`.
* @param motor_count_6020 6020 电机数量,最大 `MAX_MOTOR_COUNT`。
* Number of 6020 motors, at most `MAX_MOTOR_COUNT`.
*/
PowerControl(SuperPower& super_power, bool is_helm = false,
float chassis_static_power_loss = 0.0f, int motor_count_3508 = 4,
int motor_count_6020 = 4)
: superpower_(&super_power),
is_helm_(is_helm),
rls_(1e-5f, 0.99999f),
k3_chassis_(chassis_static_power_loss),
motor_count_3508_(motor_count_3508 > MAX_MOTOR_COUNT ? MAX_MOTOR_COUNT
: motor_count_3508),
motor_count_6020_(motor_count_6020 > MAX_MOTOR_COUNT ? MAX_MOTOR_COUNT
: motor_count_6020)
{
params_3508_(0, 0) = 2.0e-07f;
params_3508_(1, 0) = 3.0e-07f;
k1_3508_ = params_3508_(0, 0);
k2_3508_ = params_3508_(1, 0);
}
/**
* @brief 写入 3508 电机本周期的期望电流、转子转速和可选的速度跟踪误差。
* Write the desired current, rotor speed and optional speed tracking error of
* the 3508 motors for this cycle.
*
* @param output_current 期望电流数组,长度为 3508 电机数量。
* Desired current array with one entry per 3508 motor.
* @param rotorspeed_rpm 转子转速数组 (rpm),长度为 3508 电机数量。
* Rotor speed array (rpm) with one entry per 3508 motor.
* @param speed_error 速度跟踪误差数组,取绝对值使用;为空时保持上一次的值。
* Speed tracking error array, used as absolute values; the previous
* values are kept when null.
*/
void SetMotorData3508(float* output_current, float* rotorspeed_rpm,
float* speed_error = nullptr)
{
LibXR::Mutex::LockGuard lock(mutex_);
for (int i = 0; i < motor_count_3508_; i++)
{
output_current_3508_[i] = output_current[i];
rotorspeed_rpm_3508_[i] = rotorspeed_rpm[i];
if (speed_error)
{
speed_error_3508_[i] = fabsf(speed_error[i]);
}
}
}
/**
* @brief 写入 6020 电机本周期的期望电流、转子转速和可选的速度跟踪误差。
* Write the desired current, rotor speed and optional speed tracking error of
* the 6020 motors for this cycle.
*
* @param output_current 期望电流数组,长度为 6020 电机数量。
* Desired current array with one entry per 6020 motor.
* @param rotorspeed_rpm 转子转速数组 (rpm),长度为 6020 电机数量。
* Rotor speed array (rpm) with one entry per 6020 motor.
* @param speed_error 速度跟踪误差数组,取绝对值使用;为空时保持上一次的值。
* Speed tracking error array, used as absolute values; the previous
* values are kept when null.
*/
void SetMotorData6020(float* output_current, float* rotorspeed_rpm,
float* speed_error = nullptr)
{
LibXR::Mutex::LockGuard lock(mutex_);
for (int i = 0; i < motor_count_6020_; i++)
{
output_current_6020_[i] = output_current[i];
rotorspeed_rpm_6020_[i] = rotorspeed_rpm[i];
if (speed_error)
{
speed_error_6020_[i] = fabsf(speed_error[i]);
}
}
}
/**
* @brief 设置 3508 组的分配偏置,仅全向轮与麦轮路径使用。
* Set the allocation bias of the 3508 group, used by the omni-wheel and
* mecanum path.
*
* @param bias 分配偏置。
* Allocation bias.
*/
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;
}
}
/**
* @brief 读取实测底盘功率,并用 RLS 在线辨识 3508 功率模型的 `k1`、`k2`。
* Read the measured chassis power and identify `k1` and `k2` of the 3508 power
* model online with RLS.
*/
void CalculatePowerControlParam()
{
LibXR::Mutex::LockGuard lock(mutex_);
/* 用实测功率修正 3508 功率模型参数 */
measured_power_ = superpower_->GetChassisPower();
samples_3508_(0, 0) = 0;
samples_3508_(1, 0) = 0;
bool online = superpower_->IsOnline();
float mechanical_power = 0;
for (int i = 0; i < motor_count_3508_; i++)
{
samples_3508_(0, 0) += output_current_3508_[i] * output_current_3508_[i];
samples_3508_(1, 0) += rotorspeed_rpm_3508_[i] * rotorspeed_rpm_3508_[i];
mechanical_power += kt_3508_ * output_current_3508_[i] * rotorspeed_rpm_3508_[i];
}
float residual = measured_power_ - mechanical_power - k3_chassis_;
if (is_helm_)
{
float power_6020 = 0;
for (int i = 0; i < motor_count_6020_; i++)
{
power_6020 +=
calculate_motor_model_power(output_current_6020_[i], rotorspeed_rpm_6020_[i],
kt_6020_, k1_6020_, k2_6020_);
}
residual -= power_6020;
}
if (residual > 0 && online && measured_power_ > 5.0f)
{
params_3508_ = rls_.Update(samples_3508_, residual);
k1_3508_ = static_cast<float>(fmax(params_3508_(0, 0), 1.0e-07f));
k2_3508_ = static_cast<float>(fmax(params_3508_(1, 0), 1.0e-07f));
}
}
/**
* @brief 按功率上限计算限制后的输出电流,结果由 `GetPowerControlData()` 取回。
* Compute the limited output currents under the power limit; the result is
* fetched with `GetPowerControlData()`.
*
* @param max_power 底盘功率上限 (W)。
* Chassis power limit (W).
*/
void OutputLimit(float max_power)
{
LibXR::Mutex::LockGuard lock(mutex_);
if (is_helm_)
{
OutputLimitHelm(max_power);
}
else
{
OutputLimitOmni(max_power);
}
}
/**
* @brief 取回功率限制后的输出。
* Fetch the output after the power limit.
*
* @return 功率限制后的输出。
* Output after the power limit.
*/
PowerControlData GetPowerControlData()
{
LibXR::Mutex::LockGuard lock(mutex_);
return powercontrol_data_;
}
/**
* @brief 获取最近一次 `CalculatePowerControlParam()` 读到的实测功率。
* Get the measured power read by the latest `CalculatePowerControlParam()`.
*
* @return 实测功率 (W)。
* Measured power (W).
*/
float GetMeasuredPower() const { return measured_power_; }
/**
* @brief 转发 `SuperPower::GetCapEnergy()`。
* Forward `SuperPower::GetCapEnergy()`.
*
* @return 超级电容能量。
* Supercapacitor energy.
*/
float GetCapEnergy() { return superpower_->GetCapEnergy(); }
/**
* @brief 转发 `SuperPower::IsOnline()`。
* Forward `SuperPower::IsOnline()`.
*
* @return 超级电容在线为 true。
* True when the supercapacitor is online.
*/
bool IsOnline() { return superpower_->IsOnline(); }
private:
void OutputLimitOmni(float max_power)
{
float required_power_3508_sum = 0.0f;
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(
output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, k2_3508_);
if (motor_power_3508_[i] > 0)
{
required_power_3508_sum += motor_power_3508_[i];
sum_error_ += speed_error_3508_[i];
}
else
{
available_power -= motor_power_3508_[i];
}
}
if (required_power_3508_sum > available_power)
{
powercontrol_data_.is_power_limited = true;
/* 计算误差置信度: sum_error 越大, 越倾向按误差分配功率 */
if (sum_error_ > ERROR_POWERDISTRIBUTION_SET)
{
error_confidence_ = 1.0f;
}
else if (sum_error_ > POP_POWERDISTRIBUTION)
{
error_confidence_ = std::clamp(
(sum_error_ - static_cast<float>(POP_POWERDISTRIBUTION)) /
static_cast<float>(ERROR_POWERDISTRIBUTION_SET - POP_POWERDISTRIBUTION),
0.0f, 1.0f);
}
else
{
error_confidence_ = 0.0f;
}
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];
}
}
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
{
powercontrol_data_.is_power_limited = false;
for (int i = 0; i < motor_count_3508_; i++)
{
powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i];
}
}
}
void OutputLimitHelm(float max_power)
{
float required_power_3508_sum = 0.0f;
float required_power_6020_sum = 0.0f;
float sum_error_3508 = 0.0f;
float sum_error_6020 = 0.0f;
/* 舵轮需要给模型误差和控制延迟留余量 */
float available_power =
std::max(0.0f, max_power - k3_chassis_ - CHASSIS_POWER_LIMIT_MARGIN_W);
for (int i = 0; i < motor_count_3508_; i++)
{
motor_power_3508_[i] = calculate_motor_model_power(
output_current_3508_[i], rotorspeed_rpm_3508_[i], kt_3508_, k1_3508_, k2_3508_);
if (motor_power_3508_[i] > 0)
{
required_power_3508_sum += motor_power_3508_[i];
sum_error_3508 += speed_error_3508_[i];
}
}
for (int i = 0; i < motor_count_6020_; i++)
{
motor_power_6020_[i] = calculate_motor_model_power(
output_current_6020_[i], rotorspeed_rpm_6020_[i], kt_6020_, k1_6020_, k2_6020_);
if (motor_power_6020_[i] > 0)
{
required_power_6020_sum += motor_power_6020_[i];
sum_error_6020 += speed_error_6020_[i];
}
}
float total_required_power = required_power_3508_sum + required_power_6020_sum;
if (total_required_power > available_power)
{
powercontrol_data_.is_power_limited = true;
/*计算 6020 组的总功率限额*/
float limit_power_6020_total =
std::min(required_power_6020_sum, available_power * 0.8f);
/*计算 3508 组的总功率限额 (剩下的全部)*/
float limit_power_3508_total =
std::max(0.0f, available_power - limit_power_6020_total);
/* 6020 组: 误差置信度 + 混合权重分配 */
float ec_6020 = 0.0f;
if (sum_error_6020 > ERROR_POWERDISTRIBUTION_SET)
{
ec_6020 = 1.0f;
}
else if (sum_error_6020 > POP_POWERDISTRIBUTION)
{
ec_6020 = std::clamp(
(sum_error_6020 - static_cast<float>(POP_POWERDISTRIBUTION)) /
static_cast<float>(ERROR_POWERDISTRIBUTION_SET - POP_POWERDISTRIBUTION),
0.0f, 1.0f);
}
for (int i = 0; i < motor_count_6020_; i++)
{
if (motor_power_6020_[i] > 0 && required_power_6020_sum > 1e-6f)
{
float pw_err =
(sum_error_6020 > 1e-6f) ? (speed_error_6020_[i] / sum_error_6020) : 0.0f;
float pw_prop = motor_power_6020_[i] / required_power_6020_sum;
float pw = ec_6020 * pw_err + (1.0f - ec_6020) * pw_prop;
float power_quota = limit_power_6020_total * pw;
powercontrol_data_.new_output_current_6020[i] =
solve_current_for_power(power_quota, rotorspeed_rpm_6020_[i], kt_6020_,
k1_6020_, k2_6020_, output_current_6020_[i]);
}
else
{
powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i];
}
}
/*误差置信度 + 混合权重分配 */
float ec_3508 = 0.0f;
if (sum_error_3508 > ERROR_POWERDISTRIBUTION_SET)
{
ec_3508 = 1.0f;
}
else if (sum_error_3508 > POP_POWERDISTRIBUTION)
{
ec_3508 = std::clamp(
(sum_error_3508 - static_cast<float>(POP_POWERDISTRIBUTION)) /
static_cast<float>(ERROR_POWERDISTRIBUTION_SET - POP_POWERDISTRIBUTION),
0.0f, 1.0f);
}
for (int i = 0; i < motor_count_3508_; i++)
{
if (motor_power_3508_[i] > 0 && required_power_3508_sum > 1e-6f)
{
float pw_err =
(sum_error_3508 > 1e-6f) ? (speed_error_3508_[i] / sum_error_3508) : 0.0f;
float pw_prop = motor_power_3508_[i] / required_power_3508_sum;
float pw = ec_3508 * pw_err + (1.0f - ec_3508) * pw_prop;
float power_quota = limit_power_3508_total * pw;
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
{
/*功率充足,不限制*/
powercontrol_data_.is_power_limited = false;
for (int i = 0; i < motor_count_3508_; i++)
{
powercontrol_data_.new_output_current_3508[i] = output_current_3508_[i];
}
for (int i = 0; i < motor_count_6020_; i++)
{
powercontrol_data_.new_output_current_6020[i] = output_current_6020_[i];
}
}
}
private:
LibXR::Mutex mutex_;
SuperPower* superpower_;
bool is_helm_;
RLS<2> rls_; /* 在线辨识 3508 二次损耗模型参数 */
PowerControlData powercontrol_data_;
float k3_chassis_; /* 底盘静态功耗 */
float error_confidence_ = 0.0f; /* 误差置信度 */
float sum_error_ = 0.0f;
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_; /* 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] = {};
float motor_power_3508_[MAX_MOTOR_COUNT] = {};
float kt_6020_ = 1.42074505e-5f;
float k1_6020_ = 6.4276e-7f;
float k2_6020_ = 1.0e-10f;
float output_current_6020_[MAX_MOTOR_COUNT] = {};
float rotorspeed_rpm_6020_[MAX_MOTOR_COUNT] = {};
float motor_power_6020_[MAX_MOTOR_COUNT] = {};
float measured_power_ = 0.0f;
};