From 22e80298538e7baf0612e769e9719c43e28ae4e0 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 25 Oct 2025 00:17:56 +0800 Subject: [PATCH] =?UTF-8?q?lqr=E5=A5=BD=E9=9A=BE=E8=B0=83=E5=95=8A?= =?UTF-8?q?=E8=8D=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/main.c | 2 +- MDK-ARM/DevC.uvprojx | 85 ++++++++++- MDK-ARM/DevC/DevC.lnp | 17 ++- User/device/ai.h | 1 - User/module/balance_chassis.c | 136 +++--------------- User/module/balance_chassis.h | 12 -- User/module/config.c | 25 ++-- .../balance/series_legs/get_k.m | 72 +++++----- .../balance/series_legs/get_k_length.m | 6 +- 9 files changed, 166 insertions(+), 190 deletions(-) diff --git a/Core/Src/main.c b/Core/Src/main.c index 6883e26..815abda 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -74,7 +74,7 @@ void MX_FREERTOS_Init(void); * @retval int */ int main(void) -{ + { /* USER CODE BEGIN 1 */ diff --git a/MDK-ARM/DevC.uvprojx b/MDK-ARM/DevC.uvprojx index e01fb9b..265bb5b 100644 --- a/MDK-ARM/DevC.uvprojx +++ b/MDK-ARM/DevC.uvprojx @@ -784,11 +784,6 @@ 1 ..\User\component\ahrs.c - - cmd.c - 1 - ..\User\component\cmd.c - filter.c 1 @@ -824,6 +819,21 @@ 1 ..\User\component\vmc.c + + crc8.c + 1 + ..\User\component\crc8.c + + + crc16.c + 1 + ..\User\component\crc16.c + + + speed_planner.c + 1 + ..\User\component\speed_planner.c + @@ -859,6 +869,31 @@ 1 ..\User\device\motor_lz.c + + ai.c + 1 + ..\User\device\ai.c + + + bmi088.c + 1 + ..\User\device\bmi088.c + + + motor_dm.c + 1 + ..\User\device\motor_dm.c + + + motor_rm.c + 1 + ..\User\device\motor_rm.c + + + virtual_chassis.c + 1 + ..\User\device\virtual_chassis.c + @@ -874,6 +909,21 @@ 1 ..\User\module\balance_chassis.c + + cmd.c + 1 + ..\User\module\cmd.c + + + gimbal.c + 1 + ..\User\module\gimbal.c + + + shoot.c + 1 + ..\User\module\shoot.c + @@ -919,6 +969,31 @@ 1 ..\User\task\ctrl_gimbal.c + + ai.c + 1 + ..\User\task\ai.c + + + cmd.c + 1 + ..\User\task\cmd.c + + + ctrl_shoot.c + 1 + ..\User\task\ctrl_shoot.c + + + monitor.c + 1 + ..\User\task\monitor.c + + + music.c + 1 + ..\User\task\music.c + diff --git a/MDK-ARM/DevC/DevC.lnp b/MDK-ARM/DevC/DevC.lnp index 455e580..44b0d59 100644 --- a/MDK-ARM/DevC/DevC.lnp +++ b/MDK-ARM/DevC/DevC.lnp @@ -70,7 +70,6 @@ ".\devc\time.o" ".\devc\uart.o" ".\devc\ahrs.o" -".\devc\cmd.o" ".\devc\filter.o" ".\devc\limiter.o" ".\devc\pid.o" @@ -78,14 +77,25 @@ ".\devc\kinematics.o" ".\devc\lqr.o" ".\devc\vmc.o" +".\devc\crc8.o" +".\devc\crc16.o" +".\devc\speed_planner.o" ".\devc\buzzer.o" ".\devc\dm_imu.o" ".\devc\dr16.o" ".\devc\motor.o" ".\devc\motor_lk.o" ".\devc\motor_lz.o" +".\devc\ai.o" +".\devc\bmi088.o" +".\devc\motor_dm.o" +".\devc\motor_rm.o" +".\devc\virtual_chassis.o" ".\devc\config.o" ".\devc\balance_chassis.o" +".\devc\cmd_1.o" +".\devc\gimbal.o" +".\devc\shoot.o" ".\devc\blink.o" ".\devc\init.o" ".\devc\rc.o" @@ -93,6 +103,11 @@ ".\devc\atti_esti.o" ".\devc\ctrl_chassis.o" ".\devc\ctrl_gimbal.o" +".\devc\ai_1.o" +".\devc\cmd_1.o" +".\devc\ctrl_shoot.o" +".\devc\monitor.o" +".\devc\music.o" --strict --scatter ".\DevC\DevC.sct" --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols --info sizes --info totals --info unused --info veneers diff --git a/User/device/ai.h b/User/device/ai.h index 47841e0..b9fc688 100644 --- a/User/device/ai.h +++ b/User/device/ai.h @@ -4,7 +4,6 @@ #pragma once -#include #ifdef __cplusplus extern "C" { #endif diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 74a3579..9d73c14 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -42,19 +42,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */ - // 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置 - // 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃 - if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) { - c->mode = mode; - return CHASSIS_OK; - } - if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) { - c->mode = mode; - c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 - c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃 - return CHASSIS_OK; - } - Chassis_MotorEnable(c); PID_Reset(&c->pid.leg_length[0]); @@ -101,8 +88,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f); c->mode = mode; - c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 - c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃 c->wz_multi=0.0f; return CHASSIS_OK; @@ -132,11 +117,8 @@ static void Chassis_UpdateChassisState(Chassis_t *c) { c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; - // 在跳跃模式的起跳和收腿阶段暂停位移积分，避免轮子空转导致的位移误差 - if (!(c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3))) { - // 积分得到位置 - c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; - } + // 积分得到位置 + c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; } @@ -289,45 +271,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_Output(c); // 统一输出 } 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 >= 230000) { - // state 2 保持0.25s后转到state 3，确保腿部充分伸展 - c->state = 3; - c->jump_time = BSP_TIME_Get_us(); // 重置计时 - } else if (c->state == 3 && elapsed_us >= 500000) { - // state 3 保持0.4s后转到state 0，确保收腿到最高 - c->state = 0; - c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发 - } - } - - // 执行LQR控制（包含PID腿长控制） - Chassis_LQRControl(c, c_cmd); - Chassis_Output(c); // 统一输出 - break; case CHASSIS_MODE_WHELL_LEG_BALANCE: case CHASSIS_MODE_ROTOR: @@ -431,17 +374,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 构建目标状态 */ LQR_State_t target_state = {0}; - // 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直 - if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) { - target_state.theta = 0.0f; // 强制摆杆角度为0，确保腿部竖直 - } else { - target_state.theta = 0.00; // 目标摆杆角度 - } - + target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - 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.phi = 0.0f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 - target_state.x = c->chassis_state.target_x -2.07411f*(0.16f + c_cmd->height * 0.25f) + 0.41182f; + target_state.x = c->chassis_state.target_x; target_state.d_x = target_dot_x; // 目标速度 if (c->mode != CHASSIS_MODE_ROTOR){ @@ -494,13 +431,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_SetTargetState(&c->lqr[0], &target_state); LQR_Control(&c->lqr[0]); } else { - // 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f - float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f; - target_state.theta = non_contact_theta; // 非接触时的腿摆角目标 + target_state.theta = 0.16f; // 非接触时的腿摆角目标 LQR_SetTargetState(&c->lqr[0], &target_state); c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.Tp = - c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + non_contact_theta) + + c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.16f) + c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f); c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 } @@ -508,28 +443,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_SetTargetState(&c->lqr[1], &target_state); LQR_Control(&c->lqr[1]); }else { - // 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f - float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f; - target_state.theta = non_contact_theta; // 非接触时的腿摆角目标 + target_state.theta = 0.16f; // 非接触时的腿摆角目标 LQR_SetTargetState(&c->lqr[1], &target_state); c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.Tp = - c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + non_contact_theta) + + c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.16f) + c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f); c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 } /* 轮毂力矩输出（参考C++版本的减速比） */ - // 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转 - if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) { - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; - } else { - c->output.wheel[0] = - c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; - c->output.wheel[1] = - c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; - } + c->output.wheel[0] = + c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; + c->output.wheel[1] = + c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; @@ -556,39 +483,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } // 目标腿长设定（包含横滚角补偿） - switch (c->state) { - case 0: // 正常状态，根据高度指令调节腿长 - target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿 - target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿 - break; - case 1: // 准备阶段，腿长收缩 - target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿 - target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿 - break; - case 2: // 起跳阶段，腿长快速伸展 - target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿） - target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿） - break; - case 3: // 收腿阶段，腿长收缩到最高 - target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高） - target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高） - break; - default: - target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; - target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; - break; - } + target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿 + target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿 - // 腿长限幅：根据跳跃状态调整限制范围 - if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) { - // 在跳跃收腿阶段，允许更短的腿长，确保收腿到最高 - if (target_L0[0] < 0.06f) target_L0[0] = 0.06f; - if (target_L0[1] < 0.06f) target_L0[1] = 0.06f; - } else { - // 正常情况下的腿长限制：最短0.10，最长0.42m - if (target_L0[0] < 0.10f) target_L0[0] = 0.10f; - if (target_L0[1] < 0.10f) target_L0[1] = 0.10f; - } + // 腿长限幅：最短0.10，最长0.42m + if (target_L0[0] < 0.10f) target_L0[0] = 0.10f; + if (target_L0[1] < 0.10f) target_L0[1] = 0.10f; if (target_L0[0] > 0.42f) target_L0[0] = 0.42f; if (target_L0[1] > 0.42f) target_L0[1] = 0.42f; diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 98f694e..82c72c1 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -40,17 +40,9 @@ typedef enum { CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ - CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */ CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */ } Chassis_Mode_t; -typedef enum { - CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */ - CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */ - CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */ - CHASSIS_JUMP_STATE_END, /* 跳跃结束 */ -} Chassis_JumpState_t; - typedef struct { Chassis_Mode_t mode; /* 底盘模式 */ MoveVector_t move_vec; /* 底盘运动向量 */ @@ -125,10 +117,6 @@ typedef struct { VMC_t vmc_[2]; /* 两条腿的VMC */ LQR_t lqr[2]; /* 两条腿的LQR控制器 */ - int8_t state; - uint64_t jump_time; - - float angle; float height; diff --git a/User/module/config.c b/User/module/config.c index 66ecc42..0f7b956 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -254,20 +254,19 @@ Config_RobotParam_t robot_config = { } }, - .lqr_gains = { - .k11_coeff = { -2.299492214443364e+02f, 2.371048664025156e+02f, -1.091592921067477e+02f, -5.004256900558796e+00f }, // theta - .k12_coeff = { -5.160496889728024e+00f, 3.062519293243873e+00f, -9.339456933350174e+00f, -4.793280770575950e-01f }, // d_theta - .k13_coeff = { -5.187383520564029e+01f, 4.899697460241380e+01f, -1.565478588426995e+01f, -2.004694192093775e+00f }, // x - .k14_coeff = { -5.668796498905875e+01f, 5.377351750824888e+01f, -1.871718521370502e+01f, -2.691170008421150e+00f }, // d_x - .k15_coeff = { -1.449457451875909e+02f, 1.491544007677516e+02f, -5.400783326406858e+01f, 7.212250094701016e+00f }, // phi - .k16_coeff = { -9.623232912225313e+01f, 1.012699482439727e+02f, -3.862842162301360e+01f, 6.174165209610019e+00f }, // d_phi - .k21_coeff = { -1.197720213257983e+02f, 1.432558800683586e+02f, -6.332976803783767e+01f, 1.244403075562354e+01f }, // theta - .k22_coeff = { -1.641975451035667e+01f, 1.816373049685376e+01f, -6.825602612420409e+00f, 1.759808519224317e+00f }, // d_theta - .k23_coeff = { -9.525505626411980e+01f, 9.986545276773818e+01f, -3.780741158172127e+01f, 6.150010121224194e+00f }, // x - .k24_coeff = { -1.249520841685426e+02f, 1.297949868891588e+02f, -4.847618771898694e+01f, 7.842551207050604e+00f }, // d_x - .k25_coeff = { 1.098971996183004e+02f, -1.028972351169783e+02f, 3.234322461758097e+01f, 1.250515887186789e+01f }, // phi - .k26_coeff = { 1.122150774085550e+02f, -1.059683417765082e+02f, 3.379315768085248e+01f, 5.474171975795353e+00f }, // d_phi + .k11_coeff = { -2.156343028411120e+02f, 2.257665290023013e+02f, -1.002267208978328e+02f, -2.350184261149268e+00f }, // theta + .k12_coeff = { -7.743056249446016e+00f, 6.752493325294916e+00f, -8.796808854152014e+00f, -3.119669283951065e-01f }, // d_theta + .k13_coeff = { -1.118487386616928e+02f, 1.102429207742772e+02f, -3.732715045746770e+01f, -7.925992877328480e-01f }, // x + .k14_coeff = { -6.940658312417929e+01f, 6.823355321869006e+01f, -2.465299412907610e+01f, -1.060847118860617e+00f }, // d_x + .k15_coeff = { -4.583141861746078e+01f, 5.818701324121601e+01f, -2.683216623272723e+01f, 4.507357792784873e+00f }, // phi + .k16_coeff = { -1.544882596273596e+01f, 1.975202028975679e+01f, -9.521615432406449e+00f, 1.862826936219259e+00f }, // d_phi + .k21_coeff = { 9.252532546470242e+00f, 4.417889614536075e+01f, -4.710921937977012e+01f, 1.516691345722052e+01f }, // theta + .k22_coeff = { -1.018276949440906e+00f, 6.140925452791167e+00f, -4.973407169060023e+00f, 2.141324584243587e+00f }, // d_theta + .k23_coeff = { -1.020910325776861e+02f, 1.292784950839223e+02f, -6.114289602021928e+01f, 1.251012410414232e+01f }, // x + .k24_coeff = { -1.009199238331894e+02f, 1.180052040402144e+02f, -5.140001160311314e+01f, 9.859995261307304e+00f }, // d_x + .k25_coeff = { 9.176820525459368e+01f, -9.170665517126247e+01f, 3.124569590587187e+01f, 6.886312255252024e+00f }, // phi + .k26_coeff = { 4.772624933352569e+01f, -4.722944757834970e+01f, 1.599553913425377e+01f, 1.037598895224650e+00f }, // d_phi }, .theta = 0.0f, .x = 0.0f, diff --git a/utils/Simulation-master/balance/series_legs/get_k.m b/utils/Simulation-master/balance/series_legs/get_k.m index 62754ff..73f8a65 100644 --- a/utils/Simulation-master/balance/series_legs/get_k.m +++ b/utils/Simulation-master/balance/series_legs/get_k.m @@ -56,41 +56,41 @@ fprintf('},\n'); fprintf('========================================\n\n'); % 可选：显示拟合曲线图 -x0=leg; %步长为0.1 -y11=polyval(a11,x0); %返回值y0是对应于x0的函数值 -y12=polyval(a12,x0); %返回值y0是对应于x0的函数值 -y13=polyval(a13,x0); %返回值y0是对应于x0的函数值 -y14=polyval(a14,x0); %返回值y0是对应于x0的函数值 -y15=polyval(a15,x0); %返回值y0是对应于x0的函数值 -y16=polyval(a16,x0); %返回值y0是对应于x0的函数值 - -y21=polyval(a21,x0); %返回值y0是对应于x0的函数值 -y22=polyval(a22,x0); %返回值y0是对应于x0的函数值 -y23=polyval(a23,x0); %返回值y0是对应于x0的函数值 -y24=polyval(a24,x0); %返回值y0是对应于x0的函数值 -y25=polyval(a25,x0); %返回值y0是对应于x0的函数值 -y26=polyval(a26,x0); %返回值y0是对应于x0的函数值 - -figure; -subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合'); -subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合'); -subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合'); -subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合'); -subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合'); -subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合'); - -subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合'); -subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合'); -subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合'); -subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合'); -subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合'); -subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合'); - -for i = 1:12 - subplot(3,4,i); - grid on; %添加网格线 - set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5); %将网格线变成虚线 - legend('原始数据','拟合曲线','Location','best'); -end +% x0=leg; %步长为0.1 +% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值 +% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值 +% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值 +% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值 +% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值 +% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值 +% +% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值 +% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值 +% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值 +% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值 +% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值 +% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值 +% +% figure; +% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合'); +% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合'); +% subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合'); +% subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合'); +% subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合'); +% subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合'); +% +% subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合'); +% subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合'); +% subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合'); +% subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合'); +% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合'); +% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合'); +% +% for i = 1:12 +% subplot(3,4,i); +% grid on; %添加网格线 +% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5); %将网格线变成虚线 +% legend('原始数据','拟合曲线','Location','best'); +% end toc diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 1aadeb4..946f636 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -20,7 +20,7 @@ function K = get_k_length(leg_length) l1=-0.03; %机体质心距离转轴距离 mw1=0.60; %驱动轮质量 mp1=1.8; %杆质量 - M1=8.0; %机体质量 + M1=6.0; %机体质量 Iw1=mw1*R1^2; %驱动轮转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^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=double(B); - Q=diag([160 160 2000 1500 20000 5000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[140 0;0 60]; %T Tp + Q=diag([700 150 4000 200 6000 300]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[140 0;0 40]; %T Tp K=lqr(A,B,Q,R);