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-09 17:27:14 +08:00
parent 17c6a92fd0
commit dcc3b55df8
6 changed files with 395 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/module/balance_chassis.c
View File

@ -352,11 +352,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 构建目标状态 */
LQR_State_t target_state = {0};
target_state.theta = 0.0f; // 目标摆杆角度
target_state.theta = c->param->theta; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = -0.1f; // 目标俯仰角
target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x; // 目标位置
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; // 目标速度
/* 更新LQR状态 */
@ -384,8 +384,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} else {
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][6] * (-c->vmc_[0].leg.theta + 0.0f) +
c->lqr[0].K_matrix[1][7] * (-c->vmc_[0].leg.d_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->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
if (c->vmc_[1].leg.is_ground_contact)
@ -393,8 +393,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
else {
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][6] * (-c->vmc_[1].leg.theta + 0.0f) +
c->lqr[1].K_matrix[1][7] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
@ -421,8 +421,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
roll_compensation_force = -20.0f;
// 目标腿长设定（移除横滚角补偿）
target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节
target_L0[0] = 0.15f + c_cmd->height * 0.25f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.25f; // 右腿：基础腿长 + 高度调节
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@ -498,7 +498,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i]) > 20.0f) {
c->output.joint[i] = (c->output.joint[i] > 0) ? 20.0f : -20.0f;
c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
}
}

4
User/module/balance_chassis.h
View File

@ -91,6 +91,10 @@ typedef struct {
float mech_zero_yaw; /* 机械零点 */
float theta;
float x;
float phi;
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */

28
User/module/config.c
View File

@ -254,20 +254,24 @@ Config_RobotParam_t robot_config = {
}
},
.lqr_gains = {
.k11_coeff = { -1.664671105856041e+02f, 1.992942584227816e+02f, -1.128817545552375e+02f, -3.940642943456036e+00f }, // theta
.k12_coeff = { 6.755144599983973e+00f, -6.854975158723420e+00f, -7.879639603878262e+00f, -3.430989798548029e-01f }, // d_theta
.k13_coeff = { -3.418833492635837e+01f, 3.794539021170885e+01f, -1.520710246171202e+01f, -1.850485591688714e+00f }, // x
.k14_coeff = { -2.922101309139124e+01f, 3.441555363812500e+01f, -1.622627738805500e+01f, -2.575952007718017e+00f }, // d_x
.k15_coeff = { -2.999200539123322e+01f, 4.450774352569402e+01f, -2.617910446060207e+01f, 7.659044227304066e+00f }, // phi
.k16_coeff = { -6.929749592092386e+00f, 8.873396045817064e+00f, -4.631802944769120e+00f, 1.362572135878896e+00f }, // d_phi
.k21_coeff = { 1.839005962436392e+02f, -1.553482828633151e+02f, 3.110973707035321e+01f, 1.011416512244285e+01f }, // theta
.k22_coeff = { 8.875841429383772e+00f, -9.417579122119678e+00f, 3.419383245219720e+00f, 1.210428310841859e+00f }, // d_theta
.k23_coeff = { -1.702557573110695e+01f, 3.268593582331833e+01f, -2.244761887504000e+01f, 6.890533147883790e+00f }, // x
.k24_coeff = { -3.039626115318488e+01f, 4.694731664073340e+01f, -2.839718813188056e+01f, 8.384942967731277e+00f }, // d_x
.k25_coeff = { 1.199164883293124e+02f, -1.297942949573494e+02f, 5.072648972704373e+01f, 3.419795826934867e+00f }, // phi
.k26_coeff = { 1.995972391472551e+01f, -2.168740968259558e+01f, 8.589314159209088e+00f, 1.523026014539914e-01f }, // d_phi
.k11_coeff = { -1.675900301510996e+02f, 2.020661327080207e+02f, -1.145642457869424e+02f, -3.747932874943916e+00f }, // theta
.k12_coeff = { 6.610013864045129e+00f, -6.495140417893399e+00f, -8.129319798942683e+00f, -2.920993723294792e-01f }, // d_theta
.k13_coeff = { -3.558043125079351e+01f, 3.982938747687503e+01f, -1.608580664764812e+01f, -1.704388165552662e+00f }, // x
.k14_coeff = { -3.082060804555171e+01f, 3.657436465245446e+01f, -1.721864301319228e+01f, -2.425477933777937e+00f }, // d_x
.k15_coeff = { -2.915524527743928e+01f, 4.841882215140198e+01f, -3.075590139522371e+01f, 9.309579833638985e+00f }, // phi
.k16_coeff = { -6.590867790309310e+00f, 8.818728233474916e+00f, -4.823548294804866e+00f, 1.465064667201413e+00f }, // d_phi
.k21_coeff = { 2.124446115135024e+02f, -1.763726543380520e+02f, 3.279099984858797e+01f, 1.233889708835248e+01f }, // theta
.k22_coeff = { 1.207615888432813e+01f, -1.237642650067993e+01f, 4.209610950938401e+00f, 1.417825883473013e+00f }, // d_theta
.k23_coeff = { -1.482017772836761e+01f, 3.364904259474741e+01f, -2.486572037866952e+01f, 7.847915428831716e+00f }, // x
.k24_coeff = { -3.273926334829641e+01f, 5.271977032684813e+01f, -3.274024194089392e+01f, 9.730698282497507e+00f }, // d_x
.k25_coeff = { 1.663230320117893e+02f, -1.816044424207695e+02f, 7.154626742111853e+01f, 4.309975259244579e+00f }, // phi
.k26_coeff = { 2.350205493668966e+01f, -2.586342446220884e+01f, 1.038824015934221e+01f, 1.031625329478827e-01f }, // d_phi
},
.theta = 0.0f,
.x = 0.0f,
.phi = -0.1f,
.virtual_chassis_param = {
.motors = {
.can = BSP_CAN_1,

4
utils/Simulation-master/balance/parallel_legs/get_k_length.m
View File

@ -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([700 1 600 200 1000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 25]; %T Tp
Q=diag(1000 1 200 200 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[150 0;0 25]; %T Tp
K=lqr(A,B,Q,R);

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

@ -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([800 100 2500 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[140 0;0 50]; %T Tp
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
K=lqr(A,B,Q,R);

361
未命名.jdebug Normal file
View File

@ -0,0 +1,361 @@
/*********************************************************************
* (c) SEGGER Microcontroller GmbH *
* The Embedded Experts *
* www.segger.com *
**********************************************************************
File : /Users/lvzucheng/Documents/R/balance_infantry/未命名.jdebug
Created : 8 Oct 2025 23:23
Ozone Version : V3.38d
*/
/*********************************************************************
*
* OnProjectLoad
*
* Function description
* Project load routine. Required.
*
**********************************************************************
*/
void OnProjectLoad (void) {
//
// Dialog-generated settings
//
Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
Project.SetDevice ("STM32F407IG");
Project.SetHostIF ("USB", "207400620");
Project.SetTargetIF ("SWD");
Project.SetTIFSpeed ("4 MHz");
Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
//
// User settings
//
File.Open ("$(ProjectDir)/build/Debug/DevC.elf");
}
/*********************************************************************
*
* OnStartupComplete
*
* Function description
* Called when program execution has reached/passed
* the startup completion point. Optional.
*
**********************************************************************
*/
//void OnStartupComplete (void) {
//}
/*********************************************************************
*
* TargetReset
*
* Function description
* Replaces the default target device reset routine. Optional.
*
* Notes
* This example demonstrates the usage when
* debugging an application in RAM on a Cortex-M target device.
*
**********************************************************************
*/
//void TargetReset (void) {
//
// unsigned int SP;
// unsigned int PC;
// unsigned int VectorTableAddr;
//
// VectorTableAddr = Elf.GetBaseAddr();
// //
// // Set up initial stack pointer
// //
// if (VectorTableAddr != 0xFFFFFFFF) {
// SP = Target.ReadU32(VectorTableAddr);
// Target.SetReg("SP", SP);
// }
// //
// // Set up entry point PC
// //
// PC = Elf.GetEntryPointPC();
//
// if (PC != 0xFFFFFFFF) {
// Target.SetReg("PC", PC);
// } else if (VectorTableAddr != 0xFFFFFFFF) {
// PC = Target.ReadU32(VectorTableAddr + 4);
// Target.SetReg("PC", PC);
// } else {
// Util.Error("Project file error: failed to set entry point PC", 1);
// }
//}
/*********************************************************************
*
* BeforeTargetReset
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetReset (void) {
//}
/*********************************************************************
*
* AfterTargetReset
*
* Function description
* Event handler routine. Optional.
* The default implementation initializes SP and PC to reset values.
**
**********************************************************************
*/
void AfterTargetReset (void) {
_SetupTarget();
}
/*********************************************************************
*
* DebugStart
*
* Function description
* Replaces the default debug session startup routine. Optional.
*
**********************************************************************
*/
//void DebugStart (void) {
//}
/*********************************************************************
*
* TargetConnect
*
* Function description
* Replaces the default target IF connection routine. Optional.
*
**********************************************************************
*/
//void TargetConnect (void) {
//}
/*********************************************************************
*
* BeforeTargetConnect
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetConnect (void) {
//}
/*********************************************************************
*
* AfterTargetConnect
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetConnect (void) {
//}
/*********************************************************************
*
* TargetDownload
*
* Function description
* Replaces the default program download routine. Optional.
*
**********************************************************************
*/
//void TargetDownload (void) {
//}
/*********************************************************************
*
* BeforeTargetDownload
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetDownload (void) {
//}
/*********************************************************************
*
* AfterTargetDownload
*
* Function description
* Event handler routine. Optional.
* The default implementation initializes SP and PC to reset values.
*
**********************************************************************
*/
void AfterTargetDownload (void) {
_SetupTarget();
}
/*********************************************************************
*
* BeforeTargetDisconnect
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetDisconnect (void) {
//}
/*********************************************************************
*
* AfterTargetDisconnect
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetDisconnect (void) {
//}
/*********************************************************************
*
* AfterTargetHalt
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void AfterTargetHalt (void) {
//}
/*********************************************************************
*
* BeforeTargetResume
*
* Function description
* Event handler routine. Optional.
*
**********************************************************************
*/
//void BeforeTargetResume (void) {
//}
/*********************************************************************
*
* OnSnapshotLoad
*
* Function description
* Called upon loading a snapshot. Optional.
*
* Additional information
* This function is used to restore the target state in cases
* where values cannot simply be written to the target.
* Typical use: GPIO clock needs to be enabled, before
* GPIO is configured.
*
**********************************************************************
*/
//void OnSnapshotLoad (void) {
//}
/*********************************************************************
*
* OnSnapshotSave
*
* Function description
* Called upon saving a snapshot. Optional.
*
* Additional information
* This function is usually used to save values of the target
* state which can either not be trivially read,
* or need to be restored in a specific way or order.
* Typically use: Memory Mapped Registers,
* such as PLL and GPIO configuration.
*
**********************************************************************
*/
//void OnSnapshotSave (void) {
//}
/*********************************************************************
*
* OnError
*
* Function description
* Called when an error ocurred. Optional.
*
**********************************************************************
*/
//void OnError (void) {
//}
/*********************************************************************
*
* AfterProjectLoad
*
* Function description
* After Project load routine. Optional.
*
**********************************************************************
*/
//void AfterProjectLoad (void) {
//}
/*********************************************************************
*
* OnDebugStartBreakSymbolReached
*
* Function description
* Called when program execution has reached/passed
* the symbol to be breaked at during debug start. Optional.
*
**********************************************************************
*/
//void OnDebugStartBreakSymReached (void) {
//}
/*********************************************************************
*
* _SetupTarget
*
* Function description
* Setup the target.
* Called by AfterTargetReset() and AfterTargetDownload().
*
* Auto-generated function. May be overridden by Ozone.
*
**********************************************************************
*/
void _SetupTarget(void) {
unsigned int SP;
unsigned int PC;
unsigned int VectorTableAddr;
VectorTableAddr = Elf.GetBaseAddr();
//
// Set up initial stack pointer
//
SP = Target.ReadU32(VectorTableAddr);
if (SP != 0xFFFFFFFF) {
Target.SetReg("SP", SP);
}
//
// Set up entry point PC
//
PC = Elf.GetEntryPointPC();
if (PC != 0xFFFFFFFF) {
Target.SetReg("PC", PC);
} else {
Util.Error("Project script error: failed to set up entry point PC", 1);
}
}