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

add leg test

Browse Source
This commit is contained in:
Robofish 2026-03-18 10:25:19 +08:00
parent 480e41e68a
commit a3a05aa076
7 changed files with 196 additions and 19 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.

6
User/component/vmc.c
View File

@ -122,7 +122,11 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
VMC_ANGLE_NORMALIZE(leg->phi0);
// 计算角速度和长度变化率
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
float dphi0 = leg->phi0 - leg->last_phi0;
/* phi0 在 ±π 跳变时做循环差值处理 */
if (dphi0 > VMC_PI) dphi0 -= VMC_2PI;
if (dphi0 < -VMC_PI) dphi0 += VMC_2PI;
leg->d_phi0 = (vmc->dt > 0) ? (dphi0 / vmc->dt) : 0.0f;
leg->d_alpha = -leg->d_phi0;
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);

138
User/module/balance_chassis.c
View File

@ -34,6 +34,7 @@ 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_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
static void Chassis_RecoverControl(Chassis_t *c);
static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/* Private functions -------------------------------------------------------- */
@ -85,6 +86,8 @@ static void Chassis_ResetControllers(Chassis_t *c) {
PID_Reset(&c->pid.roll_force);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
PID_Reset(&c->pid.leg_test_theta[0]);
PID_Reset(&c->pid.leg_test_theta[1]);
/* 重置LQR控制器 */
LQR_Reset(&c->lqr[0]);
@ -209,6 +212,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
c->climb.state_start_time = 0;
}
/* 进入腿部测试模式：重置初始化标志，进入后锁定当前摆角 */
if (mode == CHASSIS_MODE_LEG_TEST) {
c->leg_test.initialized = false;
}
c->mode = mode;
return CHASSIS_OK;
}
@ -665,6 +673,88 @@ static void Chassis_RecoverControl(Chassis_t *c) {
}
/**
* @brief 腿部测试控制
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @note 进入时锁定当前摆角，vx/vy 增减左/右腿目标摆角
* PID控制摆角，恒力支撑腿长，轮子关闭
*/
static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 首次进入：锁定当前摆角 */
if (!c->leg_test.initialized) {
c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
c->leg_test.initialized = true;
}
float sens = c->param->leg_test.theta_sensitivity;
float deadzone = 0.05f; /* 摇杆死区 */
/* vx 控制左腿：有输入→累加目标角度（正/反转），松手→锁住当前实际角度 */
if (fabsf(c_cmd->move_vec.vx) > deadzone) {
c->leg_test.target_theta[0] += c_cmd->move_vec.vx * sens * c->dt;
} else {
c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
}
/* vy 控制右腿：同理 */
if (fabsf(c_cmd->move_vec.vy) > deadzone) {
c->leg_test.target_theta[1] += c_cmd->move_vec.vy * sens * c->dt;
} else {
c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
}
/* 目标角度归一化到 [-π, π]，防止无限增长 */
while (c->leg_test.target_theta[0] > (float)M_PI) c->leg_test.target_theta[0] -= (float)M_2PI;
while (c->leg_test.target_theta[0] < -(float)M_PI) c->leg_test.target_theta[0] += (float)M_2PI;
while (c->leg_test.target_theta[1] > (float)M_PI) c->leg_test.target_theta[1] -= (float)M_2PI;
while (c->leg_test.target_theta[1] < -(float)M_PI) c->leg_test.target_theta[1] += (float)M_2PI;
/* 轮子关闭：只清零输出，由 Chassis_Output 统一发送，避免重复 CAN 帧 */
/* 恒定支撑力 + PID摆角控制 → VMC逆解算 → 关节力矩 */
float support_force = c->param->leg_test.support_force;
for (int i = 0; i < 2; i++) {
/* 手动计算循环误差，避免 theta 在 ±π 跳变时 PID 炸 */
float error = c->leg_test.target_theta[i] - c->vmc_[i].leg.theta;
while (error > (float)M_PI) error -= (float)M_2PI;
while (error < -(float)M_PI) error += (float)M_2PI;
/* 用误差直接算 P 和 D 输出，绕过 PID 内部的 set/fdb 差值 */
float kp = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->p;
float kd = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->d;
float out_limit = c->pid.leg_test_theta[i].param->out_limit;
float Tp = kp * error - kd * c->vmc_[i].leg.d_theta;
Tp = LIMIT(Tp, -out_limit, out_limit);
if (VMC_InverseSolve(&c->vmc_[i], support_force, Tp) == 0) {
if (i == 0) {
VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[0], &c->output.joint[1]);
} else {
VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[3], &c->output.joint[2]);
}
} else {
c->output.joint[i * 2] = 0.0f;
c->output.joint[i * 2 + 1] = 0.0f;
}
}
/* 轮子输出清零 */
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
/* 安全限制 */
for (int i = 0; i < 4; i++) {
c->output.joint[i] = LIMIT(c->output.joint[i], -30.0f, 30.0f);
}
/* 发送关节指令 */
Chassis_Output(c);
}
/**
* @brief 初始化底盘
* @param c 底盘结构体指针
@ -707,12 +797,14 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
/* 初始化PID控制器 */
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.yaw, KPID_MODE_SET_D, target_freq, &param->yaw);
PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.leg_test_theta[0], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
PID_Init(&c->pid.leg_test_theta[1], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
/* 初始化LQR控制器 */
LQR_Init(&c->lqr[0], &param->lqr_gains);
@ -814,6 +906,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->recover.state[1] = RECOVER_STRETCH;
c->recover.state_start_time = 0;
/* 初始化腿部测试状态 */
c->leg_test.initialized = false;
c->leg_test.target_theta[0] = 0.0f;
c->leg_test.target_theta[1] = 0.0f;
return CHASSIS_OK;
}
@ -980,6 +1077,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_ClimbControl(c, c_cmd);
break;
case CHASSIS_MODE_LEG_TEST:
Chassis_LegTestControl(c, c_cmd);
break;
default:
return CHASSIS_ERR_MODE;
}
@ -1018,7 +1119,7 @@ void Chassis_Output(Chassis_t *c) {
// c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
// }
//cap-add//supercap-add
PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
// PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
//PowerLimit_ChassicOutput()
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
@ -1123,7 +1224,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 对齐完成，切换到平衡模式 */
c->rotor_state.exiting = false;
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
Chassis_ResetControllers(c);
/* 只重置 yaw/rotor PID，保留 chassis_state 避免 LQR 突变 */
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.rotor);
}
}
} else {
@ -1141,7 +1244,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 退出过渡减速完成后，用yaw PID对齐零点 */
if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) {
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
c->feedback.yaw.rotor_abs_angle,
c->feedback.imu.gyro.z, c->dt);
} else {
float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */
c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
@ -1158,16 +1262,30 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
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);
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false;
/* 迟滞切换：只有当另一个零点明显更近（差值>0.3rad≈17°）时才切换，
* 避免在两个零点正中间反复跳变 */
float hysteresis = 0.3f;
if (c->yaw_control.is_reversed) {
/* 当前跟踪 target_2，只有 target_1 明显更近才切 */
if (fabsf(error_to_target1) < fabsf(error_to_target2) - hysteresis) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = base_target_2;
}
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true;
/* 当前跟踪 target_1，只有 target_2 明显更近才切 */
if (fabsf(error_to_target2) < fabsf(error_to_target1) - hysteresis) {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true;
} else {
c->yaw_control.target_yaw = base_target_1;
}
}
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
c->feedback.yaw.rotor_abs_angle,
c->feedback.imu.gyro.z, c->dt);
}
/* ==================== 左腿LQR控制 ==================== */

18
User/module/balance_chassis.h
View File

@ -43,7 +43,8 @@ typedef enum {
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
CHASSIS_MODE_CLIMB_STEP /* 上台阶模式 */
CHASSIS_MODE_CLIMB_STEP, /* 上台阶模式 */
CHASSIS_MODE_LEG_TEST /* 腿部测试模式：PID锁摆角+恒力支撑，摇杆控制摆角 */
} Chassis_Mode_t;
/* 跳跃状态枚举 */
@ -174,6 +175,14 @@ typedef struct {
uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */
} climb;
/* 腿部测试参数 */
struct {
float support_force; /* 恒定支撑力 (N) */
float theta_sensitivity; /* 摇杆→摆角灵敏度 (rad/s) */
} leg_test;
KPID_Params_t leg_test_theta; /* 腿部测试摆角PID（独立于tp） */
/* 倒地自起参数 */
struct {
float stretch_velocity; /* 伸腿角速度 (rad/s) */
@ -267,6 +276,12 @@ typedef struct {
uint32_t state_start_time; /* 当前状态开始时间 (ms) */
} climb;
/* 腿部测试相关 */
struct {
float target_theta[2]; /* 左右腿目标摆角 (rad)，进入时锁定当前值 */
bool initialized; /* 是否已锁定初始摆角 */
} leg_test;
/* 电机离线检测 */
struct {
bool any_offline; /* 是否有电机离线 */
@ -286,6 +301,7 @@ typedef struct {
KPID_t tp[2];
KPID_t leg_length[2]; /* 两条腿的腿长PID */
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
KPID_t leg_test_theta[2]; /* 腿部测试摆角PID */
} pid;
/* 滤波器 */

23
User/module/config.c
View File

@ -491,6 +491,20 @@ Config_RobotParam_t robot_config = {
.phi_tolerance = 0.5f, /* 体姿接近站立容差 (rad)，约28° */
.kd = 3.0f, /* MIT速度模式阻尼 */
},
.leg_test = {
.support_force = 30.0f, /* 恒定支撑力 (N) */
.theta_sensitivity = 1.0f, /* 摇杆→摆角灵敏度 (rad/s) */
},
.leg_test_theta = {
.k = 20.0f,
.p = 5.0f,
.i = 0.0f,
.d = 1.0f,
.i_limit = 0.0f,
.out_limit = 30.0f, /* 摆角力矩限幅 (Nm) */
.d_cutoff_freq = 30.0f,
.range = M_2PI, /* 启用循环误差处理 */
},
.lqr_offset = {
.theta = 0.0f,
.x = 0.0f,
@ -522,15 +536,14 @@ Config_RobotParam_t robot_config = {
.rc_mode_map = {
#if CMD_ENABLE_MODULE_GIMBAL
.gimbal_sw_up = GIMBAL_MODE_RELAX,
// .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
.gimbal_sw_mid = GIMBAL_MODE_RECOVER,
.gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
// .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
#endif
},
},

28
User/module/gimbal.c
View File

@ -63,6 +63,17 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
}
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
/* 进入 RECOVER 时初始化双零点选择 */
if (mode == GIMBAL_MODE_RECOVER) {
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
g->recover_yaw_reversed = (err1 < err0);
}
g->mode = mode;
return 0;
}
@ -220,14 +231,27 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
} else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
/* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
/* 双零点选近：mech_zero 和 mech_zero+π，带迟滞避免临界点跳变 */
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
float hysteresis = 0.3f;
if (g->recover_yaw_reversed) {
/* 当前跟踪 zero1，只有 zero0 明显更近才切 */
if (err0 < err1 - hysteresis) {
g->recover_yaw_reversed = false;
}
} else {
/* 当前跟踪 zero0，只有 zero1 明显更近才切 */
if (err1 < err0 - hysteresis) {
g->recover_yaw_reversed = true;
}
}
g->setpoint.eulr.yaw = g->recover_yaw_reversed ? zero1 : zero0;
g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
}

2
User/module/gimbal.h
View File

@ -171,6 +171,8 @@ typedef struct {
Gimbal_Output_t out; /* 云台输出 */
Gimbal_Feedback_t feedback; /* 反馈 */
bool recover_yaw_reversed; /* RECOVER模式下跟踪的是第二个零点(+π) */
} Gimbal_t;
/* Exported functions prototypes -------------------------------------------- */