robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

重构

Browse Source
This commit is contained in:
Robofish 2025-10-02 16:59:28 +08:00
parent 1b237f9944
commit bf7533e7c2
8 changed files with 1190 additions and 729 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/FreeRTOSConfig.h
View File

@ -187,7 +187,7 @@ standard names. */
/* USER CODE BEGIN Defines */
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
#define configAPPLICATION_ALLOCATED_HEAP 1
// #define configAPPLICATION_ALLOCATED_HEAP 1
/* USER CODE END Defines */
#endif /* FREERTOS_CONFIG_H */

222
User/component/lqr.c
View File

@ -6,89 +6,82 @@
#include <string.h>
/* Private macros ----------------------------------------------------------- */
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
/* Private variables -------------------------------------------------------- */
/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */
/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */
static LQR_GainMatrix_t default_gain_matrix = {
/* K矩阵第一行 - 轮毂力矩T的增益系数 */
.k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* theta */
.k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_theta */
.k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* x */
.k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* d_x */
.k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* phi */
.k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_phi */
/* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
.k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* theta */
.k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_theta */
.k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* x */
.k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* d_x */
.k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* phi */
.k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_phi */
};
/* Private function prototypes ---------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr);
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr);
/* Public functions --------------------------------------------------------- */
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
static int8_t LQR_CalculateErrorState(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 计算状态误差 */
lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
return 0;
}
static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
if (coeff == NULL) {
return 0.0f;
}
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
float L = leg_length;
float L2 = L * L;
float L3 = L2 * L;
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
}
// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) {
// if (lqr == NULL) {
// return -1;
// }
// /* 限制轮毂力矩 */
// if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
// lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
// -lqr->param.max_wheel_torque,
// lqr->param.max_wheel_torque);
// lqr->wheel_torque_limited = true;
// }
// /* 限制髋关节力矩 */
// if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
// lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
// -lqr->param.max_joint_torque,
// lqr->param.max_joint_torque);
// lqr->joint_torque_limited = true;
// }
// return 0;
// }
/* Public functions --------------------------------------------------------- */
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
if (lqr == NULL) {
return -1;
}
/* 清零结构体 */
memset(lqr, 0, sizeof(LQR_Controller_t));
/* 设置控制限制 */
lqr->param.max_wheel_torque = max_wheel_torque;
lqr->param.max_joint_torque = max_joint_torque;
/* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */
lqr->param.R = 0.072f; /* 驱动轮半径 */
lqr->param.l = 0.03f; /* 机体重心到转轴距离 */
lqr->param.mw = 0.60f; /* 驱动轮质量 */
lqr->param.mp = 1.8f; /* 摆杆质量 */
lqr->param.M = 1.8f; /* 机体质量 */
lqr->param.g = 9.8f; /* 重力加速度 */
/* 计算转动惯量 */
lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R;
lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f;
/* 设置默认增益矩阵 */
lqr->gain_matrix = &default_gain_matrix;
/* 设置默认目标状态（平衡状态） */
memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
/* 初始化当前腿长为中等值 */
lqr->current_leg_length = 0.25f;
/* 计算初始增益矩阵 */
LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
memset(lqr, 0, sizeof(LQR_t));
lqr->gain_matrix = gain_matrix;
lqr->initialized = true;
return 0;
}
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) {
if (lqr == NULL || gain_matrix == NULL) {
return -1;
}
lqr->gain_matrix = gain_matrix;
/* 重新计算增益矩阵 */
return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
}
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *state) {
if (lqr == NULL || state == NULL) {
return -1;
}
@ -100,34 +93,25 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) {
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) {
if (lqr == NULL || target_state == NULL) {
return -1;
}
lqr->param.target_state = *target_state;
lqr->target_state = *target_state;
/* 重新计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
if (lqr == NULL || lqr->gain_matrix == NULL) {
return -1;
}
/* 限制腿长范围 */
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
lqr->current_leg_length = leg_length;
/* 更新与腿长相关的物理参数 */
lqr->param.L = leg_length / 2.0f; /* 摆杆重心到驱动轮轴距离 */
lqr->param.LM = leg_length / 2.0f; /* 摆杆重心到其转轴距离 */
/* 计算摆杆转动惯量 */
float leg_total_length = lqr->param.L + lqr->param.LM;
lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f;
/* 使用多项式拟合计算当前增益矩阵 */
lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */
lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */
@ -146,7 +130,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
return 0;
}
int8_t LQR_Control(LQR_Controller_t *lqr) {
int8_t LQR_Control(LQR_t *lqr) {
if (lqr == NULL || !lqr->initialized) {
return -1;
}
@ -175,92 +159,22 @@ int8_t LQR_Control(LQR_Controller_t *lqr) {
lqr->K_matrix[1][5] * lqr->error_state.d_phi
);
/* 应用控制限制 */
return LQR_ApplyControlLimits(lqr);
}
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) {
if (lqr == NULL || output == NULL) {
return -1;
}
*output = lqr->control_output;
return 0;
}
int8_t LQR_Reset(LQR_Controller_t *lqr) {
int8_t LQR_Reset(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 清零状态和输出 */
memset(&lqr->current_state, 0, sizeof(LQR_State_t));
memset(&lqr->target_state, 0, sizeof(LQR_State_t));
memset(&lqr->error_state, 0, sizeof(LQR_State_t));
memset(&lqr->control_output, 0, sizeof(LQR_Input_t));
memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
/* 重置限幅标志 */
lqr->wheel_torque_limited = false;
lqr->joint_torque_limited = false;
memset(&lqr->control_output, 0, sizeof(LQR_Output_t));
return 0;
}
float LQR_PolynomialCalc(const float *coeff, float leg_length) {
if (coeff == NULL) {
return 0.0f;
}
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
float L = leg_length;
float L2 = L * L;
float L3 = L2 * L;
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
}
/* Private functions -------------------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 计算状态误差 */
lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta;
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta;
lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x;
lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x;
lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi;
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi;
return 0;
}
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 重置限幅标志 */
lqr->wheel_torque_limited = false;
lqr->joint_torque_limited = false;
/* 限制轮毂力矩 */
if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
-lqr->param.max_wheel_torque,
lqr->param.max_wheel_torque);
lqr->wheel_torque_limited = true;
}
/* 限制髋关节力矩 */
if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
-lqr->param.max_joint_torque,
lqr->param.max_joint_torque);
lqr->joint_torque_limited = true;
}
return 0;
}
/* Private functions -------------------------------------------------------- */

97
User/component/lqr.h
View File

@ -1,6 +1,6 @@
/*
* LQR控制器
* 用于轮腿平衡机器人的线性二次调节器控制
* 用于轮腿平衡机器人的线性二次调节器控制：单腿建模
*/
#pragma once
@ -21,16 +21,6 @@ extern "C" {
/* Exported types ----------------------------------------------------------- */
/**
* @brief LQR状态向量
* 状态定义：
* theta: 摆杆与竖直方向夹角 (rad)
* d_theta: 摆杆角速度 (rad/s)
* x: 驱动轮位移 (m)
* d_x: 驱动轮速度 (m/s)
* phi: 机体与水平夹角 (rad)
* d_phi: 机体角速度 (rad/s)
*/
typedef struct {
float theta; /* 摆杆角度 */
float d_theta; /* 摆杆角速度 */
@ -40,19 +30,11 @@ typedef struct {
float d_phi; /* 机体俯仰角速度 */
} LQR_State_t;
/**
* @brief LQR控制输入向量
*/
typedef struct {
float T; /* 轮毂力矩 (N·m) */
float Tp; /* 髋关节力矩 (N·m) */
} LQR_Input_t;
} LQR_Output_t;
/**
* @brief LQR增益矩阵参数
* K矩阵的每个元素的多项式拟合系数
* K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
*/
typedef struct {
/* K矩阵第一行(轮毂力矩T对应的增益) */
float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */
@ -71,41 +53,18 @@ typedef struct {
float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */
} LQR_GainMatrix_t;
/**
* @brief LQR控制器参数
*/
typedef struct {
/* 系统物理参数 */
float R; /* 驱动轮半径 (m) */
float L; /* 摆杆重心到驱动轮轴距离 (m) */
float LM; /* 摆杆重心到其转轴距离 (m) */
float l; /* 机体重心到其转轴距离 (m) */
float mw; /* 驱动轮质量 (kg) */
float mp; /* 摆杆质量 (kg) */
float M; /* 机体质量 (kg) */
float Iw; /* 驱动轮转动惯量 (kg·m²) */
float Ip; /* 摆杆绕质心转动惯量 (kg·m²) */
float IM; /* 机体绕质心转动惯量 (kg·m²) */
float g; /* 重力加速度 (m/s²) */
/* 控制限制 */
float max_wheel_torque; /* 轮毂电机最大力矩 (N·m) */
float max_joint_torque; /* 关节电机最大力矩 (N·m) */
/* 目标状态 */
LQR_State_t target_state; /* 目标状态 */
} LQR_Param_t;
/**
* @brief LQR控制器主结构体
*/
typedef struct {
LQR_Param_t param; /* 控制器参数 */
LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
LQR_State_t current_state; /* 当前状态 */
LQR_State_t target_state; /* 目标状态 */
LQR_State_t error_state; /* 状态误差 */
LQR_Input_t control_output; /* 控制输出 */
LQR_Output_t control_output; /* 控制输出 */
/* 运行时变量 */
float current_leg_length; /* 当前腿长 */
@ -113,10 +72,7 @@ typedef struct {
bool initialized; /* 初始化标志 */
/* 限幅标志 */
bool wheel_torque_limited; /* 轮毂力矩是否被限幅 */
bool joint_torque_limited; /* 关节力矩是否被限幅 */
} LQR_Controller_t;
} LQR_t;
/* Exported functions prototypes -------------------------------------------- */
@ -124,20 +80,11 @@ typedef struct {
* @brief 初始化LQR控制器
*
* @param lqr LQR控制器结构体指针
* @param max_wheel_torque 轮毂电机最大力矩
* @param max_joint_torque 关节电机最大力矩
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
/**
* @brief 设置LQR增益矩阵参数
*
* @param lqr LQR控制器结构体指针
* @param gain_matrix 增益矩阵参数指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
/**
* @brief 更新当前状态
@ -146,7 +93,7 @@ int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
* @param state 当前状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
/**
* @brief 设置目标状态
@ -155,7 +102,7 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
* @param target_state 目标状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state);
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
/**
* @brief 根据当前腿长计算增益矩阵
@ -164,7 +111,7 @@ int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state
* @param leg_length 当前腿长 (m)
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
/**
* @brief 执行LQR控制计算
@ -172,16 +119,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Control(LQR_Controller_t *lqr);
/**
* @brief 获取控制输出
*
* @param lqr LQR控制器结构体指针
* @param output 输出控制指令指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
int8_t LQR_Control(LQR_t *lqr);
/**
* @brief 重置LQR控制器
@ -189,16 +127,7 @@ int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Reset(LQR_Controller_t *lqr);
/**
* @brief 根据多项式系数计算增益值
*
* @param coeff 多项式系数数组
* @param leg_length 腿长
* @return float 增益值
*/
float LQR_PolynomialCalc(const float *coeff, float leg_length);
int8_t LQR_Reset(LQR_t *lqr);
#ifdef __cplusplus
}

928
User/module/balance_chassis.c
View File

@ -1,538 +1,494 @@
#include "module/balance_chassis.h"
#include "bsp/time.h"
#include "bsp/can.h"
#include "component/user_math.h"
#include "bsp/time.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include "component/user_math.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include <math.h>
#include <stdint.h>
#include <string.h>
/**
* @brief 使能所有电机
* @param c 底盘结构体指针
* @return
* @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_motors[i]);
}
BSP_TIME_Delay_us(200);
for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
}
return CHASSIS_OK;
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
}
BSP_TIME_Delay_us(200);
for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
}
return CHASSIS_OK;
}
// /**
// * @brief 关闭所有电机
// * @param c 底盘结构体指针
// * @return
// */
// static int8_t Chassis_MotorDisable(Chassis_t *c){
// if (c == NULL) return CHASSIS_ERR_NULL;
// for (int i = 0; i < 2; i++) {
// MOTOR_LK_MotorOff(&c->param->wheel_motors[i]);
// }
// for (int i = 0; i < 4; i++) {
// MOTOR_LZ_Disable(&c->param->joint_motors[i], true);
// }
// 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 < 2; i++) {
MOTOR_LK_Relax(&c->param->wheel_motors[i]);
}
BSP_TIME_Delay_us(200);
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Relax(&c->param->joint_motors[i]);
}
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; /* 模式未改变直接返回 */
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.roll);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
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.roll);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
//清空位移
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->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
// 清空位移
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;
LQR_Reset(&c->lqr[0]);
LQR_Reset(&c->lqr[1]);
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
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;
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;
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; // 参数错误
/*初始化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]);
}
MOTOR_DM_Register(&c->param->yaw_motor);
// Chassis_MotorEnable(c);
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化pid*/
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
/*初始化LQR控制器*/
LQR_Init(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->lqr_gains);
/*初始化机体状态*/
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;
/*初始化yaw控制状态*/
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 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) {
if (joint_motor->motor.feedback.rotor_abs_angle > M_PI) {
joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
joint_motor->motor.feedback.rotor_abs_angle =
-joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整
c->feedback.joint[i] = joint_motor->motor.feedback;
}
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_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Register(&c->param->wheel_motors[i]);
}
MOTOR_DM_Register(&c->param->yaw_motor);
}
/* 更新云台电机反馈数据（yaw轴） */
MOTOR_DM_Update(&(c->param->yaw_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
if (dm_motor != NULL) {
c->feedback.yaw = dm_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;
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正解算用于状态估计
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;
fn = -25.0f;
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
&c->output.joint[1].torque);
VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
&c->output.joint[2].torque);
// Chassis_MotorEnable(c);
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
Chassis_Output(c); // 统一输出
} break;
/*初始化pid*/
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮腿平衡模式，使用LQR控制和PID腿长控制
// 执行LQR控制（包含PID腿长控制）
// Chassis_MotorRelax(c);
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
/*初始化LQR控制器*/
LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[1], &param->lqr_gains);
/*初始化机体状态*/
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;
/*初始化yaw控制状态*/
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f;
default:
return CHASSIS_ERR_MODE;
}
return CHASSIS_OK;
}
int8_t Chassis_Output(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
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], &param);
}
BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 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) {
if (joint_motor->motor.feedback.rotor_abs_angle > M_PI ){
joint_motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
joint_motor->motor.feedback.rotor_abs_angle = - joint_motor->motor.feedback.rotor_abs_angle; // 机械零点调整
c->feedback.joint[i] = joint_motor->motor.feedback;
}
}
// 更新轮子电机反馈并检查离线状态
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;
}
}
/* 更新云台电机反馈数据（yaw轴） */
MOTOR_DM_Update(&(c->param->yaw_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
if (dm_motor != NULL) {
c->feedback.yaw = dm_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:
// 放松模式，电机不输出
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
MOTOR_LZ_Relax(&c->param->joint_motors[0]);
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
MOTOR_LZ_Relax(&c->param->joint_motors[1]);
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
MOTOR_LZ_Relax(&c->param->joint_motors[2]);
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
MOTOR_LZ_Relax(&c->param->joint_motors[3]);
BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
MOTOR_LK_Relax(&c->param->wheel_motors[0]);
// BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10);
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);
Chassis_LQRControl(c, c_cmd);
break;
case CHASSIS_MODE_RECOVER:
{
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 fn;
float tp1, tp2;
fn = -25.0f;
// tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
// tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
// Chassis_MotorEnable(c);
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
Chassis_Output(c); // 统一输出
}
break;
case CHASSIS_MODE_WHELL_BALANCE:
// 更新VMC正解算用于状态估计
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控制和PID腿长控制
// 更新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控制（包含PID腿长控制）
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], &param);
}
BSP_TIME_Delay_us(400); // 等待CAN总线空闲，确保前面的命令发送完成
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 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;
}
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] = true; // 强制设置左腿着地
onground_flag[1] = true; // 强制设置右腿着地
/* 获取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;
float 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*2;
// target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
c->chassis_state.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.2f; // 目标俯仰角
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 = c->chassis_state.target_x; // 目标位置
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], &current_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;
// Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
// Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
} 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++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f;
float target_dot_x = 0.0f;
// c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x;
// c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s
// 目标设定
target_dot_x = c_cmd->move_vec.vx * 2;
c->chassis_state.target_x += target_dot_x * c->dt;
// // 修正目标yaw角度到[-pi, pi]
// if (c->yaw_control.target_yaw > M_PI) {
// c->yaw_control.target_yaw -= M_2PI;
// } else if (c->yaw_control.target_yaw < -M_PI) {
// c->yaw_control.target_yaw += M_2PI;
// }
/*更新当前状态*/
LQR_State_t current_state = {0};
current_state.theta = c->vmc_[0].leg.theta; // 左腿摆杆角度
current_state.d_theta = c->vmc_[0].leg.d_theta; // 左腿摆杆角速度
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_UpdateState(&c->lqr[0], &current_state);
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
// c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
c->yaw_control.target_yaw = c->param->mech_zero_yaw;
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);
/* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force;
/* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2];
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */
float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt);
// 目标腿长设定
target_L0[0] = 0.15f + c_cmd->height * 0.2f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
target_L0[1] = 0.15f + c_cmd->height * 0.2f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
// 获取腿长变化率
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] = leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
for (int leg = 0; leg < 2; leg++) {
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leg_length[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 + 30.0f;
// + // PID腿长控制输出
// 45.0f; // 基础支撑力
// VMC逆解算（包含摆角补偿）
// virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败
// Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
// Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) {
// if (VMC_InverseSolve(&c->vmc_[leg], 0.0, Tp[leg] + Delta_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;
}
}
current_state.theta = c->vmc_[1].leg.theta; // 右腿摆杆角度
current_state.d_theta = c->vmc_[1].leg.d_theta; // 右腿摆杆角速度
LQR_UpdateState(&c->lqr[1], &current_state);
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.2f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x; // 目标位置
target_state.d_x = target_dot_x; // 目标速度
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[0]);
LQR_Control(&c->lqr[1]);
// /* 分别计算左右腿的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.2f; // 目标俯仰角
// 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 = c->chassis_state.target_x; // 目标位置
// 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], &current_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;
// // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
// // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
// } 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->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.005f; //
// 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s
// // 修正目标yaw角度到[-pi, pi]
// if (c->yaw_control.target_yaw > M_PI) {
// c->yaw_control.target_yaw -= M_2PI;
// } else if (c->yaw_control.target_yaw < -M_PI) {
// c->yaw_control.target_yaw += M_2PI;
// }
// c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
// c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
c->yaw_control.target_yaw = c->param->mech_zero_yaw + c_cmd->move_vec.vy;
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);
/* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
/* 腿长控制和VMC逆解算（使用PID控制） */
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */
float roll_compensation =
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt);
// 目标腿长设定
target_L0[0] = 0.15f + c_cmd->height * 0.2f +
roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
target_L0[1] = 0.15f + c_cmd->height * 0.2f -
roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
// 获取腿长变化率
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, 0, c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = -Delta_Tp[0];
float out = PID_Calc(&c->pid.leg_length[0], target_L0[0], c->vmc_[0].leg.L0,
leg_d_length[0], c->dt);
c->vmc_[0].leg.F_virtual = out;
out = PID_Calc(&c->pid.leg_length[1], target_L0[1], c->vmc_[1].leg.L0,
leg_d_length[1], c->dt);
c->vmc_[1].leg.F_virtual = out;
c->vmc_[0].leg.T_virtual = c->lqr[0].control_output.Tp + Delta_Tp[0];
c->vmc_[1].leg.T_virtual = c->lqr[1].control_output.Tp + Delta_Tp[1];
VMC_InverseSolve(&c->vmc_[0], c->vmc_[0].leg.F_virtual,
c->vmc_[0].leg.T_virtual);
VMC_InverseSolve(&c->vmc_[1], c->vmc_[1].leg.F_virtual,
c->vmc_[1].leg.T_virtual);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque,
&c->output.joint[1].torque);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque,
&c->output.joint[2].torque);
/* 安全限制 */
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 < 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;
}
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;
}
return CHASSIS_OK;
}

4
User/module/balance_chassis.h
View File

@ -117,7 +117,7 @@ typedef struct {
Chassis_Output_t output;
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
@ -219,7 +219,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_Output(Chassis_t *c);
int8_t Chassis_Output(Chassis_t *c);
#ifdef __cplusplus
}

2
User/module/config.c
View File

@ -141,7 +141,7 @@ Config_RobotParam_t robot_config = {
.reverse = false,
},
.mech_zero_yaw = 1.21085676f, /* 250.5度，机械零点 */
.mech_zero_yaw = 1.20564249f, /* 250.5度，机械零点 */
.vmc_param = {
{ // 左腿

2
User/task/rc.c
View File

@ -83,7 +83,7 @@ void Task_rc(void *argument) {
cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
break;
case RC_CAN_SW_DOWN: // 下位
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
default:
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;

662
mod_wheelleg_chassis.cpp Normal file
View File

@ -0,0 +1,662 @@
#include "mod_wheelleg_chassis.hpp"
#include <cmath>
#include <tuple>
using namespace Module;
Wheelleg::Wheelleg(Param& param)
: param_(param),
roll_pid_(param.roll_pid_param, 333.0f),
yaw_pid_(param.yaw_pid_param, 333.0f),
yaccl_pid_(param.yaccl_pid_param, 333.0f),
lowfilter_(333.0f, 50.0f),
acclfilter_(333.0f, 30.0f),
manfilter_(param.manfilter_param),
ctrl_lock_(true) {
memset(&(this->cmd_), 0, sizeof(this->cmd_));
this->hip_motor_.at(0) =
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
this->hip_motor_.at(1) =
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
this->hip_motor_.at(2) =
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
this->hip_motor_.at(3) =
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
this->wheel_motor_.at(0) =
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
this->wheel_motor_.at(1) =
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
this->leglength_pid_.at(0) =
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
this->leglength_pid_.at(1) =
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
this->theta_pid_.at(0) =
new Component::PID(param.theta_pid_param.at(0), 333.0f);
this->theta_pid_.at(1) =
new Component::PID(param.theta_pid_param.at(1), 333.0f);
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
chassis->ctrl_lock_.Wait(UINT32_MAX);
switch (event) {
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
}
chassis->ctrl_lock_.Post();
};
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
event_callback, this, this->param_.EVENT_MAP);
auto chassis_thread = [](Wheelleg* chassis) {
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
auto eulr_sub =
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
auto gyro_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
auto accl_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
uint32_t last_online_time = bsp_time_get_ms();
while (1) {
eulr_sub.DumpData(chassis->eulr_); /* imu */
gyro_sub.DumpData(chassis->gyro_); /* imu */
accl_sub.DumpData(chassis->accl_);
yaw_sub.DumpData(chassis->yaw_);
cmd_sub.DumpData(chassis->cmd_);
// yaw_sub.DumpData(chassis->yaw_);
chassis->ctrl_lock_.Wait(UINT32_MAX);
chassis->MotorSetAble();
chassis->UpdateFeedback();
chassis->Calculate();
chassis->Control();
chassis->ctrl_lock_.Post();
/* 运行结束，等待下一次唤醒 */
chassis->thread_.SleepUntil(3, last_online_time);
}
};
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
System::Thread::MEDIUM);
}
void Wheelleg::MotorSetAble() {
if (this->hip_motor_flag_ == 0) {
this->hip_motor_[0]->Relax();
this->hip_motor_[1]->Relax();
this->hip_motor_[2]->Relax();
this->hip_motor_[3]->Relax();
this->dm_motor_flag_ = 0;
}
else {
if (this->dm_motor_flag_ == 0) {
for (int i = 0; i < 5; i++) {
this->hip_motor_[0]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[1]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[2]->Enable();
}
for (int i = 0; i < 5; i++) {
this->hip_motor_[3]->Enable();
}
this->dm_motor_flag_ = 1;
}
};
}
void Wheelleg::UpdateFeedback() {
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
this->last_wakeup_ = this->now_;
this->wheel_motor_[0]->Update();
this->wheel_motor_[1]->Update();
this->leg_argu_[0].phi4_ =
this->hip_motor_[0]->GetAngle() -
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->leg_argu_[0].phi1_ =
this->hip_motor_[1]->GetAngle() -
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->leg_argu_[1].phi4_ =
(-this->hip_motor_[2]->GetAngle() +
this->param_.wheel_param.mech_zero[3]); // 前关节角度
if (leg_argu_[1].phi4_ > M_PI) {
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
}
this->leg_argu_[1].phi1_ =
(-this->hip_motor_[3]->GetAngle() +
this->param_.wheel_param.mech_zero[2]); // 后关节角度
if (leg_argu_[1].phi1_ > M_PI) {
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
}
this->pit_ = this->eulr_.pit;
if (this->pit_ > M_PI) {
this->pit_ = this->eulr_.pit - 2 * M_PI;
}
/* 注意极性 */
std::tuple<float, float, float, float> result0 =
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[0].L0 = std::get<0>(result0);
this->leg_argu_[0].d_L0 = std::get<1>(result0);
this->leg_argu_[0].theta = -std::get<2>(result0);
this->leg_argu_[0].d_theta = -std::get<3>(result0);
if (leg_argu_[0].theta > M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
}
if (leg_argu_[0].theta < -M_PI) {
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
}
std::tuple<float, float, float, float> result1 =
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[1].L0 = std::get<0>(result1);
this->leg_argu_[1].d_L0 = std::get<1>(result1);
this->leg_argu_[1].theta = -std::get<2>(result1);
this->leg_argu_[1].d_theta = -std::get<3>(result1);
if (leg_argu_[1].theta > M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
}
if (leg_argu_[1].theta < -M_PI) {
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
}
/* 轮子转速近似于编码器速度 由此推测机体速度 */
this->leg_argu_[0].single_x_dot =
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
this->leg_argu_[1].single_x_dot =
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
this->move_argu_.x_dot =
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
this->move_argu_.x_dot =
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
this->move_argu_.delta_speed =
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
if (now_ > 1000000) {
this->move_argu_.x_dot_hat =
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
this->move_argu_.last_x_dot, accl_.y * 9.8f,
dt_) -
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
2;
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
}
}
void Wheelleg::Calculate() {
switch (this->mode_) {
case Wheelleg::RELAX:
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
if (this->yaw_ < -M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
break;
case Wheelleg::ROTOR:
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
default:
XB_ASSERT(false);
return;
}
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
onground0_flag_ = false;
if (leg_argu_[0].Fn > 30) {
onground0_flag_ = true;
}
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
onground1_flag_ = false;
if (leg_argu_[1].Fn > 30) {
onground1_flag_ = true;
}
std::tuple<float, float> result3;
std::tuple<float, float> result4;
switch (this->mode_) {
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
for (int i = 0; i < 12; i++) {
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
leg_argu_[0].L0);
}
for (int i = 0; i < 12; i++) {
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
leg_argu_[1].L0);
}
if (now_ > 1000000)
if (fabs(move_argu_.target_dot_x) > 0.5 ||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
((!onground0_flag_) & (!onground1_flag_))) {
leg_argu_[0].LQR_K[2] = 0;
leg_argu_[1].LQR_K[2] = 0;
this->move_argu_.xhat = 0;
move_argu_.target_x = 0;
}
if (onground0_flag_) {
leg_argu_[0].Tw =
(leg_argu_[0].LQR_K[0] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[0].Tw = 0;
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
}
if (onground1_flag_) {
leg_argu_[1].Tw =
(leg_argu_[1].LQR_K[0] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
} else {
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_) {
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
} else {
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
fabs(leg_argu_[1].theta) > 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
if (fabs(pit_) > M_PI / 2) {
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
}
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
}
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
0.65f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
0.65f, this->leg_argu_[1].L0, this->dt_);
}
if (fabs(leg_argu_[0].theta) < 1.57) {
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
leg_argu_[0].target_theta = leg_argu_[0].theta;
}
if (fabs(leg_argu_[1].theta) < 1.57) {
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
leg_argu_[1].target_theta = leg_argu_[1].theta;
}
if (leg_argu_[0].target_theta > M_PI) {
leg_argu_[0].target_theta -= 2 * M_PI;
}
if (leg_argu_[0].target_theta < -M_PI) {
leg_argu_[0].target_theta += 2 * M_PI;
}
if (leg_argu_[1].target_theta > M_PI) {
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI) {
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
} else {
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
0.10f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
0.10f, this->leg_argu_[1].L0, this->dt_);
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
0.1, leg_argu_[0].theta, dt_);
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
0.1, leg_argu_[1].theta, dt_);
clampf(&leg_argu_[0].Tp, -10, 10);
clampf(&leg_argu_[1].Tp, -10, 10);
} else {
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
}
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
}
}
void Wheelleg::Control() {
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
// wheel_motor_out_[0] = 0;
// wheel_motor_out_[1] = 0;
// if (fabs(this->pit_) > 0.5f) {
// this->hip_motor_flag_ = 0;
// }
// }
switch (this->mode_) {
case Wheelleg::RELAX:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::RESET:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
break;
}
}
void Wheelleg::SetMode(Wheelleg::Mode mode) {
if (mode == this->mode_) {
return;
}
this->leg_[0]->Reset();
this->leg_[1]->Reset();
move_argu_.x = 0;
move_argu_.x_dot = 0;
move_argu_.last_x_dot = 0;
move_argu_.target_x = move_argu_.xhat;
move_argu_.target_yaw = this->eulr_.yaw;
move_argu_.target_dot_x = 0;
move_argu_.xhat = 0;
move_argu_.x_dot_hat = 0;
this->manfilter_.reset_comp();
this->mode_ = mode;
}