From 62ed731cd2c534721495bc67d735b968b657dc3c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 8 Jan 2026 11:31:52 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9vmc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/vmc.c | 28 +++++++++++++--------------- User/component/vmc.h | 24 ++++++++---------------- 2 files changed, 21 insertions(+), 31 deletions(-) diff --git a/User/component/vmc.c b/User/component/vmc.c index f819c98..c07a275 100644 --- a/User/component/vmc.c +++ b/User/component/vmc.c @@ -35,6 +35,7 @@ #define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f) /* Private variables -------------------------------------------------------- */ +static bool inited = false; /* Private function prototypes ---------------------------------------------- */ static int8_t VMC_ValidateParams(const VMC_Param_t *param); @@ -61,7 +62,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) { // 重置状态 VMC_Reset(vmc); - vmc->initialized = true; + inited = true; return 0; } @@ -77,7 +78,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) { * - 角度: 顺时针为正 */ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return -1; } body_pitch = -body_pitch; // 机体俯仰角取反 @@ -111,8 +112,8 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl leg->phi0 = atan2f(leg->foot_y, leg->foot_x); // 计算等效摆动杆角度(相对于机体坐标系) - leg->alpha = VMC_PI_2 - leg->phi0; - leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0); + leg->alpha = M_PI_2 - leg->phi0; + leg->theta = M_PI - (M_PI_2 + body_pitch - leg->phi0); // 角度归一化 VMC_ANGLE_NORMALIZE(leg->theta); @@ -124,9 +125,6 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl leg->d_theta = body_pitch_rate + leg->d_phi0; leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt); - // 计算角加速度 - leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt); - VMC_GroundContactDetection(vmc); // 更新地面接触状态 return 0; } @@ -137,7 +135,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl * 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩 */ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return -1; } @@ -166,7 +164,7 @@ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) { * 基于虚拟力和腿部状态估计地面法向力 */ float VMC_GroundContactDetection(VMC_t *vmc) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return 0.0f; } @@ -188,7 +186,7 @@ float VMC_GroundContactDetection(VMC_t *vmc) { * @brief 获取足端位置 */ int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) { - if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) { + if (vmc == NULL || !inited || x == NULL || y == NULL) { return -1; } @@ -202,7 +200,7 @@ int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) { * @brief 获取等效摆动杆参数 */ int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return -1; } @@ -218,7 +216,7 @@ int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, flo * @brief 获取关节输出力矩 */ int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) { - if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) { + if (vmc == NULL || !inited || tau_front == NULL || tau_rear == NULL) { return -1; } @@ -248,7 +246,7 @@ void VMC_Reset(VMC_t *vmc) { * @brief 设置虚拟力和力矩 */ void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return; } @@ -262,7 +260,7 @@ void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) { * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵 */ int8_t VMC_ComputeJacobian(VMC_t *vmc) { - if (vmc == NULL || !vmc->initialized) { + if (vmc == NULL || !inited) { return -1; } @@ -301,7 +299,7 @@ int8_t VMC_ComputeJacobian(VMC_t *vmc) { */ static int8_t VMC_ValidateParams(const VMC_Param_t *param) { if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || - param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) { + param->leg_3 <= 0 || param->leg_4 <= 0) { return -1; } diff --git a/User/component/vmc.h b/User/component/vmc.h index 10969c9..d2f948c 100644 --- a/User/component/vmc.h +++ b/User/component/vmc.h @@ -8,12 +8,14 @@ extern "C" { #include #include #include -#include "kinematics.h" +#include "user_math.h" +/* Exported constants ------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ /** * @brief VMC虚拟模型控制参数结构体 + * */ typedef struct { float hip_length; // 髋关节间距 @@ -21,8 +23,6 @@ typedef struct { float leg_2; // 大腿后端长度 (L2) float leg_3; // 小腿长度 (L3) float leg_4; // 小腿前端长度 (L4) - float wheel_radius; // 轮子半径 - float wheel_mass; // 轮子质量 } VMC_Param_t; /** @@ -44,8 +44,7 @@ typedef struct { float d_L0; // 等效摆动杆长度变化率 float theta; // 等效摆动杆角度 float d_theta; // 等效摆动杆角速度 - float dd_theta; // 等效摆动杆角加速度 - + // 虚拟力和力矩 float F_virtual; // 虚拟支撑力 float T_virtual; // 虚拟摆动力矩 @@ -84,23 +83,16 @@ typedef struct { VMC_Param_t param; // VMC参数 VMC_Leg_t leg; // 腿部状态 float dt; // 控制周期 - bool initialized; // 初始化标志 } VMC_t; -/* Exported constants ------------------------------------------------------- */ - -#define VMC_PI_2 (1.5707963267948966f) -#define VMC_PI (3.1415926535897932f) -#define VMC_2PI (6.2831853071795865f) - /* Exported macros ---------------------------------------------------------- */ /** * @brief 角度范围限制到[-PI, PI] */ #define VMC_ANGLE_NORMALIZE(angle) do { \ - while((angle) > VMC_PI) (angle) -= VMC_2PI; \ - while((angle) < -VMC_PI) (angle) += VMC_2PI; \ + while((angle) > M_PI) (angle) -= M_2PI; \ + while((angle) < -M_PI) (angle) += M_2PI; \ } while(0) /* Exported functions prototypes -------------------------------------------- */ @@ -109,7 +101,7 @@ typedef struct { * @brief 初始化VMC控制器 * @param vmc VMC控制器实例 * @param param VMC参数 - * @param sample_freq 采样频率 (Hz) + * @param sample_freq 控制频率 (Hz) * @return 0:成功, -1:失败 */ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq); @@ -135,7 +127,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual); /** - * @brief 地面接触检测 + * @brief 离地检测 * @param vmc VMC控制器实例 * @return 地面法向力 (N) */