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-12 17:37:15 +08:00
parent 33dfef3288
commit 53fda34711
12 changed files with 4284 additions and 8279 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.

2
.mxproject
View File

File diff suppressed because one or more lines are too long

2
Core/Inc/FreeRTOSConfig.h
View File

@ -72,7 +72,7 @@
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 56 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)0x20000)
#define configTOTAL_HEAP_SIZE ((size_t)0x10000)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configGENERATE_RUN_TIME_STATS 1
#define configUSE_TRACE_FACILITY 1

2
Core/Src/usart.c
View File

@ -48,7 +48,7 @@ void MX_UART5_Init(void)
huart5.Instance = UART5;
huart5.Init.BaudRate = 100000;
huart5.Init.WordLength = UART_WORDLENGTH_9B;
huart5.Init.StopBits = UART_STOPBITS_1;
huart5.Init.StopBits = UART_STOPBITS_2;
huart5.Init.Parity = UART_PARITY_EVEN;
huart5.Init.Mode = UART_MODE_RX;
huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;

5
CtrBoard-H7_ALL.ioc
View File

@ -587,7 +587,7 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x2000
ProjectManager.TargetToolchain=MDK-ARM V5.32
ProjectManager.TargetToolchain=CMake
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath=
@ -748,9 +748,10 @@ TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,Period,AutoReloadPreload
TIM3.Period=10000-1
TIM3.Prescaler=24-1
UART5.BaudRate=100000
UART5.IPParameters=Mode,WordLength,Parity,BaudRate
UART5.IPParameters=Mode,WordLength,Parity,BaudRate,StopBits
UART5.Mode=MODE_RX
UART5.Parity=PARITY_EVEN
UART5.StopBits=UART_STOPBITS_2
UART5.WordLength=WORDLENGTH_9B
UART7.BaudRate=921600
UART7.IPParameters=BaudRate

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

70
User/module/balance_chassis.c
View File

@ -86,6 +86,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
PID_Reset(&c->pid.leg_length[1]);
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.roll);
PID_Reset(&c->pid.roll_force);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
@ -415,6 +416,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
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.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);
@ -805,26 +807,66 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
/* ==================== 横滚角补偿 ==================== */
float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt);
/* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
float Rl = c->param->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; /* 右腿减左腿腿长差 */
/* 限制补偿范围 */
float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
? 0.05f : 0.02f;
roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);
float A = Delta_L0 * cosf(roll_error) + 2.0f * Rl * sinf(roll_error);
float B = -Delta_L0 * sinf(roll_error) + 2.0f * Rl * cosf(roll_error);
float roll_leg_compensation_left = 0.0f;
float roll_leg_compensation_right = 0.0f;
if (fabsf(B) > 0.001f) { /* 避免除零 */
float tan_delta = A / B;
roll_leg_compensation_left = -Rl * tan_delta;
roll_leg_compensation_right = Rl * tan_delta;
}
/* 限制几何补偿范围 (适当提高限幅增强补偿效果) */
float max_leg_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
? 0.12f : 0.0f; /* 提高到0.05/0.025以提供足够补偿 */
roll_leg_compensation_left = LIMIT(roll_leg_compensation_left, -max_leg_compensation, max_leg_compensation);
roll_leg_compensation_right = LIMIT(roll_leg_compensation_right, -max_leg_compensation, max_leg_compensation);
/* 方法2: PID力补偿 (低增益避免抖动) */
float roll_force_compensation = PID_Calc(&c->pid.roll_force, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt);
/* ==================== 腿长控制 ==================== */
float target_L0[2];
float jump_extra_force[2] = {0.0f, 0.0f};
/* 基础腿长目标 */
target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
/* 基础腿长目标 (应用几何前馈补偿) */
float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE;
target_L0[0] = base_leg_length + roll_leg_compensation_left;
target_L0[1] = base_leg_length + roll_leg_compensation_right;
/* 跳跃控制：可能会修改目标腿长和额外力 */
Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
/* 腿长限幅 */
/* 智能限幅：短腿不够时将补偿转移到长腿 */
float compensation_transfer = 0.0f;
/* 检查哪条腿更短（需要缩短的那条） */
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; /* 左腿限制在最小值 */
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; /* 右腿限制在最小值 */
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);
@ -855,8 +897,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
virtual_torque_left = 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 + jump_extra_force[0];
pid_output_left + 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];
}
@ -880,8 +924,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
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 + jump_extra_force[1];
pid_output_right + 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];
}

7
User/module/balance_chassis.h
View File

@ -90,7 +90,8 @@ typedef struct {
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
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 tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
@ -107,6 +108,7 @@ typedef struct {
} jump_params;
float joint_zero[4]; /* 关节电机零点偏移位置 */
float hip_width; /* 髋宽，两腿间距 (m) */
float mech_zero_yaw; /* 机械零点 */
@ -179,7 +181,8 @@ typedef struct {
/* 反馈控制用的PID */
struct {
KPID_t yaw; /* 跟随云台用的PID */
KPID_t roll; /* 横滚角控制PID */
KPID_t roll; /* 横滚角腿长补偿PID */
KPID_t roll_force; /* 横滚角力补偿PID */
KPID_t tp[2];
KPID_t leg_length[2]; /* 两条腿的腿长PID */
KPID_t leg_theta[2]; /* 两条腿的摆角PID */

28
User/module/config.c
View File

@ -191,12 +191,23 @@ Config_RobotParam_t robot_config = {
},
.roll={
.k=0.2f,
.p=0.5f, /* 横滚角比例系数 */
.i=0.01f, /* 横滚角积分系数 */
.d=0.01f, /* 横滚角微分系数 */
.i_limit=0.2f, /* 积分限幅 */
.out_limit=1.0f, /* 输出限幅，腿长差值限制 */
.k=0.15f,
.p=0.3f, /* 横滚角腿长补偿比例系数 (降低避免抖动) */
.i=0.0f, /* 横滚角腿长补偿积分系数 (关闭避免震荡) */
.d=0.01f, /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=0.03f, /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.roll_force={
.k=5.0f,
.p=10.0f, /* 横滚角力补偿比例系数 (大幅降低避免抖动) */
.i=0.0f, /* 横滚角力补偿积分系数 */
.d=5.0f, /* 横滚角力补偿微分系数 (降低避免高频抖动) */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=20.0f, /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
@ -204,8 +215,8 @@ Config_RobotParam_t robot_config = {
.leg_length={
.k = 40.0f,
.p = 30.0f,
.i = 0.01f,
.d = 2.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 0.0f,
.out_limit = 200.0f,
.d_cutoff_freq = -1.0f,
@ -299,6 +310,7 @@ Config_RobotParam_t robot_config = {
.mech_zero_yaw = 1.78040409f, /* 机械零点 */
.joint_zero = {0.0f, 0.0f, 0.0f, 0.0f}, /* 关节电机零点偏移位置 */
.hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
.vmc_param = {
{ // 左腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)

4
User/task/user_task.c
View File

@ -12,7 +12,7 @@ const osThreadAttr_t attr_init = {
const osThreadAttr_t attr_rc = {
.name = "rc",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_atti_esit = {
.name = "atti_esit",
@ -22,7 +22,7 @@ const osThreadAttr_t attr_atti_esit = {
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_ctrl_gimbal = {
.name = "ctrl_gimbal",

2
User/task/user_task.h
View File

@ -24,7 +24,7 @@ extern "C" {
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
#define RC_INIT_DELAY (0)
#define RC_INIT_DELAY (500u)
#define ATTI_ESIT_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (0)
#define CTRL_GIMBAL_INIT_DELAY (0)