From 10ca460ebf1ea28db5b075ce690dfc3e4fcdfe36 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 19 Oct 2025 16:55:34 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=AF=E4=BB=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 77 +++++++++++++++++++++++------------ User/task/rc.c | 8 ++-- 2 files changed, 56 insertions(+), 29 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index d3e276b..74a3579 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -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; // 获取腿长变化率 diff --git a/User/task/rc.c b/User/task/rc.c index d913414..3a8d9b8 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -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;