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

暂时关闭速度规划

Browse Source
This commit is contained in:
Robofish 2026-02-07 21:16:49 +08:00
parent 79bc3f5f89
commit b5af2b9967
5 changed files with 7035 additions and 7034 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

14007
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

26
User/module/balance_chassis.c
View File

@ -598,18 +598,22 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL; if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
/* ==================== 速度控制 ==================== */ /* ==================== 速度控制 ==================== */
float raw_vx = c_cmd->move_vec.vx * 2.0f; // float raw_vx = c_cmd->move_vec.vx * 2.0f;
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx; // float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
/* 加速度限制 */ // /* 加速度限制 */
float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x; // float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
float max_velocity_change = MAX_ACCELERATION * c->dt; // float max_velocity_change = MAX_ACCELERATION * c->dt;
velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change); // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
// c->chassis_state.target_velocity_x = target_dot_x;
// c->chassis_state.last_target_velocity_x = target_dot_x;
// c->chassis_state.target_x += target_dot_x * c->dt;
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
c->chassis_state.target_velocity_x = target_dot_x;
c->chassis_state.last_target_velocity_x = target_dot_x;
c->chassis_state.target_x += target_dot_x * c->dt;
/* ==================== 状态更新 ==================== */ /* ==================== 状态更新 ==================== */
LQR_State_t current_state = { LQR_State_t current_state = {
@ -635,7 +639,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = { LQR_State_t target_state = {
.theta = 0.0f, .theta = 0.0f,
.d_theta = 0.0f, .d_theta = 0.0f,
.phi = -0.1f, .phi = 0.0f,
.d_phi = 0.0f, .d_phi = 0.0f,
.x = c->chassis_state.target_x, .x = c->chassis_state.target_x,
.d_x = c->chassis_state.target_velocity_x, .d_x = c->chassis_state.target_velocity_x,

30
User/module/config.c
View File

@ -316,20 +316,20 @@ Config_RobotParam_t robot_config = {
} }
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.110325959941390e+02f, 2.474876302869424e+02f, -1.259312069344584e+02f, -3.555067587081389e+00f }, // theta .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
.k12_coeff = { 1.729035713826111e+00f, -1.094205267383853e+00f, -8.235618497594484e+00f, -2.949091603597677e-01f }, // d_theta .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta
.k13_coeff = { -3.661055595462458e+01f, 4.066046385752247e+01f, -1.618786894025212e+01f, -1.618742454043447e+00f }, // x .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x
.k14_coeff = { -2.988926501197955e+01f, 3.445642428071118e+01f, -1.547117906530474e+01f, -2.039186188549067e+00f }, // d_x .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f }, // d_x
.k15_coeff = { -4.094585161782607e+01f, 6.119228618474273e+01f, -3.603009625243965e+01f, 1.037078067693317e+01f }, // phi .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi
.k16_coeff = { -7.625020535837323e+00f, 1.097226354374976e+01f, -6.302244199082423e+00f, 1.920720114282383e+00f }, // d_phi .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f }, // d_phi
.k21_coeff = { 3.113860878856309e+02f, -2.577426343951217e+02f, 4.982404130107013e+01f, 1.415180465415879e+01f }, // theta .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta
.k22_coeff = { 1.126082466887103e+01f, -9.812804977178935e+00f, 1.814407629267238e+00f, 1.941377111901043e+00f }, // d_theta .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f }, // d_theta
.k23_coeff = { -1.771607516002724e+01f, 4.047388400543465e+01f, -2.962942748901807e+01f, 8.971209797230671e+00f }, // x .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f }, // x
.k24_coeff = { -3.391460592939657e+01f, 5.575858169617614e+01f, -3.466996436706782e+01f, 9.833893476570065e+00f }, // d_x .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x
.k25_coeff = { 2.392778926806863e+02f, -2.574664846375947e+02f, 9.953632837845201e+01f, 2.579297039170335e+00f }, // phi .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
.k26_coeff = { 4.297556375859175e+01f, -4.657509218314731e+01f, 1.826212630358702e+01f, 1.257410509642398e-01f }, // d_phi .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi
}, },
.jump_params = { .jump_params = {
.crouch_time_ms = 300, .crouch_time_ms = 300,
@ -343,7 +343,7 @@ Config_RobotParam_t robot_config = {
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,
.phi = -0.1f, .phi = 0.0f,
}, },

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

@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
R1=0.068; %驱动轮半径 R1=0.068; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离 L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.02; %机体质心距离转轴距离 l1=0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量 mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量 mp1=1.8; %杆质量
M1=16.0; %机体质量 M1=16.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量 Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
@ -48,7 +48,7 @@ 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=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); B=double(B);
Q=diag([1 1 20 5 50 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([1 1 20 5 50 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[1.2 0;0 0.25]; %T Tp R=[1.2 0;0 0.25]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);