mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-23 19:25:09 +08:00
43 lines
2.1 KiB
C
43 lines
2.1 KiB
C
#include "balance.h"
|
|
#include "stdint.h"
|
|
#include "arm_math.h"
|
|
|
|
/**
|
|
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
|
|
* @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
|
|
*
|
|
*/
|
|
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
|
|
{
|
|
float k[12][3] = {85.365277,-164.978797,-4.643325,
|
|
-7.817260,-26.353895,0.955791,
|
|
43.461413,-37.225135,-12.040913,
|
|
34.470271,-39.045724,-7.813613,
|
|
171.639863,-172.946723,59.632812,
|
|
11.281864,-11.926075,4.239721,
|
|
-19.517972,1.032904,27.896023,
|
|
-10.715152,11.663766,2.651705,
|
|
96.039929,-99.227575,36.121143,
|
|
56.371947,-60.347262,25.166639,
|
|
-213.707672,181.799382,93.194337,
|
|
-14.287060,12.217074,3.285836};
|
|
float T[2] = {0}; // 0 T_wheel 1 T_hip
|
|
float l = p->leg_len;
|
|
float lsqr = l * l;
|
|
|
|
// float dist_limit = abs(chassis->target_dist - chassis->dist) > MAX_DIST_TRACK ? sign(chassis->target_dist - chassis->dist) * MAX_DIST_TRACK : (chassis->target_dist - chassis->dist); // todo设置值
|
|
// float vel_limit = abs(chassis->target_v - chassis->vel) > MAX_VEL_TRACK ? sign(chassis->target_v - chassis->vel) * MAX_VEL_TRACK : (chassis->target_v - chassis->vel);
|
|
|
|
for (uint8_t i = 0; i < 2; ++i)
|
|
{
|
|
uint8_t j = i * 6;
|
|
T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
|
|
(k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
|
|
(k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
|
|
(k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
|
|
(k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
|
|
(k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w;
|
|
}
|
|
p->T_wheel = T[0];
|
|
p->T_hip = T[1];
|
|
} |