基于Mahony-EKF算法的手臂运动姿态测量系统
Mahony-EKF algorithm-basing on arm motion attitude measurement system
王怡苗 1夏振华1
作者信息
摘要
针对传统姿态解算方法效率迟缓、精度低下及稳定性差等问题,提出一种基于 Mahony和扩展卡尔曼(EKF)相融合的算法,并开发出一种新型人体手臂姿态测量系统.首先,通过STM32微处理器采集 MEMS传感器测得的数据,借助 Mahony滤波器解算加速度计、磁力计和陀螺仪的数据,以此得到初步姿态四元数.其次,将初步姿态四元数作为EKF量测值,依据非重力加速度调节量测噪声协方差矩阵.然后,根据陀螺仪测得的角速度信息建立EKF状态方程,通过EKF滤波更新状态,获取解算融合后的手臂姿态数据.最后,将数据发送到上位机,通过上位机软件实时监测姿态角数据,再构建三维模型实时还原手臂的运动状态.经实验验证,应用EKF算法矫正 Mahony滤波解算出的姿态数据,不仅可以使误差减小到0.5°、消除超调量和降低噪声干扰,还能有效克服传统姿态解算方法中需要大量数据集和计算时间长问题,从而抑制了随机波动,提高姿态解算精度.
Abstract
To address the issues of slow efficiency,low accuracy,and poor robustness associated with traditional posture estimation techniques,we propose an algorithm based on Mahony-EKF fusion and develop a new human arm posture estimation technique.First,collect the data measured by MEMS sensors using an STM32 microprocessor,and solve the accelerometer,magnetometer and gyroscope data using the Mahony filter to obtain the preliminary quaternion of body attitude.Next,based on the non-gravitational acceleration,the preliminary quaternion is used as the EKF measurement and adjust the measurement matrix of noise.Then the EKF state equation is established based on the angular velocity information measured by the gyroscope,and the state is updated by EKF filtering to obtain the solved and fused attitude data of the arm.Finally,the data are sent to the host computer,the angle data of attitude are monitored in real time by the software itself and the motion state of the arm is restored in real-time by constructing the 3D model.After the experimental verification,utilize the EKF algorithm to correct the attitude data solved by Mahony filtering which can not only reduce the error to 0.5°,eliminate the overshoot and reduce the noise interference but also effectively overcome the problem of requiring a large number of data sets and long computation time in the traditional methods,to suppress the random fluctuation and improve the accuracies.
关键词
MEMS/EKF/Mahony/融合算法/姿态测量Key words
MEMS/EKF/Mahony/fusion algorithm/attitude measurement引用本文复制引用
出版年
2024
NETL
NSTL
万方数据