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

改vmc

Browse Source
This commit is contained in:
Robofish 2026-01-08 11:31:52 +08:00
parent b1300f0d7d
commit 62ed731cd2
2 changed files with 21 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
User/component/vmc.c
View File

@ -35,6 +35,7 @@
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f) #define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static bool inited = false;
/* Private function prototypes ---------------------------------------------- */ /* Private function prototypes ---------------------------------------------- */
static int8_t VMC_ValidateParams(const VMC_Param_t *param); 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_Reset(vmc);
vmc->initialized = true; inited = true;
return 0; 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) { 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; return -1;
} }
body_pitch = -body_pitch; // 机体俯仰角取反 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->phi0 = atan2f(leg->foot_y, leg->foot_x);
// 计算等效摆动杆角度(相对于机体坐标系) // 计算等效摆动杆角度(相对于机体坐标系)
leg->alpha = VMC_PI_2 - leg->phi0; leg->alpha = M_PI_2 - leg->phi0;
leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0); leg->theta = M_PI - (M_PI_2 + body_pitch - leg->phi0);
// 角度归一化 // 角度归一化
VMC_ANGLE_NORMALIZE(leg->theta); 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_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt); 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); // 更新地面接触状态 VMC_GroundContactDetection(vmc); // 更新地面接触状态
return 0; 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) { int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) { if (vmc == NULL || !inited) {
return -1; 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) { float VMC_GroundContactDetection(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) { if (vmc == NULL || !inited) {
return 0.0f; return 0.0f;
} }
@ -188,7 +186,7 @@ float VMC_GroundContactDetection(VMC_t *vmc) {
* @brief 获取足端位置 * @brief 获取足端位置
*/ */
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) { 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; return -1;
} }
@ -202,7 +200,7 @@ int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
* @brief 获取等效摆动杆参数 * @brief 获取等效摆动杆参数
*/ */
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) { 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; return -1;
} }
@ -218,7 +216,7 @@ int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, flo
* @brief 获取关节输出力矩 * @brief 获取关节输出力矩
*/ */
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) { 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; return -1;
} }
@ -248,7 +246,7 @@ void VMC_Reset(VMC_t *vmc) {
* @brief 设置虚拟力和力矩 * @brief 设置虚拟力和力矩
*/ */
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) { void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) { if (vmc == NULL || !inited) {
return; return;
} }
@ -262,7 +260,7 @@ void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵 * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
*/ */
int8_t VMC_ComputeJacobian(VMC_t *vmc) { int8_t VMC_ComputeJacobian(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) { if (vmc == NULL || !inited) {
return -1; return -1;
} }
@ -301,7 +299,7 @@ int8_t VMC_ComputeJacobian(VMC_t *vmc) {
*/ */
static int8_t VMC_ValidateParams(const VMC_Param_t *param) { static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || 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; return -1;
} }

24
User/component/vmc.h
View File

@ -8,12 +8,14 @@ extern "C" {
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <math.h> #include <math.h>
#include "kinematics.h" #include "user_math.h"
/* Exported constants ------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/** /**
* @brief VMC虚拟模型控制参数结构体 * @brief VMC虚拟模型控制参数结构体
*
*/ */
typedef struct { typedef struct {
float hip_length; // 髋关节间距 float hip_length; // 髋关节间距
@ -21,8 +23,6 @@ typedef struct {
float leg_2; // 大腿后端长度 (L2) float leg_2; // 大腿后端长度 (L2)
float leg_3; // 小腿长度 (L3) float leg_3; // 小腿长度 (L3)
float leg_4; // 小腿前端长度 (L4) float leg_4; // 小腿前端长度 (L4)
float wheel_radius; // 轮子半径
float wheel_mass; // 轮子质量
} VMC_Param_t; } VMC_Param_t;
/** /**
@ -44,8 +44,7 @@ typedef struct {
float d_L0; // 等效摆动杆长度变化率 float d_L0; // 等效摆动杆长度变化率
float theta; // 等效摆动杆角度 float theta; // 等效摆动杆角度
float d_theta; // 等效摆动杆角速度 float d_theta; // 等效摆动杆角速度
float dd_theta; // 等效摆动杆角加速度
// 虚拟力和力矩 // 虚拟力和力矩
float F_virtual; // 虚拟支撑力 float F_virtual; // 虚拟支撑力
float T_virtual; // 虚拟摆动力矩 float T_virtual; // 虚拟摆动力矩
@ -84,23 +83,16 @@ typedef struct {
VMC_Param_t param; // VMC参数 VMC_Param_t param; // VMC参数
VMC_Leg_t leg; // 腿部状态 VMC_Leg_t leg; // 腿部状态
float dt; // 控制周期 float dt; // 控制周期
bool initialized; // 初始化标志
} VMC_t; } VMC_t;
/* Exported constants ------------------------------------------------------- */
#define VMC_PI_2 (1.5707963267948966f)
#define VMC_PI (3.1415926535897932f)
#define VMC_2PI (6.2831853071795865f)
/* Exported macros ---------------------------------------------------------- */ /* Exported macros ---------------------------------------------------------- */
/** /**
* @brief 角度范围限制到[-PI, PI] * @brief 角度范围限制到[-PI, PI]
*/ */
#define VMC_ANGLE_NORMALIZE(angle) do { \ #define VMC_ANGLE_NORMALIZE(angle) do { \
while((angle) > VMC_PI) (angle) -= VMC_2PI; \ while((angle) > M_PI) (angle) -= M_2PI; \
while((angle) < -VMC_PI) (angle) += VMC_2PI; \ while((angle) < -M_PI) (angle) += M_2PI; \
} while(0) } while(0)
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
@ -109,7 +101,7 @@ typedef struct {
* @brief 初始化VMC控制器 * @brief 初始化VMC控制器
* @param vmc VMC控制器实例 * @param vmc VMC控制器实例
* @param param VMC参数 * @param param VMC参数
* @param sample_freq 采样频率 (Hz) * @param sample_freq 控制频率 (Hz)
* @return 0:成功, -1:失败 * @return 0:成功, -1:失败
*/ */
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq); 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); int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
/** /**
* @brief 地面接触检测 * @brief 离地检测
* @param vmc VMC控制器实例 * @param vmc VMC控制器实例
* @return 地面法向力 (N) * @return 地面法向力 (N)
*/ */