Kalman滤波在MEMS陀螺仪测量船舶回转率中的应用*

2021-03-26宰德广陈永冰李文魁

传感器与微系统 2021年3期

宰德广, 周岗, 陈永冰, 李文魁

(海军工程大学导航工程系,湖北武汉 430033)

0 引言

目前微机电系统(micro-electro-mechanical system,MEMS)陀螺仪的应用越来越普遍,关于如何提高陀螺仪的测量精度也越来越引起人们的关注。通常是对陀螺仪的随机噪声进行建模分析,比如利用时间序列分析的方法对噪声建立ARMA模型,获得相应的参数并设计Kalman滤波器来滤除噪声[1～3]。但对于低成本MEMS陀螺仪来说,造成其误差的因素复杂,随机性较大,很难建立准确的、具有普遍适用性的数学模型[4,5]。实验室现有一船舶回转运动模拟测量系统,其工作原理是通过驱动马达转动来模拟船舶回转运动,同时采用MEMS角速率陀螺仪敏感转台转动测得角速率(即实验室条件下模拟出的船舶回转率),但测量结果不稳定,数据波动较大,需要对测量数据进行滤波处理,以获得较为稳定的输出,实现进一步的船舶操纵控制。

本文从实际应用考虑,对测量数据直接进行滤波处理。

首先通过平均滤波和校正零偏的方法对原始采集信号进行预处理,之后设计了Kalman滤波器对信号进一步滤波,并借助船舶回转运动模拟测量系统,分别在静态、稳定回转率和回转率发生改变的情况下验证Kalman滤波器的效果,针对其动态响应较差的问题,在原有滤波器的基础上改进并设计了量测噪声方差自适应的Kalman 滤波器[6],同样情况下对其验证,以检验所设计的滤波器是否有效可行。

1 数据预处理

本文采用ADXRS系列的MEMS角速率陀螺仪,通过改变船舶回转运动模拟测量系统中自动操舵仪的舵角,即可改变回转率,使MEMS陀螺仪工作在不同状态。为保证测量准确性,每次测量前先通电工作一段时间,待稳定后再采集所需数据,而采集到的原始数据需要先经过平均滤波和校正零偏等预处理,以便于更好地进行Kalman滤波处理。

1.1 平均滤波去除野值

实验发现采集到的信号中往往存在一些数值大小异常的野值,在进行Kalman 滤波前需要对原始的采集信号预处理,剔除野值。

其中求均值公式如下

(1)

1.2 零偏校正

观察还发现,MEMS陀螺仪测量数据存在常值漂移的现象,可通过校正零偏的方法补偿陀螺仪漂移。将系统回转率设定为零,使MEMS陀螺仪工作在零输入状态。通过采集多组静态数据,并通过求均值的方法获得偏移值,此即陀螺仪的零偏,在之后的数据采集和处理中,都需要减去此零偏值以完成零偏校正。

2 Kalman滤波器设计

因为MEMS陀螺仪测得的是转台转动的角速率,故以角速率为状态量,以实际测量值为观测量,且系统噪声W和量测噪声V均为零均值高斯白噪声,系统噪声、量测噪声和被估计的角速率状态量三者互不相关,则有如下离散Kalman滤波状态方程和量测方程

(2)

式中X(k)为被估计的角速率状态量,Z(k)为角速率量测量,W(k)为系统噪声,其协方差矩阵为Q,V(k)为量测噪声,其协方差矩阵为R,A，B，H分别为状态转移矩阵、系统噪声驱动阵和量测阵。

Kalman滤波方程实质为递推公式,可分为2个预测方程和3个更新方程,如式(3)所示

(3)

设系统为一维状态系统,被估计状态量只有角速率,A,B,H分别取为单位阵,实验中只要设定滤波估计初值XKF(0)和协方差阵初值P(0),并给出系统噪声阵Q和量测噪声阵R,即可进行方程的递推和更新,实现Kalman滤波。

3 滤波器性能检验

对于固定的测量系统其系统噪声较为稳定,由经验值设定系统噪声阵Q为0.01。实际测得的角速率即为量测量Z(k),其协方差作为量测噪声阵R,其均方差的10倍设为协方差阵初值P(0)。设定好滤波器参数后,即可通过船舶回转运动模拟测量系统设定静止、恒定回转率和回转率变化三种状态,对设计的Kalman 滤波器进行性能检验。

3.1 静态测试

由上所述可知,MEMS陀螺仪需通电工作一段时间再进行数据的采集。现设定船舶回转运动模拟测量系统的回转率为零,使MEMS陀螺仪处于静止状态,采集10 min原始数据,并对其进行数据预处理和Kalman滤波处理,结果如图1(a)～(c)所示。

图1 静态数据滤波处理前后对比

从图1的对比图可看出,原始测量值存在个别野值,且整体数据出现偏移,而经过预处理之后的数据零偏现象明显改善,野值信号被滤除,再次经过Kalman滤波处理后,信号降噪效果明显,数据波动很小。计算每次数据处理后的误差均值和误差标准差,结果如表1所示。

表1 静态测试时数据统计特性

由表1可看出,经过数据预处理后的信号其误差均值和标准差都有所降低,再次经过Kalman滤波处理后更是降低一个量级之多,实验表明Kalman滤波器在静态测试时滤波效果显著。

3.2 恒定回转率测试

设定船舶回转运动模拟测量系统的回转率为30°/min,采集10 min稳定数据进行Kalman 滤波处理,结果如图2所示,计算可知,经Kalman滤波后数据误差均值由-3.776°/min降至0.243°/min,而误差标准差由2.344°/min降至0.259°/min,可以看出,在回转率恒定情况下,设计的Kalman滤波器依然具有很好的滤波效果。

图2 恒定回转率时Kalman滤波前后对比

3.3 回转率变化测试

设定船舶回转运动模拟测量系统开始时处于零回转率状态,在100 s时刻改变舵角,使得回转率发生变化,采集这一时间段内的测量数据,进行Kalman 滤波处理得到结果如图3所示。为便于对比突出Kalman 动态滤波效果,只给出了经过预处理之后的数据曲线和Kalman滤波曲线与真值的对比图。

图3 回转率变化时的Kalman滤波前后对比

从图中可看出当船舶回转率发生改变时,Kalman滤波动态性能变差,出现滞后现象,所设计的Kalman滤波器不再适用,必须对此滤波器进行改进,提高其动态响应性能。

4 改进后的自适应Kalman滤波器

关于如何提高Kalman滤波器的动态性能,很多学者提出了各种自适应Kalman滤波算法,比如过程噪声自适应、最小二乘法估计、设定指数渐消因子、加权Sage-Husa自适应滤波等[7～10]。可知Kalman滤波器中系统噪声阵Q和量测噪声阵R与滤波性能紧密相关,一般Q越大R越小系统动态性能越好,相反则稳态滤波效果越好。对于确定的测量系统其系统噪声较为稳定,而角速率测量值很大程度上受量测噪声影响,故可采用固定Q阵,改变R值的方法,实现自适应滤波。参考文献[6],可针对本实验设计量测噪声阵R自适应调节的Kalman滤波方法,具体算法流程如图4所示。

图4 自适应Kalman滤波算法流程

可知Kalman滤波方程可分为预测方程与更新方程,实现自适应的目的就是根据量测噪声变化趋势自适应改变滤波增益,以调节下一时刻的预测值和滤波估计值,而改变量测噪声阵即可达到改变滤波增益的目的。本算法思想是对每一时刻的状态预测值Xpre(k)与上一时刻的值Xpre(k-1)作差并取绝对值,根据差值大小判断状态量是否发生机动性变化,以作出相应地调整。如图4所示,d为设定的基准值,若两个时刻的预测值差值大于此基准值,则说明角速率发生改变,需要提高滤波的动态性能,R则需要相应减小,即Rg=αLR(0<αL<1)；若两个时刻的预测值差值小于此基准值,则说明角速率变化稳定,需要提高稳态滤波效果,使滤波更加稳定,R则需要相应增大,即Rg=αHR(αH>1)。量测噪声Rg更新后,滤波增益Kg也随之更新,即可实现Kalman滤波方程自适应调整,原来的滤波方程改进如下

(4)

设置实验参数时将系统噪声阵Q适当减小,量测噪声阵R适当增大,以便之后对R自适应调整。在实验过程中基准值d和调节因子αL,αH都需要根据实际情况灵活调整,以适应该滤波模型。经多次实验选定d=2,αL=0.05,αH=20。

首先对自适应Kalman滤波器进行同样的静态和恒定回转率测试,滤波后数据误差均值和误差标准差如表2所示,可以看出设计的自适应Kalman滤波器在静态和恒定回转率状态下也可以达到很好的滤波效果。

表2 自适应Kalman滤波数据统计特性

同样地,对自适应Kalman滤波器进行动态变化测试,为不失一般性,重新采集了一组数据,通过自动操舵仪在100 s时刻向右打舵使回转率增大,又在350 s时刻向左打舵减小回转率,使得回转率的变化更加复杂,以更好地验证自适应Kalman滤波器的动态性能,曲线图如图5所示。

图5 动态变化时自适应Kalman滤波效果

由图5可以看出,当角速率发生变化时滤波值能够随之变化,虽然在短时间内出现滞后,但是在达到稳态前很快作出响应变化,几乎与真值同时达到稳态,在船舶操纵控制系统中是可行的,而且在达到稳态后的两个阶段滤波效果也不错,计算误差标准差可得,150～350 s时间内由1.743°/min降0.266°/min,400～500 s时间内由1.457°/min降至0.218°/min,可知在回转率动态变化的情况下自适应Kalman滤波器是适用的。