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 2026-01-12 11:36:21 +08:00
parent a4a8bb6ccb
commit bced810ccb
7 changed files with 8694 additions and 8632 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -135,7 +135,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name> <Name>-X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -186,6 +186,22 @@
<ExecCommand></ExecCommand> <ExecCommand></ExecCommand>
<Expression>0x00000086</Expression> <Expression>0x00000086</Expression>
</Bp> </Bp>
<Bp>
<Number>2</Number>
<Type>0</Type>
<LineNumber>255</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>startup_stm32h723xx.s</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
</Breakpoint> </Breakpoint>
<WatchWindow1> <WatchWindow1>
<Ww> <Ww>
@ -256,7 +272,7 @@
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>10000000</DbgClock> <DbgClock>1000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

17193
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

74
User/module/balance_chassis.c
View File

@ -19,14 +19,14 @@
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */ #define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */ #define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
#define WHEEL_GEAR_RATIO 4.0f /* 轮毂电机扭矩 */ #define WHEEL_GEAR_RATIO 8.0f /* 轮毂电机扭矩 */
#define BASE_LEG_LENGTH 0.12f /* 基础腿长 (m) */ #define BASE_LEG_LENGTH 0.12f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */ #define LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */ #define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.42f /* 最大腿长 (m) */ #define MAX_LEG_LENGTH 0.42f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */ #define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */ #define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 65.0f /* 右腿基础支撑力 (N) */ #define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
/* Private function prototypes ---------------------------------------------- */ /* Private function prototypes ---------------------------------------------- */
static int8_t Chassis_MotorEnable(Chassis_t *c); static int8_t Chassis_MotorEnable(Chassis_t *c);
@ -148,21 +148,62 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
/** /**
* @brief 更新机体状态估计 * @brief 更新机体状态估计
* @param c 底盘结构体指针 * @param c 底盘结构体指针
* @note 基于轮腿机器人运动学的精确速度估计算法
* 综合考虑轮子速度、关节运动和机体姿态的影响
*/ */
static void Chassis_UpdateChassisState(Chassis_t *c) { static void Chassis_UpdateChassisState(Chassis_t *c) {
if (c == NULL) return; if (c == NULL) return;
/* 从轮子编码器估计机体速度 */ /* 获取轮子原始速度数据 (dps -> rad/s) */
float left_speed_dps = c->feedback.wheel[0].rotor_speed; float left_wheel_speed = c->feedback.wheel[0].rotor_speed * (M_PI / 180.0f);
float right_speed_dps = c->feedback.wheel[1].rotor_speed; float right_wheel_speed = c->feedback.wheel[1].rotor_speed * (M_PI / 180.0f);
/* 将dps转换为rad/s，再转为线速度 */ /* 获取关节速度和陀螺仪数据 (dps -> rad/s) */
float left_linear_vel = left_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS; float left_joint_speed = c->feedback.joint[1].rotor_speed * (M_PI / 180.0f); /* 左前关节 */
float right_linear_vel = right_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS; float right_joint_speed = c->feedback.joint[2].rotor_speed * (M_PI / 180.0f); /* 右前关节 */
float pitch_gyro = c->feedback.imu.gyro.y; /* 俯仰角速度 rad/s */
/* 更新机体速度和位置 */ /*
* 轮子角速度补偿算法
* 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰
* 公式来源：五连杆机构运动学分析
*/
float left_w_wheel_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
float right_w_wheel_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;
/*
* 机体速度计算算法
* 综合考虑：
* 1. 轮子接触点线速度：omega_wheel * r_wheel
* 2. 摆杆运动贡献：L0 * d_theta (摆杆角速度在x方向的分量)
* 3. 腿长变化贡献：d_L0 * sin(theta) (腿长变化在x方向的分量)
*/
float left_v_body = left_w_wheel_compensated * WHEEL_RADIUS +
c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta +
c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);
float right_v_body = right_w_wheel_compensated * WHEEL_RADIUS +
c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta +
c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);
/* 保存历史速度 */
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x; c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
c->chassis_state.velocity_x = (left_linear_vel + right_linear_vel) / 2.0f;
/* 计算平均速度 */
float vel_measured = (left_v_body + right_v_body) / 2.0f;
/*
* 接地状态检测与速度修正
* 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0
*/
if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
vel_measured = 0.0f;
}
/* 使用二阶低通滤波器平滑速度估计 */
c->chassis_state.velocity_x = LowPassFilter2p_Apply(&c->filter.velocity_est, vel_measured);
/* 位置积分更新 */
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
} }
@ -226,6 +267,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
LowPassFilter2p_Init(&c->filter.wheel_out[i], target_freq, param->low_pass_cutoff_freq.out); LowPassFilter2p_Init(&c->filter.wheel_out[i], target_freq, param->low_pass_cutoff_freq.out);
} }
LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f); /* 速度估计滤波截止频率30Hz */
Chassis_MotorEnable(c); Chassis_MotorEnable(c);
@ -356,9 +398,9 @@ void Chassis_Output(Chassis_t *c) {
if (c == NULL) return; if (c == NULL) return;
/* 关节输出滤波 */ /* 关节输出滤波 */
for (int i = 0; i < 4; i++) { // for (int i = 0; i < 4; i++) {
c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]); // c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
} // }
/* 发送关节控制指令 */ /* 发送关节控制指令 */
MOTOR_LZ_MotionParam_t motion_param = { MOTOR_LZ_MotionParam_t motion_param = {
@ -375,9 +417,9 @@ void Chassis_Output(Chassis_t *c) {
} }
/* 轮子输出滤波并发送 */ /* 轮子输出滤波并发送 */
for (int i = 0; i < 2; i++) { // for (int i = 0; i < 2; i++) {
c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]); // c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
} // }
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]); MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]); MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
} }

5
User/module/balance_chassis.h
View File

@ -151,8 +151,9 @@ typedef struct {
/* 滤波器 */ /* 滤波器 */
struct { struct {
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */ LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */ LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */
} filter; } filter;
} Chassis_t; } Chassis_t;

24
User/module/config.c
View File

@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -2.702074813939778e+02f, 2.894104138810802e+02f, -1.211372850409846e+02f, -6.933300731936785e-01f }, // theta .k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
.k12_coeff = { -3.327095836963479e+00f, 5.164647957579151e+00f, -7.601554284013210e+00f, 2.552123800155830e-01f }, // d_theta .k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f }, // d_theta
.k13_coeff = { -4.196624710163955e+01f, 4.127695960453792e+01f, -1.402278033829385e+01f, 7.645039434796388e-03f }, // x .k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f }, // x
.k14_coeff = { -5.313365798300281e+01f, 5.256471525738568e+01f, -1.848736219345810e+01f, -1.124172698659643e-03f }, // d_x .k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f }, // d_x
.k15_coeff = { -6.095849580858518e+01f, 7.441367039791133e+01f, -3.453552694561191e+01f, 7.000025215843615e+00f }, // phi .k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
.k16_coeff = { -9.662388366457952e+00f, 1.288897516182337e+01f, -6.650550674687217e+00f, 1.558651627757777e+00f }, // d_phi .k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f }, // d_phi
.k21_coeff = { 1.194019591863143e+02f, -7.294395555463191e+01f, -5.502401903255857e+00f, 1.056061060815681e+01f }, // theta .k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
.k22_coeff = { 1.953914704492318e+01f, -1.853477522511211e+01f, 5.522863854227361e+00f, 2.440732134290017e-01f }, // d_theta .k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f }, // d_theta
.k23_coeff = { -3.113451478996141e+01f, 3.917196236114254e+01f, -1.888229690044390e+01f, 4.105978636011114e+00f }, // x .k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f }, // x
.k24_coeff = { -3.864995803122945e+01f, 4.873841855321316e+01f, -2.361615127826612e+01f, 5.240190452941591e+00f }, // d_x .k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f }, // d_x
.k25_coeff = { 2.402815287983674e+02f, -2.353554477048087e+02f, 7.938206986720357e+01f, 7.179992245652441e-01f }, // phi .k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
.k26_coeff = { 5.110194134606057e+01f, -5.057473149871059e+01f, 1.733200146622070e+01f, -2.070892022734363e-01f }, // d_phi .k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f }, // d_phi
}, },
.theta = 0.0f, .theta = 0.0f,
.x = 0.0f, .x = 0.0f,

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

@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
R1=0.068; %驱动轮半径 R1=0.068; %驱动轮半径
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.5; %杆质量 mp1=1.8; %杆质量
M1=12.0; %机体质量 M1=12.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; %摆杆转动惯量
@ -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([500 1 600 200 8000 100]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[200 0;0 60]; %T Tp R=[250 0;0 50]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);