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

lqr好难调啊草

Browse Source
This commit is contained in:
Robofish 2025-10-25 00:17:56 +08:00
parent f87d2a29a6
commit 22e8029853
9 changed files with 166 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Src/main.c
View File

@ -74,7 +74,7 @@ void MX_FREERTOS_Init(void);
* @retval int
*/
int main(void)
{
{
/* USER CODE BEGIN 1 */

85
MDK-ARM/DevC.uvprojx
View File

@ -784,11 +784,6 @@
<FileType>1</FileType>
<FilePath>..\User\component\ahrs.c</FilePath>
</File>
<File>
<FileName>cmd.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\cmd.c</FilePath>
</File>
<File>
<FileName>filter.c</FileName>
<FileType>1</FileType>
@ -824,6 +819,21 @@
<FileType>1</FileType>
<FilePath>..\User\component\vmc.c</FilePath>
</File>
<File>
<FileName>crc8.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\crc8.c</FilePath>
</File>
<File>
<FileName>crc16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\crc16.c</FilePath>
</File>
<File>
<FileName>speed_planner.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\speed_planner.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -859,6 +869,31 @@
<FileType>1</FileType>
<FilePath>..\User\device\motor_lz.c</FilePath>
</File>
<File>
<FileName>ai.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\ai.c</FilePath>
</File>
<File>
<FileName>bmi088.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\bmi088.c</FilePath>
</File>
<File>
<FileName>motor_dm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_dm.c</FilePath>
</File>
<File>
<FileName>motor_rm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_rm.c</FilePath>
</File>
<File>
<FileName>virtual_chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\virtual_chassis.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -874,6 +909,21 @@
<FileType>1</FileType>
<FilePath>..\User\module\balance_chassis.c</FilePath>
</File>
<File>
<FileName>cmd.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\cmd.c</FilePath>
</File>
<File>
<FileName>gimbal.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\gimbal.c</FilePath>
</File>
<File>
<FileName>shoot.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\shoot.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -919,6 +969,31 @@
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_gimbal.c</FilePath>
</File>
<File>
<FileName>ai.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ai.c</FilePath>
</File>
<File>
<FileName>cmd.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\cmd.c</FilePath>
</File>
<File>
<FileName>ctrl_shoot.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_shoot.c</FilePath>
</File>
<File>
<FileName>monitor.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\monitor.c</FilePath>
</File>
<File>
<FileName>music.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\music.c</FilePath>
</File>
</Files>
</Group>
<Group>

17
MDK-ARM/DevC/DevC.lnp
View File

@ -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

1
User/device/ai.h
View File

@ -4,7 +4,6 @@
#pragma once
#include <sys/cdefs.h>
#ifdef __cplusplus
extern "C" {
#endif

136
User/module/balance_chassis.c
View File

@ -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;

12
User/module/balance_chassis.h
View File

@ -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;

25
User/module/config.c
View File

@ -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,

72
utils/Simulation-master/balance/series_legs/get_k.m
View File

@ -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

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

@ -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);