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-01 20:53:02 +08:00
parent 650ebc1f87
commit 1b237f9944
3 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
User/module/balance_chassis.c
View File

@ -402,8 +402,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_state.d_theta = 0.0f; // 目标摆杆角速度
// target_state.x = 0; // 目标位置
// target_state.d_x = 0.0f; // 目标速度
target_state.phi = 0.0f; // 目标俯仰角
target_state.d_phi = -0.4f; // 目标俯仰角速度
target_state.phi = -0.2f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
// target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
// target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.x = c->chassis_state.target_x; // 目标位置

28
User/module/config.c
View File

@ -159,20 +159,20 @@ Config_RobotParam_t robot_config = {
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains = {
.k11_coeff = { -1.518517857889991e+02f, 1.947427875897828e+02f, -1.148120822481098e+02f, 2.924883559935997e+00f }, // theta
.k12_coeff = { 1.210535496345407e+01f, -9.650112118530574e+00f, -7.795614823592060e+00f, 4.657327020761534e-01f }, // d_theta
.k13_coeff = { -1.165837444184349e+02f, 1.210192009641869e+02f, -4.426716583574083e+01f, 2.303172586380976e+00f }, // x
.k14_coeff = { -1.286052871916038e+02f, 1.353354867883034e+02f, -5.203199795488686e+01f, 2.622579832615006e+00f }, // d_x
.k15_coeff = { -1.105374347374162e+01f, 3.229730763109554e+01f, -2.423106279412239e+01f, 6.195204290845447e+00f }, // phi
.k16_coeff = { 8.260175276799853e-01f, 2.318023492852569e+00f, -2.927935219327240e+00f, 8.556202879801391e-01f }, // d_phi
.k21_coeff = { 5.970052798954075e+02f, -5.263867571125131e+02f, 1.275655583254851e+02f, 1.140189988320814e+01f }, // theta
.k22_coeff = { 7.451585479025395e+01f, -7.785445709151199e+01f, 2.827386050355351e+01f, -1.578620610065337e-01f }, // d_theta
.k23_coeff = { 4.622306333044981e+01f, 4.325117706159210e+00f, -3.407203207833582e+01f, 1.455475256803718e+01f }, // x
.k24_coeff = { 6.870578295004115e+01f, -1.301773374139566e+01f, -3.211146583952794e+01f, 1.644195610244500e+01f }, // d_x
.k25_coeff = { 4.493994591642744e+02f, -4.563158073471503e+02f, 1.605396531175411e+02f, -3.691208683727941e+00f }, // phi
.k26_coeff = { 5.771012099214764e+01f, -5.915171391239870e+01f, 2.096920183531924e+01f, -1.205283154896878e+00f }, // d_phi
},
.lqr_gains = {
.k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
.k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta
.k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x
.k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x
.k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
.k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi
.k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
.k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta
.k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x
.k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x
.k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
.k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi
},
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
.lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm
},

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([700 1 2000 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[140 0;0 15]; %T Tp
Q=diag([700 100 2000 1500 5000 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);