197 lines
5.7 KiB
C
197 lines
5.7 KiB
C
#pragma once
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <math.h>
|
|
#include "kinematics.h"
|
|
|
|
/* Exported types ----------------------------------------------------------- */
|
|
|
|
/**
|
|
* @brief VMC虚拟模型控制参数结构体
|
|
*/
|
|
typedef struct {
|
|
float hip_length; // 髋关节间距
|
|
float leg_1; // 大腿前端长度 (L1)
|
|
float leg_2; // 大腿后端长度 (L2)
|
|
float leg_3; // 小腿长度 (L3)
|
|
float leg_4; // 小腿前端长度 (L4)
|
|
float wheel_radius; // 轮子半径
|
|
float wheel_mass; // 轮子质量
|
|
} VMC_Param_t;
|
|
|
|
/**
|
|
* @brief VMC腿部运动学状态结构体
|
|
*/
|
|
typedef struct {
|
|
// 关节角度
|
|
float phi1; // 后髋关节角度 (rad)
|
|
float phi2; // 大腿后端角度 (rad)
|
|
float phi3; // 小腿角度 (rad)
|
|
float phi4; // 前髋关节角度 (rad)
|
|
|
|
// 足端坐标
|
|
float foot_x; // 足端x坐标
|
|
float foot_y; // 足端y坐标
|
|
|
|
// 等效摆动杆参数
|
|
float L0; // 等效摆动杆长度
|
|
float d_L0; // 等效摆动杆长度变化率
|
|
float theta; // 等效摆动杆角度
|
|
float d_theta; // 等效摆动杆角速度
|
|
float dd_theta; // 等效摆动杆角加速度
|
|
|
|
// 虚拟力和力矩
|
|
float F_virtual; // 虚拟支撑力
|
|
float T_virtual; // 虚拟摆动力矩
|
|
|
|
// 雅可比矩阵元素
|
|
float J11, J12, J21, J22;
|
|
|
|
// 输出力矩
|
|
float tau_hip_front; // 前髋关节输出力矩
|
|
float tau_hip_rear; // 后髋关节输出力矩
|
|
|
|
// 内部计算变量
|
|
float XB, YB, XC, YC, XD, YD; // 各关键点坐标
|
|
float lBD; // BD连杆长度
|
|
float A0, B0, C0; // 运动学计算中间变量
|
|
float phi0; // 足端极角
|
|
float alpha; // 等效摆动杆与竖直方向夹角
|
|
float d_phi0; // 足端极角变化率
|
|
float d_alpha; // alpha角变化率
|
|
|
|
// 历史值（用于数值微分）
|
|
float last_phi0;
|
|
float last_L0;
|
|
float last_d_L0;
|
|
float last_d_theta;
|
|
|
|
// 地面接触检测
|
|
float Fn; // 地面法向力
|
|
bool is_ground_contact; // 地面接触标志
|
|
} VMC_Leg_t;
|
|
|
|
/**
|
|
* @brief VMC控制器结构体
|
|
*/
|
|
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(0)
|
|
|
|
/* Exported functions prototypes -------------------------------------------- */
|
|
|
|
/**
|
|
* @brief 初始化VMC控制器
|
|
* @param vmc VMC控制器实例
|
|
* @param param VMC参数
|
|
* @param sample_freq 采样频率 (Hz)
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
|
|
|
|
/**
|
|
* @brief VMC五连杆正解算
|
|
* @param vmc VMC控制器实例
|
|
* @param phi1 后髋关节角度 (rad)
|
|
* @param phi4 前髋关节角度 (rad)
|
|
* @param body_pitch 机体pitch角 (rad)
|
|
* @param body_pitch_rate 机体pitch角速度 (rad/s)
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);
|
|
|
|
/**
|
|
* @brief VMC五连杆逆解算(力矩分配)
|
|
* @param vmc VMC控制器实例
|
|
* @param F_virtual 期望虚拟支撑力 (N)
|
|
* @param T_virtual 期望虚拟摆动力矩 (N*m)
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
|
|
|
|
/**
|
|
* @brief 地面接触检测
|
|
* @param vmc VMC控制器实例
|
|
* @return 地面法向力 (N)
|
|
*/
|
|
float VMC_GroundContactDetection(VMC_t *vmc);
|
|
|
|
/**
|
|
* @brief 获取足端位置(直角坐标)
|
|
* @param vmc VMC控制器实例
|
|
* @param x 足端x坐标输出
|
|
* @param y 足端y坐标输出
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);
|
|
|
|
/**
|
|
* @brief 获取等效摆动杆参数
|
|
* @param vmc VMC控制器实例
|
|
* @param length 等效摆动杆长度输出
|
|
* @param angle 等效摆动杆角度输出
|
|
* @param d_length 等效摆动杆长度变化率输出
|
|
* @param d_angle 等效摆动杆角速度输出
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);
|
|
|
|
/**
|
|
* @brief 获取关节输出力矩
|
|
* @param vmc VMC控制器实例
|
|
* @param tau_front 前髋关节力矩输出
|
|
* @param tau_rear 后髋关节力矩输出
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);
|
|
|
|
/**
|
|
* @brief 重置VMC控制器状态
|
|
* @param vmc VMC控制器实例
|
|
*/
|
|
void VMC_Reset(VMC_t *vmc);
|
|
|
|
/**
|
|
* @brief 设置虚拟力和力矩
|
|
* @param vmc VMC控制器实例
|
|
* @param F_virtual 虚拟支撑力 (N)
|
|
* @param T_virtual 虚拟摆动力矩 (N*m)
|
|
*/
|
|
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);
|
|
|
|
/**
|
|
* @brief 计算雅可比矩阵
|
|
* @param vmc VMC控制器实例
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t VMC_ComputeJacobian(VMC_t *vmc);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|