✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信
🔥 内容介绍
针对两轮平台（自平衡车、两轮机器人）俯仰角估算中 “加速度计易受振动干扰、陀螺仪存在积分漂移” 的核心矛盾，提出一种 EKF（扩展卡尔曼滤波）与 Madgwick 滤波器融合的俯仰角估算方案。首先建立两轮平台动力学模型与传感器误差模型，明确俯仰角与角速度、加速度的映射关系；其次设计双滤波器融合架构：EKF 利用动力学模型实现姿态角最优估计，抑制陀螺仪漂移，Madgwick 滤波器通过梯度下降算法快速响应姿态变化，补偿 EKF 动态滞后；最后通过数据级融合策略（自适应权重分配）整合双滤波器输出，实现 “高精度 - 低延迟 - 强鲁棒性” 的俯仰角估算。实验基于自制两轮自平衡车平台，对比单一 EKF、单一 Madgwick 滤波器与融合算法，结果表明：融合算法的俯仰角估算误差降低 38.7%，漂移量控制在 0.2°/min 以内，动态响应延迟≤5ms，在正弦激励（频率 0.5~5Hz）与随机振动场景下鲁棒性显著优于单一滤波方法，为两轮平台的稳定控制提供可靠姿态感知支撑。
1 引言
1.1 研究背景与工程意义
两轮平台（如自平衡车、两轮配送机器人、移动检测设备）凭借结构紧凑、机动性强的优势，广泛应用于物流配送、室内巡检、教育科研等领域 [1]。俯仰角（绕横轴的倾斜角度）是两轮平台姿态控制的核心状态量，其估算精度直接决定平台的平衡稳定性与运动控制性能 [2]。例如，自平衡车需通过实时准确的俯仰角反馈，调整车轮转速抵消倾斜趋势；两轮机器人的路径跟踪控制也依赖高精度姿态角实现动态平衡与轨迹修正。
然而，两轮平台俯仰角估算面临两大核心挑战：① 传感器特性局限：加速度计易受平台振动、加减速运动的干扰，静态精度高但动态失真严重；陀螺仪能快速响应角速度变化，但存在零偏漂移，长期积分误差累积 [3]；② 平台动态干扰：两轮平台运动过程中（启动、加速、转向、颠簸路面行驶）产生的非线性动力学扰动，进一步加剧姿态角估算误差 [4]。传统单一传感器或单一滤波方法难以兼顾动态响应速度与长期估算精度，亟需构建多传感器融合的滤波方案。
1.2 研究现状与技术缺口
现有两轮平台俯仰角估算方法主要分为三类：① 单一滤波方法：EKF 通过建立系统动力学模型实现最优估计，但模型误差与过程噪声难以精准建模，动态响应滞后；Madgwick 滤波器基于梯度下降算法，计算量小、响应快，但抗干扰能力弱 [5]；② 传感器直接融合：将加速度计与陀螺仪数据加权平均，权重固定，无法适配平台动态变化 [6]；③ 多滤波器融合：如 EKF + 互补滤波，但融合机制简单，未充分利用两种滤波器的优势互补特性 [7]。
现有研究存在三大技术缺口：① 未针对两轮平台的非线性动力学特性优化滤波模型，模型失配导致估算误差增大；② 双滤波器融合多采用固定权重，无法根据平台运动状态（静态、动态、振动）自适应调整；③ 缺乏在复杂动态场景（如快速转向、颠簸路面）下的鲁棒性验证，工程实用性不足。
Image
Image
Image
Image
⛳️ 运行结果
Image
Image
📣 部分代码
%% Main Kalman alg
function [mu, Sigma] = kalman_alg( mu_0, Sigma_0, z, R, Q, t )
%KALMAN Kalman algorithm
% Takes initial guess mu_0, intial variance Sigma_0, measurements z,
% covariance matrices of process noise and measurement noise R and Q
% respectively and a time vector. Runs the Kalman algorithm and returns
% the mean of the state vector, mu.
% Initalize
N = size(z,2);
mu = zeros(2,N);
Sigma = zeros(2,2,N);
%Init with initial conditions
mu(1,1) = mu_0(1);
mu(2,1) = mu_0(2);
Sigma(:,:,1) = Sigma_0;
z = [zeros(3,1), z];
%Kalman alg
for i=2:N
deltaT = t(i) - t(i-1);
A = [1, deltaT;0, 1];
mu_bar = A*mu(:,i-1);
H = [0, 1;cos(mu_bar(1)), 0;-sin(mu_bar(1)), 0];
Sigma_bar = A*Sigma(:,:,i-1)*A' + R;
K=Sigma_bar*H'/(H*Sigma_bar*H'+Q);
mu(:,i) = mu_bar + K*nu(z(:,i), mu_bar);
Sigma(:,:,i) = (eye(2)-K*H)*Sigma_bar;
end
end
%% Innovation
function nu = nu(z, x)
%Calculates the innovation of the measurement z and mean x
nu = z - [x(2) sin(x(1)) cos(x(1))]';
end
🔗 参考文献
🎈 部分理论引用网络文献，若有侵权联系博主删除
