提交
This commit is contained in:
parent
a4a8bb6ccb
commit
bced810ccb
@ -135,7 +135,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
@ -186,6 +186,22 @@
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression>0x00000086</Expression>
|
||||
</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>
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
@ -256,7 +272,7 @@
|
||||
<EnableFlashSeq>1</EnableFlashSeq>
|
||||
<EnableLog>0</EnableLog>
|
||||
<Protocol>2</Protocol>
|
||||
<DbgClock>10000000</DbgClock>
|
||||
<DbgClock>1000000</DbgClock>
|
||||
</DebugDescription>
|
||||
</TargetOption>
|
||||
</Target>
|
||||
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@ -19,14 +19,14 @@
|
||||
|
||||
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m) */
|
||||
#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 LEG_LENGTH_RANGE 0.22f /* 腿长调节范围 (m) */
|
||||
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
|
||||
#define MAX_LEG_LENGTH 0.42f /* 最大腿长 (m) */
|
||||
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */
|
||||
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
|
||||
#define RIGHT_BASE_FORCE 65.0f /* 右腿基础支撑力 (N) */
|
||||
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
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 更新机体状态估计
|
||||
* @param c 底盘结构体指针
|
||||
* @note 基于轮腿机器人运动学的精确速度估计算法
|
||||
* 综合考虑轮子速度、关节运动和机体姿态的影响
|
||||
*/
|
||||
static void Chassis_UpdateChassisState(Chassis_t *c) {
|
||||
if (c == NULL) return;
|
||||
|
||||
/* 从轮子编码器估计机体速度 */
|
||||
float left_speed_dps = c->feedback.wheel[0].rotor_speed;
|
||||
float right_speed_dps = c->feedback.wheel[1].rotor_speed;
|
||||
/* 获取轮子原始速度数据 (dps -> rad/s) */
|
||||
float left_wheel_speed = c->feedback.wheel[0].rotor_speed * (M_PI / 180.0f);
|
||||
float right_wheel_speed = c->feedback.wheel[1].rotor_speed * (M_PI / 180.0f);
|
||||
|
||||
/* 将dps转换为rad/s，再转为线速度 */
|
||||
float left_linear_vel = left_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS;
|
||||
float right_linear_vel = right_speed_dps * (M_PI / 180.0f) * WHEEL_RADIUS;
|
||||
/* 获取关节速度和陀螺仪数据 (dps -> rad/s) */
|
||||
float left_joint_speed = c->feedback.joint[1].rotor_speed * (M_PI / 180.0f); /* 左前关节 */
|
||||
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.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;
|
||||
}
|
||||
|
||||
@ -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++) {
|
||||
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);
|
||||
|
||||
@ -356,9 +398,9 @@ void Chassis_Output(Chassis_t *c) {
|
||||
if (c == NULL) return;
|
||||
|
||||
/* 关节输出滤波 */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
|
||||
}
|
||||
// for (int i = 0; i < 4; i++) {
|
||||
// c->output.joint[i] = LowPassFilter2p_Apply(&c->filter.joint_out[i], c->output.joint[i]);
|
||||
// }
|
||||
|
||||
/* 发送关节控制指令 */
|
||||
MOTOR_LZ_MotionParam_t motion_param = {
|
||||
@ -375,9 +417,9 @@ void Chassis_Output(Chassis_t *c) {
|
||||
}
|
||||
|
||||
/* 轮子输出滤波并发送 */
|
||||
for (int i = 0; i < 2; i++) {
|
||||
c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
|
||||
}
|
||||
// for (int i = 0; i < 2; 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[1], c->output.wheel[1]);
|
||||
}
|
||||
|
||||
@ -151,8 +151,9 @@ typedef struct {
|
||||
|
||||
/* 滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
|
||||
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
|
||||
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
|
||||
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
|
||||
LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */
|
||||
} filter;
|
||||
|
||||
} Chassis_t;
|
||||
|
||||
@ -189,7 +189,7 @@ Config_RobotParam_t robot_config = {
|
||||
.d_cutoff_freq=30.0f,
|
||||
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
|
||||
},
|
||||
|
||||
|
||||
.roll={
|
||||
.k=0.2f,
|
||||
.p=0.5f, /* 横滚角比例系数 */
|
||||
@ -317,18 +317,18 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
|
||||
.lqr_gains = {
|
||||
.k11_coeff = { -2.702074813939778e+02f, 2.894104138810802e+02f, -1.211372850409846e+02f, -6.933300731936785e-01f }, // theta
|
||||
.k12_coeff = { -3.327095836963479e+00f, 5.164647957579151e+00f, -7.601554284013210e+00f, 2.552123800155830e-01f }, // d_theta
|
||||
.k13_coeff = { -4.196624710163955e+01f, 4.127695960453792e+01f, -1.402278033829385e+01f, 7.645039434796388e-03f }, // x
|
||||
.k14_coeff = { -5.313365798300281e+01f, 5.256471525738568e+01f, -1.848736219345810e+01f, -1.124172698659643e-03f }, // d_x
|
||||
.k15_coeff = { -6.095849580858518e+01f, 7.441367039791133e+01f, -3.453552694561191e+01f, 7.000025215843615e+00f }, // phi
|
||||
.k16_coeff = { -9.662388366457952e+00f, 1.288897516182337e+01f, -6.650550674687217e+00f, 1.558651627757777e+00f }, // d_phi
|
||||
.k21_coeff = { 1.194019591863143e+02f, -7.294395555463191e+01f, -5.502401903255857e+00f, 1.056061060815681e+01f }, // theta
|
||||
.k22_coeff = { 1.953914704492318e+01f, -1.853477522511211e+01f, 5.522863854227361e+00f, 2.440732134290017e-01f }, // d_theta
|
||||
.k23_coeff = { -3.113451478996141e+01f, 3.917196236114254e+01f, -1.888229690044390e+01f, 4.105978636011114e+00f }, // x
|
||||
.k24_coeff = { -3.864995803122945e+01f, 4.873841855321316e+01f, -2.361615127826612e+01f, 5.240190452941591e+00f }, // d_x
|
||||
.k25_coeff = { 2.402815287983674e+02f, -2.353554477048087e+02f, 7.938206986720357e+01f, 7.179992245652441e-01f }, // phi
|
||||
.k26_coeff = { 5.110194134606057e+01f, -5.057473149871059e+01f, 1.733200146622070e+01f, -2.070892022734363e-01f }, // d_phi
|
||||
.k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
|
||||
.k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f }, // d_theta
|
||||
.k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f }, // x
|
||||
.k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f }, // d_x
|
||||
.k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
|
||||
.k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f }, // d_phi
|
||||
.k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
|
||||
.k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f }, // d_theta
|
||||
.k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f }, // x
|
||||
.k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f }, // d_x
|
||||
.k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
|
||||
.k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f }, // d_phi
|
||||
},
|
||||
.theta = 0.0f,
|
||||
.x = 0.0f,
|
||||
|
||||
@ -17,9 +17,9 @@ function K = get_k_length(leg_length)
|
||||
R1=0.068; %驱动轮半径
|
||||
L1=leg_length/2; %摆杆重心到驱动轮轴距离
|
||||
LM1=leg_length/2; %摆杆重心到其转轴距离
|
||||
l1=-0.01; %机体质心距离转轴距离
|
||||
l1=0.01; %机体质心距离转轴距离
|
||||
mw1=0.60; %驱动轮质量
|
||||
mp1=1.5; %杆质量
|
||||
mp1=1.8; %杆质量
|
||||
M1=12.0; %机体质量
|
||||
Iw1=mw1*R1^2; %驱动轮转动惯量
|
||||
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=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
|
||||
R=[200 0;0 60]; %T Tp
|
||||
Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||
R=[250 0;0 50]; %T Tp
|
||||
|
||||
K=lqr(A,B,Q,R);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user