384 lines
11 KiB
C
384 lines
11 KiB
C
/*
|
||
* VMC虚拟模型控制器实现
|
||
*
|
||
* 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
|
||
* 主要功能包括:
|
||
* 1. 五连杆机构的正逆运动学解算
|
||
* 2. 虚拟力到关节力矩的映射
|
||
* 3. 地面接触检测
|
||
* 4. 等效摆动杆模型转换
|
||
*
|
||
* 参考文献:
|
||
* - 韭菜的菜 知乎: 平衡步兵控制系统设计
|
||
* - 上交轮腿电控开源方案 (2023)
|
||
*/
|
||
|
||
#include "vmc.h"
|
||
#include <string.h>
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define VMC_EPSILON (1e-6f) // 数值计算精度
|
||
#define VMC_MAX_ITER (10) // 最大迭代次数
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
|
||
/**
|
||
* @brief 限制数值范围
|
||
*/
|
||
#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
|
||
|
||
/**
|
||
* @brief 安全开方
|
||
*/
|
||
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
|
||
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
|
||
static int8_t VMC_ValidateParams(const VMC_Param_t *param);
|
||
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
|
||
static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
|
||
static float VMC_ComputeNumericDerivative(float current, float previous, float dt);
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 初始化VMC控制器
|
||
*/
|
||
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
|
||
if (vmc == NULL || param == NULL || sample_freq <= 0) {
|
||
return -1;
|
||
}
|
||
|
||
// 复制参数
|
||
memcpy(&vmc->param, param, sizeof(VMC_Param_t));
|
||
|
||
// 设置控制周期
|
||
vmc->dt = 1.0f / sample_freq;
|
||
|
||
// 重置状态
|
||
VMC_Reset(vmc);
|
||
|
||
vmc->initialized = true;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief VMC五连杆正解算
|
||
*
|
||
* 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
|
||
*
|
||
* 坐标系定义:
|
||
* - x轴: 机体前进方向为正
|
||
* - y轴: 竖直向下为正
|
||
* - 角度: 顺时针为正
|
||
*/
|
||
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return -1;
|
||
}
|
||
body_pitch = -body_pitch; // 机体俯仰角取反
|
||
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 保存历史值
|
||
leg->last_phi0 = leg->phi0;
|
||
leg->last_L0 = leg->L0;
|
||
leg->last_d_L0 = leg->d_L0;
|
||
leg->last_d_theta = leg->d_theta;
|
||
|
||
// 更新关节角度
|
||
leg->phi1 = phi1;
|
||
leg->phi4 = phi4;
|
||
|
||
// 更新运动学状态
|
||
VMC_UpdateKinematics(vmc, phi1, phi4);
|
||
|
||
// 求解闭环运动学
|
||
if (VMC_SolveClosedLoop(vmc) != 0) {
|
||
return -1;
|
||
}
|
||
|
||
// 计算足端坐标
|
||
leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
|
||
leg->foot_y = leg->YC;
|
||
|
||
// 计算等效摆动杆参数
|
||
leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
|
||
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);
|
||
|
||
// 角度归一化
|
||
VMC_ANGLE_NORMALIZE(leg->theta);
|
||
VMC_ANGLE_NORMALIZE(leg->phi0);
|
||
|
||
// 计算角速度和长度变化率
|
||
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
|
||
leg->d_alpha = -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->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
|
||
|
||
VMC_GroundContactDetection(vmc); // 更新地面接触状态
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief VMC五连杆逆解算(力矩分配)
|
||
*
|
||
* 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
|
||
*/
|
||
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return -1;
|
||
}
|
||
|
||
// 保存虚拟力和力矩
|
||
vmc->leg.F_virtual = F_virtual;
|
||
vmc->leg.T_virtual = -T_virtual;
|
||
|
||
// 计算雅可比矩阵
|
||
if (VMC_ComputeJacobian(vmc) != 0) {
|
||
return -1;
|
||
}
|
||
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 通过雅可比转置计算关节力矩
|
||
// tau = J^T * F_virtual
|
||
leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
|
||
leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 地面接触检测
|
||
*
|
||
* 基于虚拟力和腿部状态估计地面法向力
|
||
*/
|
||
float VMC_GroundContactDetection(VMC_t *vmc) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return 0.0f;
|
||
}
|
||
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 计算地面法向力
|
||
// Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
|
||
leg->Fn = leg->F_virtual * cosf(leg->theta) +
|
||
leg->T_virtual * sinf(leg->theta) / leg->L0 +
|
||
10.0f; // 添加轮子重力，假设轮子质量约1kg
|
||
|
||
// 地面接触判断
|
||
leg->is_ground_contact = (leg->Fn > 20.0f); // 10N阈值
|
||
|
||
return leg->Fn;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取足端位置
|
||
*/
|
||
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
|
||
if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
*x = vmc->leg.foot_x;
|
||
*y = vmc->leg.foot_y;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取等效摆动杆参数
|
||
*/
|
||
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return -1;
|
||
}
|
||
|
||
if (length) *length = vmc->leg.L0;
|
||
if (angle) *angle = vmc->leg.theta;
|
||
if (d_length) *d_length = vmc->leg.d_L0;
|
||
if (d_angle) *d_angle = vmc->leg.d_theta;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @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) {
|
||
return -1;
|
||
}
|
||
|
||
*tau_front = vmc->leg.tau_hip_front;
|
||
*tau_rear = vmc->leg.tau_hip_rear;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 重置VMC控制器状态
|
||
*/
|
||
void VMC_Reset(VMC_t *vmc) {
|
||
if (vmc == NULL) {
|
||
return;
|
||
}
|
||
|
||
// 清零腿部状态
|
||
memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
|
||
|
||
// 设置初始值
|
||
vmc->leg.L0 = 0.15f; // 默认腿长15cm
|
||
vmc->leg.theta = 0.0f;
|
||
}
|
||
|
||
/**
|
||
* @brief 设置虚拟力和力矩
|
||
*/
|
||
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return;
|
||
}
|
||
|
||
vmc->leg.F_virtual = F_virtual;
|
||
vmc->leg.T_virtual = T_virtual;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算雅可比矩阵
|
||
*
|
||
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
|
||
*/
|
||
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
|
||
if (vmc == NULL || !vmc->initialized) {
|
||
return -1;
|
||
}
|
||
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 检查分母不为零
|
||
float sin_diff = sinf(leg->phi3 - leg->phi2);
|
||
if (fabsf(sin_diff) < VMC_EPSILON) {
|
||
return -1; // 奇异配置
|
||
}
|
||
|
||
// 计算雅可比矩阵元素
|
||
// J11: 后髋关节到支撑力的雅可比
|
||
leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) *
|
||
sinf(leg->phi1 - leg->phi2)) / sin_diff;
|
||
|
||
// J12: 后髋关节到摆动力矩的雅可比
|
||
leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) *
|
||
sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
|
||
|
||
// J21: 前髋关节到支撑力的雅可比
|
||
leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) *
|
||
sinf(leg->phi3 - leg->phi4)) / sin_diff;
|
||
|
||
// J22: 前髋关节到摆动力矩的雅可比
|
||
leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) *
|
||
sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
|
||
|
||
return 0;
|
||
}
|
||
|
||
/* Private functions -------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 验证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) {
|
||
return -1;
|
||
}
|
||
|
||
// 检查腿部几何约束
|
||
if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
|
||
return -1; // 不满足闭环几何约束
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新基本运动学参数
|
||
*/
|
||
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 计算关键点坐标
|
||
// 点B (后髋关节末端)
|
||
leg->XB = vmc->param.leg_1 * cosf(phi1);
|
||
leg->YB = vmc->param.leg_1 * sinf(phi1);
|
||
|
||
// 点D (前髋关节末端)
|
||
leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
|
||
leg->YD = vmc->param.leg_4 * sinf(phi4);
|
||
|
||
// 计算BD连杆长度
|
||
float dx = leg->XD - leg->XB;
|
||
float dy = leg->YD - leg->YB;
|
||
leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
|
||
}
|
||
|
||
/**
|
||
* @brief 求解闭环运动学方程
|
||
*
|
||
* 根据两个髋关节角度，求解中间关节角度
|
||
*/
|
||
static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
|
||
VMC_Leg_t *leg = &vmc->leg;
|
||
|
||
// 使用余弦定理求解phi2
|
||
leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
|
||
leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
|
||
leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 +
|
||
leg->lBD * leg->lBD -
|
||
vmc->param.leg_3 * vmc->param.leg_3;
|
||
|
||
// 检查判别式
|
||
float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
|
||
if (discriminant < 0) {
|
||
return -1; // 无解
|
||
}
|
||
|
||
float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
|
||
|
||
// 计算phi2 (选择合适的解)
|
||
leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
|
||
|
||
// 计算phi3
|
||
leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
|
||
leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
|
||
|
||
// 计算足端坐标点C
|
||
leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
|
||
leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算数值微分
|
||
*/
|
||
static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
|
||
if (dt <= 0) {
|
||
return 0.0f;
|
||
}
|
||
|
||
return (current - previous) / dt;
|
||
}
|