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

跳跃

Browse Source
This commit is contained in:
Robofish 2026-03-03 00:16:50 +08:00
parent 2a51286ca3
commit 2c5c77645d
7 changed files with 249 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

278
User/module/balance_chassis.c
View File

@ -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: 几何前馈腿长补偿 (参考底盘文件夹算法) */
@ -934,6 +985,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
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;
/* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
@ -1017,6 +1070,7 @@ 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 {

18
User/module/balance_chassis.h
View File

@ -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;

44
User/module/config.c
View File

@ -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 = {

8
User/task/ctrl_chassis.c
View File

@ -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,

51
User/task/rc.c
View File

@ -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); // 重置消息队列，防止阻塞

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([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);