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

改k

Browse Source
This commit is contained in:
Robofish 2025-10-06 02:09:43 +08:00
parent cedc8bbab1
commit 9b5d806e5f
4 changed files with 29 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
User/device/virtual_chassis.c
View File

@ -177,7 +177,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
if (y_int & 0x800000) {
y_int |= 0xFF000000;
}
chassis->data.imu.gyro.y = (float)y_int / 1000.0f; // 直接除以1000，保持rad/s
chassis->data.imu.gyro.y = -(float)y_int / 1000.0f; // 直接除以1000，保持rad/s
// z: 16位有符号整数 (字节6-7) - 精度0.01 rad/s (不再转换单位)
int16_t z_int;
@ -206,7 +206,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
if (pit_int & 0x800000) {
pit_int |= 0xFF000000;
}
chassis->data.imu.euler.pit = (float)pit_int / 10000.0f; // 直接除以10000，保持rad
chassis->data.imu.euler.pit = -(float)pit_int / 10000.0f; // 直接除以10000，保持rad
// roll: 16位有符号整数 (字节6-7) - 精度0.001 rad (不再转换单位)
int16_t rol_int;

22
User/module/balance_chassis.c
View File

@ -217,18 +217,20 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
break;
case CHASSIS_MODE_RECOVER: {
float fn;
fn = -25.0f;
float fn, tp;
fn = -20.0f;
tp = 0.0f;
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
VMC_InverseSolve(&c->vmc_[1], fn, tp);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
&c->output.joint[2]);
// Chassis_MotorEnable(c);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
c->output.wheel[0] = c_cmd->move_vec.vx *0.2f;
c->output.wheel[1] = c_cmd->move_vec.vx *0.2f;
Chassis_Output(c); // 统一输出
} break;
@ -269,6 +271,7 @@ void Chassis_Output(Chassis_t *c) {
// out[3] = 0.0f;
Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f);
// for (int i = 0; i < 2; i++) {
}
@ -316,7 +319,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = {0};
target_state.theta = 0.0f; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = -0.1f; // 目标俯仰角
target_state.phi = -0.2f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x; // 目标位置
target_state.d_x = target_dot_x; // 目标速度
@ -373,7 +376,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[0].leg.d_theta, c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = -Delta_Tp[0];
// 左腿控制
{
@ -382,7 +384,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 + 20.0f ;
virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 10.0f ;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@ -403,7 +405,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 + 20.0f ;
virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 10.0f ;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],

25
User/module/config.c
View File

@ -113,19 +113,20 @@ Config_RobotParam_t robot_config = {
}
},
.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
.k11_coeff = { -2.018567892252635e+02f, 2.443886947748371e+02f, -1.170514252719224e+02f, 1.756967283355012e+00f }, // theta
.k12_coeff = { 5.755541993064226e+00f, -2.330435203830081e+00f, -5.839389482510768e+00f, 3.102177559173085e-01f }, // d_theta
.k13_coeff = { -4.626648375801587e+01f, 4.957163915561897e+01f, -1.892978626686148e+01f, 1.163361730642856e+00f }, // x
.k14_coeff = { -6.133244696547617e+01f, 6.581150547694307e+01f, -2.562467010643359e+01f, 1.546359441777505e+00f }, // d_x
.k15_coeff = { -9.787512606327831e-01f, 8.581307530201126e+00f, -7.593138673503293e+00f, 1.797419370480695e+00f }, // phi
.k16_coeff = { 2.873430467178664e+00f, -8.356289550331172e-01f, -1.159458299556627e+00f, 5.473036343381187e-01f }, // d_phi
.k21_coeff = { 4.048940262085570e+02f, -3.365968455386673e+02f, 6.490322687755379e+01f, 1.009032151585340e+01f }, // theta
.k22_coeff = { 3.731821338634553e+01f, -3.863396990944306e+01f, 1.309004485801674e+01f, -8.491362495876140e-02f }, // d_theta
.k23_coeff = { 3.197848557628166e+01f, -1.613497787175245e+01f, -5.139546674944345e+00f, 4.089512188250406e+00f }, // x
.k24_coeff = { 4.042762833518352e+01f, -2.015511550072881e+01f, -6.901438674565010e+00f, 5.459259288213627e+00f }, // d_x
.k25_coeff = { 9.671666382924865e+01f, -1.007923006268712e+02f, 3.656525147129298e+01f, -7.438566326110798e-01f }, // phi
.k26_coeff = { 2.324856410306975e+01f, -2.529653580362136e+01f, 9.749203865654970e+00f, -5.110687534172851e-01f }, // d_phi
},
.virtual_chassis_param = {
.motors = {
.can = BSP_CAN_1,

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 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
Q=diag([2000 1 600 200 2000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 50]; %T Tp
K=lqr(A,B,Q,R);