﻿ MEMS四元数卡尔曼滤波算法的电梯姿态估计
 计算机系统应用  2020, Vol. 29 Issue (3): 246-252 PDF
MEMS四元数卡尔曼滤波算法的电梯姿态估计

1. 福建师范大学光电与信息工程学院, 福州 350007;
2. 数字福建环境监测物联网实验室, 福州 350117;
3. 福建省光电传感应用工程技术研究中心, 福州 350007

Elevator Attitude Estimation Based on MEMS Quaternion Kalman Filter Algorithm
GUO Wei1, WU Yun-Ping1,2,3, WANG Ting-Yin1,2,3
1. College of Photonic and Electronic Engineering, Fujian Normal University, Fuzhou 350007, China;
2. Digital Fujian Environmental Monitoring IoT Laboratory, Fuzhou 350117, China;
3. Fujian Provincial Engineering Technology Research Center of Photoelectric Sensing Application, Fuzhou 350007, China
Foundation item: Key Project of Joint Fund for the Promotion of Scientific Cooperation across the Taiwan Straits of National Natural Science Foundation of China (U1805263); Guiding Project of Fujian Province (2019H0009); Natural Science Foundation of Fujian Province (2019J01427)
Abstract: The multi-dimensional MEMS sensor is applied to elevator monitoring. According to the working characteristics of the elevator, the quaternion complementary filtering method is modified to correct the gyroscopedata to solve the real-time attitude of the elevator, and then the Kalman filtering method is applied to further improve the attitude monitoring accuracy. The actual verification shows that the method can improve the accuracy of elevator attitude monitoring data,and use the attitude angle and acceleration kurtosis to analyze and compare, which can provide critical data basis for elevator safety and comfort assessment.
Key words: MEMS     elevator attitude     Kalman filter     comfort

1 引言

2 数据采集设计 2.1 硬件设计

2.2 软件设计

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

 图 2 软件设计流程图

3 数据处理 3.1 四元数姿态解算

 图 3 姿态角表示图

 $q = q_0 + q_1 i + q_2 j + q_3 k = {[ q_0 , q_1 , q_2 , q_3 ]}^{\rm T}$ (1)

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

 $\left( \begin{array}{l} {{q'}_0}\\ {{q'}_1}\\ {{q'}_2}\\ {{q'}_3} \end{array} \right) = \frac{1}{2}\left( {\begin{array}{*{20}{c}} 0&{ - \omega _x^b}&{ - \omega _y^b}&{ - \omega _z^b}\\ {\omega _x^b}&0&{\omega _z^b}&{ - \omega _y^b}\\ {\omega _y^b}&{ - \omega _z^b}&0&{ - \omega _x^b}\\ {\omega _z^b}&{\omega _y^b}&{ - \omega _x^b}&0 \end{array}} \right)\left( \begin{array}{l} {q_0}\\ {q_1}\\ {q_2}\\ {q_3} \end{array} \right){\rm{ = }}\frac{1}{2}{\Omega _{{b}}}.q$ (2)
 $C_{{n}}^{ b} = \left( {\begin{array}{*{20}{c}} {q_0^2 + q_1^2 + q_2^2 + q_3^2}&\!\!\!\!\!\!{2({q_1}{q_2} + {q_0}{q_3})}&\!\!\!\!\!\!{2({q_1}{q_3} + {q_0}{q_2})}\!\!\!\!\!\!\!\! \\ {2({q_1}{q_2} + {q_0}{q_3})}&\!\!\!\!\!\!{q_0^2 + q_1^2 + q_2^2 + q{{_3^2}}}&\!\!\!\!\!\!{2({q_2}{q_3} + {q_0}{q_1})} \\ {2({q_1}{q_3} + {q_0}{q_2})}&\!\!\!\!\!\!\!\!{2({q_2}{q_3} + {q_0}{q_1})}&\!\!\!\!\!\!\!\!{q_0^2 + q_1^2 + q_2^2 + q_3^2} \end{array}}\!\!\!\! \right)$ (3)

 $q(t + T) = q(t) + T{\Omega _{\rm b}}(t)q(t)$ (4)
 \left\{\begin{aligned} & \theta = \arcsin (2({q_0}{q_2}- {q_1}{q_3})) \\ & \varphi = \arctan \frac{{2({q_2}{q_3} + {q_0}{q_1})}}{{(1 - 2(q_1^2 + q_2^2))}} \\ & \psi = \arctan \frac{{2({q_1}{q_2} + {q_0}{q_3})}}{{1- 2(q_2^2 + q_3^2)}} \end{aligned}\right. (5)

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

3.2 电梯姿态解算

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

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

 $X(k) = AX(k - 1) + W(k)$ (6)
 $Z(k) = HX(k) + V(k)$ (7)

1) 状态一步预测

 $X(k|k - 1) = AX(k - 1|k - 1)$ (8)

2) 状态一步预测协方差

 $P(k|k - 1) = AP(k - 1|k - 1)A^{\rm T} + Q$ (9)

3) 卡尔曼增益

 $Kg(k) = P(k|k - 1)H^{\rm T}/(HP(k|k - 1)H^{\rm T} + R)$ (10)

4) 状态更新

 $X(k|k) = X(k|k - 1) + Kg(k)(Z(k) - HX(k|k - 1))$ (11)

5) 状态协方差更新

 $P(k|k) = (1 - Kg(k)H)P(k|k - 1)$ (12)

3.3 电梯速度计算

 $V = S\!ample(n - 1)*T + (S\!ample(n) - S\!ample(n - 1)/2)*T$

3.4 滤波性能分析

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

4 实验分析 4.1 电梯姿态解算算法优化比对

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

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

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

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

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

 图 11 电梯A的速度图

 图 12 电梯B的速度图

 图 13 电梯A的加速度图

5 总结

 图 14 电梯B的加速度图

 [1] 潘鹏, 吴允平, 王廷银, 等. 基于Logistic回归的电梯健康评估. 计算机系统应用, 2018, 27(10): 255-260. DOI:10.15888/j.cnki.csa.006531 [2] 恩旺, 刘子金, 张淼. 中国电梯行业的技术发展与趋势. 建筑科学, 2018, 34(9): 110-118. [3] 李东风. 基于物联网技术的电梯安全运行监控系统. 通信设计与应用, 2017(17): 8-9. [4] 施京京. 不断提升电梯安全水平——《关于加强电梯质量安全工作的意见》解读. 中国质量技术监督, 2018(3): 18-19. DOI:10.3969/j.issn.1008-1607.2018.03.006 [5] 姚长鸿, 夏钟兴. 电梯轿厢舒适度与异常震动监测系统的设计探讨. 价值工程, 2016, 35(9): 87-88. [6] 顾雯雯, 孙勇, 路成龙. 影响电梯乘坐舒适度因素分析. 中国设备工程, 2018(24): 59-60. DOI:10.3969/j.issn.1671-0711.2018.24.035 [7] 宋涵, 郑尚透. 基于AHP综合评价法的电梯舒适度评估体系研究. 机电工程技术, 2019, 48(2): 21-22, 81. DOI:10.3969/j.issn.1009-9492.2019.02.006 [8] 曹景伟, 朱宝全. 应用MEMS陀螺仪和加速度计的汽车运动姿态测量. 重庆理工大学学报(自然科学), 2018, 32(4): 48-54. [9] 王贇贇, 李建民, 侯文. 基于组合导航的汽车姿态数据采集系统设计. 电子产品世界, 2017, 24(1): 44-47, 26. [10] Shi G, Li XS, Jiang ZF. An improved yaw estimation algorithm for land vehicles using MARG sensors. Sensors, 2018, 18(10): 3251. DOI:10.3390/s18103251 [11] 刘飞飞, 任安虎, 张博超. MPU9250九轴传感器的危岩危石监测系统. 单片机与嵌入式系统应用, 2018, 18(12): 62-65, 73. [12] 张煌军, 徐雪松, 罗伟, 等. 基于平方根容积卡尔曼滤波的四旋翼无人机的姿态解算. 科学技术与工程, 2019, 19(12): 248-253. DOI:10.3969/j.issn.1671-1815.2019.12.036 [13] 王龙, 章政, 王立. 改进扩展卡尔曼滤波的四旋翼姿态估计算法. 计算机应用, 2017, 37(4): 1122-1128. DOI:10.11772/j.issn.1001-9081.2017.04.1122 [14] 孙章国. 基于ARM的航姿参考系统研究[硕士学位论文]. 上海: 上海交通大学, 2010. [15] 周绍磊, 丛源材, 李娟, 等. 方向余弦矩阵中四元数提取算法比较. 中国惯性技术学报, 2008, 16(4): 415-418. [16] 张荣辉, 贾宏光, 陈涛, 等. 基于四元数法的捷联式惯性导航系统的姿态解算. 光学精密工程, 2008, 16(10): 1963-1970. [17] 泰永元. 惯性导航. 2版. 北京: 科学出版社, 2014. [18] Mahony R, Hamel T, Pflimin JM. Nonlinear complementary fllters on the special orthogonal group. IEEE Transactions on Automatic Control, 2008, 53(5): 1203-1218. DOI:10.1109/TAC.2008.923738 [19] 彭孝东, 张铁民, 李继宇, 等. 基于传感器校正与融合的农用小型无人机姿态估计算法. 自动化学报, 2015, 41(4): 854-860. [20] 李二闯, 张建杰, 袁亮, 等. 基于四元数互补滤波的小型四旋翼姿态解算. 组合机床与自动化加工技术, 2019, 37(3): 37-39, 43. [21] 杜瑾, 赵华超, 郑哲, 等. 捷联惯导互补滤波姿态融合算法设计. 传感技术学报, 2018, 31(10): 1547-1552. [22] 孙金秋, 游有鹏, 傅忠云. 基于共轭梯度法和互补滤波相结合的姿态解算算法. 传感技术学报, 2014, 27(4): 524-529. DOI:10.3969/j.issn.1004-1699.2014.04.019 [23] 叶松庆. 非线性卡尔曼滤波算法研究[硕士学位论文]. 重庆: 中国科学院重庆绿色智能技术研究院, 2016. [24] 王学民. 偏度和峰度概念的认识误区. 统计与决策, 2008(12): 145-146.