Robofish
5b89b3c4f4
Added kalman_filter.c to the project structure for chassis state estimation. Updated Keil MDK project files (.uvprojx, .uvoptx) to include the new component and modified the debug watch window to monitor chassis variables.
918 lines
32 KiB
C
918 lines
32 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) */
|
||
#define MAX_ACCELERATION 2.0f /* 最大加速度 (m/s²) */
|
||
#define WHEEL_GEAR_RATIO 4.5f /* 轮毂电机扭矩 */
|
||
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
|
||
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
|
||
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
|
||
#define MAX_LEG_LENGTH 0.33f /* 最大腿长 (m) */
|
||
#define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */
|
||
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
|
||
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
|
||
|
||
|
||
// 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);
|
||
|
||
/* 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.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_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;
|
||
|
||
Chassis_MotorEnable(c);
|
||
Chassis_ResetControllers(c);
|
||
Chassis_SelectYawZeroPoint(c);
|
||
|
||
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]
|
||
*/
|
||
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); /* 当前时间 ms */
|
||
uint32_t elapsed_time = current_time - c->jump.state_start_time;
|
||
|
||
/* 初始化额外力为0 */
|
||
extra_force[0] = 0.0f;
|
||
extra_force[1] = 0.0f;
|
||
|
||
/* 检测跳跃触发 */
|
||
if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
|
||
c->jump.state = JUMP_CROUCH;
|
||
c->jump.state_start_time = current_time;
|
||
c->jump.trigger = false; /* 清除触发标志 */
|
||
}
|
||
|
||
/* 跳跃状态机 */
|
||
switch (c->jump.state) {
|
||
case JUMP_IDLE:
|
||
/* 待机状态，使用正常腿长控制 */
|
||
break;
|
||
|
||
case JUMP_CROUCH:
|
||
/* 蓄力阶段：缩短腿长 */
|
||
target_L0[0] = c->param->jump_params.crouch_leg_length;
|
||
target_L0[1] = c->param->jump_params.crouch_leg_length;
|
||
|
||
if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
|
||
c->jump.state = JUMP_LAUNCH;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
break;
|
||
|
||
case JUMP_LAUNCH:
|
||
/* 起跳阶段：发力向下推地面 */
|
||
target_L0[0] = MAX_LEG_LENGTH; /* 腿伸长 */
|
||
target_L0[1] = MAX_LEG_LENGTH;
|
||
extra_force[0] = c->param->jump_params.launch_force; /* 额外向下的力 */
|
||
extra_force[1] = c->param->jump_params.launch_force;
|
||
|
||
if (elapsed_time >= c->param->jump_params.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_params.retract_leg_length;
|
||
target_L0[1] = c->param->jump_params.retract_leg_length;
|
||
extra_force[0] = c->param->jump_params.retract_force; /* 前馈力帮助收腿 */
|
||
extra_force[1] = c->param->jump_params.retract_force;
|
||
|
||
/* 检测落地或超时 */
|
||
if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
|
||
c->jump.state = JUMP_LAND;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
break;
|
||
|
||
case JUMP_LAND:
|
||
/* 落地阶段：缓冲并恢复正常 */
|
||
/* 使用正常腿长，让PID自动恢复 */
|
||
|
||
if (elapsed_time >= c->param->jump_params.land_time_ms) {
|
||
c->jump.state = JUMP_IDLE;
|
||
c->jump.state_start_time = current_time;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
c->jump.state = JUMP_IDLE;
|
||
break;
|
||
}
|
||
}
|
||
|
||
/* Public functions --------------------------------------------------------- */
|
||
|
||
/**
|
||
* @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.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_params.crouch_leg_length;
|
||
c->jump.launch_force = c->param->jump_params.launch_force;
|
||
c->jump.retract_leg_length = c->param->jump_params.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->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;
|
||
}
|
||
|
||
/* 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: {
|
||
float fn = -20.0f, tp = 0.0f;
|
||
Chassis_LQRControl(c, c_cmd);
|
||
|
||
/* 计算弹簧力补偿 */
|
||
float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
|
||
float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
|
||
float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left);
|
||
float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right);
|
||
|
||
VMC_InverseSolve(&c->vmc_[0], fn_left, tp);
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
VMC_InverseSolve(&c->vmc_[1], fn_right, tp);
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
||
|
||
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
|
||
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
|
||
Chassis_Output(c);
|
||
} 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 = 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_zero_yaw + manual_offset;
|
||
float base_target_2 = c->param->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 = 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 + 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 = 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 + NON_CONTACT_THETA) +
|
||
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
}
|
||
|
||
/* ==================== 轮毂输出 ==================== */
|
||
c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
|
||
c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
|
||
|
||
/* ==================== 横滚角补偿 ==================== */
|
||
float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
|
||
c->feedback.imu.gyro.x, c->dt);
|
||
|
||
/* 限制补偿范围 */
|
||
float max_compensation = (c->vmc_[0].leg.is_ground_contact && c->vmc_[1].leg.is_ground_contact)
|
||
? 0.05f : 0.02f;
|
||
roll_compensation = LIMIT(roll_compensation, -max_compensation, max_compensation);
|
||
|
||
/* ==================== 腿长控制 ==================== */
|
||
float target_L0[2];
|
||
float jump_extra_force[2] = {0.0f, 0.0f};
|
||
|
||
/* 基础腿长目标 */
|
||
target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
|
||
target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
|
||
|
||
/* 跳跃控制：可能会修改目标腿长和额外力 */
|
||
Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
|
||
|
||
/* 腿长限幅 */
|
||
target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
|
||
target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_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; /* 处理无效值 */
|
||
|
||
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
|
||
float virtual_force_left, virtual_torque_left;
|
||
if (c->jump.state == JUMP_RETRACT) {
|
||
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
|
||
virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
|
||
virtual_torque_left = 0.0f; /* 收腿时不控制摆角 */
|
||
} else {
|
||
virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
|
||
pid_output_left + LEFT_BASE_FORCE - spring_force_left + jump_extra_force[0];
|
||
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; /* 处理无效值 */
|
||
|
||
/* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
|
||
float virtual_force_right, virtual_torque_right;
|
||
if (c->jump.state == JUMP_RETRACT) {
|
||
/* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
|
||
virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
|
||
virtual_torque_right = 0.0f; /* 收腿时不控制摆角 */
|
||
} else {
|
||
virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
|
||
pid_output_right + RIGHT_BASE_FORCE - spring_force_right + jump_extra_force[1];
|
||
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;
|
||
}
|
||
|
||
/**
|
||
* @brief 触发跳跃
|
||
* @param c 底盘结构体指针
|
||
*/
|
||
void Chassis_TriggerJump(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
/* 只在平衡模式且待机状态下可触发跳跃 */
|
||
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->jump.state == JUMP_IDLE) {
|
||
c->jump.trigger = true;
|
||
}
|
||
}
|