MEMS四元数卡尔曼滤波算法的电梯姿态估计①

郭 威,吴允平,2,3,王廷银,2,3

1(福建师范大学光电与信息工程学院,福州 350007)

2(数字福建环境监测物联网实验室,福州 350117)

3(福建省光电传感应用工程技术研究中心,福州 350007)

1 引言

电梯,已逐渐成为城市生活的基础设施,是现代人类出行活动最后50 米的重要交通工具[1].随着我国城市化进程的发展,电梯保有量、年产量、年增量均为世界第一[2].与其他发达国家相比,我国电梯使用呈现负载高、运量大、随机性强等特点,电梯事故时有发生[3].近十年来,通过信息技术手段如物联网对电梯运行等安全性监测,使得电梯事故率持续下降[4].

随着电梯安全性能提升的同时,电梯舒适性也渐渐成为人们乘梯、用梯的关注重点.考虑到电梯轿厢异常震动是电梯安全性能的关键,也是电梯乘坐舒适度的关键指标之一,为此,2016 年姚长鸿等通过振动传感器来采集异常震动,将震动的突变嵌入到安全管理[5],给电梯的安全管理注入的新观点.顾雯雯等[6]从乘客的视觉角度及电梯的运行性能两方面着手,重点分析了影响电梯乘坐舒适感的主要原因与影响因素.宋涵通过电梯的装潢、结构、速度控制方式构建了基于AHP模糊综合评价法对电梯的舒适性进行评估的体系[7],通过分析得出各个因素对影响电梯舒适性的程度.这些研究从电梯的构成、装饰等方面尝试对电梯的安全性和舒适性进行了初步研究.

这些年,MEMS(Micro Electro Mechanical System)技术呈现体积小、灵敏度高、维度多等发展趋势[8],已广泛应用于汽车工业、地质灾害以及航空航天等众多领域.九轴传感器就是MEMS 技术发展的典型代表,它采集加速度、磁场和角速度等多维度数据,为深化行业应用提供了多维数据视角.王贇贇等应用九轴传感器采集汽车的数据,通过互补滤波实现误差补偿,再使用四元数法进行姿态解算,得到汽车的姿态角,进一步研究了汽车运动姿态和安全行驶之间的相关性[9].Shi G 等使用九轴传感器采集汽车的数据,利用传统的线性卡尔曼滤波实现四元数的姿态解算并提出了新的方法来单独新更磁力计,消除了对姿态估计的影响[10].刘飞飞等应用九轴传感器采集危岩的数据,经内置运动处理引擎(DMP)处理得到危石四元数后,解算出危岩危石的姿态角,来监测其滚动和震动的情况[11].张煌军等通过应用九轴传感器采集无人机数据,将加速计和磁力计的数据通过平方根容积卡尔曼滤波来纠正陀螺仪误差,再用四元数进行姿态解算,从而提高无人机飞行姿态解算的精度[12].

电梯作为在有限空间内导轨上运行的特种设备,其姿态、加速度、速度等直接影响乘梯用梯的舒适性以及安全性等体验,因此,将九轴MEMS 传感器应用于电梯监测领域中,解算获得电梯姿态,无疑有助于开展电梯的安全性评估和舒适性分析,是一项具有重大意义的研究.

2 数据采集设计

2.1 硬件设计

九轴MEMS 数据采集电路图如图1 所示,传感器内部包含了三轴加速计、三轴陀螺仪、三轴磁力计,可以采集加速度、角速度、磁场的数据,RX 和TX 分别为传感器的UART 接收端和UART 发送端,与嵌入式微处理器进行数据交互,由嵌入式微处理器对接收到的数据进行计算分析、完成姿态解算后进行持久化存储.

2.2 软件设计

嵌入式软件的流程图如图2 所示.总体方案是:通过定时器接收传感器发送的数据.首先,对数据进行完整性和可靠性校验,以队列的方式存储校验通过的数据,然后进行电梯运行姿态和状态的计算.将采集到的加速计和磁力计数据利用互补滤波纠正陀螺仪数据通过四元数微分方程进行姿态角初步求解,最后卡尔曼滤波对初步求解的姿态角进行数据精确化,得到最终的姿态角.同时,利用加速度对时间进行积分得到电梯的运行姿态.最后进行持久化存储,方便数据的整理分析.

图1 九轴MEMS 数据采集电路图

图2 软件设计流程图

3 数据处理

3.1 四元数姿态解算

通常,通过引入导航坐标系(简称n系,由Zn、Xn、Yn组成)和载体坐标系(简称b系,由Zb、Xb、Yb组成)来描述姿态,如图3 所示.b系可以由n系绕Xn轴、Yn轴、Zn轴转动表示,其中旋转的角分别是俯仰角φ、横滚角θ、航向角ψ,它们统称为欧拉角或姿态角[13].

求解姿态角方法有欧拉角法、方向余弦法、四元数法[14],其中,欧拉角法存在奇异点现象无法表示物体的全姿态,方向余弦法则存在计算量大和解算复杂度高的问题[15],而四元数法具有计算量小、无奇异点现象等优点,因此,主要通过对陀螺仪数据用四元数法求解姿态[16].捷联惯导系统理论定义了四元数q[17]:

q0为四元数标量部分,q1、q2、q3为四元数矢量部分;还定义了陀螺仪和四元数的微分关系以及四元数与n、b两系之间旋转矩阵的关系,如式(2)和式(3).

通过一阶龙格库塔法求解四元数微分方程如式(4),T为采样周期.求出四元数得到旋转矩阵可计算出姿态角如式(5).

由于陀螺仪存在长时间工作会误差漂移、加速计存在动态响应较慢、磁力计存在数据采集容易受环境影响等因素,在具体应用时往往还会将这三者数据进行补偿从而获得准确的姿态[18-22].图4 就是一种典型的互补滤波姿态解算算法[20].

图4 基于MEMS 的姿态角互补滤波算法框图

图4 中,虚线部分为互补滤波的过程,xba为b系载体的加速度,n系中标准重力加速度gn=[0,0,1]T,通过旋转矩阵表示为b系标准重力加速度并与xba做向量积,得到b系下加速度向量误差;xbm为b系载体的磁力,通过旋转矩阵将xbm转到n系下表示xnm=[hx,hy,hz]T,n系下磁力计理想输出数据[bx,by,bz]T与xnm的关系为通过旋转矩阵将其转化为b系下表示并与xbm做向量积得到b系磁力计向量误差,将加速度向量误差与磁力计向量误差相加得到总向量误差e,通过PI 调节系统输出的修正补偿量为W;Ubg为b系载体的角速度,Ub表示为经过修正后陀螺仪的角速度,kp为比例系数,决定了滤波器的截止频率和响应速度,ki为消除稳态误差的积分系数.最后通过四元数微分方程求解出四元数并转换成姿态角.

3.2 电梯姿态解算

由于MEMS 磁力计数据易受周围金属影响以及互补滤波的截止频率不易确定[21,22],解算后电梯姿态角的误差较大,精度不高,所以在互补滤波姿态解算上开展进一步工作,由此,MEMS 四元数卡尔曼滤波算法的电梯姿态求解如图5 所示.首先,在解算俯仰角和横滚角时,把互补滤波方法中使用的磁力计参数排除,仅用加速计数据计算向量误差来校正陀螺仪,来避免磁力计数据误差对横滚角和俯仰角计算的影响;其次,卡尔曼滤波是最优递推估计技术,其中线性化的扩展卡尔曼滤波(EKF)已经广泛用于工业非线性系统[23],结合电梯运行规律以及姿态估计系统的非线性特点,通过扩展卡尔曼滤波将初步解算的姿态角作为观测量对预测量进行修正,以便得到当前时刻的最优状态估计,从而帮助提高电梯姿态角的精度.

图5 四元数卡尔曼滤波的姿态解算图

EKF 是将卡尔曼滤波状态方程和测量方程非线性部分线性化,得到线性模型如式(6)和式(7):

预测更新过程如下:

1)状态一步预测

式中,X(k|k-1)是用k-1 时刻状态预测的结果,X(k-1|k-1)是k-1 时刻状态最优结果.

2)状态一步预测协方差

式中,P(k|k-1)是X(k|k-1)的协方差,P(k-1|k-1)是X(k-1|k-1)的协方差,Q为系统噪声协方差.

3)卡尔曼增益

式中,R为测量噪声协方差.

4)状态更新

5)状态协方差更新

式中,X(k)为状态值用于状态预测,式(7)中Z(k)为初步解算后的姿态角,式(8)～式(10)是状态估计部分,即当前时刻预测值和协方差以及增益;式(11)和式(12)是状态更新的部分即当前时刻的最优状态,然后将当前时刻的电梯最优姿态估计用于下一个时刻的预测,最终得到精确后的电梯姿态角.

3.3 电梯速度计算

根据数学知识可知,加速度在时间上的积分等于速度,积分的值可无限近似成区域面积的总和.由于实际采样时间不趋于0 这样会有误差累加的情况出现.为了减少误差,通过以下方法来求得电梯速度:

其中,Sample(n)为当前时刻加速度,Sample(n-1)为前一时刻加速度,T为时间间隔.

3.4 滤波性能分析

就互补滤波算法与卡尔曼滤波算法在收敛性进行对比分析,如图6 所示.卡尔曼滤波的姿态角比较于互补滤波的姿态角抖动较小,滤波的收敛效果更好,具有良好的实时性.

图6 互补滤波和卡尔曼滤波的收敛性分析图

4 实验分析

4.1 电梯姿态解算算法优化比对

为验证本方法的效果,在2015 年安装使用的某电梯上安装了本装置,分别应用常规互补滤波法、本算法进行俯仰角、航向角以及横滚角结算,对比如图7所示.本方法解算的姿态角相对平滑,抖动小.

4.2 电梯运行状态的数据分析

为进一步对比验证本方法的合理性,电梯A 及电梯B 分别安装了本装置,对解算的姿态角进行分析.图8～图10 为电梯A 和电梯B 某时段横滚角、俯仰角、航向角的对比数据.可以看出,两部电梯在运行过程中横滚角和俯仰角的偏离值只有零点几度.但在航向角的比较中,电梯A 的偏离值为14 而电梯B 的偏离值为8,两者相差较大,导致电梯A 的晃动感较为明显.

图7 互补滤波姿态解算和本文算法姿态解算角度对比图

图8 电梯A 和B 横滚角对比图

图9 电梯A 和B 俯仰角对比图

图11 为电梯A 速度数据,图12 为电梯B 速度数据,速度正值表示电梯为上行姿态,速度负值表示电梯为下行姿态,速度为零时则表示电梯静止,通过速度可知电梯的当前状态;可见,电梯B 在上下运行过程中的速度略快于电梯A.

图10 电梯A 和B 航向角对比图

图13 为电梯A 的加速度,图14 为电梯B 的加速度,可知两者的加速度数据总体正常.统计学中定义峰度为波形尖峭程度的一个量[24],采用峰度来表示电梯加速度的波形特征,可以较好地比较两部电梯的舒适性,峰度越大即加速度波形越尖锐,说明电梯在加减速的过程越急速,导致不舒适的乘坐感,峰度越小即加速度波形越平滑电梯在加减速的过程越平缓能带来良好的乘坐体验.由表1 和表2 得出,电梯A 的峰度比电梯B 来的大,即电梯A 的舒适性不如电梯B.

因此,综合电梯的横滚角、俯仰角、航向角以及加速度峰度可以得出结论为,电梯A 的舒适性与安全性皆不如电梯B,这和电梯A、电梯B 的实际情况相吻合;电梯A 为2007 年投入使用的货梯,长时间大负荷使用导致器械老化,电梯B 为2016 年投入使用的客梯,设施状况和维保较好.

图11 电梯A 的速度图

图12 电梯B 的速度图

图13 电梯A 的加速度图

5 总结

在电梯监测中应用九轴MEMS 传感器,在应用四元数算法时需结合电梯金属件多、运行突变性较小、姿态变化率不大等运行特点,首先在求解横滚角和俯仰角时用加速计数据而不是磁力计数据来校正陀螺仪,其次,用卡尔曼滤波算法来预测估计电梯状态,最终提高了解算电梯姿态角的精度;通过分析电梯运行时的姿态角和加速度的峰度以及电梯的速度,为评估对电梯的安全性与舒适性提供了数据支持.

图14 电梯B 的加速度图

表1 电梯A 加速度波形的峰度

表2 电梯B 加速度波形的峰度