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-06 21:40:11 +08:00
parent 6e2e973dab
commit df420f0324
4 changed files with 27 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
User/device/virtual_chassis.c
View File

@ -119,12 +119,12 @@ static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint1
#define ACCEL_CAN_MIN (-58.8f)
#define GYRO_CAN_MAX (34.88f)
#define GYRO_CAN_MIN (-34.88f)
#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */
#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */
#define ROLL_CAN_MAX (M_2PI)
#define ROLL_CAN_MIN (-M_2PI)
#define YAW_CAN_MAX (M_2PI)
#define YAW_CAN_MIN (-M_PI_2) /* -π 弧度 ≈ -180° */
#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */
#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */
#define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
#define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
#define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
#define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
#define QUATERNION_MIN (-1.0f)
#define QUATERNION_MAX (1.0f)

10
User/module/balance_chassis.c
View File

@ -194,10 +194,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-c->feedback.imu.euler.rol, -c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-c->feedback.imu.euler.rol, -c->feedback.imu.gyro.y);
/*根据底盘模式执行不同的控制逻辑*/
switch (c->mode) {
@ -214,6 +214,8 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
fn = -20.0f;
tp = 0.0f;
Chassis_LQRControl(c, c_cmd);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
@ -295,7 +297,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
current_state.d_theta = c->vmc_[0].leg.d_theta;
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = c->feedback.imu.euler.pit;
current_state.phi = -c->feedback.imu.euler.rol;
current_state.d_phi = c->feedback.imu.gyro.y;
LQR_UpdateState(&c->lqr[0], &current_state);
@ -312,7 +314,7 @@ 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.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = -0.1f; // 目标俯仰角
target_state.phi = 0.0f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x; // 目标位置
target_state.d_x = target_dot_x; // 目标速度

24
User/module/config.c
View File

@ -254,18 +254,18 @@ Config_RobotParam_t robot_config = {
}
},
.lqr_gains = {
.k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
.k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta
.k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x
.k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x
.k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
.k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi
.k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
.k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta
.k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x
.k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x
.k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
.k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi
.k11_coeff = { -2.018567892252635e+02f, 2.443886947748371e+02f, -1.170514252719224e+02f, 1.756967283355012e+00f }, // theta
.k12_coeff = { 5.755541993064226e+00f, -2.330435203830081e+00f, -5.839389482510768e+00f, 3.102177559173085e-01f }, // d_theta
.k13_coeff = { -4.626648375801587e+01f, 4.957163915561897e+01f, -1.892978626686148e+01f, 1.163361730642856e+00f }, // x
.k14_coeff = { -6.133244696547617e+01f, 6.581150547694307e+01f, -2.562467010643359e+01f, 1.546359441777505e+00f }, // d_x
.k15_coeff = { -9.787512606327831e-01f, 8.581307530201126e+00f, -7.593138673503293e+00f, 1.797419370480695e+00f }, // phi
.k16_coeff = { 2.873430467178664e+00f, -8.356289550331172e-01f, -1.159458299556627e+00f, 5.473036343381187e-01f }, // d_phi
.k21_coeff = { 4.048940262085570e+02f, -3.365968455386673e+02f, 6.490322687755379e+01f, 1.009032151585340e+01f }, // theta
.k22_coeff = { 3.731821338634553e+01f, -3.863396990944306e+01f, 1.309004485801674e+01f, -8.491362495876140e-02f }, // d_theta
.k23_coeff = { 3.197848557628166e+01f, -1.613497787175245e+01f, -5.139546674944345e+00f, 4.089512188250406e+00f }, // x
.k24_coeff = { 4.042762833518352e+01f, -2.015511550072881e+01f, -6.901438674565010e+00f, 5.459259288213627e+00f }, // d_x
.k25_coeff = { 9.671666382924865e+01f, -1.007923006268712e+02f, 3.656525147129298e+01f, -7.438566326110798e-01f }, // phi
.k26_coeff = { 2.324856410306975e+01f, -2.529653580362136e+01f, 9.749203865654970e+00f, -5.110687534172851e-01f }, // d_phi
},
.virtual_chassis_param = {

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

@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.03; %机体质心距离转轴距离
l1=-0.01; %机体质心距离转轴距离
mw1=0.80; %驱动轮质量
mp1=1.11; %杆质量
M1=2.0; %机体质量
M1=12.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,7 +48,7 @@ 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([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
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
K=lqr(A,B,Q,R);