引言

卡尔曼滤波器是Kalman于1960年提出的一种应用于离散线性滤波的迭代算法^[1],其目的是尽可能地减少测量噪声的影响,并从含有噪声的测量值中得到系统状态的最优估计。在小型无人机自动驾驶仪的设计中,受有效载荷重量和安装体积的限制,业界多采用微机电系统(MEMS)惯性测量单元来获取无人机的飞行姿态参数^{[2, 3, 4]}。同时,受MEMS传感器制造工艺的限制,由MEMS加速度计和陀螺仪组合而成的惯性测量系统不可避免地存在零漂误差,且测试数据极易受到机体振动的干扰。为此,本文着重对小型四轴飞行器纵摇和横摇姿态(分别对应俯仰角和横滚角)测量进行研究。

1 卡尔曼滤波器的构造

取姿态角α和陀螺仪输出的角速率偏差β为状态变量,构造系统的离散状态方程为:

式中,Δt为采样时间间隔,ω_k－1为k-1(k=1,2,…，n)时刻MEMS陀螺仪输出的角速率,w_g为陀螺仪输出的白噪声。将式(1)写为标准形式:

式中,X(k)为状态变量,A、B分别为系统矩阵和控制输入矩阵,U(k-1)为陀螺仪在k-1时刻输出的角速率,W为系统高斯白噪声。

为消除MEMS陀螺仪的零漂误差和积分误差,需利用MEMS加速度计测得的角度值Z(k)对姿态角进行校正,即所谓的数据融合。为此构造观测方程(又称量测方程):

式中w_a为MEMS加速度计输出的白噪声。将式(3)写为标准形式:

式中,H为量测矩阵,V(k)为量测高斯白噪声。由式(2)和式(4),结合经典的卡尔曼滤波理论可得到便于数值计算的迭代过程。首先由k-1时刻的最优估计值预测当前时刻的状态值:

式中,X(k－1|k－1)为k-1时刻系统的最优估计值,X(k|k－1)为k时刻的初步预测值。预测误差的协方差记作P(k|k-1),则有:

式中,P(k|k-1)为X(k|k-1)对应的协方差,P(k-1|k-1)为X(k-1|k-1)对应的协方差,Q为系统噪声协方差。式(5)和式(6)是对系统的预测。然后由初步预测值X(kk－1),结合当前时刻的测量值Z(k)即可得到当前时刻的最优估计值:

式中K_g(k)为卡尔曼增益,可表示为：

式中R为量测噪声协方差。为了让运算可以自动迭代下去,还需要更新当前时刻最优估计值的协方差:

根据式(5)～(9)就可以编写计算机程序完成卡尔曼滤波。

2 卡尔曼滤波实验与结果分析

如图 1所示,采用三轴MEMS陀螺仪L3G4200D,三轴MEMS加速度计MMA8451Q和16位Freescale单片机MC9S12XS128构成惯性姿态测量单元。其中,加速度计和陀螺仪通过IIC总线与单片机GPIO口的E2、E3引脚相连,经卡尔曼滤波后的数据通过Tx、Rx引脚发送到PC机。 软件流程图如图 2所示,流程包括传感器初始化、IIC数据采集、卡尔曼数据融合和串口数据发送等4个主要步骤。

惯性测量单元安装在JZJ-1型自准直仪转台上,通过单片机定时器设置采样间隔Δt为0.1 s,则式(5)～(9)进行迭代计算所需的系统矩阵A、控制输入矩阵B和量测矩阵H分别为:

系统变量X、预测误差协方差P、系统噪声协方差Q和量测噪声协方差R的初值分别为:

首先将JZJ-1转台的俯仰角保持在22°附近,采样时间为50 s左右。在此期间用橡皮锤对转台底座施加机械冲击载荷,测试曲线如图 3所示。由图 3可见,MEMS陀螺仪输出的俯仰角度值随着时间的推移明显偏离实际值,这是典型的零漂现象。在机械冲击载荷作用的时刻,由加速度值换算得到的角度值会明显偏离实际值。而经卡尔曼滤波后输出的角度值零漂现象不明显,且对外界冲击载荷的抗干扰能力较强。

当转台从-5°附近开始产生动态俯仰运动,在测试过程中对底座施加1次锤击,测试曲线如图 4所示。由图可见,在动态工况下,卡尔曼滤波后的动态角度曲线比单纯依靠加速度计测得的角度曲线更平滑且无偏移。此时,卡尔曼滤波器同样对机械冲击载荷有较好的抑制作用。

3 结 论

本文利用卡尔曼滤波器对MEMS陀螺仪和角速度计的输出信号进行了数据融合,较好地解决了零漂和机械振动干扰问题。其基本原理是利用MEMS陀螺仪的信号作为预测值,利用加速度计输出信号对其校正。所设计的卡尔曼滤波器可运行在16位飞思卡尔单片机上,受单片机浮点运算能力的限制,采样时间间隔设置为0.1 s。如何在32位ARM芯片上实现多通道卡尔曼滤波,并提高采样频率将是今后研究工作的重点。