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-02-25 17:57:05 +08:00
parent c5d5928212
commit 2a51286ca3
7 changed files with 7717 additions and 7512 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

14815
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

255
User/module/balance_chassis.c
View File

@ -18,16 +18,8 @@
/* Private defines ---------------------------------------------------------- */ /* Private defines ---------------------------------------------------------- */
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x))) #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_RADIUS 0.068f /* 轮子半径 (m)，固定机械尺寸 */
#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) */
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f; // float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
@ -120,7 +112,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
*/ */
static void Chassis_SelectYawZeroPoint(Chassis_t *c) { static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
float current_yaw = c->feedback.yaw.rotor_abs_angle; 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 zero_point_2 = zero_point_1 + M_PI;
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI); float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
@ -294,10 +286,14 @@ static float Chassis_CalcSpringForce(float leg_length)
* @param c_cmd 控制指令 * @param c_cmd 控制指令
* @param target_L0 输出的目标腿长数组[2] * @param target_L0 输出的目标腿长数组[2]
* @param extra_force 输出的额外力数组[2] * @param extra_force 输出的额外力数组[2]
*
* @note 跳跃流程：
* IDLE -> CROUCH(蓄力下蹲) -> LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE
* 蓄力阶段：用强力前馈把腿压短，同时等腿真正到位再起跳
* 起跳阶段：直接给大力蹬地，绕过正常PID/LQR
*/ */
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) { 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 current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
uint32_t elapsed_time = current_time - c->jump.state_start_time;
/* 初始化额外力为0 */ /* 初始化额外力为0 */
extra_force[0] = 0.0f; extra_force[0] = 0.0f;
@ -307,60 +303,86 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
if (c->jump.trigger && c->jump.state == JUMP_IDLE) { if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
c->jump.state = JUMP_CROUCH; c->jump.state = JUMP_CROUCH;
c->jump.state_start_time = current_time; c->jump.state_start_time = current_time;
c->jump.trigger = false; /* 清除触发标志 */ c->jump.trigger = false;
} }
/* elapsed_time 必须在触发逻辑之后计算，否则第一帧会用旧的 state_start_time */
uint32_t elapsed_time = current_time - c->jump.state_start_time;
/* 跳跃状态机 */ /* 跳跃状态机 */
switch (c->jump.state) { switch (c->jump.state) {
case JUMP_IDLE: case JUMP_IDLE:
/* 待机状态，使用正常腿长控制 */
break; break;
case JUMP_CROUCH: case JUMP_CROUCH: {
/* 蓄力阶段：缩短腿长 */ /* 蓄力阶段：目标腿长设短 */
target_L0[0] = c->param->jump_params.crouch_leg_length; target_L0[0] = c->param->jump.crouch_leg_length;
target_L0[1] = c->param->jump_params.crouch_leg_length; target_L0[1] = c->param->jump.crouch_leg_length;
extra_force[0] = 0.0f;
extra_force[1] = 0.0f;
if (elapsed_time >= c->param->jump_params.crouch_time_ms) { /* 位置驱动：腿缩到位就起跳（差值<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 = JUMP_LAUNCH;
c->jump.state_start_time = current_time; c->jump.state_start_time = current_time;
} }
break; /* 超时保护 */
if (elapsed_time >= c->param->jump.crouch_time_ms) {
c->jump.state = JUMP_LAUNCH;
c->jump.state_start_time = current_time;
}
} break;
case JUMP_LAUNCH: case JUMP_LAUNCH: {
/* 起跳阶段：发力向下推地面 */ /* 起跳阶段：腿伸长 + 额外蹬地力 */
target_L0[0] = MAX_LEG_LENGTH; /* 腿伸长 */ target_L0[0] = c->param->leg.max_length;
target_L0[1] = MAX_LEG_LENGTH; target_L0[1] = c->param->leg.max_length;
extra_force[0] = c->param->jump_params.launch_force; /* 额外向下的力 */ extra_force[0] = c->param->jump.launch_force;
extra_force[1] = c->param->jump_params.launch_force; extra_force[1] = c->param->jump.launch_force;
if (elapsed_time >= c->param->jump_params.launch_time_ms) { /* 位置驱动：腿伸到位就收腿（差值<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 = JUMP_RETRACT;
c->jump.state_start_time = current_time; 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: case JUMP_RETRACT: {
/* 收腿阶段：腿收缩准备落地 */ /* 收腿阶段 */
target_L0[0] = c->param->jump_params.retract_leg_length; target_L0[0] = c->param->jump.retract_leg_length;
target_L0[1] = c->param->jump_params.retract_leg_length; target_L0[1] = c->param->jump.retract_leg_length;
extra_force[0] = c->param->jump_params.retract_force; /* 前馈力帮助收腿 */ extra_force[0] = c->param->jump.retract_force;
extra_force[1] = c->param->jump_params.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 = JUMP_LAND;
c->jump.state_start_time = current_time; 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: case JUMP_LAND:
/* 落地阶段：缓冲并恢复正常 */ /* 落地缓冲：时间到就回 IDLE */
/* 使用正常腿长，让PID自动恢复 */ if (elapsed_time >= c->param->jump.land_time_ms) {
if (elapsed_time >= c->param->jump_params.land_time_ms) {
c->jump.state = JUMP_IDLE; c->jump.state = JUMP_IDLE;
c->jump.state_start_time = current_time;
} }
break; break;
@ -499,9 +521,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->jump.state = JUMP_IDLE; c->jump.state = JUMP_IDLE;
c->jump.trigger = false; c->jump.trigger = false;
c->jump.state_start_time = 0; c->jump.state_start_time = 0;
c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length; c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
c->jump.launch_force = c->param->jump_params.launch_force; c->jump.launch_force = c->param->jump.launch_force;
c->jump.retract_leg_length = c->param->jump_params.retract_leg_length; c->jump.retract_leg_length = c->param->jump.retract_leg_length;
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -532,7 +554,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->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 +604,8 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */ /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */
if (c_cmd->jump_trigger && 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->mode == CHASSIS_MODE_JUMP_TEST) &&
c->jump.state == JUMP_IDLE) { c->jump.state == JUMP_IDLE) {
c->jump.trigger = true; c->jump.trigger = true;
} }
@ -660,6 +683,71 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_Output(c); Chassis_Output(c);
break; 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: default:
return CHASSIS_ERR_MODE; return CHASSIS_ERR_MODE;
} }
@ -716,7 +804,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 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); // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change; // float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
@ -750,9 +838,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 目标状态 ==================== */ /* ==================== 目标状态 ==================== */
LQR_State_t target_state = { LQR_State_t target_state = {
.theta = 0.1f, .theta = 0.0f,
.d_theta = 0.0f, .d_theta = 0.0f,
.phi = -0.1f, .phi = 0.0f,
.d_phi = 0.0f, .d_phi = 0.0f,
.x = c->chassis_state.target_x, .x = c->chassis_state.target_x,
.d_x = c->chassis_state.target_velocity_x, .d_x = c->chassis_state.target_velocity_x,
@ -763,8 +851,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; c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
float manual_offset = c_cmd->move_vec.vy * M_PI_2; 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_1 = c->param->mech.mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + 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_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); float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
@ -786,11 +874,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]); LQR_Control(&c->lqr[0]);
} else { } else {
target_state.theta = NON_CONTACT_THETA; target_state.theta = c->param->motion.non_contact_theta;
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp = 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->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
c->yaw_control.yaw_force = 0.0f; c->yaw_control.yaw_force = 0.0f;
} }
@ -800,22 +888,22 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]); LQR_Control(&c->lqr[1]);
} else { } else {
target_state.theta = NON_CONTACT_THETA; target_state.theta = c->param->motion.non_contact_theta;
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp = 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->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
c->yaw_control.yaw_force = 0.0f; 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[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 / 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;
/* ==================== 横滚角补偿 ==================== */ /* ==================== 横滚角补偿 ==================== */
/* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */ /* 方法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 roll_error = c->feedback.imu.euler.rol - 0.0f; /* Roll角误差 */
float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0; /* 右腿减左腿腿长差 */ float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0; /* 右腿减左腿腿长差 */
@ -846,7 +934,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float jump_extra_force[2] = {0.0f, 0.0f}; float jump_extra_force[2] = {0.0f, 0.0f};
/* 基础腿长目标 (应用几何前馈补偿) */ /* 基础腿长目标 (应用几何前馈补偿) */
float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE; 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[0] = base_leg_length + roll_leg_compensation_left;
target_L0[1] = base_leg_length + roll_leg_compensation_right; target_L0[1] = base_leg_length + roll_leg_compensation_right;
@ -859,23 +947,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] < target_L0[1]) {
/* 左腿更短，检查是否触碰下限 */ /* 左腿更短，检查是否触碰下限 */
if (target_L0[0] < MIN_LEG_LENGTH) { if (target_L0[0] < c->param->leg.min_length) {
compensation_transfer = MIN_LEG_LENGTH - target_L0[0]; /* 计算短缺量 */ compensation_transfer = c->param->leg.min_length - target_L0[0]; /* 计算短缺量 */
target_L0[0] = MIN_LEG_LENGTH; /* 左腿限制在最小值 */ target_L0[0] = c->param->leg.min_length; /* 左腿限制在最小值 */
target_L0[1] += compensation_transfer; /* 右腿补偿增加 */ target_L0[1] += compensation_transfer; /* 右腿补偿增加 */
} }
} else if (target_L0[1] < target_L0[0]) { } else if (target_L0[1] < target_L0[0]) {
/* 右腿更短，检查是否触碰下限 */ /* 右腿更短，检查是否触碰下限 */
if (target_L0[1] < MIN_LEG_LENGTH) { if (target_L0[1] < c->param->leg.min_length) {
compensation_transfer = MIN_LEG_LENGTH - target_L0[1]; /* 计算短缺量 */ compensation_transfer = c->param->leg.min_length - target_L0[1]; /* 计算短缺量 */
target_L0[1] = MIN_LEG_LENGTH; /* 右腿限制在最小值 */ target_L0[1] = c->param->leg.min_length; /* 右腿限制在最小值 */
target_L0[0] += compensation_transfer; /* 左腿补偿增加 */ target_L0[0] += compensation_transfer; /* 左腿补偿增加 */
} }
} }
/* 最终安全限幅 */ /* 最终安全限幅 */
target_L0[0] = LIMIT(target_L0[0], 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], MIN_LEG_LENGTH, MAX_LEG_LENGTH); target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_length);
/* 获取腿长变化率 */ /* 获取腿长变化率 */
float leg_d_length[2]; float leg_d_length[2];
@ -895,18 +983,20 @@ 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], 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); c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0); 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; float virtual_force_left, virtual_torque_left;
if (c->jump.state == JUMP_RETRACT) { if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */ /* 跳跃阶段：完全接管力控 */
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0]; virtual_force_left = jump_extra_force[0] + pid_output_left;
virtual_torque_left = 0.0f; /* 收腿时不控制摆角 */ virtual_torque_left = 0.0f;
/* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
} else { } else {
/* 正常状态：添加Roll力补偿 (低增益避免抖动) */ /* 正常状态：添加Roll力补偿 (低增益避免抖动) */
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) + 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; jump_extra_force[0] + roll_force_compensation;
virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0]; virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
} }
@ -922,18 +1012,17 @@ 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], 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); c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0); 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; float virtual_force_right, virtual_torque_right;
if (c->jump.state == JUMP_RETRACT) { if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */ /* 跳跃阶段：完全接管力控 */
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1]; virtual_force_right = jump_extra_force[1] + pid_output_right;
virtual_torque_right = 0.0f; /* 收腿时不控制摆角 */ virtual_torque_right = 0.0f;
} else { } else {
/* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */ /* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) + 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; jump_extra_force[1] - roll_force_compensation;
virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1]; virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
} }

73
User/module/balance_chassis.h
View File

@ -42,7 +42,8 @@ typedef enum {
CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */ // CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/ CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
CHASSIS_MODE_JUMP_TEST /* 跳跃测试模式：架空测试，无平衡控制，纯验证跳跃动作 */
} Chassis_Mode_t; } Chassis_Mode_t;
/* 跳跃状态枚举 */ /* 跳跃状态枚举 */
@ -89,37 +90,61 @@ typedef struct {
VMC_Param_t vmc_param[2]; /* VMC参数 */ VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */ LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */ KPID_Params_t yaw; /* yaw轴PID控制参数 */
KPID_Params_t roll; /* roll轴腿长补偿PID控制参数 */ KPID_Params_t roll; /* roll轴腿长补偿PID */
KPID_Params_t roll_force; /* roll轴力补偿PID控制参数 */ KPID_Params_t roll_force; /* roll轴力补偿PID */
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */ KPID_Params_t tp; /* 摆力矩PID */
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */ KPID_Params_t leg_length; /* 腿长PID */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */ KPID_Params_t leg_theta; /* 摆角PID */
/* 机械参数 */
struct { struct {
uint32_t crouch_time_ms; /* 蓄力时间 (ms) */ float wheel_radius; /* 轮子半径 (m) */
uint32_t launch_time_ms; /* 起跳发力时间 (ms) */ float wheel_gear_ratio; /* 轮毂电机减速比 */
uint32_t retract_time_ms; /* 收腿时间 (ms) */ float hip_width; /* 髋宽，两腿间距 (m) */
uint32_t land_time_ms; /* 落地缓冲时间 (ms) */ float joint_zero[4]; /* 关节电机零点偏移 */
float crouch_leg_length; /* 蓄力时腿长 (m) */ float mech_zero_yaw; /* yaw轴机械零点 */
float launch_force; /* 起跳力 (N) */ } mech;
float retract_leg_length; /* 收腿时腿长 (m) */
float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */
} jump_params;
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; struct {
float phi; float max_acceleration; /* 最大加速度 (m/s²) */
float non_contact_theta; /* 离地时摆角目标 (rad) */
} motion;
/* LQR 目标状态偏移 */
struct {
float theta;
float x;
float phi;
} lqr_offset;
/* 低通滤波器截止频率 */ /* 低通滤波器截止频率 */
struct { struct {
float in; /* 输入 */ float in;
float out; /* 输出 */ float out;
} low_pass_cutoff_freq; } low_pass_cutoff_freq;
} Chassis_Params_t; } Chassis_Params_t;

50
User/module/config.c
View File

@ -214,7 +214,7 @@ Config_RobotParam_t robot_config = {
.leg_length={ .leg_length={
.k = 40.0f, .k = 40.0f,
.p = 20.0f, .p = 50.0f,
.i = 0.01f, .i = 0.01f,
.d = 2.0f, .d = 2.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
@ -308,10 +308,29 @@ Config_RobotParam_t robot_config = {
}, },
}, },
.mech_zero_yaw = 1.78040409f, /* 机械零点 */ .mech = {
.joint_zero = {0.0f, 0.0f, 3.84780002f, -1.16999996f}, /* 关节电机零点偏移位置 */ .wheel_radius = 0.068f,
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ .wheel_gear_ratio = 1.0f,
.vmc_param = { .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) */
},
.motion = {
.max_acceleration = 5.0f, /* 最大加速度 (m/s^2) */
.non_contact_theta = 0.15f, /* 离地摆角目标 (rad) */
},
.vmc_param = {
{ // 左腿 { // 左腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m) .leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
@ -344,19 +363,22 @@ Config_RobotParam_t robot_config = {
}, },
.jump_params = { .jump = {
.crouch_time_ms = 100, .crouch_time_ms = 500,
.launch_time_ms = 120, .launch_time_ms = 300,
.retract_time_ms = 80, .retract_time_ms = 500,
.land_time_ms = 300, .land_time_ms = 500,
.crouch_leg_length = 0.14f, .crouch_leg_length = 0.13f, /* 不用蹲太低 */
.launch_force = 200.0f, .launch_force = 20.0f, /* 起跳力减小 */
.retract_leg_length = 0.1f, /* 收腿目标更短 */ .retract_leg_length = 0.12f, /* 收腿目标 */
.retract_force = -120.0f, /* 收腿前馈力加大 */ .retract_force = -20.0f, /* 收腿前馈力减小 */
}, },
.lqr_offset = {
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,
.phi = 0.0f, .phi = 0.0f,
},
}, },

4
User/task/ctrl_chassis.c
View File

@ -42,12 +42,16 @@ static int print_chassis(const void *data, char *buf, size_t size) {
const Chassis_t *chassis = (const Chassis_t *)data; const Chassis_t *chassis = (const Chassis_t *)data;
return MRobot_Snprintf(buf, size, return MRobot_Snprintf(buf, size,
" Mode : %d\r\n" " Mode : %d\r\n"
" Jump : state=%d trigger=%d\r\n"
" LegLen : L=%.3f R=%.3f (m)\r\n"
" IMU : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n" " IMU : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n"
" Motors : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n" " Motors : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n"
" Wheels : W0=%.1f W1=%.1f (rpm)\r\n" " Wheels : W0=%.1f W1=%.1f (rpm)\r\n"
" Output : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n" " Output : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n"
" VelX : %.3f m/s\r\n", " VelX : %.3f m/s\r\n",
chassis->mode, chassis->mode,
chassis->jump.state, chassis->jump.trigger,
chassis->vmc_[0].leg.L0, chassis->vmc_[1].leg.L0,
chassis->feedback.imu.euler.rol, chassis->feedback.imu.euler.pit, chassis->feedback.imu.euler.yaw, 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[0].rotor_speed, chassis->feedback.joint[1].rotor_speed,
chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed, chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed,

4
User/task/rc.c
View File

@ -169,8 +169,8 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break; break;
case DR16_SW_DOWN: case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; cmd_for_chassis.mode = CHASSIS_MODE_JUMP_TEST;
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR; // cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE; // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
break; break;