robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改参数

Browse Source
This commit is contained in:
Robofish 2025-10-15 22:05:05 +08:00
parent 8b5a7ec6cb
commit 45e340156c
3 changed files with 29 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/module/balance_chassis.c
View File

@ -409,9 +409,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = {0};
target_state.theta = c->param->theta; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.phi = 11.9601*pow((0.18f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.18f + c_cmd->height * 0.25f),2) + 3.87083*(0.18f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
target_state.x = c->chassis_state.target_x -2.07411f*(0.18f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){
@ -434,7 +434,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]);
} else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
target_state.theta = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
@ -446,7 +446,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]);
}else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
target_state.theta = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
@ -483,12 +483,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 目标腿长设定（移除横滚角补偿）
switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
target_L0[0] = 0.18f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.18f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
target_L0[0] = 0.18f; // 左腿：收缩到基础腿长
target_L0[1] = 0.18f; // 右腿：收缩到基础腿长
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
@ -499,8 +499,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_L0[1] = 0.1f; // 右腿：缓冲腿长
break;
default:
target_L0[0] = 0.15f + c_cmd->height * 0.22f;
target_L0[1] = 0.15f + c_cmd->height * 0.22f;
target_L0[0] = 0.18f + c_cmd->height * 0.22f;
target_L0[1] = 0.18f + c_cmd->height * 0.22f;
break;
}

34
User/module/config.c
View File

@ -169,9 +169,9 @@ Config_RobotParam_t robot_config = {
.chassis_param = {
.yaw={
.k=1.0f,
.p=1.0f,
.i=0.01f,
.d=0.05f,
.p=0.8f,
.i=0.0f,
.d=0.15f,
.i_limit=0.0f,
.out_limit=1.0f,
.d_cutoff_freq=30.0f,
@ -211,8 +211,8 @@ Config_RobotParam_t robot_config = {
},
.tp={
.k=2.0f,
.p=2.0f, /* 俯仰角比例系数 */
.k=4.0f,
.p=3.0f, /* 俯仰角比例系数 */
.i=0.0f, /* 俯仰角积分系数 */
.d=0.0f, /* 俯仰角微分系数 */
.i_limit=0.0f, /* 积分限幅 */
@ -255,18 +255,18 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta
.k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f }, // d_theta
.k13_coeff = { -3.146938649452867e+01f, 3.671781823697795e+01f, -1.548019975847165e+01f, -3.659482046966758e-01f }, // x
.k14_coeff = { -3.363190268966418e+01f, 4.073114332036178e+01f, -1.877821151363973e+01f, -6.755848684037919e-01f }, // d_x
.k15_coeff = { 1.813346556940570e+00f, 1.542139674818206e+01f, -1.784151260000496e+01f, 6.975189972486323e+00f }, // phi
.k16_coeff = { -1.683339907923917e+00f, 3.762911505427638e+00f, -2.966405826579746e+00f, 1.145611903160937e+00f }, // d_phi
.k21_coeff = { 3.683683451383080e+02f, -3.184826128050574e+02f, 7.012281968985167e+01f, 1.232691707185163e+01f }, // theta
.k22_coeff = { 2.256451382235226e+01f, -2.387074220964917e+01f, 8.355357863648345e+00f, 1.148384164091676e+00f }, // d_theta
.k23_coeff = { 1.065026516142075e+01f, 1.016717837738013e+01f, -1.810442639595643e+01f, 7.368019318950717e+00f }, // x
.k24_coeff = { 2.330418054102148e+00f, 2.193671212435775e+01f, -2.502198732490354e+01f, 9.621144599080463e+00f }, // d_x
.k25_coeff = { 1.784444885521937e+02f, -2.030738611028434e+02f, 8.375405599993576e+01f, -2.204042546242858e-01f }, // phi
.k26_coeff = { 2.646425532528492e+01f, -3.034702782249508e+01f, 1.275100635568078e+01f, -4.878639273974432e-01f }, // d_phi
.k11_coeff = { -1.365991212772591e+02f, 1.817682520209751e+02f, -1.090145744768390e+02f, -1.357841047348700e+00f }, // theta
.k12_coeff = { 1.066191238841660e+01f, -9.003325414315036e+00f, -6.468059178062407e+00f, -3.576912197025609e-02f }, // d_theta
.k13_coeff = { -4.401223711894286e+01f, 5.240645764730768e+01f, -2.259350423458009e+01f, -3.125930762749351e-01f }, // x
.k14_coeff = { -3.589096446318186e+01f, 4.504832892816042e+01f, -2.180583719935267e+01f, -6.131521769198509e-01f }, // d_x
.k15_coeff = { 7.755509171684017e+00f, 9.703025198473474e+00f, -1.617988750531686e+01f, 6.969677381136859e+00f }, // phi
.k16_coeff = { -1.054369868423547e+00f, 3.078146872733054e+00f, -2.719931242949781e+00f, 1.137139318462496e+00f }, // d_phi
.k21_coeff = { 4.187447897772694e+02f, -3.681828885583142e+02f, 8.516335489092428e+01f, 1.307642410705911e+01f }, // theta
.k22_coeff = { 2.733059508651422e+01f, -2.951697374399402e+01f, 1.083275594166739e+01f, 1.186192378498821e+00f }, // d_theta
.k23_coeff = { 2.607118229545885e+01f, 5.493744611623077e+00f, -2.426883873185716e+01f, 1.096947115837726e+01f }, // x
.k24_coeff = { 1.079657629365681e+01f, 1.807070716568389e+01f, -2.737138856658193e+01f, 1.153864525902007e+01f }, // d_x
.k25_coeff = { 1.861482062842389e+02f, -2.152844878785277e+02f, 9.051129761064519e+01f, -1.063399918986778e+00f }, // phi
.k26_coeff = { 2.777098508680760e+01f, -3.223625515767142e+01f, 1.376100864562188e+01f, -6.731370513814361e-01f }, // d_phi
},
.theta = 0.0f,

4
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[300 0;0 55]; %T Tp
Q=diag([1500 100 5000 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[300 0;0 50]; %T Tp
K=lqr(A,B,Q,R);