【姿态估计】IMU测量的空中机器人系统姿态估计显式互补滤波器附Matlab实现

原创于 2025-10-02 11:24:21 发布 · 900 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #matlab #开发语言

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

在空中机器人（如多旋翼无人机、无人直升机）的飞行控制中，姿态估计是保障稳定飞行的核心环节 —— 需实时获取机器人在三维空间中的滚转角（Roll）、俯仰角（Pitch）与偏航角（Yaw），为控制指令生成提供精准的状态反馈。惯性测量单元（Inertial Measurement Unit, IMU）作为常用传感器，可输出角速度（陀螺仪）与线加速度（加速度计）数据，但单一传感器存在固有缺陷：陀螺仪易受漂移误差累积影响，长期精度差；加速度计受运动加速度干扰，短期动态响应弱。

显式互补滤波器（Explicit Complementary Filter, ECF）通过频率域互补融合策略，充分发挥陀螺仪与加速度计的优势：在高频段依赖陀螺仪的快速动态响应抑制噪声，在低频段利用加速度计（结合重力向量特性）校正陀螺仪漂移，最终实现高精度、低延迟的姿态估计，且因算法复杂度低、实时性强，特别适用于算力有限的空中机器人嵌入式系统。

一、IMU 传感器信号特性与互补滤波核心逻辑

1. IMU 测量信号的频率特性

陀螺仪（Gyroscope）：输出绕 x/y/z 轴的角速度（单位：rad/s），优点是动态响应快（高频特性好），能捕捉空中机器人快速姿态变化（如突发转向、机动飞行）；缺点是存在零偏漂移（Zero Drift），误差随时间累积（低频段误差显著），例如长期静置时角度估计偏差会持续增大。

加速度计（Accelerometer）：输出 x/y/z 轴的比力（单位：m/s²，含重力与运动加速度），优点是无累积误差（低频稳定性好）—— 当空中机器人匀速飞行或悬停时，比力向量近似等于重力向量（可通过重力方向反推姿态角）；缺点是对运动加速度敏感（高频段噪声大），例如加速上升或俯冲时，测量值会严重偏离重力向量，导致姿态估计失真。

2. 显式互补滤波的核心思想

互补滤波的本质是在频率域对两种传感器信号进行 “分工” 与 “融合”：

设定一个截止频率（Cut-off Frequency），将姿态估计的频率范围划分为高频段与低频段；

高频段：采用陀螺仪的角速度积分结果更新姿态，利用其动态响应优势跟踪快速姿态变化；

低频段：采用加速度计解算的姿态结果校正陀螺仪积分的累积误差，利用其无漂移特性保障长期精度；

“显式” 的含义：通过明确的数学公式（如传递函数、时域迭代方程）定义高频与低频信号的融合权重，而非依赖自适应调整（区别于自适应互补滤波器），算法逻辑更清晰，实时计算量更小。

二、显式互补滤波器的数学模型与姿态解算

空中机器人的姿态通常用四元数（Quaternion）或欧拉角（Euler Angle）描述。考虑到欧拉角存在 “万向锁” 问题，实际工程中多采用四元数表示姿态，显式互补滤波的核心流程围绕 “四元数更新 - 误差校正” 展开。

⛳️ 运行结果

📣 部分代码

close all

% clear all

clc

% Load dataset

load("all_data_in_ws.mat")

% Extract real data IMU sensor

acc_IMU = ts_imu_acc.Data; % accelerometer measurements

gyro_IMU = ts_imu_gyro.Data; % gyroscope measurements

mag_IMU = ts_imu_mag.Data; % magnetometer measurements

numsamples = size(acc_IMU,1); % total number of samples

fs = 100;

% Convert data from imu frame to wand frame

% Compute Cimu^wand matrix:

% define IMU mounting:

% - the IMU X axis is aligned with -Y wand (vicon) axis

% - the IMU Y axis is aligned with -X wand (vicon) axis

% thus the rotation from IMU axis to wand_axis is :

% first Rz(90), then Ry(180)

acc_IMU_wand = zeros(numsamples,3);

gyro_IMU_wand = zeros(numsamples,3);

mag_IMU_wand = zeros(numsamples,3);

for i = 1 : size(acc_IMU,1)

acc_IMU_wand(i,:) = (r_imu_mount * (acc_IMU(i,:)'))';

gyro_IMU_wand(i,:) = (r_imu_mount * (gyro_IMU(i,:)'))';

mag_IMU_wand(i,:) = (r_imu_mount * (mag_IMU(i,:)'))';

end

for i = 1 : size(mag_IMU,1)

q_in = quaternion(ts_imu_quat.Data(i,[4 1 2 3]));

C_IMU_imu = rotmat(q_in,'frame'); %this rot mat transforms imu to IMU

mag_fromimutoIMU(i,:) = (C_IMU_imu' * mag_IMU(i,:)')';

eul_out(i,:) = rad2deg(rotm2eul(C_IMU_imu','ZYX'));

q_in = quaternion(ts_vicon_quat.Data(i,[4 1 2 3]));

C_VICON_wand = rotmat(q_in,'frame'); % this rot mat transforms wand to VICON

mag_fromwandtoVICON(i,:) = (C_VICON_wand' * mag_IMU_wand(i,:)')';

end

real_att_IMUimu = eul_out';

% magnetic field vector in IMU frame

mag_IMU_forfilter = mean(mag_fromimutoIMU);

% magnetic field vectori in VICON frame

mag_VICON_forfilter = mean(mag_fromwandtoVICON);

%% Create a new dataset containing only the measurements of interest:

real_data_IMU = [];

% % IMU measurements in imu frame

real_data_IMU.acc_imu = acc_IMU;

real_data_IMU.gyro_imu = gyro_IMU;

real_data_IMU.mag_imu = mag_IMU;

% IMU measurements in wand frame

real_data_IMU.acc_wand = acc_IMU_wand;

real_data_IMU.gyro_wand = gyro_IMU_wand;

real_data_IMU.mag_wand = mag_IMU_wand;

% Magnetic field measurements in body frame (imu)

real_data_IMU.mag_body = mag_IMU;

% Magnetic field measurements in body frame rotated in IMU frame

real_data_IMU.mag_fromimutoIMU = mag_fromimutoIMU;

real_data_IMU.numSamples = numsamples; % total samples measurements

real_data_IMU.mag_VICON = mag_VICON_forfilter'; % Mean magnetic field in VICON frame

real_data_IMU.mag_IMU = mag_IMU_forfilter'; % Mean magnetic field in IMU frame

real_data_IMU.C_IMU_VICON = C_IMU_VICON;

C_imu_VICON = C_IMU_VICON * C_IMU_imu';

real_data_IMU.C_imu_VICON = C_imu_VICON;

save('real_data.mat','real_data_IMU');

%% Plot

plot_real_data = [];

t = (0:(numsamples-1))/fs; % time when measurements are provided

plot_real_data.time = t;

plot_real_data.real_att_IMUimu = real_att_IMUimu;

plot_real_data.ts = ts_VICON_wand.Time;

plot_real_data.real_att_VICONwand = (ts_VICON_wand.Data*180/pi)';

save('plot_real_data','plot_real_data')

🔗 参考文献

Minh-Duc Hua, Guillaume Ducard, Tarek Hamel, Robert Mahony. Introduction to Nonlinear Attitude Estimation for Aerial Robotic Systems. Aerospace Lab, 2014, pp.AL08-04. 10.12762/2014.AL08-04 .hal-01113138