Compare commits
4 Commits
c5d5928212
...
6480f59e1e
| Author | SHA1 | Date | |
|---|---|---|---|
| 6480f59e1e | |||
| 5ca466dd45 | |||
| 2c5c77645d | |||
| 2a51286ca3 |
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@ -18,16 +18,8 @@
|
||||
/* Private defines ---------------------------------------------------------- */
|
||||
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||||
|
||||
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
|
||||
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
|
||||
#define WHEEL_GEAR_RATIO 4.5f /* 轮毂电机扭矩 */
|
||||
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
|
||||
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
|
||||
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
|
||||
#define MAX_LEG_LENGTH 0.33f /* 最大腿长 (m) */
|
||||
#define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */
|
||||
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
|
||||
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
|
||||
/* 物理常量（不可调） */
|
||||
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m)，固定机械尺寸 */
|
||||
|
||||
|
||||
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
|
||||
@ -39,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 -------------------------------------------------------- */
|
||||
|
||||
@ -120,7 +113,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
|
||||
*/
|
||||
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
|
||||
float current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||||
float zero_point_1 = c->param->mech_zero_yaw;
|
||||
float zero_point_1 = c->param->mech.mech_zero_yaw;
|
||||
float zero_point_2 = zero_point_1 + M_PI;
|
||||
|
||||
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
|
||||
@ -145,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;
|
||||
}
|
||||
@ -289,78 +312,96 @@ static float Chassis_CalcSpringForce(float leg_length)
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 跳跃控制状态机
|
||||
* @brief 跳跃控制
|
||||
* @param c 底盘结构体指针
|
||||
* @param c_cmd 控制指令
|
||||
* @param target_L0 输出的目标腿长数组[2]
|
||||
* @param extra_force 输出的额外力数组[2]
|
||||
*
|
||||
* @note 跳跃流程：
|
||||
* 触发后直接进入 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); /* 当前时间 ms */
|
||||
uint32_t elapsed_time = current_time - c->jump.state_start_time;
|
||||
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
|
||||
|
||||
/* 初始化额外力为0 */
|
||||
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; /* 清除触发标志 */
|
||||
c->jump.trigger = false;
|
||||
}
|
||||
|
||||
/* 跳跃状态机 */
|
||||
/* 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_params.crouch_leg_length;
|
||||
target_L0[1] = c->param->jump_params.crouch_leg_length;
|
||||
|
||||
if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
|
||||
c->jump.state = JUMP_LAUNCH;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
/* CROUCH状态已废弃，直接跳到LAUNCH */
|
||||
c->jump.state = JUMP_LAUNCH;
|
||||
c->jump.state_start_time = current_time;
|
||||
break;
|
||||
|
||||
case JUMP_LAUNCH:
|
||||
/* 起跳阶段：发力向下推地面 */
|
||||
target_L0[0] = MAX_LEG_LENGTH; /* 腿伸长 */
|
||||
target_L0[1] = MAX_LEG_LENGTH;
|
||||
extra_force[0] = c->param->jump_params.launch_force; /* 额外向下的力 */
|
||||
extra_force[1] = c->param->jump_params.launch_force;
|
||||
case JUMP_LAUNCH: {
|
||||
/* 起跳阶段：腿伸长 + 大推力 */
|
||||
target_L0[0] = c->param->leg.max_length;
|
||||
target_L0[1] = c->param->leg.max_length;
|
||||
|
||||
if (elapsed_time >= c->param->jump_params.launch_time_ms) {
|
||||
/* 参考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) &&
|
||||
(c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
|
||||
if (leg_ext) {
|
||||
c->jump.state = JUMP_RETRACT;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
break;
|
||||
/* 超时保护 */
|
||||
if (elapsed_time >= c->param->jump.launch_time_ms) {
|
||||
c->jump.state = JUMP_RETRACT;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
} break;
|
||||
|
||||
case JUMP_RETRACT:
|
||||
/* 收腿阶段：腿收缩准备落地 */
|
||||
target_L0[0] = c->param->jump_params.retract_leg_length;
|
||||
target_L0[1] = c->param->jump_params.retract_leg_length;
|
||||
extra_force[0] = c->param->jump_params.retract_force; /* 前馈力帮助收腿 */
|
||||
extra_force[1] = c->param->jump_params.retract_force;
|
||||
case JUMP_RETRACT: {
|
||||
/* 收腿阶段 */
|
||||
target_L0[0] = c->param->jump.retract_leg_length;
|
||||
target_L0[1] = c->param->jump.retract_leg_length;
|
||||
extra_force[0] = c->param->jump.retract_force;
|
||||
extra_force[1] = c->param->jump.retract_force;
|
||||
|
||||
/* 检测落地或超时 */
|
||||
if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
|
||||
/* 位置驱动：腿缩到位就进落地缓冲 */
|
||||
float rt = c->param->jump.retract_leg_length;
|
||||
bool leg_retracted = (c->vmc_[0].leg.L0 < rt + 0.02f) &&
|
||||
(c->vmc_[1].leg.L0 < rt + 0.02f);
|
||||
if (leg_retracted) {
|
||||
c->jump.state = JUMP_LAND;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
break;
|
||||
/* 超时保护 */
|
||||
if (elapsed_time >= c->param->jump.retract_time_ms) {
|
||||
c->jump.state = JUMP_LAND;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
} break;
|
||||
|
||||
case JUMP_LAND:
|
||||
/* 落地阶段：缓冲并恢复正常 */
|
||||
/* 使用正常腿长，让PID自动恢复 */
|
||||
|
||||
if (elapsed_time >= c->param->jump_params.land_time_ms) {
|
||||
/* 落地缓冲：时间到就回 IDLE */
|
||||
if (elapsed_time >= c->param->jump.land_time_ms) {
|
||||
c->jump.state = JUMP_IDLE;
|
||||
c->jump.state_start_time = current_time;
|
||||
}
|
||||
break;
|
||||
|
||||
@ -370,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 初始化底盘
|
||||
@ -499,9 +603,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
|
||||
c->jump.state = JUMP_IDLE;
|
||||
c->jump.trigger = false;
|
||||
c->jump.state_start_time = 0;
|
||||
c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length;
|
||||
c->jump.launch_force = c->param->jump_params.launch_force;
|
||||
c->jump.retract_leg_length = c->param->jump_params.retract_leg_length;
|
||||
c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
|
||||
c->jump.launch_force = c->param->jump.launch_force;
|
||||
c->jump.retract_leg_length = c->param->jump.retract_leg_length;
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
@ -532,7 +636,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
|
||||
c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
|
||||
|
||||
/* 应用零点偏移 */
|
||||
c->feedback.joint[i].rotor_abs_angle -= c->param->joint_zero[i];
|
||||
c->feedback.joint[i].rotor_abs_angle -= c->param->mech.joint_zero[i];
|
||||
}
|
||||
|
||||
/* 更新轮子电机反馈 */
|
||||
@ -582,7 +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_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) &&
|
||||
c->jump.state == JUMP_IDLE) {
|
||||
c->jump.trigger = true;
|
||||
}
|
||||
@ -615,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: {
|
||||
@ -716,7 +850,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
|
||||
// /* 加速度限制 */
|
||||
// float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
|
||||
// float max_velocity_change = MAX_ACCELERATION * c->dt;
|
||||
// float max_velocity_change = c->param->motion.max_acceleration * c->dt;
|
||||
// velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
|
||||
|
||||
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
|
||||
@ -750,9 +884,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
|
||||
/* ==================== 目标状态 ==================== */
|
||||
LQR_State_t target_state = {
|
||||
.theta = 0.1f,
|
||||
.theta = 0.0f,
|
||||
.d_theta = 0.0f,
|
||||
.phi = -0.1f,
|
||||
.phi = 0.0f,
|
||||
.d_phi = 0.0f,
|
||||
.x = c->chassis_state.target_x,
|
||||
.d_x = c->chassis_state.target_velocity_x,
|
||||
@ -763,8 +897,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||||
|
||||
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
|
||||
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
|
||||
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
|
||||
float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
|
||||
float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
|
||||
|
||||
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
|
||||
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
|
||||
@ -786,11 +920,11 @@ 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 = NON_CONTACT_THETA;
|
||||
target_state.theta = c->param->motion.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 + NON_CONTACT_THETA) +
|
||||
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + c->param->motion.non_contact_theta) +
|
||||
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
|
||||
c->yaw_control.yaw_force = 0.0f;
|
||||
}
|
||||
@ -800,22 +934,27 @@ 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 = NON_CONTACT_THETA;
|
||||
target_state.theta = c->param->motion.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 + NON_CONTACT_THETA) +
|
||||
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + c->param->motion.non_contact_theta) +
|
||||
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
|
||||
c->yaw_control.yaw_force = 0.0f;
|
||||
}
|
||||
|
||||
/* ==================== 轮毂输出 ==================== */
|
||||
c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
|
||||
c->output.wheel[1] = c->lqr[1].control_output.T / 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: 几何前馈腿长补偿 (参考底盘文件夹算法) */
|
||||
float Rl = c->param->hip_width / 2.0f; /* 髋宽的一半 */
|
||||
float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */
|
||||
float roll_error = c->feedback.imu.euler.rol - 0.0f; /* Roll角误差 */
|
||||
float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0; /* 右腿减左腿腿长差 */
|
||||
|
||||
@ -846,7 +985,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
float jump_extra_force[2] = {0.0f, 0.0f};
|
||||
|
||||
/* 基础腿长目标 (应用几何前馈补偿) */
|
||||
float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE;
|
||||
/* 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;
|
||||
|
||||
@ -859,23 +999,23 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
/* 检查哪条腿更短（需要缩短的那条） */
|
||||
if (target_L0[0] < target_L0[1]) {
|
||||
/* 左腿更短，检查是否触碰下限 */
|
||||
if (target_L0[0] < MIN_LEG_LENGTH) {
|
||||
compensation_transfer = MIN_LEG_LENGTH - target_L0[0]; /* 计算短缺量 */
|
||||
target_L0[0] = MIN_LEG_LENGTH; /* 左腿限制在最小值 */
|
||||
if (target_L0[0] < c->param->leg.min_length) {
|
||||
compensation_transfer = c->param->leg.min_length - target_L0[0]; /* 计算短缺量 */
|
||||
target_L0[0] = c->param->leg.min_length; /* 左腿限制在最小值 */
|
||||
target_L0[1] += compensation_transfer; /* 右腿补偿增加 */
|
||||
}
|
||||
} else if (target_L0[1] < target_L0[0]) {
|
||||
/* 右腿更短，检查是否触碰下限 */
|
||||
if (target_L0[1] < MIN_LEG_LENGTH) {
|
||||
compensation_transfer = MIN_LEG_LENGTH - target_L0[1]; /* 计算短缺量 */
|
||||
target_L0[1] = MIN_LEG_LENGTH; /* 右腿限制在最小值 */
|
||||
if (target_L0[1] < c->param->leg.min_length) {
|
||||
compensation_transfer = c->param->leg.min_length - target_L0[1]; /* 计算短缺量 */
|
||||
target_L0[1] = c->param->leg.min_length; /* 右腿限制在最小值 */
|
||||
target_L0[0] += compensation_transfer; /* 左腿补偿增加 */
|
||||
}
|
||||
}
|
||||
|
||||
/* 最终安全限幅 */
|
||||
target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
|
||||
target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
|
||||
target_L0[0] = LIMIT(target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
|
||||
target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_length);
|
||||
|
||||
/* 获取腿长变化率 */
|
||||
float leg_d_length[2];
|
||||
@ -895,18 +1035,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
||||
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
||||
float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||||
if (isnan(spring_force_left)) spring_force_left = 0.0f; /* 处理无效值 */
|
||||
if (isnan(spring_force_left)) spring_force_left = 0.0f;
|
||||
|
||||
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
|
||||
float virtual_force_left, virtual_torque_left;
|
||||
if (c->jump.state == JUMP_RETRACT) {
|
||||
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
|
||||
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
|
||||
virtual_torque_left = 0.0f; /* 收腿时不控制摆角 */
|
||||
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;
|
||||
/* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
|
||||
c->output.wheel[0] = 0.0f;
|
||||
c->output.wheel[1] = 0.0f;
|
||||
} else {
|
||||
/* 正常状态：添加Roll力补偿 (低增益避免抖动) */
|
||||
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
|
||||
pid_output_left + 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];
|
||||
}
|
||||
@ -922,18 +1065,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
||||
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
||||
float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||||
if (isnan(spring_force_right)) spring_force_right = 0.0f; /* 处理无效值 */
|
||||
if (isnan(spring_force_right)) spring_force_right = 0.0f;
|
||||
|
||||
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
|
||||
float virtual_force_right, virtual_torque_right;
|
||||
if (c->jump.state == JUMP_RETRACT) {
|
||||
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
|
||||
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
|
||||
virtual_torque_right = 0.0f; /* 收腿时不控制摆角 */
|
||||
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 + 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];
|
||||
}
|
||||
|
||||
@ -41,7 +41,7 @@ typedef enum {
|
||||
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
|
||||
CHASSIS_MODE_RECOVER, /* 复位模式 */
|
||||
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
|
||||
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
|
||||
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
|
||||
CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
|
||||
} Chassis_Mode_t;
|
||||
|
||||
@ -54,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; /* 底盘运动向量 */
|
||||
@ -89,37 +97,61 @@ typedef struct {
|
||||
VMC_Param_t vmc_param[2]; /* VMC参数 */
|
||||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||||
|
||||
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
|
||||
KPID_Params_t roll; /* roll轴腿长补偿PID控制参数 */
|
||||
KPID_Params_t roll_force; /* roll轴力补偿PID控制参数 */
|
||||
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
|
||||
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
|
||||
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
|
||||
KPID_Params_t yaw; /* yaw轴PID控制参数 */
|
||||
KPID_Params_t roll; /* roll轴腿长补偿PID */
|
||||
KPID_Params_t roll_force; /* roll轴力补偿PID */
|
||||
KPID_Params_t tp; /* 摆力矩PID */
|
||||
KPID_Params_t leg_length; /* 腿长PID */
|
||||
KPID_Params_t leg_theta; /* 摆角PID */
|
||||
|
||||
/* 机械参数 */
|
||||
struct {
|
||||
uint32_t crouch_time_ms; /* 蓄力时间 (ms) */
|
||||
uint32_t launch_time_ms; /* 起跳发力时间 (ms) */
|
||||
uint32_t retract_time_ms; /* 收腿时间 (ms) */
|
||||
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */
|
||||
float crouch_leg_length; /* 蓄力时腿长 (m) */
|
||||
float launch_force; /* 起跳力 (N) */
|
||||
float retract_leg_length; /* 收腿时腿长 (m) */
|
||||
float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */
|
||||
} jump_params;
|
||||
float wheel_radius; /* 轮子半径 (m) */
|
||||
float wheel_gear_ratio; /* 轮毂电机减速比 */
|
||||
float hip_width; /* 髋宽，两腿间距 (m) */
|
||||
float joint_zero[4]; /* 关节电机零点偏移 */
|
||||
float mech_zero_yaw; /* yaw轴机械零点 */
|
||||
} mech;
|
||||
|
||||
float joint_zero[4]; /* 关节电机零点偏移位置 */
|
||||
float hip_width; /* 髋宽，两腿间距 (m) */
|
||||
/* 腿长控制参数 */
|
||||
struct {
|
||||
float base_length; /* 基础腿长 (m) */
|
||||
float length_range; /* 腿长调节范围 (m)，波轮控制 */
|
||||
float min_length; /* 最小腿长 (m) */
|
||||
float max_length; /* 最大腿长 (m) */
|
||||
float left_base_force; /* 左腿基础支撑力 (N) */
|
||||
float right_base_force; /* 右腿基础支撑力 (N) */
|
||||
} leg;
|
||||
|
||||
float mech_zero_yaw; /* 机械零点 */
|
||||
/* 跳跃参数 */
|
||||
struct {
|
||||
uint32_t crouch_time_ms; /* 蓄力超时 (ms) */
|
||||
uint32_t launch_time_ms; /* 起跳超时 (ms) */
|
||||
uint32_t retract_time_ms; /* 收腿超时 (ms) */
|
||||
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */
|
||||
float crouch_leg_length; /* 蓄力时腿长 (m) */
|
||||
float launch_force; /* 起跳额外力 (N) */
|
||||
float retract_leg_length; /* 收腿时腿长 (m) */
|
||||
float retract_force; /* 收腿前馈力 (N)，负值=向上 */
|
||||
} jump;
|
||||
|
||||
float theta;
|
||||
float x;
|
||||
float phi;
|
||||
/* 运动控制参数 */
|
||||
struct {
|
||||
float max_acceleration; /* 最大加速度 (m/s²) */
|
||||
float non_contact_theta; /* 离地时摆角目标 (rad) */
|
||||
} motion;
|
||||
|
||||
/* LQR 目标状态偏移 */
|
||||
struct {
|
||||
float theta;
|
||||
float x;
|
||||
float phi;
|
||||
} lqr_offset;
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
struct {
|
||||
float in; /* 输入 */
|
||||
float out; /* 输出 */
|
||||
float in;
|
||||
float out;
|
||||
} low_pass_cutoff_freq;
|
||||
} Chassis_Params_t;
|
||||
|
||||
@ -173,6 +205,11 @@ typedef struct {
|
||||
float retract_leg_length; /* 收腿时的腿长 */
|
||||
} jump;
|
||||
|
||||
/* 自起控制相关 */
|
||||
struct {
|
||||
Recover_State_t state[2]; /* 左右腿自起状态 */
|
||||
} recover;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
AHRS_Eulr_t chassis;
|
||||
|
||||
@ -66,7 +66,7 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
.mech_zero = {
|
||||
.yaw = 0.0f,
|
||||
.pit = 2.2f,
|
||||
.pit = 0.137291431f,
|
||||
},
|
||||
.travel = {
|
||||
.yaw = -1.0f,
|
||||
@ -214,11 +214,11 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
.leg_length={
|
||||
.k = 40.0f,
|
||||
.p = 20.0f,
|
||||
.p = 50.0f,
|
||||
.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,
|
||||
},
|
||||
@ -308,10 +308,29 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
},
|
||||
|
||||
.mech_zero_yaw = 1.78040409f, /* 机械零点 */
|
||||
.joint_zero = {0.0f, 0.0f, 3.84780002f, -1.16999996f}, /* 关节电机零点偏移位置 */
|
||||
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
|
||||
.vmc_param = {
|
||||
.mech = {
|
||||
.wheel_radius = 0.068f,
|
||||
.wheel_gear_ratio = 4.5f,
|
||||
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
|
||||
.joint_zero = {0.0f, 0.0f, 1.32422018f, 1.16195965f}, /* 关节电机零点偏移 */
|
||||
.mech_zero_yaw = 2.96925735f, /* 机械零点 */
|
||||
},
|
||||
|
||||
.leg = {
|
||||
.base_length = 0.16f, /* 基础腿长 (m) */
|
||||
.length_range = 0.20f, /* 腿长调节范围 (m) */
|
||||
.min_length = 0.14f, /* 最小腿长 (m) */
|
||||
.max_length = 0.36f, /* 最大腿长 (m) */
|
||||
.left_base_force = 65.0f, /* 左腿基础支撑力 (N) */
|
||||
.right_base_force = 65.0f, /* 右腿基础支撑力 (N) */
|
||||
},
|
||||
|
||||
.motion = {
|
||||
.max_acceleration = 5.0f, /* 最大加速度 (m/s^2) */
|
||||
.non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */
|
||||
},
|
||||
|
||||
.vmc_param = {
|
||||
{ // 左腿
|
||||
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
|
||||
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
|
||||
@ -327,36 +346,38 @@ Config_RobotParam_t robot_config = {
|
||||
.hip_length = 0.0f // 髋宽 (L5) (m)
|
||||
}
|
||||
},
|
||||
|
||||
.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
|
||||
.k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
|
||||
.k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta
|
||||
.k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x
|
||||
.k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f }, // d_x
|
||||
.k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi
|
||||
.k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f }, // d_phi
|
||||
.k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta
|
||||
.k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f }, // d_theta
|
||||
.k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f }, // x
|
||||
.k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x
|
||||
.k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
|
||||
.k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi
|
||||
},
|
||||
|
||||
|
||||
.jump_params = {
|
||||
.crouch_time_ms = 100,
|
||||
.launch_time_ms = 120,
|
||||
.retract_time_ms = 80,
|
||||
.land_time_ms = 300,
|
||||
.crouch_leg_length = 0.14f,
|
||||
.launch_force = 200.0f,
|
||||
.retract_leg_length = 0.1f, /* 收腿目标更短 */
|
||||
.retract_force = -120.0f, /* 收腿前馈力加大 */
|
||||
.jump = {
|
||||
.crouch_time_ms = 500,
|
||||
.launch_time_ms = 300,
|
||||
.retract_time_ms = 500,
|
||||
.land_time_ms = 500,
|
||||
.crouch_leg_length = 0.13f, /* 不用蹲太低 */
|
||||
.launch_force = 20.0f, /* 起跳力减小 */
|
||||
.retract_leg_length = 0.12f, /* 收腿目标 */
|
||||
.retract_force = -20.0f, /* 收腿前馈力减小 */
|
||||
},
|
||||
|
||||
.lqr_offset = {
|
||||
.theta = 0.0f,
|
||||
.x = 0.0f,
|
||||
.phi = 0.0f,
|
||||
},
|
||||
|
||||
},
|
||||
|
||||
|
||||
@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||||
|
||||
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
|
||||
memset(&s->output,0,sizeof(s->output));
|
||||
s->target_variable.target_rpm=2000.0f; /* 归一化目标转速 */
|
||||
s->target_variable.target_rpm=6800.0f; /* 归一化目标转速 */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -41,13 +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,
|
||||
|
||||
@ -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_RECOVER;
|
||||
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
|
||||
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||||
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
|
||||
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); // 重置消息队列，防止阻塞
|
||||
|
||||
@ -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 500 50 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[150 0;0 5]; %T Tp
|
||||
|
||||
K=lqr(A,B,Q,R);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user