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-19 16:55:34 +08:00
parent 1850429757
commit 10ca460ebf
2 changed files with 56 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
User/module/balance_chassis.c
View File

@ -132,8 +132,11 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
c->chassis_state.velocity_x =
(left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
// 在跳跃模式的起跳和收腿阶段暂停位移积分，避免轮子空转导致的位移误差
if (!(c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3))) {
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
}
@ -310,12 +313,12 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 160000) {
// state 2 保持0.2s后转到state 3
} else if (c->state == 2 && elapsed_us >= 230000) {
// state 2 保持0.25s后转到state 3，确保腿部充分伸展
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 160000) {
// state 3 保持0.3s后转到state 0
} else if (c->state == 3 && elapsed_us >= 500000) {
// state 3 保持0.4s后转到state 0，确保收腿到最高
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
}
@ -427,7 +430,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 构建目标状态 */
LQR_State_t target_state = {0};
target_state.theta = 0.00; // 目标摆杆角度
// 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
target_state.theta = 0.0f; // 强制摆杆角度为0，确保腿部竖直
} else {
target_state.theta = 0.00; // 目标摆杆角度
}
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
@ -484,11 +494,13 @@ 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.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) +
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + non_contact_theta) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
@ -496,20 +508,28 @@ 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.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + non_contact_theta) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
/* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] =
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] =
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
// 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转
if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) {
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
} else {
c->output.wheel[0] =
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] =
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
}
/* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2];
float target_L0[2];
@ -542,16 +562,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.16f + roll_compensation_length; // 左腿：收缩到基础腿长 + 横滚补偿
target_L0[1] = 0.16f - roll_compensation_length; // 右腿：收缩到基础腿长 - 横滚补偿
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿
target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.55f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
break;
case 3: // 着地阶段，腿长缓冲
target_L0[0] = 0.1f; // 左腿：缓冲腿长（着地时不进行横滚补偿）
target_L0[1] = 0.1f; // 右腿：缓冲腿长（着地时不进行横滚补偿）
case 3: // 收腿阶段，腿长收缩到最高
target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高）
target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高）
break;
default:
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
@ -559,10 +579,17 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
break;
}
// 腿长限幅：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
// 腿长限幅：根据跳跃状态调整限制范围
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
// 在跳跃收腿阶段，允许更短的腿长，确保收腿到最高
if (target_L0[0] < 0.06f) target_L0[0] = 0.06f;
if (target_L0[1] < 0.06f) target_L0[1] = 0.06f;
} else {
// 正常情况下的腿长限制：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
}
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
// 获取腿长变化率

8
User/task/rc.c
View File

@ -61,13 +61,13 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;