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

上弹簧了

Browse Source
This commit is contained in:
Robofish 2026-01-13 13:48:44 +08:00
parent 4ed3b712f5
commit d521f0b485
7 changed files with 8798 additions and 8671 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -158,27 +158,32 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
<ItemText>L_length</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>L_fn</ItemText>
<ItemText>R_length</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>R_fn</ItemText>
<ItemText>L_fn</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>L_length</ItemText>
<ItemText>R_fn</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>R_length</ItemText>
<ItemText>LF</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>RF</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

91
User/module/balance_chassis.c
View File

@ -21,13 +21,13 @@
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO 8.0f /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */
#define LEG_LENGTH_RANGE 0.18f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (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;
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
/* Private function prototypes ---------------------------------------------- */
static int8_t Chassis_MotorEnable(Chassis_t *c);
static int8_t Chassis_MotorRelax(Chassis_t *c);
@ -207,6 +207,38 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
/**
* @brief 计算五杆机构弹簧等效支撑力
* @param leg_length 腿长 L_AE (m)，有效范围 [0.0391, 0.4689]
* @return 等效支撑力 (N)，无解时返回 NAN
*/
static float Chassis_CalcSpringForce(float leg_length)
{
// 五杆机构几何参数 (单位: m)
const float L_AB = 0.215f; // 基座长度
const float L_AC = 0.042f; // 弹簧上端高度
const float L_BD = 0.050f; // 杆BD长度
const float L_BE = 0.254f; // 杆BE长度
const float Fs = 360.0f; // 弹簧恒力 (N)
// 通过余弦定理计算∠ABE的余弦值
float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);
// 检查解的有效性
if (fabsf(cos_theta) > 1.0f) {
return NAN; // 无物理解
}
float theta = acosf(cos_theta);
float sin_theta = sinf(theta);
// 计算等效支撑力
// F_eq = (Fs × |力臂| × L_AB × sin(θ)) / (L_CD × L_BE × L_AE)
return (Fs * fabsf(L_BD*cos_theta*L_AC - L_AB*L_BD*sin_theta) * L_AB*sin_theta)
/ (sqrtf((L_AB - L_BD*cos_theta)*(L_AB - L_BD*cos_theta)
+ (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
}
/* Public functions --------------------------------------------------------- */
/**
@ -357,6 +389,18 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
/* 修正离地检测：加上弹簧提供的支撑力 */
float spring_Fn_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
float spring_Fn_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
if (!isnan(spring_Fn_left)) {
c->vmc_[0].leg.Fn += spring_Fn_left;
c->vmc_[0].leg.is_ground_contact = (c->vmc_[0].leg.Fn > 20.0f);
}
if (!isnan(spring_Fn_right)) {
c->vmc_[1].leg.Fn += spring_Fn_right;
c->vmc_[1].leg.is_ground_contact = (c->vmc_[1].leg.Fn > 20.0f);
}
/* 根据模式执行控制 */
switch (c->mode) {
case CHASSIS_MODE_RELAX:
@ -368,9 +412,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float fn = -20.0f, tp = 0.0f;
Chassis_LQRControl(c, c_cmd);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
/* 计算弹簧力补偿 */
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);
VMC_InverseSolve(&c->vmc_[0], fn_left, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn, tp);
VMC_InverseSolve(&c->vmc_[1], fn_right, tp);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
@ -378,19 +428,22 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_Output(c);
} break;
case CHASSIS_MODE_CALIBRATE: {
// case CHASSIS_MODE_CALIBRATE: {
Chassis_LQRControl(c, c_cmd);
// Chassis_LQRControl(c, c_cmd);
// LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
// RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
// L_fn = -LF;
// VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
// VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
// VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
// VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
Chassis_Output(c);
} break;
// c->output.wheel[0] = 0.0f;
// c->output.wheel[1] = 0.0f;
// Chassis_Output(c);
// } break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
Chassis_LQRControl(c, c_cmd);
@ -579,8 +632,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 左腿VMC控制 ==================== */
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; /* 处理无效值 */
float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
pid_output_left + LEFT_BASE_FORCE;
pid_output_left + LEFT_BASE_FORCE - spring_force_left;
if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
@ -593,8 +648,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 右腿VMC控制 ==================== */
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; /* 处理无效值 */
float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
pid_output_right + RIGHT_BASE_FORCE;
pid_output_right + RIGHT_BASE_FORCE - spring_force_right;
if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {

2
User/module/balance_chassis.h
View File

@ -39,7 +39,7 @@ extern "C" {
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_CALIBRATE, /* 校准模式 */
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t;

4
User/task/rc.c
View File

@ -115,8 +115,8 @@ switch (dr16.data.sw_l) {
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
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;

2
utils/算弹簧/cacl.py
View File

@ -7,7 +7,7 @@ L_AB = 215.0
L_AC = 42.0
L_BD = 50.0
L_BE = 254.0 # 50 + 204
Fs = 220.0 # 弹簧力 220N
Fs = 280.0 # 弹簧力 220N
def calculate_force_ae(L_mm):
# 1. 几何计算