From 2c5c77645d0f0708ad5bb96e0d9e723e83cf730b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 3 Mar 2026 00:16:50 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B7=B3=E8=B7=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .DS_Store | Bin 8196 -> 8196 bytes User/module/balance_chassis.c | 284 +++++++++++------- User/module/balance_chassis.h | 18 +- User/module/config.c | 44 +-- User/task/ctrl_chassis.c | 8 +- User/task/rc.c | 51 +++- .../balance/series_legs/get_k_length.m | 4 +- 7 files changed, 249 insertions(+), 160 deletions(-) diff --git a/.DS_Store b/.DS_Store index 6a4bf7c5798e071a0622a60afbea6d957ffe6ec0..211125cb8b6d49d5f52ef48ee838e15bdf99890d 100644 GIT binary patch delta 40 qcmZp1XmOa}&nUSuU^hRb;trx?Hh diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index c9b7b06..e7601ed 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -31,6 +31,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c); static void Chassis_ResetControllers(Chassis_t *c); static void Chassis_SelectYawZeroPoint(Chassis_t *c); static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force); +static void Chassis_RecoverControl(Chassis_t *c); /* Private functions -------------------------------------------------------- */ @@ -137,10 +138,40 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c == NULL) return CHASSIS_ERR_NULL; if (mode == c->mode) return CHASSIS_OK; + /* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */ + if (c->mode == CHASSIS_MODE_RECOVER && + mode == CHASSIS_MODE_WHELL_LEG_BALANCE) { + return CHASSIS_OK; + } + Chassis_MotorEnable(c); Chassis_ResetControllers(c); Chassis_SelectYawZeroPoint(c); + /* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */ + if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) { + float pitch = c->feedback.imu.euler.pit; + /* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */ + if (fabsf(pitch) > 0.2f) { + /* 根据 theta 判断腿朝上还是朝下 */ + float theta_l = c->vmc_[0].leg.theta; + float theta_r = c->vmc_[1].leg.theta; + c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + c->mode = CHASSIS_MODE_RECOVER; + return CHASSIS_OK; + } + /* 否则直接进入平衡模式 */ + } + + /* 直接进入 RECOVER 时也重置状态机 */ + if (mode == CHASSIS_MODE_RECOVER) { + float theta_l = c->vmc_[0].leg.theta; + float theta_r = c->vmc_[1].leg.theta; + c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + } + c->mode = mode; return CHASSIS_OK; } @@ -281,16 +312,14 @@ static float Chassis_CalcSpringForce(float leg_length) } /** - * @brief 跳跃控制状态机 + * @brief 跳跃控制 * @param c 底盘结构体指针 * @param c_cmd 控制指令 * @param target_L0 输出的目标腿长数组[2] * @param extra_force 输出的额外力数组[2] * * @note 跳跃流程： - * IDLE -> CROUCH(蓄力下蹲) -> LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE - * 蓄力阶段：用强力前馈把腿压短，同时等腿真正到位再起跳 - * 起跳阶段：直接给大力蹬地，绕过正常PID/LQR + * 触发后直接进入 LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE */ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) { uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000); @@ -299,49 +328,39 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float extra_force[0] = 0.0f; extra_force[1] = 0.0f; - /* 检测跳跃触发 */ + /* ==================== 跳跃触发检测 ==================== */ + /* 触发后直接进入LAUNCH */ if (c->jump.trigger && c->jump.state == JUMP_IDLE) { - c->jump.state = JUMP_CROUCH; + c->jump.state = JUMP_LAUNCH; /* 直接进入起跳阶段 */ c->jump.state_start_time = current_time; c->jump.trigger = false; } - /* elapsed_time 必须在触发逻辑之后计算，否则第一帧会用旧的 state_start_time */ + /* elapsed_time 必须在触发逻辑之后计算 */ uint32_t elapsed_time = current_time - c->jump.state_start_time; - /* 跳跃状态机 */ + /* ==================== 跳跃状态机 ==================== */ switch (c->jump.state) { case JUMP_IDLE: + /* 待机状态，预收腿逻辑已在上面处理 */ break; - case JUMP_CROUCH: { - /* 蓄力阶段：目标腿长设短 */ - target_L0[0] = c->param->jump.crouch_leg_length; - target_L0[1] = c->param->jump.crouch_leg_length; - extra_force[0] = 0.0f; - extra_force[1] = 0.0f; - - /* 位置驱动：腿缩到位就起跳（差值<2cm） */ - float ct = c->param->jump.crouch_leg_length; - bool leg_ready = (c->vmc_[0].leg.L0 < ct + 0.02f) && - (c->vmc_[1].leg.L0 < ct + 0.02f); - if (leg_ready) { - c->jump.state = JUMP_LAUNCH; - c->jump.state_start_time = current_time; - } - /* 超时保护 */ - if (elapsed_time >= c->param->jump.crouch_time_ms) { - c->jump.state = JUMP_LAUNCH; - c->jump.state_start_time = current_time; - } - } break; + case JUMP_CROUCH: + /* CROUCH状态已废弃，直接跳到LAUNCH */ + c->jump.state = JUMP_LAUNCH; + c->jump.state_start_time = current_time; + break; case JUMP_LAUNCH: { - /* 起跳阶段：腿伸长 + 额外蹬地力 */ + /* 起跳阶段：腿伸长 + 大推力 */ target_L0[0] = c->param->leg.max_length; target_L0[1] = c->param->leg.max_length; - extra_force[0] = c->param->jump.launch_force; - extra_force[1] = c->param->jump.launch_force; + + /* 参考Chassis项目：直接给大推力，不依赖PID增益 */ + /* 腿长误差约0.17m，Kp=70000时，力=70000*0.17=11900N */ + /* 考虑到机器人重量和跳跃高度，给5000-8000N的推力 */ + extra_force[0] = 6000.0f; /* 直接给推力(N) */ + extra_force[1] = 6000.0f; /* 位置驱动：腿伸到位就收腿（差值<3cm） */ bool leg_ext = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) && @@ -392,7 +411,70 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float } } -/* Public functions --------------------------------------------------------- */ +/** + * @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 初始化底盘 @@ -604,8 +686,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */ if (c_cmd->jump_trigger && - (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR || - c->mode == CHASSIS_MODE_JUMP_TEST) && + (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) && c->jump.state == JUMP_IDLE) { c->jump.trigger = true; } @@ -638,23 +719,53 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CHASSIS_MODE_RECOVER: { - float fn = -20.0f, tp = 0.0f; - Chassis_LQRControl(c, c_cmd); + /* 运行自起状态机 */ + 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); - float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left); - float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right); + if (isnan(spring_left)) spring_left = 0.0f; + if (isnan(spring_right)) spring_right = 0.0f; - VMC_InverseSolve(&c->vmc_[0], fn_left, tp); + /* 左腿 */ + 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, tp); + 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] = c_cmd->move_vec.vx * 0.2f; - c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f; + 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: { @@ -683,71 +794,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_Output(c); break; - case CHASSIS_MODE_JUMP_TEST: { - /* 跳跃测试模式：架空测试，无平衡/LQR，纯腿长PID + 跳跃状态机 */ - c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; - - /* 轮子不输出 */ - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; - - /* 基础腿长目标 */ - float test_target_L0[2]; - float test_extra_force[2] = {0.0f, 0.0f}; - float test_base_leg = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range; - test_target_L0[0] = test_base_leg; - test_target_L0[1] = test_base_leg; - - /* 跳跃控制 */ - Chassis_JumpControl(c, c_cmd, test_target_L0, test_extra_force); - - /* 安全限幅 */ - test_target_L0[0] = LIMIT(test_target_L0[0], c->param->leg.min_length, c->param->leg.max_length); - test_target_L0[1] = LIMIT(test_target_L0[1], c->param->leg.min_length, c->param->leg.max_length); - - /* 腿长变化率 */ - float test_leg_dl[2]; - VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &test_leg_dl[0], NULL); - VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &test_leg_dl[1], NULL); - - /* 弹簧力补偿 */ - float test_spring_L = Chassis_CalcSpringForce(c->vmc_[0].leg.L0); - if (isnan(test_spring_L)) test_spring_L = 0.0f; - float test_spring_R = Chassis_CalcSpringForce(c->vmc_[1].leg.L0); - if (isnan(test_spring_R)) test_spring_R = 0.0f; - - /* 左腿：PID + 基础支撑力(对抗重力) - 弹簧力 + 跳跃前馈力 */ - float test_pid_L = PID_Calc(&c->pid.leg_length[0], test_target_L0[0], - c->vmc_[0].leg.L0, test_leg_dl[0], c->dt); - float test_Fn_L = test_pid_L + c->param->leg.left_base_force - test_spring_L + test_extra_force[0]; - - if (VMC_InverseSolve(&c->vmc_[0], test_Fn_L, 0.0f) == 0) { - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - } else { - c->output.joint[0] = 0.0f; - c->output.joint[1] = 0.0f; - } - - /* 右腿 */ - float test_pid_R = PID_Calc(&c->pid.leg_length[1], test_target_L0[1], - c->vmc_[1].leg.L0, test_leg_dl[1], c->dt); - float test_Fn_R = test_pid_R + c->param->leg.right_base_force - test_spring_R + test_extra_force[1]; - - if (VMC_InverseSolve(&c->vmc_[1], test_Fn_R, 0.0f) == 0) { - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - } else { - c->output.joint[2] = 0.0f; - c->output.joint[3] = 0.0f; - } - - /* 安全限幅 */ - for (int i = 0; i < 4; i++) { - c->output.joint[i] = LIMIT(c->output.joint[i], -60.0f, 60.0f); - } - - Chassis_Output(c); - } break; - default: return CHASSIS_ERR_MODE; } @@ -898,8 +944,13 @@ 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 + c->yaw_control.yaw_force; - c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - c->yaw_control.yaw_force; + /* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */ + float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f; + float yaw_scale = c->param->leg.base_length / avg_L0; /* 以基础腿长为基准归一化 */ + float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale; + + 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; /* ==================== 横滚角补偿 ==================== */ /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */ @@ -932,8 +983,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== 腿长控制 ==================== */ float target_L0[2]; float jump_extra_force[2] = {0.0f, 0.0f}; - + /* 基础腿长目标 (应用几何前馈补偿) */ + /* height 可以为负值用于收腿（预蹲），正值用于伸腿 */ float base_leg_length = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range; target_L0[0] = base_leg_length + roll_leg_compensation_left; target_L0[1] = base_leg_length + roll_leg_compensation_right; @@ -988,6 +1040,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float virtual_force_left, virtual_torque_left; if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) { /* 跳跃阶段：完全接管力控 */ + /* 所有跳跃阶段统一处理：PID + 额外力 */ virtual_force_left = jump_extra_force[0] + pid_output_left; virtual_torque_left = 0.0f; /* 跳跃阶段同时清零轮子，防止平衡控制干扰 */ @@ -996,7 +1049,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } else { /* 正常状态：添加Roll力补偿 (低增益避免抖动) */ virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) + - pid_output_left + c->param->leg.left_base_force - spring_force_left + + pid_output_left + c->param->leg.left_base_force - spring_force_left + jump_extra_force[0] + roll_force_compensation; virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0]; } @@ -1017,12 +1070,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float virtual_force_right, virtual_torque_right; if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) { /* 跳跃阶段：完全接管力控 */ + /* 所有跳跃阶段统一处理：PID + 额外力 */ virtual_force_right = jump_extra_force[1] + pid_output_right; virtual_torque_right = 0.0f; } else { /* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */ virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) + - pid_output_right + c->param->leg.right_base_force - spring_force_right + + pid_output_right + c->param->leg.right_base_force - spring_force_right + jump_extra_force[1] - roll_force_compensation; virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1]; } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index dd3f3ec..6f04b0e 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -41,9 +41,8 @@ typedef enum { CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */ // CHASSIS_MODE_CALIBRATE, /* 校准模式 */ - CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ - CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/ - CHASSIS_MODE_JUMP_TEST /* 跳跃测试模式：架空测试，无平衡控制，纯验证跳跃动作 */ + CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */ + CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/ } Chassis_Mode_t; /* 跳跃状态枚举 */ @@ -55,6 +54,14 @@ typedef enum { JUMP_LAND /* 落地阶段：恢复正常控制 */ } Jump_State_t; +/* 自起状态枚举 */ +typedef enum { + RECOVER_FLIP, /* 翻身阶段：theta朝上时，先慢慢翻转到腿朝下 */ + RECOVER_STRETCH, /* 伸腿阶段：腿伸到最大长度 */ + RECOVER_BACKLEG, /* 后甩腿阶段：phi0 趋近目标角度 */ + RECOVER_COMPLETE /* 完成：切换到平衡模式 */ +} Recover_State_t; + typedef struct { Chassis_Mode_t mode; /* 底盘模式 */ MoveVector_t move_vec; /* 底盘运动向量 */ @@ -198,6 +205,11 @@ typedef struct { float retract_leg_length; /* 收腿时的腿长 */ } jump; + /* 自起控制相关 */ + struct { + Recover_State_t state[2]; /* 左右腿自起状态 */ + } recover; + /* PID计算的目标值 */ struct { AHRS_Eulr_t chassis; diff --git a/User/module/config.c b/User/module/config.c index f5a17c1..90c8e3d 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -218,7 +218,7 @@ Config_RobotParam_t robot_config = { .i = 0.01f, .d = 2.0f, .i_limit = 0.0f, - .out_limit = 200.0f, + .out_limit = 500.0f, .d_cutoff_freq = -1.0f, .range = -1.0f, }, @@ -310,19 +310,19 @@ Config_RobotParam_t robot_config = { .mech = { .wheel_radius = 0.068f, - .wheel_gear_ratio = 1.0f, + .wheel_gear_ratio = 4.5f, .hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ .joint_zero = {0.0f, 0.0f, 3.84780002f, -1.16999996f}, /* 关节电机零点偏移 */ .mech_zero_yaw = 1.78040409f, /* 机械零点 */ }, .leg = { - .base_length = 0.18f, /* 基础腿长 (m) */ - .length_range = 0.07f, /* 腿长调节范围 (m) */ - .min_length = 0.12f, /* 最小腿长 (m) */ - .max_length = 0.30f, /* 最大腿长 (m) */ - .left_base_force = 40.0f, /* 左腿基础支撑力 (N) */ - .right_base_force = 40.0f, /* 右腿基础支撑力 (N) */ + .base_length = 0.16f, /* 基础腿长 (m) */ + .length_range = 0.14f, /* 腿长调节范围 (m) */ + .min_length = 0.14f, /* 最小腿长 (m) */ + .max_length = 0.32f, /* 最大腿长 (m) */ + .left_base_force = 65.0f, /* 左腿基础支撑力 (N) */ + .right_base_force = 65.0f, /* 右腿基础支撑力 (N) */ }, .motion = { @@ -347,20 +347,20 @@ Config_RobotParam_t robot_config = { } }, - .lqr_gains = { - .k11_coeff = { -2.110325959941390e+02f, 2.474876302869424e+02f, -1.259312069344584e+02f, -3.555067587081389e+00f }, // theta - .k12_coeff = { 1.729035713826111e+00f, -1.094205267383853e+00f, -8.235618497594484e+00f, -2.949091603597677e-01f }, // d_theta - .k13_coeff = { -3.661055595462458e+01f, 4.066046385752247e+01f, -1.618786894025212e+01f, -1.618742454043447e+00f }, // x - .k14_coeff = { -2.988926501197955e+01f, 3.445642428071118e+01f, -1.547117906530474e+01f, -2.039186188549067e+00f }, // d_x - .k15_coeff = { -4.094585161782607e+01f, 6.119228618474273e+01f, -3.603009625243965e+01f, 1.037078067693317e+01f }, // phi - .k16_coeff = { -7.625020535837323e+00f, 1.097226354374976e+01f, -6.302244199082423e+00f, 1.920720114282383e+00f }, // d_phi - .k21_coeff = { 3.113860878856309e+02f, -2.577426343951217e+02f, 4.982404130107013e+01f, 1.415180465415879e+01f }, // theta - .k22_coeff = { 1.126082466887103e+01f, -9.812804977178935e+00f, 1.814407629267238e+00f, 1.941377111901043e+00f }, // d_theta - .k23_coeff = { -1.771607516002724e+01f, 4.047388400543465e+01f, -2.962942748901807e+01f, 8.971209797230671e+00f }, // x - .k24_coeff = { -3.391460592939657e+01f, 5.575858169617614e+01f, -3.466996436706782e+01f, 9.833893476570065e+00f }, // d_x - .k25_coeff = { 2.392778926806863e+02f, -2.574664846375947e+02f, 9.953632837845201e+01f, 2.579297039170335e+00f }, // phi - .k26_coeff = { 4.297556375859175e+01f, -4.657509218314731e+01f, 1.826212630358702e+01f, 1.257410509642398e-01f }, // d_phi - }, +.lqr_gains = { + .k11_coeff = { -2.207343401461175e+02f, 2.556834548508151e+02f, -1.234911826220423e+02f, -4.363637275740159e+00f }, // theta + .k12_coeff = { -3.770331874039195e+00f, 4.741937570219265e+00f, -8.819635513801229e+00f, -4.038842223045839e-01f }, // d_theta + .k13_coeff = { -1.734094544170626e+01f, 1.978491180834041e+01f, -8.072812769263495e+00f, -1.340589378097037e+00f }, // x + .k14_coeff = { -1.472211724053756e+01f, 1.819479665184211e+01f, -8.589647618765262e+00f, -2.293363271026126e+00f }, // d_x + .k15_coeff = { -7.852280760746451e+01f, 1.298695612217853e+02f, -8.129414466725356e+01f, 2.292846364959872e+01f }, // phi + .k16_coeff = { -9.928427113080220e+00f, 1.357080611082777e+01f, -7.548018651928150e+00f, 2.271157830136482e+00f }, // d_phi + .k21_coeff = { 2.759964883186658e+02f, -2.086511949454606e+02f, 2.331823984594646e+01f, 2.000175147642214e+01f }, // theta + .k22_coeff = { 8.647051328755200e+00f, -3.808605265669558e+00f, -2.849484014325586e+00f, 2.885576178300258e+00f }, // d_theta + .k23_coeff = { -8.076374471738090e+00f, 2.409302487254470e+01f, -1.916050906481694e+01f, 5.795464166404872e+00f }, // x + .k24_coeff = { -4.239237551080097e+01f, 6.262543107118653e+01f, -3.577382630475294e+01f, 9.121364590201878e+00f }, // d_x + .k25_coeff = { 5.289371139851175e+02f, -5.748160815705720e+02f, 2.236549747702434e+02f, 1.625392607061903e+01f }, // phi + .k26_coeff = { 3.803765357892968e+01f, -4.291478873067381e+01f, 1.776277142102920e+01f, 7.733428194766802e-01f }, // d_phi +}, .jump = { diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 55a34b3..5e9b2df 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -41,17 +41,21 @@ Chassis_IMU_t chassis_imu; static int print_chassis(const void *data, char *buf, size_t size) { const Chassis_t *chassis = (const Chassis_t *)data; return MRobot_Snprintf(buf, size, - " Mode : %d\r\n" + " Mode : %d Recover: L=%d R=%d\r\n" " Jump : state=%d trigger=%d\r\n" " LegLen : L=%.3f R=%.3f (m)\r\n" + " Theta : L=%.3f R=%.3f (rad)\r\n" + " Phi0 : L=%.3f R=%.3f (rad)\r\n" " IMU : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n" " Motors : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n" " Wheels : W0=%.1f W1=%.1f (rpm)\r\n" " Output : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n" " VelX : %.3f m/s\r\n", - chassis->mode, + chassis->mode, chassis->recover.state[0], chassis->recover.state[1], chassis->jump.state, chassis->jump.trigger, chassis->vmc_[0].leg.L0, chassis->vmc_[1].leg.L0, + chassis->vmc_[0].leg.theta, chassis->vmc_[1].leg.theta, + chassis->vmc_[0].leg.phi0, chassis->vmc_[1].leg.phi0, chassis->feedback.imu.euler.rol, chassis->feedback.imu.euler.pit, chassis->feedback.imu.euler.yaw, chassis->feedback.joint[0].rotor_speed, chassis->feedback.joint[1].rotor_speed, chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed, diff --git a/User/task/rc.c b/User/task/rc.c index b10d567..0105f82 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -141,18 +141,37 @@ void Task_rc(void *argument) { osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0); /************************* 底盘命令 **************************************/ - /* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */ - static bool ch_res_was_min = false; /* 记录上次是否在最低位置 */ - static uint8_t jump_trigger_hold_cnt = 0; /* 触发保持计数，避免单拍脉冲被队列重置吞掉 */ - const float CH_RES_MIN_THRESHOLD = -0.9f; /* 判定为最低位置的阈值 */ - const float CH_RES_RELEASE_THRESHOLD = -0.3f; /* 判定为松开的阈值 */ + /* 蓄力-释放跳跃逻辑：向上推蓄力收腿，松开回弹时触发跳跃 */ + static float last_ch_res = 0.0f; /* 上一次拨杆位置 */ + static float min_ch_res = 0.0f; /* 记录最小值（最大蓄力） */ + static uint8_t jump_trigger_hold_cnt = 0; /* 触发保持计数 */ + static bool in_charge_state = false; /* 是否处于蓄力状态 */ - if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) { - ch_res_was_min = true; /* 记录已到达最低位置 */ - } else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) { - /* 从最低位置松开，触发跳跃 */ - jump_trigger_hold_cnt = 5; /* 保持5个RC周期，确保ctrl任务一定能收到 */ - ch_res_was_min = false; /* 重置状态 */ + const float CHARGE_THRESHOLD = -0.4f; /* 蓄力阈值：低于此值开始蓄力 */ + const float RELEASE_THRESHOLD = -0.2f; /* 释放阈值：回到此值以上触发跳跃 */ + const float MIN_CHARGE = -0.5f; /* 最小蓄力量：至少要推到此值才能触发 */ + + /* 检测蓄力：拨杆向上推 */ + if (dr16.data.ch_res < CHARGE_THRESHOLD) { + in_charge_state = true; + if (dr16.data.ch_res < min_ch_res) { + min_ch_res = dr16.data.ch_res; /* 更新最小值 */ + } + } + + /* 检测释放：拨杆回弹到阈值以上 && 之前有足够蓄力 */ + if (in_charge_state && + dr16.data.ch_res > RELEASE_THRESHOLD && + min_ch_res < MIN_CHARGE) { + jump_trigger_hold_cnt = 5; /* 触发跳跃，保持5个RC周期 */ + in_charge_state = false; /* 退出蓄力状态 */ + min_ch_res = 0.0f; /* 重置蓄力状态 */ + } + + /* 拨杆回到中位，重置蓄力 */ + if (dr16.data.ch_res > -0.1f) { + in_charge_state = false; + min_ch_res = 0.0f; } cmd_for_chassis.jump_trigger = (jump_trigger_hold_cnt > 0); @@ -160,6 +179,8 @@ void Task_rc(void *argument) { jump_trigger_hold_cnt--; } + last_ch_res = dr16.data.ch_res; /* 保存当前值 */ + switch (dr16.data.sw_l) { case DR16_SW_UP: cmd_for_chassis.mode = CHASSIS_MODE_RELAX; @@ -169,10 +190,7 @@ switch (dr16.data.sw_l) { cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; break; case DR16_SW_DOWN: - cmd_for_chassis.mode = CHASSIS_MODE_JUMP_TEST; - // cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR; - // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE; + cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR; break; default: cmd_for_chassis.mode = CHASSIS_MODE_RELAX; @@ -181,7 +199,8 @@ switch (dr16.data.sw_l) { cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y; cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x; cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x; - cmd_for_chassis.height = max(dr16.data.ch_res, 0.0f); + /* height 传递拨杆位置，负值表示下推收腿，正值表示上推伸腿 */ + cmd_for_chassis.height = dr16.data.ch_res; osMessageQueueReset( task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞 diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 1237a19..d46203b 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -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([1 1 20 5 200 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[1.5 0;0 0.25]; %T Tp + Q=diag([4000 200 1000 500 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[150 0;0 25]; %T Tp K=lqr(A,B,Q,R);