#include "module/balance_chassis.h" #include "bsp/time.h" #include "bsp/can.h" #include "component/user_math.h" #include "component/kinematics.h" #include #include float fn=0.0f; float tp=0.0f; float t1=0.0f; float t2=0.0f; 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; /* 模式未改变直接返回 */ MOTOR_LK_MotorOn(&c->param->wheel_motors[0]); MOTOR_LK_MotorOn(&c->param->wheel_motors[1]); for (int i = 0; i < 4; i++) { MOTOR_LZ_Enable(&c->param->joint_motors[i]); } // 清空pid积分 PID_Reset(&c->pid.leglength[0]); PID_Reset(&c->pid.leglength[1]); c->mode = mode; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 return CHASSIS_OK; } /* 更新机体状态估计 */ static void Chassis_UpdateChassisState(Chassis_t *c) { if (c == NULL) return; // 从轮子编码器估计机体速度 (参考C++代码) float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒) float right_wheel_speed_dps = c->feedback.wheel[1].rotor_speed; // dps (度每秒) // 将dps转换为rad/s float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s float wheel_radius = 0.072f; float left_wheel_linear_vel = left_wheel_speed * wheel_radius; float right_wheel_linear_vel = right_wheel_speed * wheel_radius; // 机体x方向速度 (轮子中心速度) c->chassis_state.last_velocity_x = c->chassis_state.velocity_x; c->chassis_state.velocity_x = (left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f; // 积分得到位置 c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; } int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ if (c == NULL || param == NULL || target_freq <= 0.0f) { return -1; // 参数错误 } c->param = param; /*初始化can*/ BSP_CAN_Init(); /*初始化和注册所有电机*/ MOTOR_LZ_Init(); for (int i = 0; i < 4; i++) { MOTOR_LZ_Register(&c->param->joint_motors[i]); } for (int i = 0; i < 2; i++) { MOTOR_LK_Register(&c->param->wheel_motors[i]); } /*初始化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.leglength[0], KPID_MODE_CALC_D, target_freq, ¶m->leglength); PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, ¶m->leglength); /*初始化LQR控制器*/ LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); LQR_SetGainMatrix(&c->lqr[0], ¶m->lqr_gains); LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); LQR_SetGainMatrix(&c->lqr[1], ¶m->lqr_gains); /*初始化机体状态*/ c->chassis_state.position_x = 0.0f; c->chassis_state.velocity_x = 0.0f; c->chassis_state.last_velocity_x = 0.0f; return CHASSIS_OK; } int8_t Chassis_UpdateFeedback(Chassis_t *c){ if (c == NULL) { return -1; // 参数错误 } /*更新电机反馈*/ for (int i = 0; i < 4; i++) { MOTOR_LZ_Update(&c->param->joint_motors[i]); } MOTOR_LK_Update(&c->param->wheel_motors[0]); MOTOR_LK_Update(&c->param->wheel_motors[1]); /*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/ // 更新关节电机反馈并检查离线状态 for (int i = 0; i < 4; i++) { MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]); if (joint_motor != NULL) { c->feedback.joint[i] = joint_motor->motor.feedback; // c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整 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; // 机械零点调整 } } // 更新轮子电机反馈并检查离线状态 for (int i = 0; i < 2; i++) { MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]); if (wheel_motor != NULL) { c->feedback.wheel[i] = wheel_motor->motor.feedback; } } // 更新机体状态估计 Chassis_UpdateChassisState(c); return 0; } int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){ if (c == NULL) { return -1; // 参数错误 } c->feedback.imu = imu; // c->feedback.imu.euler.pit = - c->feedback.imu.euler.pit; return 0; } 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; // 设置模式失败 } /*根据底盘模式执行不同的控制逻辑*/ switch (c->mode) { case CHASSIS_MODE_RELAX: // 放松模式，电机不输出 MOTOR_LZ_Relax(&c->param->joint_motors[0]); MOTOR_LZ_Relax(&c->param->joint_motors[1]); MOTOR_LZ_Relax(&c->param->joint_motors[2]); MOTOR_LZ_Relax(&c->param->joint_motors[3]); MOTOR_LK_Relax(&c->param->wheel_motors[0]); MOTOR_LK_Relax(&c->param->wheel_motors[1]); // 更新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); // VMC_InverseSolve(&c->vmc_[0], fn, tp); // VMC_GetJointTorques(&c->vmc_[0], &t1, &t2); // MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1}); if (Chassis_LQRControl(c, c_cmd) != 0) { // LQR控制失败，切换到安全模式 return CHASSIS_ERR; } break; case CHASSIS_MODE_RECOVER: // 恢复模式，使用简单的关节位置控制回到初始姿态 // TODO: 实现恢复逻辑 break; case CHASSIS_MODE_WHELL_BALANCE: // 更新VMC正解算用于状态估计 // MOTOR_LZ_Relax(&c->param->joint_motors[0]); // MOTOR_LZ_Relax(&c->param->joint_motors[1]); // MOTOR_LZ_Relax(&c->param->joint_motors[2]); // MOTOR_LZ_Relax(&c->param->joint_motors[3]); // MOTOR_LK_Relax(&c->param->wheel_motors[0]); // MOTOR_LK_Relax(&c->param->wheel_motors[1]); for (int i = 0; i < 4; i++) { c->output.joint[i].torque = 0.0f; } for (int i = 0; i < 2; i++) { c->output.wheel[i] = 0.0f; } // 更新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); // VMC_InverseSolve(&c->vmc_[1], fn, tp); // VMC_GetJointTorques(&c->vmc_[1], &t1, &t2); // c->output.joint[3].torque = t1; // c->output.joint[2].torque = t2; Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新 // c->output.wheel[0] = 0.2f; // c->output.wheel[1] = 0.2f; Chassis_Output(c); // 统一输出 break; case CHASSIS_MODE_WHELL_LEG_BALANCE: // 轮腿平衡模式，使用LQR控制 // 更新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); // 执行LQR控制 // if (Chassis_LQRControl(c, c_cmd) != 0) { // // LQR控制失败，切换到安全模式 // return CHASSIS_ERR; // } // Chassis_Output(c); // 统一输出 break; default: return CHASSIS_ERR_MODE; } return CHASSIS_OK; } void Chassis_Output(Chassis_t *c) { if (c == NULL) return; for (int i = 0; i < 4; i++) { MOTOR_LZ_MotionParam_t param = {0}; param.torque = c->output.joint[i].torque; MOTOR_LZ_MotionControl(&c->param->joint_motors[i], ¶m); } for (int i = 0; i < 2; i++) { MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); } // 这个函数已经在各个模式中直接调用了电机输出函数 // 如果需要统一输出，可以在这里实现 // 现在的设计是在控制逻辑中直接输出，所以这里留空 } int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { if (c == NULL || c_cmd == NULL) { return CHASSIS_ERR_NULL; } /* 参考C++版本实现的LQR控制 */ /* 地面接触检测（参考C++版本） */ float leg_fn[2]; bool onground_flag[2]; // 简化的地面接触检测，可以后续改进 leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]); leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]); onground_flag[0] = (leg_fn[0] > 30.0f); onground_flag[1] = (leg_fn[1] > 30.0f); /* 获取VMC状态（等效摆杆参数） */ float leg_L0[2], leg_theta[2], leg_d_theta[2]; VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]); VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]); /* 运动参数（参考C++版本的状态估计） */ static float xhat = 0.0f, x_dot_hat = 0.0f; static float target_x = 0.0f, target_dot_x = 0.0f; // 简化的速度估计（后续可以改进为C++版本的复杂滤波） x_dot_hat = c->chassis_state.velocity_x; xhat = c->chassis_state.position_x; // 目标设定 target_dot_x = c_cmd->move_vec.vx; target_x += target_dot_x * c->dt; /* 分别计算左右腿的LQR控制 */ float Tw[2] = {0.0f, 0.0f}; // 轮毂力矩 float Tp[2] = {0.0f, 0.0f}; // 髋关节力矩 for (int leg = 0; leg < 2; leg++) { /* 构建当前状态 */ LQR_State_t current_state = {0}; current_state.theta = leg_theta[leg]; current_state.d_theta = leg_d_theta[leg]; current_state.x = xhat; current_state.d_x = x_dot_hat; current_state.phi = -c->feedback.imu.euler.pit; current_state.d_phi = -c->feedback.imu.gyro.y; /* 构建目标状态 */ LQR_State_t target_state = {0}; target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.x = 0; // 目标位置 target_state.d_x = 0.0f; // 目标速度 target_state.phi = 0.0f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 // target_state.d_theta = 0.0f; // 目标摆杆角速度 // target_state.x = target_x - 0.56f; // 目标位置 // target_state.d_x = target_dot_x; // 目标速度 // target_state.phi = 0.16f; // 目标俯仰角 // target_state.d_phi = 0.0f; // 目标俯仰角速度 /* 根据当前腿长更新增益矩阵 */ LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]); /* 更新LQR状态 */ LQR_SetTargetState(&c->lqr[leg], &target_state); LQR_UpdateState(&c->lqr[leg], ¤t_state); if (onground_flag[leg]) { /* 接地状态：使用LQR控制器计算输出 */ if (LQR_Control(&c->lqr[leg]) == 0) { LQR_Input_t lqr_output = {0}; if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) { Tw[leg] = lqr_output.T; Tp[leg] = lqr_output.Tp; } else { Tw[leg] = 0.0f; Tp[leg] = 0.0f; } } else { Tw[leg] = 0.0f; Tp[leg] = 0.0f; } } else { /* 离地状态：简化控制，只控制腿部摆动 */ Tw[leg] = 0.0f; // 只控制摆杆角度 float theta_error = current_state.theta - target_state.theta; float d_theta_error = current_state.d_theta - target_state.d_theta; Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error); } } /* 转向控制（参考C++版本） */ float yaw_force = 0.0f; if (onground_flag[0] && onground_flag[1]) { // 简化的转向控制，后续可以改进为PID yaw_force = -c_cmd->move_vec.wz * 0.1f; } /* 轮毂力矩输出（参考C++版本的减速比） */ c->output.wheel[0] = Tw[0] / 4.9256f - yaw_force; // 左轮 c->output.wheel[1] = Tw[1] / 4.9256f + yaw_force; // 右轮 /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; float leg_d_length[2]; // 腿长变化率 // 目标腿长设定 target_L0[0] = 0.13f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节 target_L0[1] = 0.13f + c_cmd->height * 0.1f; // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); for (int leg = 0; leg < 2; leg++) { // 使用PID进行腿长控制 float pid_output = PID_Calc(&c->pid.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 virtual_force[leg] = Tp[leg] * sinf(leg_theta[leg]) + pid_output + // PID腿长控制输出 50.0f; // 基础支撑力 // VMC逆解算 if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) { if (leg == 0) { VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); } else { VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque); } } else { // VMC失败，设为0力矩 if (leg == 0) { c->output.joint[0].torque = 0.0f; c->output.joint[1].torque = 0.0f; } else { c->output.joint[2].torque = 0.0f; c->output.joint[3].torque = 0.0f; } } } /* 安全限制 */ for (int i = 0; i < 2; i++) { if (fabsf(c->output.wheel[i]) > 0.8f) { c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f; } } for (int i = 0; i < 4; i++) { if (fabsf(c->output.joint[i].torque) > 20.0f) { c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f; } } return CHASSIS_OK; }