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 2025-10-17 23:02:42 +08:00
parent 45e340156c
commit 3ebd0927a8
5 changed files with 131 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
User/module/balance_chassis.c
View File

@ -64,7 +64,23 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
PID_Reset(&c->pid.tp[0]); PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]); PID_Reset(&c->pid.tp[1]);
c->yaw_control.target_yaw = c->feedback.yaw.rotor_abs_angle; // 双零点自动选择逻辑（使用user_math的CircleError函数）
float current_yaw = c->feedback.yaw.rotor_abs_angle;
float zero_point_1 = c->param->mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI; // 第二个零点，相差180度
// 使用CircleError计算到两个零点的角度差（范围为M_2PI）
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
// 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
c->yaw_control.target_yaw = zero_point_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
// 清空位移 // 清空位移
c->chassis_state.position_x = 0.0f; c->chassis_state.position_x = 0.0f;
@ -182,6 +198,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->yaw_control.target_yaw = 0.0f; c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f; c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f; c->yaw_control.yaw_force = 0.0f;
c->yaw_control.is_reversed = false;
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -363,7 +380,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
xhat = c->chassis_state.position_x; xhat = c->chassis_state.position_x;
// 目标速度设定（原始期望速度） // 目标速度设定（原始期望速度）
float desired_velocity = c_cmd->move_vec.vx * 2; float raw_vx = c_cmd->move_vec.vx * 3;
// 根据零点选择情况决定是否反转前后方向
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
// 加速度限制处理 // 加速度限制处理
float velocity_change = float velocity_change =
@ -407,26 +427,56 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 构建目标状态 */ /* 构建目标状态 */
LQR_State_t target_state = {0}; LQR_State_t target_state = {0};
target_state.theta = c->param->theta; // 目标摆杆角度 target_state.theta = 0.00; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.18f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.18f + c_cmd->height * 0.25f),2) + 3.87083*(0.18f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 target_state.phi = 11.9601*pow((0.16f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.16f + c_cmd->height * 0.25f),2) + 3.87083*(0.16f + c_cmd->height * 0.25f) + -0.4154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.18f + c_cmd->height * 0.25f) + 0.41182f; target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.d_x = target_dot_x; // 目标速度
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.target_yaw =
c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2;
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);
}else{
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.current_yaw + 0.5f;
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
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_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
// 使用CircleError计算到两个目标的角度误差
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; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
c->yaw_control.yaw_force = c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, 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, 0.0f, c->dt);
}else{
// 小陀螺模式：使用速度环控制
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
c->wz_multi += 0.005f;
if (c->wz_multi > 1.2f)
c->wz_multi = 1.2f;
// 目标角速度（rad/s），可根据需要调整最大速度
float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
// 当前角速度反馈来自IMU陀螺仪
float current_yaw_velocity = c->feedback.imu.gyro.z;
// 使用PID控制角速度，输出力矩
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, target_yaw_velocity,
current_yaw_velocity, 0.0f, c->dt);
} }
if (c->vmc_[0].leg.is_ground_contact) { if (c->vmc_[0].leg.is_ground_contact) {
@ -434,7 +484,7 @@ 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 = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
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 =
@ -446,7 +496,7 @@ 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 = 0.18f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.16f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
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 =
@ -466,44 +516,55 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float leg_d_length[2]; // 腿长变化率 float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */ /* 横滚角PID补偿计算 */
// 使用PID控制器计算横滚角补偿力，目标横滚角为0 // 使用PID控制器计算横滚角补偿长度，目标横滚角为0
float roll_compensation_force = float roll_compensation_length =
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt); c->feedback.imu.gyro.x, c->dt);
// 限制补偿力范围，防止过度补偿 // 限制补偿长度范围，防止过度补偿
// 如果任一腿离地，限制补偿力为20 // 如果任一腿离地，限制补偿长度
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) { if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_force > 20.0f) if (roll_compensation_length > 0.02f)
roll_compensation_force = 20.0f; roll_compensation_length = 0.02f;
if (roll_compensation_force < -20.0f) if (roll_compensation_length < -0.02f)
roll_compensation_force = -20.0f; roll_compensation_length = -0.02f;
} else {
if (roll_compensation_length > 0.05f)
roll_compensation_length = 0.05f;
if (roll_compensation_length < -0.05f)
roll_compensation_length = -0.05f;
} }
// 目标腿长设定（移除横滚角补偿） // 目标腿长设定（包含横滚角补偿）
switch (c->state) { switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长 case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.18f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
target_L0[1] = 0.18f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break; break;
case 1: // 准备阶段，腿长收缩 case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.18f; // 左腿：收缩到基础腿长 target_L0[0] = 0.16f + roll_compensation_length; // 左腿：收缩到基础腿长 + 横滚补偿
target_L0[1] = 0.18f; // 右腿：收缩到基础腿长 target_L0[1] = 0.16f - roll_compensation_length; // 右腿：收缩到基础腿长 - 横滚补偿
break; break;
case 2: // 起跳阶段，腿长快速伸展 case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长 target_L0[0] = 0.55f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.55f; // 右腿：伸展到最大腿长 target_L0[1] = 0.55f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
break; break;
case 3: // 着地阶段，腿长缓冲 case 3: // 着地阶段，腿长缓冲
target_L0[0] = 0.1f; // 左腿：缓冲腿长 target_L0[0] = 0.1f; // 左腿：缓冲腿长（着地时不进行横滚补偿）
target_L0[1] = 0.1f; // 右腿：缓冲腿长 target_L0[1] = 0.1f; // 右腿：缓冲腿长（着地时不进行横滚补偿）
break; break;
default: default:
target_L0[0] = 0.18f + c_cmd->height * 0.22f; target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
target_L0[1] = 0.18f + c_cmd->height * 0.22f; target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length;
break; break;
} }
// 腿长限幅：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
// 获取腿长变化率 // 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
@ -526,11 +587,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0], float pid_output = 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);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
// 横滚角补偿力（左腿减少支撑力）
virtual_force[0] = virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 55.0f + roll_compensation_force; pid_output + 60.0f;
// VMC逆解算（包含摆角补偿） // VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
@ -551,11 +611,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1], float pid_output = 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);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
// 横滚角补偿力（右腿增加支撑力）
virtual_force[1] = virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 60.0f - roll_compensation_force; pid_output + 60.0f;
// VMC逆解算（包含摆角补偿） // VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],

1
User/module/balance_chassis.h
View File

@ -147,6 +147,7 @@ typedef struct {
float target_yaw; /* 目标yaw角度 */ float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */ float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */ float yaw_force; /* yaw轴控制力矩 */
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control; } yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */ float wz_multi; /* 小陀螺模式旋转方向 */

46
User/module/config.c
View File

@ -179,23 +179,23 @@ Config_RobotParam_t robot_config = {
}, },
.roll={ .roll={
.k=30.0f, .k=1.0f,
.p=20.0f, /* 横滚角比例系数 */ .p=1.0f, /* 横滚角比例系数 */
.i=1.0f, /* 横滚角积分系数 */ .i=0.01f, /* 横滚角积分系数 */
.d=0.5f, /* 横滚角微分系数 */ .d=0.01f, /* 横滚角微分系数 */
.i_limit=20.0f, /* 积分限幅 */ .i_limit=0.2f, /* 积分限幅 */
.out_limit=100.0f, /* 输出限幅，腿长差值限制 */ .out_limit=1.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */ .d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */ .range=-1.0f, /* 不使用循环误差处理 */
}, },
.leg_length={ .leg_length={
.k = 50.0f, .k = 25.0f,
.p = 10.0f, .p = 20.0f,
.i = 0.0f, .i = 0.01f,
.d = 1.0f, .d = 2.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 100.0f, .out_limit = 200.0f,
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = -1.0f, .range = -1.0f,
}, },
@ -255,18 +255,18 @@ Config_RobotParam_t robot_config = {
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -1.365991212772591e+02f, 1.817682520209751e+02f, -1.090145744768390e+02f, -1.357841047348700e+00f }, // theta .k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
.k12_coeff = { 1.066191238841660e+01f, -9.003325414315036e+00f, -6.468059178062407e+00f, -3.576912197025609e-02f }, // d_theta .k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta
.k13_coeff = { -4.401223711894286e+01f, 5.240645764730768e+01f, -2.259350423458009e+01f, -3.125930762749351e-01f }, // x .k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x
.k14_coeff = { -3.589096446318186e+01f, 4.504832892816042e+01f, -2.180583719935267e+01f, -6.131521769198509e-01f }, // d_x .k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x
.k15_coeff = { 7.755509171684017e+00f, 9.703025198473474e+00f, -1.617988750531686e+01f, 6.969677381136859e+00f }, // phi .k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
.k16_coeff = { -1.054369868423547e+00f, 3.078146872733054e+00f, -2.719931242949781e+00f, 1.137139318462496e+00f }, // d_phi .k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi
.k21_coeff = { 4.187447897772694e+02f, -3.681828885583142e+02f, 8.516335489092428e+01f, 1.307642410705911e+01f }, // theta .k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
.k22_coeff = { 2.733059508651422e+01f, -2.951697374399402e+01f, 1.083275594166739e+01f, 1.186192378498821e+00f }, // d_theta .k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta
.k23_coeff = { 2.607118229545885e+01f, 5.493744611623077e+00f, -2.426883873185716e+01f, 1.096947115837726e+01f }, // x .k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x
.k24_coeff = { 1.079657629365681e+01f, 1.807070716568389e+01f, -2.737138856658193e+01f, 1.153864525902007e+01f }, // d_x .k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x
.k25_coeff = { 1.861482062842389e+02f, -2.152844878785277e+02f, 9.051129761064519e+01f, -1.063399918986778e+00f }, // phi .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
.k26_coeff = { 2.777098508680760e+01f, -3.223625515767142e+01f, 1.376100864562188e+01f, -6.731370513814361e-01f }, // d_phi .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,

7
User/task/rc.c
View File

@ -61,11 +61,12 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break; break;
case DR16_SW_MID: case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// 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_WHELL_LEG_BALANCE; cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP; // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break; break;
default: default:

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

@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
R1=0.072; %驱动轮半径 R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离 L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.01; %机体质心距离转轴距离 l1=-0.01; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量 mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量 mp1=1.8; %杆质量
M1=12.0; %机体质量 M1=14.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量 Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量 IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B); B=double(B);
Q=diag([1500 100 5000 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([1000 1 8000 100 20000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[300 0;0 50]; %T Tp R=[280 0;0 30]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);