当前位置：首页 > news >正文

单连杆倾角估计：互补滤波器的 MATLAB 仿真实现

news 来源：原创 2025/6/13 7:20:03

一、问题背景与目标

在倾角 θ 随时间变化的斜面上，通过惯性传感器测量 y’轴加速度和 z’轴角速度，需要设计互补滤波器融合两类传感器数据，实现对倾角 θ 的准确估计。惯性传感器中：

加速度计易受运动加速度干扰，但长期漂移小
陀螺仪短期精度高，但存在零偏和积分漂移

二、互补滤波器原理

互补滤波器通过权重分配融合两类传感器数据：

加速度计角度估计：利用重力分量计算 θ_acc = -arcsin(a_y/g)
陀螺仪角度估计：通过角速度积分 θ_gyro = θ_prev + ω·dt
融合公式：θ_est = α·θ_gyro + (1-α)·θ_acc
- α为滤波系数（本文取 0.98），体现对陀螺仪的信任度

三、MATLAB 仿真实现

%% 单连杆倾角估计互补滤波仿真
clear; clc; close all;%% 参数设置
g = 9.81;         % 重力加速度(m/s^2)
dt = 0.01;        % 采样时间(s)
T = 10;           % 总时长(s)
t = 0:dt:T;       % 时间向量(1001个点)
alpha = 0.98;     % 滤波系数，更信任陀螺仪短期稳定性%% 生成真实倾角信号
theta_true = sin(0.5*t);  % 周期为4π的正弦波%% 传感器模拟
% 陀螺仪信号（含噪声和零偏）
gyro_bias = 0.05;         % 零偏(rad/s)
gyro_noise = 0.01;        % 噪声标准差
omega_true = gradient(theta_true, dt);  % 真实角速度
omega_meas = omega_true + gyro_bias + gyro_noise*randn(size(t));% 加速度计信号（含运动加速度和噪声）
accel_noise = 0.1;        % 噪声标准差(m/s^2)
a_motion = 0.3*cos(2*t);  % 假设的运动加速度分量
a_y = -g*sin(theta_true) + a_motion + accel_noise*randn(size(t));%% 互补滤波实现
theta_est = zeros(size(t));      % 初始化角度估计
theta_acc = -asin(a_y/g);        % 加速度计角度一次性计算for k = 2:length(t)% 陀螺仪角度递推：前一时刻角度 + 角速度积分theta_gyro = theta_est(k-1) + omega_meas(k-1)*dt;% 互补滤波融合theta_est(k) = alpha*theta_gyro + (1-alpha)*theta_acc(k);
end%% 结果可视化
figure;
subplot(2,1,1);
plot(t, theta_true, 'b', t, theta_est, 'r--');
legend('真实角度','估计角度');
xlabel('时间(s)'); ylabel('角度(rad)'); title('角度估计结果');
grid on;subplot(2,1,2);
plot(t, theta_est - theta_true);
xlabel('时间(s)'); ylabel('误差(rad)'); title('估计误差');
grid on;%% 性能指标计算
RMSE = sqrt(mean((theta_est - theta_true).^2));
fprintf('均方根误差(RMSE): %.4f rad\n', RMSE);

四、关键模块解析

传感器噪声建模：
- 陀螺仪：添加固定零偏gyro_bias和高斯噪声gyro_noise
- 加速度计：包含运动加速度a_motion和测量噪声accel_noise
互补滤波核心逻辑：
- 加速度计提供长期稳定的角度基准，但易受运动干扰
- 陀螺仪通过积分提供连续角度，但存在累积误差
- alpha=0.98表明算法更依赖陀螺仪的短期精度，加速度计用于周期性校准
性能评估指标：
- 均方根误差 (RMSE) 量化估计精度，本文仿真结果约为 0.02 rad