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-05 20:15:37 +08:00
parent 11aef0acb2
commit cedc8bbab1
3 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
User/module/balance_chassis.c
View File

@ -357,8 +357,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (roll_compensation_force < -20.0f) roll_compensation_force = -20.0f;
// 目标腿长设定（移除横滚角补偿）
target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节
target_L0[0] = 0.12f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.12f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@ -382,7 +382,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力）
virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 40.0f ;
virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 20.0f ;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@ -403,7 +403,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力）
virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 40.0f ;
virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 20.0f ;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],

28
User/module/config.c
View File

@ -112,20 +112,20 @@ Config_RobotParam_t robot_config = {
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains = {
.k11_coeff = { -1.916376919295156e+02f, 2.009487240966917e+02f, -9.481460086781939e+01f, -4.748704486775178e+00f }, // theta
.k12_coeff = { -1.794538872095484e+00f, -1.557720009681701e-02f, -7.253705624763870e+00f, -9.066344876912042e-01f }, // d_theta
.k13_coeff = { -5.530128764310525e+01f, 5.441066051951745e+01f, -1.855725101721958e+01f, -1.532320658646968e+00f }, // x
.k14_coeff = { -5.226956984912729e+01f, 5.134367619659140e+01f, -1.878250555345751e+01f, -2.194040977657715e+00f }, // d_x
.k15_coeff = { -8.041587662748894e+00f, 1.194985309170939e+01f, -6.358536570979702e+00f, 8.475914154881636e-01f }, // phi
.k16_coeff = { -1.171602430557222e+01f, 1.424681188601595e+01f, -6.769563511869035e+00f, 1.358696951640962e+00f }, // d_phi
.k21_coeff = { -1.589916277171124e+01f, 3.688717311486668e+01f, -2.693106123880470e+01f, 8.468880380216705e+00f }, // theta
.k22_coeff = { -4.056650578339882e+00f, 5.627051302682392e+00f, -2.802037254431724e+00f, 1.258712322539219e+00f }, // d_theta
.k23_coeff = { -4.615940205649521e+01f, 5.298830026615487e+01f, -2.321905486010759e+01f, 4.733000143959848e+00f }, // x
.k24_coeff = { -6.198443605956307e+01f, 6.867018097135634e+01f, -2.873375258895661e+01f, 5.632056979434964e+00f }, // d_x
.k25_coeff = { -1.149934231218892e+01f, 9.960303123832579e+00f, -2.826210920406189e+00f, 4.384038656352771e+00f }, // phi
.k26_coeff = { 1.452344963148120e+01f, -1.451377402367933e+01f, 5.020766559053281e+00f, 1.099371055071753e+00f }, // d_phi
},
.lqr_gains = {
.k11_coeff = { -1.971875171647865e+02f, 2.333085109680034e+02f, -1.246153590631013e+02f, -1.174312882115276e+00f }, // theta
.k12_coeff = { 1.013831636839851e+01f, -9.401172931240684e+00f, -7.093019361028483e+00f, -1.965070661986029e-01f }, // d_theta
.k13_coeff = { -8.071626496047382e+01f, 8.488840381053994e+01f, -3.185529183755884e+01f, 8.124359207371032e-01f }, // x
.k14_coeff = { -9.146471594077464e+01f, 9.719345618895781e+01f, -3.842805305561099e+01f, 7.255654659220748e-01f }, // d_x
.k15_coeff = { 6.971426904628133e+00f, 1.184154915473761e+00f, -5.491870448330107e+00f, 1.440694962464955e+00f }, // phi
.k16_coeff = { -1.435148366342453e+00f, 3.514683607111185e+00f, -2.799267599341074e+00f, 7.265018312372623e-01f }, // d_phi
.k21_coeff = { 2.190542850560747e+02f, -1.777611537012371e+02f, 2.829510877414540e+01f, 1.179295840423852e+01f }, // theta
.k22_coeff = { 1.499600560463937e+01f, -1.645293844666804e+01f, 6.159388841822305e+00f, 1.123989134168646e+00f }, // d_theta
.k23_coeff = { -2.101291454047308e+00f, 2.194459164295155e+01f, -2.170715123911771e+01f, 7.721879713384771e+00f }, // x
.k24_coeff = { -8.999340091522836e+00f, 3.152322448910924e+01f, -2.733558805289098e+01f, 9.654200348790059e+00f }, // d_x
.k25_coeff = { 2.294462014541718e+01f, -2.777789794229430e+01f, 1.160794075992378e+01f, 2.826138680231716e+00f }, // phi
.k26_coeff = { 1.624214174273862e+01f, -1.723094946447953e+01f, 6.402650924792812e+00f, 8.755827436652930e-02f }, // d_phi
},
.virtual_chassis_param = {
.motors = {
.can = BSP_CAN_1,

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

@ -20,7 +20,7 @@ function K = get_k_length(leg_length)
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=6.0; %机体质量
M1=10.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
@ -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([500 200 1500 800 2500 200]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[100 0;0 80]; %T Tp
Q=diag([700 100 2000 1500 2300 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[140 0;0 50]; %T Tp
K=lqr(A,B,Q,R);