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

好qr

Browse Source
This commit is contained in:
Robofish 2025-10-10 00:21:11 +08:00
parent dcc3b55df8
commit 64216877e5
5 changed files with 105 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
User/module/balance_chassis.c
View File

@ -42,6 +42,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (mode == c->mode) if (mode == c->mode)
return CHASSIS_OK; /* 模式未改变直接返回 */ return CHASSIS_OK; /* 模式未改变直接返回 */
// 在CHASSIS_MODE_JUMP和CHASSIS_MODE_WHELL_LEG_BALANCE之间切换时不重置
if ((c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) ||
(c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP)) {
c->mode = mode;
return CHASSIS_OK;
}
Chassis_MotorEnable(c); Chassis_MotorEnable(c);
PID_Reset(&c->pid.leg_length[0]); PID_Reset(&c->pid.leg_length[0]);
@ -73,6 +80,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
c->mode = mode; c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0;
c->wz_multi=0.0f; c->wz_multi=0.0f;
return CHASSIS_OK; return CHASSIS_OK;
@ -106,6 +114,8 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
} }
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
if (c == NULL || param == NULL || target_freq <= 0.0f) { if (c == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误 return -1; // 参数错误
@ -253,15 +263,54 @@ 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:
// 跳跃模式状态机
// 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
// jump_time == 0: 未开始跳跃
// jump_time == 1: 已完成跳跃（不再触发）
// jump_time > 1: 跳跃进行中
// 检测是否需要开始新的跳跃（state为0且jump_time为0）
if (c->state == 0 && c->jump_time == 0) {
// 开始跳跃，进入state 1
c->state = 1;
c->jump_time = BSP_TIME_Get_us();
}
// 只有在跳跃进行中时才处理状态转换（jump_time > 1）
if (c->jump_time > 1) {
// 计算已经过的时间（微秒）
uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
// 状态转换逻辑
if (c->state == 1 && elapsed_us >= 300000) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 200000) {
// state 2 保持0.2s后转到state 3
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 300000) {
// state 3 保持0.3s后转到state 0
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
}
}
case CHASSIS_MODE_WHELL_LEG_BALANCE:
case CHASSIS_MODE_ROTOR:
// 执行LQR控制（包含PID腿长控制） // 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd); Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
case CHASSIS_MODE_ROTOR:
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出 Chassis_Output(c); // 统一输出
break; break;
break;
default: default:
return CHASSIS_ERR_MODE; return CHASSIS_ERR_MODE;
} }
@ -301,7 +350,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 运动参数（参考C++版本的状态估计） */ /* 运动参数（参考C++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f; static float xhat = 0.0f, x_dot_hat = 0.0f;
float target_dot_x = 0.0f; float target_dot_x = 0.0f;
float max_acceleration = 3.0f; // 最大加速度限制: 1 m/s² float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s²
// 简化的速度估计（后续可以改进为C++版本的复杂滤波） // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x; x_dot_hat = c->chassis_state.velocity_x;
@ -359,10 +408,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f; target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.d_x = target_dot_x; // 目标速度
/* 更新LQR状态 */
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_SetTargetState(&c->lqr[1], &target_state);
if (c->mode != CHASSIS_MODE_ROTOR){ if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
c->yaw_control.target_yaw = c->yaw_control.target_yaw =
@ -380,17 +425,24 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} }
if (c->vmc_[0].leg.is_ground_contact) { if (c->vmc_[0].leg.is_ground_contact) {
/* 更新LQR状态 */
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]); LQR_Control(&c->lqr[0]);
} else { } else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
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 + 0.0f) + c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f); c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
} }
if (c->vmc_[1].leg.is_ground_contact) if (c->vmc_[1].leg.is_ground_contact){
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]); LQR_Control(&c->lqr[1]);
else { }else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
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 + 0.0f) + c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
@ -421,8 +473,28 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
roll_compensation_force = -20.0f; roll_compensation_force = -20.0f;
// 目标腿长设定（移除横滚角补偿） // 目标腿长设定（移除横滚角补偿）
target_L0[0] = 0.15f + c_cmd->height * 0.25f; // 左腿：基础腿长 + 高度调节 switch (c->state) {
target_L0[1] = 0.15f + c_cmd->height * 0.25f; // 右腿：基础腿长 + 高度调节 case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
target_L0[1] = 0.55f; // 右腿：伸展到最大腿长
break;
case 3: // 着地阶段，腿长缓冲
target_L0[0] = 0.1f; // 左腿：缓冲腿长
target_L0[1] = 0.1f; // 右腿：缓冲腿长
break;
default:
target_L0[0] = 0.15f + c_cmd->height * 0.22f;
target_L0[1] = 0.15f + c_cmd->height * 0.22f;
break;
}
// 获取腿长变化率 // 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@ -504,3 +576,5 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
return CHASSIS_OK; return CHASSIS_OK;
} }

4
User/module/balance_chassis.h
View File

@ -40,6 +40,7 @@ typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */ CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} Chassis_Mode_t; } Chassis_Mode_t;
@ -125,6 +126,8 @@ typedef struct {
LQR_t lqr[2]; /* 两条腿的LQR控制器 */ LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state; int8_t state;
uint64_t jump_time;
float angle; float angle;
float height; float height;
@ -220,6 +223,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/** /**
* \brief 底盘输出值 * \brief 底盘输出值
* *

26
User/module/config.c
View File

@ -20,7 +20,7 @@ Config_RobotParam_t robot_config = {
.shoot_param = { .shoot_param = {
.trig_step_angle=M_2PI/8, .trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f, .shot_delay_time=0.05f,
.shot_burst_num=1, .shot_burst_num=50,
.fric_motor_param[0] = { .fric_motor_param[0] = {
.can = BSP_CAN_2, .can = BSP_CAN_2,
.id = 0x201, .id = 0x201,
@ -254,18 +254,18 @@ Config_RobotParam_t robot_config = {
} }
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -1.675900301510996e+02f, 2.020661327080207e+02f, -1.145642457869424e+02f, -3.747932874943916e+00f }, // theta .k11_coeff = { -1.541653276294261e+02f, 1.944240977607161e+02f, -1.094196687050347e+02f, -1.991862922476537e+00f }, // theta
.k12_coeff = { 6.610013864045129e+00f, -6.495140417893399e+00f, -8.129319798942683e+00f, -2.920993723294792e-01f }, // d_theta .k12_coeff = { 7.958802216156968e+00f, -6.496301522626967e+00f, -6.793798563394576e+00f, -9.768935145474957e-02f }, // d_theta
.k13_coeff = { -3.558043125079351e+01f, 3.982938747687503e+01f, -1.608580664764812e+01f, -1.704388165552662e+00f }, // x .k13_coeff = { -3.265025394715387e+01f, 3.775539494427401e+01f, -1.577293629786055e+01f, -6.143705005307794e-01f }, // x
.k14_coeff = { -3.082060804555171e+01f, 3.657436465245446e+01f, -1.721864301319228e+01f, -2.425477933777937e+00f }, // d_x .k14_coeff = { -3.342861178786544e+01f, 4.027907112212562e+01f, -1.860920837741005e+01f, -1.011945860330850e+00f }, // d_x
.k15_coeff = { -2.915524527743928e+01f, 4.841882215140198e+01f, -3.075590139522371e+01f, 9.309579833638985e+00f }, // phi .k15_coeff = { -4.173395770434105e+00f, 2.208051577637197e+01f, -2.060462131200154e+01f, 7.510277938071905e+00f }, // phi
.k16_coeff = { -6.590867790309310e+00f, 8.818728233474916e+00f, -4.823548294804866e+00f, 1.465064667201413e+00f }, // d_phi .k16_coeff = { -2.731905689135932e+00f, 4.861408834763942e+00f, -3.379865397362515e+00f, 1.218262454432878e+00f }, // d_phi
.k21_coeff = { 2.124446115135024e+02f, -1.763726543380520e+02f, 3.279099984858797e+01f, 1.233889708835248e+01f }, // theta .k21_coeff = { 3.310056906118938e+02f, -2.839582288120425e+02f, 6.078882971562280e+01f, 1.248056097881360e+01f }, // theta
.k22_coeff = { 1.207615888432813e+01f, -1.237642650067993e+01f, 4.209610950938401e+00f, 1.417825883473013e+00f }, // d_theta .k22_coeff = { 2.029863884910664e+01f, -2.130214391291087e+01f, 7.397797146713744e+00f, 1.234955263546594e+00f }, // d_theta
.k23_coeff = { -1.482017772836761e+01f, 3.364904259474741e+01f, -2.486572037866952e+01f, 7.847915428831716e+00f }, // x .k23_coeff = { 4.920315528337404e+00f, 1.585029096342149e+01f, -2.001513921771265e+01f, 7.602599395836243e+00f }, // x
.k24_coeff = { -3.273926334829641e+01f, 5.271977032684813e+01f, -3.274024194089392e+01f, 9.730698282497507e+00f }, // d_x .k24_coeff = { -6.228021022229131e+00f, 2.987205717969141e+01f, -2.731854197984320e+01f, 9.786304250377514e+00f }, // d_x
.k25_coeff = { 1.663230320117893e+02f, -1.816044424207695e+02f, 7.154626742111853e+01f, 4.309975259244579e+00f }, // phi .k25_coeff = { 1.782118404877848e+02f, -2.009255508273753e+02f, 8.203108121734316e+01f, 7.691004625305012e-01f }, // phi
.k26_coeff = { 2.350205493668966e+01f, -2.586342446220884e+01f, 1.038824015934221e+01f, 1.031625329478827e-01f }, // d_phi .k26_coeff = { 2.601707340816693e+01f, -2.955760901249582e+01f, 1.229540979299611e+01f, -3.567767355508533e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,

2
User/module/shoot.c
View File

@ -133,7 +133,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->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output)); memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=200.0f; s->target_variable.target_rpm=5000.0f;
return 0; return 0;
} }

2
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -49,7 +49,7 @@ function K = get_k_length(leg_length)
B=double(B); B=double(B);
Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[140 0;0 40]; %T Tp R=[250 0;0 50]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);