655 lines
24 KiB
C
655 lines
24 KiB
C
#include "module/balance_chassis.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include "component/filter.h"
|
||
#include "component/kinematics.h"
|
||
#include "component/limiter.h"
|
||
#include "component/user_math.h"
|
||
#include "device/motor_lz.h"
|
||
#include "device/virtual_chassis.h"
|
||
#include <math.h>
|
||
#include <stddef.h>
|
||
#include <string.h>
|
||
|
||
static Virtual_Chassis_t virtual_chassis;
|
||
|
||
/**
|
||
* @brief 使能所有电机
|
||
* @param c 底盘结构体指针
|
||
* @return
|
||
*/
|
||
static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||
if (c == NULL)
|
||
return CHASSIS_ERR_NULL;
|
||
|
||
Virtual_Chassis_Enable(&virtual_chassis);
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
static int8_t Chassis_MotorRelax(Chassis_t *c) {
|
||
if (c == NULL)
|
||
return CHASSIS_ERR_NULL;
|
||
float relax_torque[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||
Virtual_Chassis_SendJointTorque(&virtual_chassis, relax_torque);
|
||
Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f);
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
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);
|
||
|
||
PID_Reset(&c->pid.leg_length[0]);
|
||
PID_Reset(&c->pid.leg_length[1]);
|
||
PID_Reset(&c->pid.yaw);
|
||
PID_Reset(&c->pid.yaw_omega);
|
||
PID_Reset(&c->pid.roll);
|
||
PID_Reset(&c->pid.tp[0]);
|
||
PID_Reset(&c->pid.tp[1]);
|
||
|
||
// 双零点自动选择逻辑（使用user_math的CircleError函数）
|
||
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; // 第二个零点，相差180度
|
||
|
||
// 使用CircleError计算到两个零点的角度差（范围为M_2PI）
|
||
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; // 使用第二个零点，需要反转前后方向
|
||
}
|
||
|
||
// 清空位移
|
||
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;
|
||
|
||
LQR_Reset(&c->lqr[0]);
|
||
LQR_Reset(&c->lqr[1]);
|
||
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.velocity_est, 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.phi, 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.d_phi, 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.d_theta[0], 0.0f);
|
||
LowPassFilter2p_Reset(&c->filter.d_theta[1], 0.0f);
|
||
|
||
c->mode = mode;
|
||
c->wz_multi=0.0f;
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/* 更新机体状态估计 */
|
||
static void Chassis_UpdateChassisState(Chassis_t *c) {
|
||
if (c == NULL)
|
||
return;
|
||
|
||
/*
|
||
* 基于轮腿机器人运动学的精确速度估计算法
|
||
* 参考C++代码：综合考虑轮子速度、关节运动和机体姿态的影响
|
||
*/
|
||
|
||
// 获取轮子原始速度数据 (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
|
||
|
||
/*
|
||
* 轮子角速度补偿算法
|
||
* 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰
|
||
* 公式来源：五连杆机构运动学分析
|
||
*/
|
||
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方向的分量)
|
||
*/
|
||
const float wheel_radius = 0.072f; // 轮子半径，单位：米
|
||
|
||
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;
|
||
|
||
// 计算平均速度
|
||
float vel_measured = (left_v_body + right_v_body) / 2.0f;
|
||
|
||
/*
|
||
* 接地状态检测与速度修正
|
||
* 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0
|
||
* 阈值20.0N基于实验调试得出
|
||
*/
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
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;
|
||
|
||
c->mode = CHASSIS_MODE_RELAX;
|
||
|
||
/*初始化can*/
|
||
BSP_CAN_Init();
|
||
|
||
/*初始化和注册所有电机*/
|
||
MOTOR_LZ_Init();
|
||
|
||
Virtual_Chassis_Init(&virtual_chassis, &c->param->virtual_chassis_param);
|
||
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.yaw_omega, KPID_MODE_CALC_D, target_freq, ¶m->yaw_omega);
|
||
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);
|
||
|
||
LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq,
|
||
param->low_pass_cutoff_freq.joint);
|
||
LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq,
|
||
param->low_pass_cutoff_freq.joint);
|
||
LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq,
|
||
param->low_pass_cutoff_freq.joint);
|
||
LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq,
|
||
param->low_pass_cutoff_freq.joint);
|
||
LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq,
|
||
param->low_pass_cutoff_freq.wheel);
|
||
LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq,
|
||
param->low_pass_cutoff_freq.wheel);
|
||
|
||
/* 初始化速度估计滤波器 */
|
||
LowPassFilter2p_Init(&c->filter.velocity_est, target_freq,
|
||
param->low_pass_cutoff_freq.velocity_est);
|
||
|
||
/* 初始化俯仰角和俯仰角速度滤波器 */
|
||
LowPassFilter2p_Init(&c->filter.phi, target_freq,
|
||
param->low_pass_cutoff_freq.phi);
|
||
LowPassFilter2p_Init(&c->filter.d_phi, target_freq,
|
||
param->low_pass_cutoff_freq.d_phi);
|
||
|
||
/* 初始化摆杆角速度滤波器 */
|
||
LowPassFilter2p_Init(&c->filter.d_theta[0], target_freq,
|
||
param->low_pass_cutoff_freq.d_theta);
|
||
LowPassFilter2p_Init(&c->filter.d_theta[1], target_freq,
|
||
param->low_pass_cutoff_freq.d_theta);
|
||
|
||
/*初始化机体状态*/
|
||
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;
|
||
|
||
/*初始化yaw控制状态*/
|
||
c->yaw_control.target_yaw = 0.0f;
|
||
c->yaw_control.current_yaw = 0.0f;
|
||
c->yaw_control.yaw_force = 0.0f;
|
||
c->yaw_control.is_reversed = false;
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
|
||
if (c == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
Virtual_Chassis_Update(&virtual_chassis);
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
c->feedback.joint[i] = virtual_chassis.data.joint[i];
|
||
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++) {
|
||
c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
|
||
}
|
||
c->feedback.imu.accl = virtual_chassis.data.imu.accl;
|
||
c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
|
||
c->feedback.imu.euler = virtual_chassis.data.imu.euler;
|
||
|
||
// 更新机体状态估计
|
||
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;
|
||
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; // 设置模式失败
|
||
}
|
||
|
||
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);
|
||
|
||
/*根据底盘模式执行不同的控制逻辑*/
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX:
|
||
// 放松模式，电机不输出
|
||
Chassis_MotorRelax(c);
|
||
|
||
Chassis_LQRControl(c, c_cmd);
|
||
|
||
break;
|
||
|
||
case CHASSIS_MODE_RECOVER: {
|
||
float fn, tp;
|
||
fn = -20.0f;
|
||
tp = 0.0f;
|
||
|
||
Chassis_LQRControl(c, c_cmd);
|
||
|
||
VMC_InverseSolve(&c->vmc_[0], fn, tp);
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
||
VMC_InverseSolve(&c->vmc_[1], fn, 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_WHELL_LEG_BALANCE:
|
||
case CHASSIS_MODE_ROTOR:
|
||
|
||
// 执行LQR控制（包含PID腿长控制）
|
||
Chassis_LQRControl(c, c_cmd);
|
||
Chassis_Output(c); // 统一输出
|
||
break;
|
||
|
||
break;
|
||
default:
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
void Chassis_Output(Chassis_t *c) {
|
||
if (c == NULL)
|
||
return;
|
||
c->output.joint[0] =
|
||
LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]);
|
||
c->output.joint[1] =
|
||
LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]);
|
||
c->output.joint[2] =
|
||
LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
|
||
c->output.joint[3] =
|
||
LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
|
||
float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
|
||
c->output.joint[3]};
|
||
Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
|
||
c->output.wheel[0] =
|
||
LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
|
||
c->output.wheel[1] =
|
||
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
|
||
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
|
||
c->output.wheel[1]);
|
||
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
|
||
// 0.0f); // 暂时不让轮子动
|
||
}
|
||
|
||
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||
if (c == NULL || c_cmd == NULL) {
|
||
return CHASSIS_ERR_NULL;
|
||
}
|
||
|
||
/* 运动参数（参考C++版本的状态估计） */
|
||
static float xhat = 0.0f, x_dot_hat = 0.0f;
|
||
float target_dot_x = 0.0f;
|
||
float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s²
|
||
|
||
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
|
||
x_dot_hat = c->chassis_state.velocity_x;
|
||
xhat = c->chassis_state.position_x;
|
||
|
||
// 目标速度设定（原始期望速度）
|
||
float raw_vx = c_cmd->move_vec.vx * 2;
|
||
|
||
// 根据零点选择情况决定是否反转前后方向
|
||
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; // 最大允许的速度变化
|
||
|
||
// 限制速度变化率（加速度限制）
|
||
if (velocity_change > max_velocity_change) {
|
||
velocity_change = max_velocity_change;
|
||
} else if (velocity_change < -max_velocity_change) {
|
||
velocity_change = -max_velocity_change;
|
||
}
|
||
|
||
// 应用加速度限制后的目标速度
|
||
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_x = c->chassis_state.position_x + c->param->x; // 暂时不让底盘动
|
||
/* 分别计算左右腿的LQR控制 */
|
||
/* 构建当前状态 */
|
||
LQR_State_t current_state = {0};
|
||
current_state.theta = c->vmc_[0].leg.theta;
|
||
current_state.d_theta = LowPassFilter2p_Apply(&c->filter.d_theta[0], c->vmc_[0].leg.d_theta);
|
||
current_state.x = xhat;
|
||
current_state.d_x = x_dot_hat;
|
||
|
||
/* 使用滤波后的俯仰角和俯仰角速度 */
|
||
current_state.phi = LowPassFilter2p_Apply(&c->filter.phi, -c->feedback.imu.euler.pit);
|
||
current_state.d_phi = LowPassFilter2p_Apply(&c->filter.d_phi, -c->feedback.imu.gyro.y);
|
||
|
||
/* 保存滤波后的摆杆角速度，用于后续计算 */
|
||
float filtered_d_theta[2];
|
||
filtered_d_theta[0] = current_state.d_theta; // 左腿已经滤波过的d_theta
|
||
|
||
LQR_UpdateState(&c->lqr[0], ¤t_state);
|
||
|
||
current_state.theta = c->vmc_[1].leg.theta;
|
||
current_state.d_theta = LowPassFilter2p_Apply(&c->filter.d_theta[1], c->vmc_[1].leg.d_theta);
|
||
filtered_d_theta[1] = current_state.d_theta; // 右腿已经滤波过的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 = {0};
|
||
|
||
target_state.theta = 0.0f; // 目标摆杆角度
|
||
target_state.d_theta = 0.0f; // 目标摆杆角速度
|
||
target_state.phi = 0.0f; // 目标俯仰角
|
||
target_state.d_phi = 0.0f; // 目标俯仰角速度
|
||
target_state.x = c->chassis_state.target_x;
|
||
target_state.d_x = target_dot_x; // 目标速度
|
||
|
||
if (c->mode != CHASSIS_MODE_ROTOR){
|
||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||
|
||
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
|
||
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;
|
||
|
||
// 使用CircleError计算到两个目标的角度误差
|
||
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; // 使用第二个零点，需要反转前后方向
|
||
}
|
||
|
||
// 双环PID控制：位置环输出作为速度环的目标
|
||
float target_yaw_velocity = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
|
||
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
|
||
|
||
// 速度环：将位置环输出作为目标角速度，反馈为电机角速度（dps转rad/s）
|
||
c->yaw_control.yaw_force =
|
||
PID_Calc(&c->pid.yaw_omega, target_yaw_velocity,
|
||
c->feedback.yaw.rotor_speed * M_PI / 180.0f, 0.0f, c->dt);
|
||
}else{
|
||
// 小陀螺模式：使用速度环控制
|
||
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
|
||
|
||
// 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
|
||
c->wz_multi += 0.005f;
|
||
if (c->wz_multi > 1.2f)
|
||
c->wz_multi = 1.2f;
|
||
|
||
// 目标角速度（rad/s），可根据需要调整最大速度
|
||
float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
|
||
|
||
// 当前角速度反馈来自IMU陀螺仪（小陀螺用IMU更准确）
|
||
float current_yaw_velocity = c->feedback.imu.gyro.z;
|
||
|
||
// 使用速度环PID控制角速度，输出力矩
|
||
c->yaw_control.yaw_force =
|
||
PID_Calc(&c->pid.yaw_omega, target_yaw_velocity,
|
||
current_yaw_velocity, 0.0f, c->dt);
|
||
}
|
||
|
||
if (c->vmc_[0].leg.is_ground_contact) {
|
||
/* 更新LQR状态 */
|
||
LQR_SetTargetState(&c->lqr[0], &target_state);
|
||
LQR_Control(&c->lqr[0]);
|
||
} else {
|
||
target_state.theta = 0.16f; // 非接触时的腿摆角目标
|
||
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 + 0.16f) +
|
||
c->lqr[0].K_matrix[1][1] * (-filtered_d_theta[0] + 0.0f);
|
||
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
|
||
}
|
||
if (c->vmc_[1].leg.is_ground_contact){
|
||
LQR_SetTargetState(&c->lqr[1], &target_state);
|
||
LQR_Control(&c->lqr[1]);
|
||
}else {
|
||
target_state.theta = 0.16f; // 非接触时的腿摆角目标
|
||
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 + 0.16f) +
|
||
c->lqr[1].K_matrix[1][1] * (-filtered_d_theta[1] + 0.0f);
|
||
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
|
||
}
|
||
|
||
/* 轮毂力矩输出（参考C++版本的减速比） */
|
||
c->output.wheel[0] =
|
||
c->lqr[0].control_output.T / 5.0f + c->yaw_control.yaw_force;
|
||
c->output.wheel[1] =
|
||
c->lqr[1].control_output.T / 5.0f - c->yaw_control.yaw_force;
|
||
/* 腿长控制和VMC逆解算（使用PID控制） */
|
||
float virtual_force[2];
|
||
float target_L0[2];
|
||
float leg_d_length[2]; // 腿长变化率
|
||
|
||
/* 横滚角PID补偿计算 */
|
||
// 使用PID控制器计算横滚角补偿长度，目标横滚角为0
|
||
float roll_compensation_length =
|
||
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
|
||
c->feedback.imu.gyro.x, c->dt);
|
||
|
||
// 限制补偿长度范围，防止过度补偿
|
||
// 如果任一腿离地，限制补偿长度
|
||
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
|
||
if (roll_compensation_length > 0.10f)
|
||
roll_compensation_length = 0.10f;
|
||
if (roll_compensation_length < -0.10f)
|
||
roll_compensation_length = -0.10f;
|
||
} else {
|
||
if (roll_compensation_length > 0.15f)
|
||
roll_compensation_length = 0.15f;
|
||
if (roll_compensation_length < -0.15f)
|
||
roll_compensation_length = -0.15f;
|
||
}
|
||
|
||
// 目标腿长设定（包含横滚角补偿）
|
||
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
|
||
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
|
||
|
||
// 腿长限幅：最短0.10，最长0.42m
|
||
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
|
||
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
|
||
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
|
||
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
|
||
|
||
// 获取腿长变化率
|
||
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
|
||
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
|
||
|
||
/* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
|
||
float Delta_Tp[2];
|
||
// 使用tp_pid进行左右腿摆角同步控制
|
||
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
|
||
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, filtered_d_theta[0], c->dt);
|
||
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
|
||
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, filtered_d_theta[1], c->dt);
|
||
|
||
// 左腿控制
|
||
{
|
||
// 使用PID进行腿长控制
|
||
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
||
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
||
|
||
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
|
||
virtual_force[0] =
|
||
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
|
||
pid_output + 60.0f;
|
||
|
||
// VMC逆解算（包含摆角补偿）
|
||
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
|
||
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
|
||
&c->output.joint[1]);
|
||
|
||
} else {
|
||
// VMC失败，设为0力矩
|
||
c->output.joint[0] = 0.0f;
|
||
c->output.joint[1] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// 右腿控制
|
||
{
|
||
// 使用PID进行腿长控制
|
||
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
||
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
||
|
||
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
|
||
virtual_force[1] =
|
||
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
|
||
pid_output + 65.0f;
|
||
|
||
// VMC逆解算（包含摆角补偿）
|
||
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
|
||
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
|
||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
|
||
&c->output.joint[2]);
|
||
} else {
|
||
// VMC失败，设为0力矩
|
||
c->output.joint[2] = 0.0f;
|
||
c->output.joint[3] = 0.0f;
|
||
}
|
||
}
|
||
|
||
/* 安全限制 */
|
||
for (int i = 0; i < 2; i++) {
|
||
if (fabsf(c->output.wheel[i]) > 1.0f) {
|
||
c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f;
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
if (fabsf(c->output.joint[i]) > 20.0f) {
|
||
c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
|
||
}
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|