1101 lines
39 KiB
C
1101 lines
39 KiB
C
/**
|
||
* @file balance_chassis.c
|
||
* @brief 平衡底盘控制模块
|
||
*/
|
||
|
||
#include "module/balance_chassis.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include "component/filter.h"
|
||
#include "component/kalman_filter.h"
|
||
#include "component/user_math.h"
|
||
#include "device/motor_lk.h"
|
||
#include "device/motor_lz.h"
|
||
#include <math.h>
|
||
#include <stddef.h>
|
||
#include <string.h>
|
||
|
||
/* Private defines ---------------------------------------------------------- */
|
||
#define LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||
|
||
/* 物理常量（不可调） */
|
||
#define WHEEL_RADIUS 0.068f /* 轮子半径 (m)，固定机械尺寸 */
|
||
|
||
|
||
// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
static int8_t Chassis_MotorEnable(Chassis_t *c);
|
||
static int8_t Chassis_MotorRelax(Chassis_t *c);
|
||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode);
|
||
static void Chassis_UpdateChassisState(Chassis_t *c);
|
||
static void Chassis_ResetControllers(Chassis_t *c);
|
||
static void Chassis_SelectYawZeroPoint(Chassis_t *c);
|
||
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
|
||
static void Chassis_RecoverControl(Chassis_t *c);
|
||
|
||
/* Private functions -------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 使能所有电机
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Enable(&c->param->joint_param[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_MotorOn(&c->param->wheel_param[i]);
|
||
}
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 放松所有电机
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_MotorRelax(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Relax(&c->param->joint_param[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_Relax(&c->param->wheel_param[i]);
|
||
}
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 重置所有控制器
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
static void Chassis_ResetControllers(Chassis_t *c) {
|
||
/* 重置PID控制器 */
|
||
PID_Reset(&c->pid.leg_length[0]);
|
||
PID_Reset(&c->pid.leg_length[1]);
|
||
PID_Reset(&c->pid.yaw);
|
||
PID_Reset(&c->pid.roll);
|
||
PID_Reset(&c->pid.roll_force);
|
||
PID_Reset(&c->pid.tp[0]);
|
||
PID_Reset(&c->pid.tp[1]);
|
||
|
||
/* 重置LQR控制器 */
|
||
LQR_Reset(&c->lqr[0]);
|
||
LQR_Reset(&c->lqr[1]);
|
||
|
||
/* 重置滤波器 */
|
||
for (int i = 0; i < 4; i++) {
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[i], 0.0f);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
|
||
}
|
||
|
||
/* 重置卡尔曼滤波器 */
|
||
KF_Reset(&c->kf_state_est);
|
||
|
||
/* 清空机体状态 */
|
||
c->chassis_state.position_x = 0.0f;
|
||
c->chassis_state.velocity_x = 0.0f;
|
||
c->chassis_state.last_velocity_x = 0.0f;
|
||
c->chassis_state.target_x = 0.0f;
|
||
c->chassis_state.target_velocity_x = 0.0f;
|
||
c->chassis_state.last_target_velocity_x = 0.0f;
|
||
}
|
||
|
||
/**
|
||
* @brief 选择yaw轴零点（双零点自动选择）
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
|
||
float current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||
float zero_point_1 = c->param->mech.mech_zero_yaw;
|
||
float zero_point_2 = zero_point_1 + M_PI;
|
||
|
||
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
|
||
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
|
||
|
||
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
|
||
c->yaw_control.target_yaw = zero_point_1;
|
||
c->yaw_control.is_reversed = false;
|
||
} else {
|
||
c->yaw_control.target_yaw = zero_point_2;
|
||
c->yaw_control.is_reversed = true;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 设置底盘模式
|
||
* @param c 底盘结构体指针
|
||
* @param mode 目标模式
|
||
* @return 操作结果
|
||
*/
|
||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
if (mode == c->mode) return CHASSIS_OK;
|
||
|
||
/* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */
|
||
if (c->mode == CHASSIS_MODE_RECOVER &&
|
||
mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
Chassis_MotorEnable(c);
|
||
Chassis_ResetControllers(c);
|
||
Chassis_SelectYawZeroPoint(c);
|
||
|
||
/* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */
|
||
if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) {
|
||
float pitch = c->feedback.imu.euler.pit;
|
||
/* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */
|
||
if (fabsf(pitch) > 0.2f) {
|
||
/* 根据 theta 判断腿朝上还是朝下 */
|
||
float theta_l = c->vmc_[0].leg.theta;
|
||
float theta_r = c->vmc_[1].leg.theta;
|
||
c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
|
||
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
|
||
c->mode = CHASSIS_MODE_RECOVER;
|
||
return CHASSIS_OK;
|
||
}
|
||
/* 否则直接进入平衡模式 */
|
||
}
|
||
|
||
/* 直接进入 RECOVER 时也重置状态机 */
|
||
if (mode == CHASSIS_MODE_RECOVER) {
|
||
float theta_l = c->vmc_[0].leg.theta;
|
||
float theta_r = c->vmc_[1].leg.theta;
|
||
c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
|
||
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
|
||
}
|
||
|
||
c->mode = mode;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新机体状态估计
|
||
* @param c 底盘结构体指针
|
||
* @note 基于轮腿机器人运动学的精确速度估计算法
|
||
* 综合考虑轮子速度、关节运动和机体姿态的影响
|
||
*/
|
||
static void Chassis_UpdateChassisState(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
/* 获取轮子原始速度数据 (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_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 */
|
||
|
||
/*
|
||
* 轮子角速度补偿算法
|
||
* w = w_wheel - pitch_gyro + joint_speed
|
||
*/
|
||
float left_w_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
|
||
float right_w_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;
|
||
|
||
/*
|
||
* 机体速度计算 (参考港大开源)
|
||
* v = w*R + L0*dtheta*cos(theta) + dL0*sin(theta)
|
||
*/
|
||
float left_v_body = left_w_compensated * WHEEL_RADIUS +
|
||
c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta * cosf(c->vmc_[0].leg.theta) +
|
||
c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);
|
||
|
||
float right_v_body = right_w_compensated * WHEEL_RADIUS +
|
||
c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta * cosf(c->vmc_[1].leg.theta) +
|
||
c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);
|
||
|
||
/* 保存历史速度 */
|
||
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
|
||
|
||
/* 计算平均轮子速度 */
|
||
float vel_measured = (left_v_body + right_v_body) / 2.0f;
|
||
|
||
/*
|
||
* 获取IMU前向加速度（世界坐标系x方向）
|
||
* 使用旋转矩阵将机体加速度(已去重力)转到世界系
|
||
* axE = cos(pitch)*ax_body + sin(roll)*sin(pitch)*ay_body + cos(roll)*sin(pitch)*az_body
|
||
*/
|
||
float pitch = c->feedback.imu.euler.pit;
|
||
float roll = c->feedback.imu.euler.rol;
|
||
float ax_body = c->feedback.imu.accl.x;
|
||
float ay_body = c->feedback.imu.accl.y;
|
||
float az_body = c->feedback.imu.accl.z;
|
||
|
||
/* 去除重力分量（机体系下） */
|
||
float gravity_bx = -9.81f * sinf(pitch);
|
||
float gravity_by = 9.81f * sinf(roll) * cosf(pitch);
|
||
float gravity_bz = 9.81f * cosf(roll) * cosf(pitch);
|
||
float motion_ax = ax_body - gravity_bx;
|
||
float motion_ay = ay_body - gravity_by;
|
||
float motion_az = az_body - gravity_bz;
|
||
|
||
/* 旋转到世界坐标系x方向 */
|
||
float accel_world_x = cosf(pitch) * motion_ax +
|
||
sinf(roll) * sinf(pitch) * motion_ay +
|
||
cosf(roll) * sinf(pitch) * motion_az;
|
||
|
||
/* ==================== 卡尔曼滤波器融合 ==================== */
|
||
/* 动态更新状态转移矩阵 F（根据实际dt）*/
|
||
float dt = c->dt;
|
||
if (dt <= 0.0f || dt > 0.1f) dt = 0.001f;
|
||
|
||
/* F = [1, dt; 0, 1] */
|
||
c->kf_state_est.F_data[0] = 1.0f;
|
||
c->kf_state_est.F_data[1] = dt;
|
||
c->kf_state_est.F_data[2] = 0.0f;
|
||
c->kf_state_est.F_data[3] = 1.0f;
|
||
|
||
/* 量测向量: z = [v_wheel, a_imu]
|
||
* 注意：必须写入 measured_vector，KF_Measure 会将其拷贝到 z_data
|
||
* 直接写 z_data 会被 KF_Measure 用 measured_vector(全零) 覆盖！ */
|
||
c->kf_state_est.measured_vector[0] = vel_measured;
|
||
c->kf_state_est.measured_vector[1] = accel_world_x;
|
||
|
||
/* 动态调整R矩阵：离地时降低轮速信任度 */
|
||
if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
|
||
c->kf_state_est.R_data[0] = 100000.0f; /* 离地：不信任轮速 */
|
||
} else {
|
||
c->kf_state_est.R_data[0] = 100.0f; /* 接地：信任轮速 */
|
||
}
|
||
/* R[1][1] = 1000000.0f 保持不变，始终不信任加速度量测 */
|
||
|
||
/* 执行卡尔曼滤波 */
|
||
float *kf_result = KF_Update(&c->kf_state_est);
|
||
|
||
/* KF输出: [velocity, acceleration] */
|
||
if (kf_result != NULL) {
|
||
c->chassis_state.velocity_x = kf_result[0];
|
||
/* 位移 = 滤波后的速度积分（不让加速度误差被二次积分放大） */
|
||
c->chassis_state.position_x += c->chassis_state.velocity_x * dt;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 计算五杆机构弹簧等效支撑力
|
||
* @param leg_length 腿长 L_AE (m)，有效范围 [0.0391, 0.4689]
|
||
* @return 等效支撑力 (N)，无解时返回 NAN
|
||
*/
|
||
static float Chassis_CalcSpringForce(float leg_length)
|
||
{
|
||
// 五杆机构几何参数 (单位: m)
|
||
const float L_AB = 0.215f; // 基座长度
|
||
const float L_AC = 0.042f; // 弹簧上端高度
|
||
const float L_BD = 0.050f; // 杆BD长度
|
||
const float L_BE = 0.254f; // 杆BE长度
|
||
const float Fs = 360.0f; // 弹簧恒力 (N)
|
||
|
||
// 通过余弦定理计算∠ABE的余弦值
|
||
float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);
|
||
|
||
// 检查解的有效性
|
||
if (fabsf(cos_theta) > 1.0f) {
|
||
return NAN; // 无物理解
|
||
}
|
||
|
||
float theta = acosf(cos_theta);
|
||
float sin_theta = sinf(theta);
|
||
|
||
// 计算等效支撑力
|
||
// F_eq = (Fs × |力臂| × L_AB × sin(θ)) / (L_CD × L_BE × L_AE)
|
||
return (Fs * fabsf(L_BD*cos_theta*L_AC - L_AB*L_BD*sin_theta) * L_AB*sin_theta)
|
||
/ (sqrtf((L_AB - L_BD*cos_theta)*(L_AB - L_BD*cos_theta)
|
||
+ (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
|
||
}
|
||
|
||
/**
|
||
* @brief 跳跃控制
|
||
* @param c 底盘结构体指针
|
||
* @param c_cmd 控制指令
|
||
* @param target_L0 输出的目标腿长数组[2]
|
||
* @param extra_force 输出的额外力数组[2]
|
||
*
|
||
* @note 跳跃流程：
|
||
* 触发后直接进入 LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE
|
||
*/
|
||
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
|
||
uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
|
||
|
||
/* 初始化额外力为0 */
|
||
extra_force[0] = 0.0f;
|
||
extra_force[1] = 0.0f;
|
||
|
||
/* ==================== 跳跃触发检测 ==================== */
|
||
/* 触发后直接进入LAUNCH */
|
||
if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
|
||
c->jump.state = JUMP_LAUNCH; /* 直接进入起跳阶段 */
|
||
c->jump.state_start_time = current_time;
|
||
c->jump.trigger = false;
|
||
}
|
||
|
||
/* elapsed_time 必须在触发逻辑之后计算 */
|
||
uint32_t elapsed_time = current_time - c->jump.state_start_time;
|
||
|
||
/* ==================== 跳跃状态机 ==================== */
|
||
switch (c->jump.state) {
|
||
case JUMP_IDLE:
|
||
/* 待机状态，预收腿逻辑已在上面处理 */
|
||
break;
|
||
|
||
case JUMP_CROUCH:
|
||
/* CROUCH状态已废弃，直接跳到LAUNCH */
|
||
c->jump.state = JUMP_LAUNCH;
|
||
c->jump.state_start_time = current_time;
|
||
break;
|
||
|
||
case JUMP_LAUNCH: {
|
||
/* 起跳阶段：腿伸长 + 大推力 */
|
||
target_L0[0] = c->param->leg.max_length;
|
||
target_L0[1] = c->param->leg.max_length;
|
||
|
||
/* 参考Chassis项目：直接给大推力，不依赖PID增益 */
|
||
/* 腿长误差约0.17m，Kp=70000时，力=70000*0.17=11900N */
|
||
/* 考虑到机器人重量和跳跃高度，给5000-8000N的推力 */
|
||
extra_force[0] = 6000.0f; /* 直接给推力(N) */
|
||
extra_force[1] = 6000.0f;
|
||
|
||
/* 位置驱动：腿伸到位就收腿（差值<3cm） */
|
||
bool leg_ext = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
|
||
(c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
|
||
if (leg_ext) {
|
||
c->jump.state = JUMP_RETRACT;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
/* 超时保护 */
|
||
if (elapsed_time >= c->param->jump.launch_time_ms) {
|
||
c->jump.state = JUMP_RETRACT;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
} break;
|
||
|
||
case JUMP_RETRACT: {
|
||
/* 收腿阶段 */
|
||
target_L0[0] = c->param->jump.retract_leg_length;
|
||
target_L0[1] = c->param->jump.retract_leg_length;
|
||
extra_force[0] = c->param->jump.retract_force;
|
||
extra_force[1] = c->param->jump.retract_force;
|
||
|
||
/* 位置驱动：腿缩到位就进落地缓冲 */
|
||
float rt = c->param->jump.retract_leg_length;
|
||
bool leg_retracted = (c->vmc_[0].leg.L0 < rt + 0.02f) &&
|
||
(c->vmc_[1].leg.L0 < rt + 0.02f);
|
||
if (leg_retracted) {
|
||
c->jump.state = JUMP_LAND;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
/* 超时保护 */
|
||
if (elapsed_time >= c->param->jump.retract_time_ms) {
|
||
c->jump.state = JUMP_LAND;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
} break;
|
||
|
||
case JUMP_LAND:
|
||
/* 落地缓冲：时间到就回 IDLE */
|
||
if (elapsed_time >= c->param->jump.land_time_ms) {
|
||
c->jump.state = JUMP_IDLE;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
c->jump.state = JUMP_IDLE;
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 倒地自起控制
|
||
* @note 自起流程（左右腿独立状态机）：
|
||
* STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成)
|
||
* 两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式
|
||
*
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
static void Chassis_RecoverControl(Chassis_t *c) {
|
||
float theta_l = c->vmc_[0].leg.theta;
|
||
float theta_r = c->vmc_[1].leg.theta;
|
||
|
||
/* ===== 左腿状态机 ===== */
|
||
switch (c->recover.state[0]) {
|
||
case RECOVER_FLIP:
|
||
/* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */
|
||
/* 等待 theta 转到腿朝下（|theta| < π/2） */
|
||
if (fabsf(theta_l) < (float)M_PI_2) {
|
||
c->recover.state[0] = RECOVER_STRETCH;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_STRETCH:
|
||
/* 伸腿：等待腿伸到位 */
|
||
if (c->vmc_[0].leg.L0 > 0.28f) {
|
||
c->recover.state[0] = RECOVER_BACKLEG;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_BACKLEG:
|
||
/* 后甩腿：等待 phi0 趋近目标角度 */
|
||
if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) {
|
||
c->recover.state[0] = RECOVER_COMPLETE;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_COMPLETE:
|
||
break;
|
||
}
|
||
|
||
/* ===== 右腿状态机 ===== */
|
||
switch (c->recover.state[1]) {
|
||
case RECOVER_FLIP:
|
||
if (fabsf(theta_r) < (float)M_PI_2) {
|
||
c->recover.state[1] = RECOVER_STRETCH;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_STRETCH:
|
||
if (c->vmc_[1].leg.L0 > 0.28f) {
|
||
c->recover.state[1] = RECOVER_BACKLEG;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_BACKLEG:
|
||
if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) {
|
||
c->recover.state[1] = RECOVER_COMPLETE;
|
||
}
|
||
break;
|
||
|
||
case RECOVER_COMPLETE:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化底盘
|
||
* @param c 底盘结构体指针
|
||
* @param param 底盘参数指针
|
||
* @param target_freq 目标控制频率
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
|
||
if (c == NULL || param == NULL || target_freq <= 0.0f) {
|
||
return CHASSIS_ERR;
|
||
}
|
||
|
||
c->param = param;
|
||
c->mode = CHASSIS_MODE_RELAX;
|
||
|
||
/* 初始化CAN */
|
||
BSP_CAN_Init();
|
||
|
||
/* 初始化和注册电机 */
|
||
MOTOR_LZ_Init();
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < 2; i++) {
|
||
if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
MOTOR_DM_Register(&c->param->yaw_motor);
|
||
|
||
/* 初始化VMC控制器 */
|
||
VMC_Init(&c->vmc_[0], ¶m->vmc_param[0], target_freq);
|
||
VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq);
|
||
|
||
/* 初始化PID控制器 */
|
||
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw);
|
||
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll);
|
||
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, ¶m->roll_force);
|
||
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||
|
||
/* 初始化LQR控制器 */
|
||
LQR_Init(&c->lqr[0], ¶m->lqr_gains);
|
||
LQR_Init(&c->lqr[1], ¶m->lqr_gains);
|
||
|
||
/* 初始化滤波器 */
|
||
for (int i = 0; i < 4; i++) {
|
||
LowPassFilter2p_Init(&c->filter.joint_out[i], target_freq, param->low_pass_cutoff_freq.out);
|
||
}
|
||
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 */
|
||
|
||
/*
|
||
* 初始化卡尔曼滤波器（参考港大开源方案）
|
||
* 状态向量 x = [v, a] (2维：速度和加速度)
|
||
* 量测向量 z = [v, a] (2维：轮速和IMU加速度)
|
||
* 位移在KF外部用滤波后的速度积分，避免加速度误差被二次积分放大
|
||
*/
|
||
KF_Init(&c->kf_state_est, 2, 0, 2);
|
||
|
||
float dt_init = 1.0f / target_freq;
|
||
|
||
/* 初始协方差矩阵 P (2x2) */
|
||
static float P_Init[4] = {
|
||
1.0f, 0.0f,
|
||
0.0f, 1.0f,
|
||
};
|
||
memcpy(c->kf_state_est.P_data, P_Init, sizeof(P_Init));
|
||
|
||
/* 状态转移矩阵 F (2x2): v(k) = v(k-1) + a*dt, a(k) = a(k-1) */
|
||
float F_Init[4] = {
|
||
1.0f, dt_init,
|
||
0.0f, 1.0f,
|
||
};
|
||
memcpy(c->kf_state_est.F_data, F_Init, sizeof(F_Init));
|
||
|
||
/* 过程噪声协方差矩阵 Q (2x2) */
|
||
static float Q_Init[4] = {
|
||
0.1f, 0.0f,
|
||
0.0f, 0.1f,
|
||
};
|
||
memcpy(c->kf_state_est.Q_data, Q_Init, sizeof(Q_Init));
|
||
|
||
/* 状态最小方差（防止过度收敛） */
|
||
static float state_min_var[2] = {0.005f, 0.1f};
|
||
memcpy(c->kf_state_est.state_min_variance, state_min_var, sizeof(state_min_var));
|
||
|
||
/* 关闭自动量测调整（手动管理H/R） */
|
||
c->kf_state_est.use_auto_adjustment = 0;
|
||
|
||
/* 量测矩阵 H (2x2): z = H*x, 直接观测速度和加速度 */
|
||
static float H_Init[4] = {
|
||
1.0f, 0.0f,
|
||
0.0f, 1.0f,
|
||
};
|
||
memcpy(c->kf_state_est.H_data, H_Init, sizeof(H_Init));
|
||
|
||
/*
|
||
* 量测噪声协方差矩阵 R (2x2)
|
||
* 关键：加速度噪声设极大(1000000)，几乎不信任加速度量测
|
||
* KF主要靠轮速量测修正，加速度仅在预测环节(F矩阵)起辅助作用
|
||
*/
|
||
static float R_Init[4] = {
|
||
100.0f, 0.0f,
|
||
0.0f, 1000000.0f,
|
||
};
|
||
memcpy(c->kf_state_est.R_data, R_Init, sizeof(R_Init));
|
||
|
||
Chassis_MotorEnable(c);
|
||
|
||
/* 初始化状态变量 */
|
||
memset(&c->chassis_state, 0, sizeof(c->chassis_state));
|
||
memset(&c->yaw_control, 0, sizeof(c->yaw_control));
|
||
|
||
/* 初始化跳跃状态 */
|
||
c->jump.state = JUMP_IDLE;
|
||
c->jump.trigger = false;
|
||
c->jump.state_start_time = 0;
|
||
c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
|
||
c->jump.launch_force = c->param->jump.launch_force;
|
||
c->jump.retract_leg_length = c->param->jump.retract_leg_length;
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新底盘反馈数据
|
||
* @param c 底盘结构体指针
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* 更新所有电机数据 */
|
||
MOTOR_LZ_UpdateAll();
|
||
MOTOR_LK_UpdateAll();
|
||
|
||
/* 更新关节电机反馈 */
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
|
||
if (joint_motor != NULL) {
|
||
c->feedback.joint[i] = joint_motor->motor.feedback;
|
||
}
|
||
|
||
/* 机械零点调整 */
|
||
if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
|
||
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
|
||
}
|
||
c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
|
||
|
||
/* 应用零点偏移 */
|
||
c->feedback.joint[i].rotor_abs_angle -= c->param->mech.joint_zero[i];
|
||
}
|
||
|
||
/* 更新轮子电机反馈 */
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
|
||
if (wheel_motor != NULL) {
|
||
c->feedback.wheel[i] = wheel_motor->motor.feedback;
|
||
}
|
||
}
|
||
|
||
/* 更新机体状态估计 */
|
||
Chassis_UpdateChassisState(c);
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新IMU数据
|
||
* @param c 底盘结构体指针
|
||
* @param imu IMU数据
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->feedback.imu = imu;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 底盘控制主函数
|
||
* @param c 底盘结构体指针
|
||
* @param c_cmd 控制指令
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* 计算时间间隔 */
|
||
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
|
||
c->lask_wakeup = BSP_TIME_Get_us();
|
||
|
||
/* 设置底盘模式 */
|
||
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
/* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */
|
||
if (c_cmd->jump_trigger &&
|
||
(c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) &&
|
||
c->jump.state == JUMP_IDLE) {
|
||
c->jump.trigger = true;
|
||
}
|
||
|
||
/* VMC正解算 */
|
||
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);
|
||
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);
|
||
|
||
/* 修正离地检测：加上弹簧提供的支撑力 */
|
||
float spring_Fn_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||
float spring_Fn_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||
if (!isnan(spring_Fn_left)) {
|
||
c->vmc_[0].leg.Fn += spring_Fn_left;
|
||
c->vmc_[0].leg.is_ground_contact = (c->vmc_[0].leg.Fn > 20.0f);
|
||
}
|
||
if (!isnan(spring_Fn_right)) {
|
||
c->vmc_[1].leg.Fn += spring_Fn_right;
|
||
c->vmc_[1].leg.is_ground_contact = (c->vmc_[1].leg.Fn > 20.0f);
|
||
}
|
||
|
||
/* 根据模式执行控制 */
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX:
|
||
Chassis_MotorRelax(c);
|
||
Chassis_LQRControl(c, c_cmd);
|
||
break;
|
||
|
||
case CHASSIS_MODE_RECOVER: {
|
||
/* 运行自起状态机 */
|
||
Chassis_RecoverControl(c);
|
||
|
||
/* 根据状态决定输出力 */
|
||
float fn_left, fn_right;
|
||
float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||
float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||
if (isnan(spring_left)) spring_left = 0.0f;
|
||
if (isnan(spring_right)) spring_right = 0.0f;
|
||
|
||
/* 左腿 */
|
||
if (c->recover.state[0] == RECOVER_FLIP) {
|
||
fn_left = 30.0f - spring_left; /* 收腿，借重力慢慢翻转 */
|
||
} else if (c->recover.state[0] == RECOVER_STRETCH) {
|
||
fn_left = -80.0f - spring_left; /* 大力伸腿 */
|
||
} else if (c->recover.state[0] == RECOVER_BACKLEG) {
|
||
fn_left = -40.0f - spring_left; /* 维持伸腿 */
|
||
} else {
|
||
fn_left = -20.0f - spring_left;
|
||
}
|
||
|
||
/* 右腿 */
|
||
if (c->recover.state[1] == RECOVER_FLIP) {
|
||
fn_right = 30.0f - spring_right;
|
||
} else if (c->recover.state[1] == RECOVER_STRETCH) {
|
||
fn_right = -80.0f - spring_right;
|
||
} else if (c->recover.state[1] == RECOVER_BACKLEG) {
|
||
fn_right = -40.0f - spring_right;
|
||
} else {
|
||
fn_right = -20.0f - spring_right;
|
||
}
|
||
|
||
VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f);
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f);
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
c->output.wheel[0] = 0.0f;
|
||
c->output.wheel[1] = 0.0f;
|
||
Chassis_Output(c);
|
||
|
||
/* 两腿均完成，自动切换到平衡模式 */
|
||
if (c->recover.state[0] == RECOVER_COMPLETE &&
|
||
c->recover.state[1] == RECOVER_COMPLETE) {
|
||
Chassis_ResetControllers(c);
|
||
Chassis_SelectYawZeroPoint(c);
|
||
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||
}
|
||
} break;
|
||
|
||
// case CHASSIS_MODE_CALIBRATE: {
|
||
// Chassis_LQRControl(c, c_cmd);
|
||
// LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||
// RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||
// L_fn = -LF;
|
||
// VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
|
||
// VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
// VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
|
||
// VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
|
||
// c->output.wheel[0] = 0.0f;
|
||
// c->output.wheel[1] = 0.0f;
|
||
// Chassis_Output(c);
|
||
// } break;
|
||
|
||
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
||
Chassis_LQRControl(c, c_cmd);
|
||
Chassis_Output(c);
|
||
break;
|
||
case CHASSIS_MODE_BALANCE_ROTOR:
|
||
Chassis_LQRControl(c, c_cmd);
|
||
c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
|
||
c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
|
||
Chassis_Output(c);
|
||
break;
|
||
|
||
default:
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 底盘输出
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
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]);
|
||
// }
|
||
|
||
/* 发送关节控制指令 */
|
||
MOTOR_LZ_MotionParam_t motion_param = {
|
||
.torque = 0.0f,
|
||
.target_angle = 0.0f,
|
||
.target_velocity = 0.0f,
|
||
.kp = 0.0f,
|
||
.kd = 0.0f,
|
||
};
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
motion_param.torque = c->output.joint[i];
|
||
MOTOR_LZ_MotionControl(&c->param->joint_param[i], &motion_param);
|
||
}
|
||
|
||
/* 轮子输出滤波并发送 */
|
||
// 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]);
|
||
}
|
||
|
||
/**
|
||
* @brief LQR控制
|
||
* @param c 底盘结构体指针
|
||
* @param c_cmd 控制指令
|
||
* @return 操作结果
|
||
*/
|
||
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* ==================== 速度控制 ==================== */
|
||
// float raw_vx = c_cmd->move_vec.vx * 2.0f;
|
||
// float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
|
||
|
||
// /* 加速度限制 */
|
||
// float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
|
||
// float max_velocity_change = c->param->motion.max_acceleration * c->dt;
|
||
// velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
|
||
|
||
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
|
||
// c->chassis_state.target_velocity_x = target_dot_x;
|
||
// c->chassis_state.last_target_velocity_x = target_dot_x;
|
||
// c->chassis_state.target_x += target_dot_x * c->dt;
|
||
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
|
||
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
|
||
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
|
||
|
||
|
||
/* ==================== 状态更新 ==================== */
|
||
LQR_State_t current_state = {
|
||
.theta = c->vmc_[0].leg.theta,
|
||
.d_theta = c->vmc_[0].leg.d_theta,
|
||
.x = c->chassis_state.position_x,
|
||
.d_x = c->chassis_state.velocity_x,
|
||
.phi = c->feedback.imu.euler.pit,
|
||
.d_phi = -c->feedback.imu.gyro.x,
|
||
};
|
||
|
||
LQR_UpdateState(&c->lqr[0], ¤t_state);
|
||
|
||
current_state.theta = c->vmc_[1].leg.theta;
|
||
current_state.d_theta = c->vmc_[1].leg.d_theta;
|
||
LQR_UpdateState(&c->lqr[1], ¤t_state);
|
||
|
||
/* 根据腿长更新增益矩阵 */
|
||
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
|
||
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
|
||
|
||
/* ==================== 目标状态 ==================== */
|
||
LQR_State_t target_state = {
|
||
.theta = 0.0f,
|
||
.d_theta = 0.0f,
|
||
.phi = 0.0f,
|
||
.d_phi = 0.0f,
|
||
.x = c->chassis_state.target_x,
|
||
.d_x = c->chassis_state.target_velocity_x,
|
||
};
|
||
|
||
/* ==================== Yaw轴控制 ==================== */
|
||
if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
|
||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||
|
||
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
|
||
float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
|
||
float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
|
||
|
||
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
|
||
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
|
||
|
||
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
|
||
c->yaw_control.target_yaw = base_target_1;
|
||
c->yaw_control.is_reversed = false;
|
||
} else {
|
||
c->yaw_control.target_yaw = base_target_2;
|
||
c->yaw_control.is_reversed = true;
|
||
}
|
||
|
||
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
|
||
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
|
||
}
|
||
|
||
/* ==================== 左腿LQR控制 ==================== */
|
||
if (c->vmc_[0].leg.is_ground_contact) {
|
||
LQR_SetTargetState(&c->lqr[0], &target_state);
|
||
LQR_Control(&c->lqr[0]);
|
||
} else {
|
||
target_state.theta = c->param->motion.non_contact_theta;
|
||
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 + c->param->motion.non_contact_theta) +
|
||
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
}
|
||
|
||
/* ==================== 右腿LQR控制 ==================== */
|
||
if (c->vmc_[1].leg.is_ground_contact) {
|
||
LQR_SetTargetState(&c->lqr[1], &target_state);
|
||
LQR_Control(&c->lqr[1]);
|
||
} else {
|
||
target_state.theta = c->param->motion.non_contact_theta;
|
||
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 + c->param->motion.non_contact_theta) +
|
||
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
}
|
||
|
||
/* ==================== 轮毂输出 ==================== */
|
||
/* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */
|
||
float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
|
||
float yaw_scale = c->param->leg.base_length / avg_L0; /* 以基础腿长为基准归一化 */
|
||
float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale;
|
||
|
||
c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force;
|
||
c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force;
|
||
|
||
/* ==================== 横滚角补偿 ==================== */
|
||
/* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
|
||
float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */
|
||
float roll_error = c->feedback.imu.euler.rol - 0.0f; /* Roll角误差 */
|
||
float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0; /* 右腿减左腿腿长差 */
|
||
|
||
float A = Delta_L0 * cosf(roll_error) + 2.0f * Rl * sinf(roll_error);
|
||
float B = -Delta_L0 * sinf(roll_error) + 2.0f * Rl * cosf(roll_error);
|
||
|
||
float roll_leg_compensation_left = 0.0f;
|
||
float roll_leg_compensation_right = 0.0f;
|
||
|
||
if (fabsf(B) > 0.001f) { /* 避免除零 */
|
||
float tan_delta = A / B;
|
||
roll_leg_compensation_left = -Rl * tan_delta;
|
||
roll_leg_compensation_right = Rl * tan_delta;
|
||
}
|
||
|
||
/* 限制几何补偿范围 (适当提高限幅增强补偿效果) */
|
||
float max_leg_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
|
||
? 0.12f : 0.0f; /* 提高到0.05/0.025以提供足够补偿 */
|
||
roll_leg_compensation_left = LIMIT(roll_leg_compensation_left, -max_leg_compensation, max_leg_compensation);
|
||
roll_leg_compensation_right = LIMIT(roll_leg_compensation_right, -max_leg_compensation, max_leg_compensation);
|
||
|
||
/* 方法2: PID力补偿 (低增益避免抖动) */
|
||
float roll_force_compensation = PID_Calc(&c->pid.roll_force, 0.0f, c->feedback.imu.euler.rol,
|
||
c->feedback.imu.gyro.x, c->dt);
|
||
|
||
/* ==================== 腿长控制 ==================== */
|
||
float target_L0[2];
|
||
float jump_extra_force[2] = {0.0f, 0.0f};
|
||
|
||
/* 基础腿长目标 (应用几何前馈补偿) */
|
||
/* height 可以为负值用于收腿（预蹲），正值用于伸腿 */
|
||
float base_leg_length = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range;
|
||
target_L0[0] = base_leg_length + roll_leg_compensation_left;
|
||
target_L0[1] = base_leg_length + roll_leg_compensation_right;
|
||
|
||
/* 跳跃控制：可能会修改目标腿长和额外力 */
|
||
Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
|
||
|
||
/* 智能限幅：短腿不够时将补偿转移到长腿 */
|
||
float compensation_transfer = 0.0f;
|
||
|
||
/* 检查哪条腿更短（需要缩短的那条） */
|
||
if (target_L0[0] < target_L0[1]) {
|
||
/* 左腿更短，检查是否触碰下限 */
|
||
if (target_L0[0] < c->param->leg.min_length) {
|
||
compensation_transfer = c->param->leg.min_length - target_L0[0]; /* 计算短缺量 */
|
||
target_L0[0] = c->param->leg.min_length; /* 左腿限制在最小值 */
|
||
target_L0[1] += compensation_transfer; /* 右腿补偿增加 */
|
||
}
|
||
} else if (target_L0[1] < target_L0[0]) {
|
||
/* 右腿更短，检查是否触碰下限 */
|
||
if (target_L0[1] < c->param->leg.min_length) {
|
||
compensation_transfer = c->param->leg.min_length - target_L0[1]; /* 计算短缺量 */
|
||
target_L0[1] = c->param->leg.min_length; /* 右腿限制在最小值 */
|
||
target_L0[0] += compensation_transfer; /* 左腿补偿增加 */
|
||
}
|
||
}
|
||
|
||
/* 最终安全限幅 */
|
||
target_L0[0] = LIMIT(target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
|
||
target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_length);
|
||
|
||
/* 获取腿长变化率 */
|
||
float leg_d_length[2];
|
||
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
|
||
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
|
||
|
||
/* ==================== 左右腿摆角同步补偿 ==================== */
|
||
float Delta_Tp[2];
|
||
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
|
||
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
|
||
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
|
||
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
|
||
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
|
||
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
|
||
|
||
/* ==================== 左腿VMC控制 ==================== */
|
||
float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
||
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
||
float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||
if (isnan(spring_force_left)) spring_force_left = 0.0f;
|
||
|
||
float virtual_force_left, virtual_torque_left;
|
||
if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
|
||
/* 跳跃阶段：完全接管力控 */
|
||
/* 所有跳跃阶段统一处理：PID + 额外力 */
|
||
virtual_force_left = jump_extra_force[0] + pid_output_left;
|
||
virtual_torque_left = 0.0f;
|
||
/* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
|
||
c->output.wheel[0] = 0.0f;
|
||
c->output.wheel[1] = 0.0f;
|
||
} else {
|
||
/* 正常状态：添加Roll力补偿 (低增益避免抖动) */
|
||
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
|
||
pid_output_left + c->param->leg.left_base_force - spring_force_left +
|
||
jump_extra_force[0] + roll_force_compensation;
|
||
virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
|
||
}
|
||
|
||
if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left, virtual_torque_left) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
} else {
|
||
c->output.joint[0] = 0.0f;
|
||
c->output.joint[1] = 0.0f;
|
||
}
|
||
|
||
/* ==================== 右腿VMC控制 ==================== */
|
||
float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
||
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
||
float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||
if (isnan(spring_force_right)) spring_force_right = 0.0f;
|
||
|
||
float virtual_force_right, virtual_torque_right;
|
||
if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
|
||
/* 跳跃阶段：完全接管力控 */
|
||
/* 所有跳跃阶段统一处理：PID + 额外力 */
|
||
virtual_force_right = jump_extra_force[1] + pid_output_right;
|
||
virtual_torque_right = 0.0f;
|
||
} else {
|
||
/* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
|
||
virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
|
||
pid_output_right + c->param->leg.right_base_force - spring_force_right +
|
||
jump_extra_force[1] - roll_force_compensation;
|
||
virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
|
||
}
|
||
|
||
if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right, virtual_torque_right) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
} else {
|
||
c->output.joint[2] = 0.0f;
|
||
c->output.joint[3] = 0.0f;
|
||
}
|
||
|
||
/* ==================== 安全限制 ==================== */
|
||
for (int i = 0; i < 2; i++) {
|
||
c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
|
||
}
|
||
for (int i = 0; i < 4; i++) {
|
||
c->output.joint[i] = LIMIT(c->output.joint[i], -60.0f, 60.0f);
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|