From bc9801a60a49335f67805f8f08411282ef169c41 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 09:55:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8B=BF=E5=87=BAvmc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 148 +++++----------------------------- User/module/balance_chassis.h | 10 +++ 2 files changed, 30 insertions(+), 128 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 60d1fb6..90d6289 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -411,70 +411,6 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float } } -/** - * @brief 倒地自起控制 - * @note 自起流程（左右腿独立状态机）： - * STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成) - * 两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式 - * - * @param c 底盘结构体指针 - */ -static void Chassis_RecoverControl(Chassis_t *c) { - float theta_l = c->vmc_[0].leg.theta; - float theta_r = c->vmc_[1].leg.theta; - - /* ===== 左腿状态机 ===== */ - switch (c->recover.state[0]) { - case RECOVER_FLIP: - /* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */ - /* 等待 theta 转到腿朝下（|theta| < π/2） */ - if (fabsf(theta_l) < (float)M_PI_2) { - c->recover.state[0] = RECOVER_STRETCH; - } - break; - - case RECOVER_STRETCH: - /* 伸腿：等待腿伸到位 */ - if (c->vmc_[0].leg.L0 > 0.28f) { - c->recover.state[0] = RECOVER_BACKLEG; - } - break; - - case RECOVER_BACKLEG: - /* 后甩腿：等待 phi0 趋近目标角度 */ - if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) { - c->recover.state[0] = RECOVER_COMPLETE; - } - break; - - case RECOVER_COMPLETE: - break; - } - - /* ===== 右腿状态机 ===== */ - switch (c->recover.state[1]) { - case RECOVER_FLIP: - if (fabsf(theta_r) < (float)M_PI_2) { - c->recover.state[1] = RECOVER_STRETCH; - } - break; - - case RECOVER_STRETCH: - if (c->vmc_[1].leg.L0 > 0.28f) { - c->recover.state[1] = RECOVER_BACKLEG; - } - break; - - case RECOVER_BACKLEG: - if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) { - c->recover.state[1] = RECOVER_COMPLETE; - } - break; - - case RECOVER_COMPLETE: - break; - } -} /** * @brief 初始化底盘 @@ -716,81 +652,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { case CHASSIS_MODE_RELAX: Chassis_MotorRelax(c); Chassis_LQRControl(c, c_cmd); + Chassis_VMCControl(c, c_cmd); break; - case CHASSIS_MODE_RECOVER: { - /* 运行自起状态机 */ - Chassis_RecoverControl(c); - - /* 根据状态决定输出力 */ - float fn_left, fn_right; - float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0); - float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0); - if (isnan(spring_left)) spring_left = 0.0f; - if (isnan(spring_right)) spring_right = 0.0f; - - /* 左腿 */ - if (c->recover.state[0] == RECOVER_FLIP) { - fn_left = 30.0f - spring_left; /* 收腿，借重力慢慢翻转 */ - } else if (c->recover.state[0] == RECOVER_STRETCH) { - fn_left = -80.0f - spring_left; /* 大力伸腿 */ - } else if (c->recover.state[0] == RECOVER_BACKLEG) { - fn_left = -40.0f - spring_left; /* 维持伸腿 */ - } else { - fn_left = -20.0f - spring_left; - } - - /* 右腿 */ - if (c->recover.state[1] == RECOVER_FLIP) { - fn_right = 30.0f - spring_right; - } else if (c->recover.state[1] == RECOVER_STRETCH) { - fn_right = -80.0f - spring_right; - } else if (c->recover.state[1] == RECOVER_BACKLEG) { - fn_right = -40.0f - spring_right; - } else { - fn_right = -20.0f - spring_right; - } - - VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f); - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f); - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; - Chassis_Output(c); - - /* 两腿均完成，自动切换到平衡模式 */ - if (c->recover.state[0] == RECOVER_COMPLETE && - c->recover.state[1] == RECOVER_COMPLETE) { - Chassis_ResetControllers(c); - Chassis_SelectYawZeroPoint(c); - c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - } - } break; - - // case CHASSIS_MODE_CALIBRATE: { - // Chassis_LQRControl(c, c_cmd); - // LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0); - // RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0); - // L_fn = -LF; - // VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp); - // VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - // VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp); - // VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - - // c->output.wheel[0] = 0.0f; - // c->output.wheel[1] = 0.0f; - // Chassis_Output(c); - // } break; + // case CHASSIS_MODE_RECOVER: + // Chassis_RecoverControl(c); + // break; case CHASSIS_MODE_WHELL_LEG_BALANCE: Chassis_LQRControl(c, c_cmd); + Chassis_VMCControl(c, c_cmd); Chassis_Output(c); break; case CHASSIS_MODE_BALANCE_ROTOR: Chassis_LQRControl(c, c_cmd); c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f; c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f; + Chassis_VMCControl(c, c_cmd); Chassis_Output(c); break; @@ -952,6 +830,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force; c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force; + return CHASSIS_OK; +} + +/** + * @brief VMC控制（虚拟模型控制） + * @param c 底盘结构体指针 + * @param c_cmd 控制指令 + * @return 操作结果 + * @note 将LQR输出的力矩通过VMC逆解算转换为关节力矩， + * 同时处理横滚补偿、腿长控制、跳跃控制和摆角同步 + */ +int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL; + /* ==================== 横滚角补偿 ==================== */ /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */ float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */ diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 363a487..e0c498a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -291,6 +291,16 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd); */ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); +/** + * \brief VMC控制（虚拟模型控制） + * + * \param c 包含底盘数据的结构体 + * \param c_cmd 底盘控制指令 + * + * \return 函数运行结果 + */ +int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); + /** * \brief 底盘输出值 *