针对电液伺服动态疲劳试验机的实际受力计算需求(需扣除夹具/传感器等非试样部件的惯性力),以下提供基于多体动力学建模的C代码实现方案,严格区分试样受力与系统寄生力:
物理模型与公式
实际试样受力 = 应变传感器总力 - 附加质量惯性力
F_specimen = F_total - (m_fixture + m_grip + m_adapter)
a
其中:
-
m_fixture
:夹具连接件总质量(已预先标定) -
m_grip
:夹头质量+试样质量(试样如参与计算需实测填写+已选夹头预标定质量) -
m_adapter
:传感器适配器质量 -
a
:加速度计实测值(需方向对齐)
代码架构(带动态标定功能)
#include <stdint.h>
#include <math.h>
// 系统参数配置
#define SAMPLE_RATE 2000 // 2kHz控制频率
#define MASS_FIXTURE 2.7 // 夹具质量(kg) 需实际标定
#define MASS_GRIP 1.2 // 夹头质量(kg)
#define MASS_ADAPTER 0.8 // 适配器质量(kg)
#define GRAVITY 9.80665 // 标准重力
// 传感器校准结构体
typedef struct {
float strain_offset; // 应变传感器零点偏移
float strain_sensitivity; // 应变传感器灵敏度(N/count)
float accel_sensitivity; // 加速度计灵敏度(g/count)
float accel_offset[3]; // 三轴加速度计零点
} CalibrationParams;
// 实时数据容器
typedef struct {
int32_t raw_strain; // 应变传感器原始AD值
int32_t raw_accel[3]; // 三轴加速度原始AD值
float acceleration; // 有效轴向加速度(m/s²)
float force_total; // 传感器测量总力(N)
float force_specimen; // 试样实际受力(N)
} DynamicForceData;
// 全局参数
static CalibrationParams calib;
static const float parasitic_mass = MASS_FIXTURE + MASS_GRIP + MASS_ADAPTER;
// IIR低通滤波器实现(截止频率50Hz)
typedef struct {
float coeff_b[3];
float coeff_a[2];
float state_x[2];
float state_y[2];
} IIRFilter;
float iir_filter_process(IIRFilter* filter, float input) {
float output = filter->coeff_b[0] * input +
filter->coeff_b[1] * filter->state_x[0] +
filter->coeff_b[2] * filter->state_x[1] -
filter->coeff_a[0] * filter->state_y[0] -
filter->coeff_a[1] * filter->state_y[1];
// 更新状态
filter->state_x[1] = filter->state_x[0];
filter->state_x[0] = input;
filter->state_y[1] = filter->state_y[0];
filter->state_y[0] = output;
return output;
}
// 加速度矢量合成(根据试验机运动方向)
float calculate_effective_accel(int32_t accel_raw[3]) {
// 假设运动方向为X轴(根据实际安装调整)
float accel_x = (accel_raw[0] - calib.accel_offset[0]) * calib.accel_sensitivity * GRAVITY;
// 可选:多轴合成(复杂运动情况)
// float accel_y = ... ;
// float accel_z = ... ;
// return sqrt(accel_x*accel_x + accel_y*accel_y + accel_z*accel_z);
return accel_x;
}
// 动态力计算核心函数
void compute_specimen_force(DynamicForceData* data) {
// 1. 应变传感器处理
data->force_total = (data->raw_strain - calib.strain_offset) * calib.strain_sensitivity;
// 2. 加速度处理
data->acceleration = calculate_effective_accel(data->raw_accel);
// 3. 去除寄生质量惯性力
data->force_specimen = data->force_total - parasitic_mass * data->acceleration;
}
// 自动标定函数(需在无试样状态下运行)
void calibrate_parasitic_mass() {
IIRFilter accel_filter = { /* 初始化滤波器系数 */ };
IIRFilter force_filter = { /* 初始化滤波器系数 */ };
float sum_force = 0, sum_accel = 0;
const uint16_t num_samples = 500; // 采集500个点
for(int i=0; i<num_samples; i++) {
DynamicForceData data;
data.raw_strain = read_strain_sensor();
data.raw_accel[0] = read_accel_x();
// 滤波处理
float f = iir_filter_process(&force_filter,
(data.raw_strain - calib.strain_offset) * calib.strain_sensitivity);
float a = iir_filter_process(&accel_filter,
(data.raw_accel[0] - calib.accel_offset[0]) * calib.accel_sensitivity * GRAVITY);
sum_force += f;
sum_accel += a;
}
// 计算等效寄生质量(F = m*a → m = F/a)
float avg_force = sum_force / num_samples;
float avg_accel = sum_accel / num_samples;
float calibrated_mass = fabs(avg_force / avg_accel);
// 更新全局参数
parasitic_mass = calibrated_mass; // 需持久化存储
}
// 中断服务例程(实时控制)
void __attribute__((interrupt)) Timer_IRQHandler() {
static DynamicForceData current_data;
// 1. 读取传感器原始数据
current_data.raw_strain = ADC_Read(STRAIN_CH);
current_data.raw_accel[0] = ADC_Read(ACCEL_X_CH);
// ... 其他轴数据
// 2. 执行核心计算
compute_specimen_force(¤t_data);
// 3. 安全检测
if(fabs(current_data.force_specimen) > SAFETY_LIMIT) {
emergency_shutdown();
}
// 4. 伺服控制输出
servo_control(current_data.force_specimen);
}
// 主控制循环
int main() {
// 初始化硬件
adc_init();
load_calibration(&calib);
servo_init();
// 启动前自动标定
calibrate_parasitic_mass();
while(1) {
// 后台任务(通信、日志等)
send_telemetry_data();
}
}
关键增强功能
-
1动态质量标定(提高系统适应性):
void adaptive_mass_calibration(float force, float accel) {
static float sum_force = 0, sum_accel = 0;
static uint16_t count = 0;
// 仅在加速度超过阈值时采样(确保信噪比)
if(fabs(accel) > 0.2*GRAVITY) {
sum_force += force;
sum_accel += accel;
count++;
if(count >= 1000) {
float new_mass = sum_force / sum_accel;
parasitic_mass = 0.9*parasitic_mass + 0.1*new_mass; // 平滑更新
sum_force = sum_accel = count = 0;
}
}
}
-
2.运动方向补偿(应对多轴振动):
float vectorial_compensation(int32_t accel_raw[3]) {
// 转换到试验机坐标系
float accel_x = (accel_raw[0]-calib.accel_offset[0])*calib.accel_sensitivity;
float accel_y = (accel_raw[1]-calib.accel_offset[1])*calib.accel_sensitivity;
float accel_z = (accel_raw[2]-calib.accel_offset[2])*calib.accel_sensitivity;
// 根据夹具运动学模型合成有效加速度
return (0.92*accel_x + 0.15*accel_y - 0.35*accel_z) * GRAVITY; // 系数需实测标定
}
-
3温度漂移补偿:
void temperature_compensation(float temp) {
// 应变传感器温漂模型(需根据标定数据确定)
float temp_effect = 0.005 * (temp - 25.0) - 0.0001 * pow(temp-25.0, 2);
calib.strain_offset += temp_effect * calib.strain_sensitivity;
// 加速度计温漂补偿
calib.accel_offset[0] *= (1.0 + 0.0002*(temp - 25.0));
}
(实时嵌入式系统)
#include <stdint.h>
#include <math.h>
#include <stdbool.h>
// 硬件参数配置
#define SAMPLE_RATE 2000 // 2kHz采样率
#define FILTER_CUTOFF 50.0 // 低通截止频率(Hz)
#define SENSOR_RANGE_STRAIN 10000 // 应变传感器量程±10kN
#define SENSOR_RANGE_ACCEL 20 // 加速度计量程±20g
// 传感器校准结构体
typedef struct {
float strain_offset; // 应变传感器零点偏移
float accel_offset[3]; // 三轴加速度计零点偏移(XYZ)
float sensitivity_strain; // 应变传感器灵敏度(N/count)
float sensitivity_accel; // 加速度计灵敏度(g/count)
float specimen_mass; // 试样等效质量(kg)
} SensorCalibration;
// 实时数据容器
typedef struct {
int32_t raw_strain; // 原始应变AD值
int32_t raw_accel[3]; // 三轴加速度AD值
float filtered_strain; // 滤波后应变力
float accel_axis; // 有效加速度轴(根据安装方向)
float f_actual; // 补偿后实际力
} ForceData;
// 全局校准参数
static SensorCalibration calib;
// 低通滤波器实现(二阶Butterworth)
float low_pass_filter(float input, float* state, float* coeffs) {
float output = coeffs[0]*input + coeffs[1]*state[0] + coeffs[2]*state[1]
- coeffs[3]*state[2] - coeffs[4]*state[3];
// 更新状态变量
state[1] = state[0];
state[0] = input;
state[3] = state[2];
state[2] = output;
return output;
}
// 动态力计算核心函数
void compute_actual_force(ForceData* data) {
// 1. 应变传感器处理
float f_strain = (data->raw_strain - calib.strain_offset) * calib.sensitivity_strain;
// 2. 加速度计处理(选择有效轴)
float acceleration = (data->raw_accel[0] - calib.accel_offset[0]) * calib.sensitivity_accel * 9.81; // 转换为m/s²
// 3. 惯性力补偿计算
float f_inertial = calib.specimen_mass * acceleration;
// 4. 实际受力计算
data->f_actual = f_strain - f_inertial;
}
// 中断服务例程(定时采样)
void __attribute__((interrupt)) ADC_IRQHandler() {
static ForceData current_data;
// 读取ADC值(假设已配置DMA)
current_data.raw_strain = ADC_GetValue(ADC_CH_STRAIN);
current_data.raw_accel[0] = ADC_GetValue(ADC_CH_ACCEL_X);
// ...其他轴同理
// 执行滤波和计算
static float strain_filter_state[4] = {0};
static float accel_filter_state[4] = {0};
const float lp_coeffs[5] = {0.0201, 0.0402, 0.0201, -1.561, 0.6414}; // 50Hz截止
current_data.filtered_strain = low_pass_filter(
(current_data.raw_strain - calib.strain_offset) * calib.sensitivity_strain,
strain_filter_state,
lp_coeffs
);
float filtered_accel = low_pass_filter(
(current_data.raw_accel[0] - calib.accel_offset[0]) * calib.sensitivity_accel,
accel_filter_state,
lp_coeffs
);
compute_actual_force(¤t_data);
// 触发控制闭环
servo_control_update(current_data.f_actual);
}
// 伺服阀控制函数
void servo_control_update(float f_actual) {
static float integral_error = 0;
float setpoint = get_force_setpoint(); // 从上位机获取目标力
float error = setpoint - f_actual;
// PID算法(需根据系统响应调参)
float Kp = 0.12, Ki = 0.003, Kd = 0.05;
integral_error += error * (1.0/SAMPLE_RATE);
float derivative = (error - prev_error) * SAMPLE_RATE;
prev_error = error;
float output = Kp*error + Ki*integral_error + Kd*derivative;
// 驱动伺服阀(PWM或电流输出)
set_servo_valve(output);
}
// 主初始化函数
int main() {
// 加载校准参数
load_calibration(&calib);
// 硬件初始化
adc_init();
servo_valve_init();
enable_interrupts();
while(1) {
// 低优先级任务(如通信、状态监控)
send_telemetry_data();
check_safety_limits();
}
}
工程实施要点
-
机械参数标定:
-
空载状态下运行正弦扫频,通过
F_total = m_parasitic * a
关系标定寄生质量 -
使用高精度质量块进行静态验证
-
-
信号同步处理:
-
采用FPGA实现传感器数据的硬件级同步采样
-
对时延敏感路径使用RTOS(如FreeRTOS)确保实时性
误差分析:
void error_analysis() { // 主要误差源: // 1. 加速度计非线性误差(<0.1% FSR) // 2. 应变传感器蠕变(需定期零点校准) // 3. 机械连接间隙引起的额外惯性冲击 // 4. 多体耦合振动(需有限元模态分析) }
安全监控:
-
void safety_monitor(DynamicForceData* data) {
// 力传感器过载保护
if(fabs(data->force_total) > SENSOR_RANGE * 1.1) {
emergency_stop(OVERRUN_FAULT);
}
// 加速度冲击保护
if(fabs(data->acceleration) > MAX_ACCEL * GRAVITY) {
emergency_stop(IMPACT_FAULT);
}
// 试样力突变检测(防止断裂失控)
static float prev_force = 0;
float delta = fabs(data->force_specimen - prev_force);
if(delta > MAX_DELTA_FORCE) {
emergency_stop(FRACTURE_DETECTED);
}
prev_force = data->force_specimen;
}
关键算法优化
-
动态质量标定(防止试样质量变化引入误差):
void dynamic_mass_calibration(ForceData* data) {
// 在静态条件下(a=0)自动更新质量参数
static int calibration_counter = 0;
static float sum_f_strain = 0;
if(is_static_condition()) { // 通过加速度计判断静止
sum_f_strain += data->filtered_strain;
calibration_counter++;
if(calibration_counter >= 100) { // 采集100个点平均
calib.specimen_mass = sum_f_strain / (calibration_counter * 9.81);
sum_f_strain = 0;
calibration_counter = 0;
}
}
}
-
多轴加速度合成(针对复杂运动情况):
float vectorial_accel_compensation(int32_t accel_raw[3]) {
float ax = (accel_raw[0]-calib.accel_offset[0])*calib.sensitivity_accel;
float ay = (accel_raw[1]-calib.accel_offset[1])*calib.sensitivity_accel;
float az = (accel_raw[2]-calib.accel_offset[2])*calib.sensitivity_accel;
// 根据试验机运动学模型合成有效加速度
return sqrt(ax*ax + ay*ay + (az-1.0)*9.81); // 扣除重力基准
}
-
过载保护机制:
void check_safety_limits() {
static ForceData data;
if(fabs(data.f_actual) > SENSOR_RANGE_STRAIN * 1.2) {
emergency_shutdown();
log_error("Force overload: %.1f N", data.f_actual);
}
if(fabs(data.accel_axis) > SENSOR_RANGE_ACCEL * 0.9) {
emergency_shutdown();
log_error("Acceleration overload: %.1f g", data.accel_axis);
}
}
工程实施要点
-
传感器同步采样:
使用ADC的同步采样模式或FPGA触发,确保应变与加速度数据时间对齐 -
非线性补偿:
增加应变传感器的温度漂移补偿算法:float temperature_compensation(float raw, float temp) { return raw - (0.0021*temp*temp + 0.15*temp); // 多项式拟合结果 }
-
实时性能优化:
-
采用定点数运算加速(Q格式)
-
使用ARM CMSIS-DSP库优化滤波计算
-
分配计算任务到不同优先级中断
-
完整系统需配合以下硬件模块:
-
24位高精度ADC(如ADS131M04)
-
电液伺服阀驱动器(PWM/4-20mA输出)
-
CAN/EtherCAT工业通信接口
-
选择低噪声加速度计(如Kistler 8692C)/ 三轴MEMS加速度计(如ADXL355)
-
采用六线制应变传感器连接法消除导线电阻影响
-
为高速ADC配置独立的电源和地平面
完整工程需根据具体试验机型号调整运动学模型参数,建议通过有限元分析验证多体动力学补偿效果。如需定制开发或获取针对特定型号的优化代码,请提供详细机械结构参数。