mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
51 lines
2.3 KiB
C
51 lines
2.3 KiB
C
#include "balance.h"
|
||
#include "stdint.h"
|
||
#include "arm_math.h"
|
||
|
||
/**
|
||
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
|
||
* @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
|
||
*
|
||
*/
|
||
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
|
||
{
|
||
static float k[12][3] = {152.073959,-322.787113,4.602472,
|
||
-18.091924,-50.039341,3.160226,
|
||
124.677598,-113.871572,-13.279757,
|
||
86.314525,-98.184173,-8.140564,
|
||
162.907111,-185.705008,74.114015,
|
||
14.280240,-18.420891,8.593112,
|
||
-148.548074,99.259778,48.506697,
|
||
-37.972970,41.871624,3.715364,
|
||
192.255883,-224.641960,95.484838,
|
||
93.227296,-117.550338,60.336158,
|
||
-343.472953,311.036102,57.349362,
|
||
-41.609057,38.668058,-0.809331,};
|
||
float T[2] = {0}; // 0 T_wheel 1 T_hip
|
||
float l = p->leg_len;
|
||
float lsqr = l * l;
|
||
|
||
uint8_t i, j;
|
||
|
||
// 离地时轮子输出置0
|
||
i = 0; j = i * 6;
|
||
T[i] = p->fly_flag ? 0 :
|
||
((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);
|
||
|
||
// 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
|
||
i = 1; 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 + (p->fly_flag ? 0 :
|
||
((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];
|
||
} |