基于改进互补滤波算法的姿态解算

基于改进互补滤波算法的姿态解算

孙宝增

江南机电设计研究所  贵阳  550009

摘   要：本文主要是将可穿戴设备分别佩戴于运动员不同的部位，对由三个传感器采集的数据进行融合处理，求解高山滑雪运动员的动作姿态角变化。由于每种仪器都有自己的优缺点，通过调整加速度计、磁力计采样得到的姿态角和陀螺仪采样得到的姿态角之间的权重，通过改进的互补滤波算法进行数据融合，求解出最终的姿态角，姿态角包含横滚角、俯仰角、偏航角三个角度，可以为我国高山滑雪运动员训练提供参考。

关键字:互补滤波算法；姿态解算；数据融合;姿态角。

1引言

可穿戴设备由加速度计、陀螺仪、磁力计组成，分别配戴于运动员的胸部、腰部、右大腿、左大腿、右足、左足6个部位，便可测得运动员这6个部位的三维九轴数据[1]。各个传感器有着自己的优缺点，存在误差，利用比较合适的算法将多个传感器采集到的数据进行数据融合，是目前研究工作的重点[2]。

在目前的研究中，主要是采用陀螺仪为主体，对陀螺仪求解的姿态角“信任度”比较高[3]。但是在求解过程中，陀螺仪在短时间内十分精准，但长期积分求解过程只出现积分累计误差，导致精度变差[4]。在姿态解算的过程中，利用三个传感器之间的性质互补性，为了提高解算精度将数据进行融合，这也就成为姿态解算的研究的主要方向[5]。

2基于互补滤波算法的姿态解算

通过加速度计求解横滚角、俯仰角，通过磁力计求解出偏航角，通过陀螺仪求解出横滚角、俯仰角、偏航角。利用互补滤波算法将加速度计、磁力计、陀螺仪求解的姿态角进行数据融合，求解出最终的姿态角。

2.1利用加速度计得到横滚角与俯仰角

当运动员处于静止时期，根据第二牛顿定律，运动员只受重力，加速度计测得的是重力加速度，设静止状态下加速度计测量到的加速度向量为，对做归一化处理，有。

是重力加速度向量，对做归一化处理，有，可得：

                            (1)

可得到横滚角与俯仰角：

                              (2)

可以求解出横滚角和俯仰角，为了方便区分和经陀螺仪得到的横滚角、俯仰角，将由加速度计计算得到的横滚角记作，将由加速度计计算得到的俯仰角记作。

2.2 利用磁力计求解偏航角

设静止状态下磁力计测量到的地磁场向量为，并对做归一化处理，有。

为导航坐标系的归一化地磁场向量，有，可得：

(3)

得出偏航角的计算公式为

                          (4)

2.3 利用陀螺仪积分求解欧拉角

设是陀螺仪在某个时刻的采样值，采样的时间间隔为，然后使用积分，得到角度在的变化量：

                            (5)

在本文中，将得到的结果进行累加，即可得到若干个时刻的角度变化值。将由陀螺仪得到的横滚角、俯仰角、偏航角记作。

2.4互补滤波算法

互补滤波算法是通过将三种仪器采集到的不同类数据进行数据融合的算法。互补滤波算法是在利用三个传感器的优缺点互补，使得姿态解算的结果更加准确。利用加速度计可以求解出横滚角、俯仰角，磁力计可以求解出偏航角。陀螺仪可以求解出横滚角、俯仰角、偏航角。互补滤波算法主要将利用加速度计、磁力计求解出来的姿态角和利用陀螺仪求解的姿态角进行加权融合。

                          (6)

其中，，是加速度计求出的横滚角，俯仰角。利用磁力计求出俯仰角。是利用陀螺仪求出的横滚角、俯仰角、偏航角。

3基于改进的互补滤波算法的姿态解算

根据互补滤波算法的原理：运动员在前期静止时，应该对加速度计、磁力计信任度增加，在运动时刻，对陀螺仪开始信任，但是会产生积分误差，需要用到加速度计、磁力计去修正误差。在静止时期，加速度计、磁力计是主体，在运动时期，陀螺仪是主体。

设权重参数为，其中，为参数，x的作用是调节陀螺仪与加速度计、磁力计输出姿态角之间的权重参数。

设数据融合后的横滚角、俯仰角、偏航角为，有：

              (7)

其中，，,，利用加速度计得到横滚角，俯仰角，利用“倾斜补偿后”的磁力计得到角度为，利用陀螺仪得到数据。

通过式（7）可以求解出各个时刻上的角度，可以得到变化的角度，即为人体姿态的变化。

4总结

在确定运动姿态旋转顺序为Z-Y-X的前提下，利用加速度计求解出的俯仰角和横滚角对磁力计输出数值进行补偿。由于Z轴首先发生变化，会导致磁力计测得数值发生变化，不利于求解偏航角。经过补偿后输出的数据，为改进的互补滤波算法提供更准确、更稳定的数据。

根据传感器的性质和数据特点，构造一个时间函数来调整利用加速度计、磁力计求解的姿态角和陀螺仪求解的姿态角之间的权重。利用互补滤波算法求解姿态角时，在运动员静止时求解出的姿态角会发生较大的波动，为了解决这一问题，提出权重的调整方案进行数据融合，改善了互补滤波算法在求解过程中的缺点，更加合理的求解出运动员部位的姿态角。

参考文献

[1]陈浩. 可穿戴计算机信息捕获及处理系统的研究与实现[D]. 电子科技大学, 2008: 2-4页.

[2]Makni A, Kibangou A Y, et.al. Data Fusion-Based Descriptor Approach for Attitude Estimation underaccelerated maneuvers[J]. Asian Journal of Control, 2019(4): 1433-1437P.

[3]Sabatini A M. Estimating Three-Dimensional Orientation of Human Body Parts by Inertial/Magnetic Sensing[J]. Sensors, 2011, 11(2):1489-1525.

[4]杜杉杉, 吴昊, et.al. 一种面向AHRS的改进互补滤波融合算法[J]. 国外电子测量技术, 2015(3): 13-18页.

[5]姜强, 曾勇, et.al. 四旋翼飞行器姿态航向参考系统设计与实现[J]. 控制工程, 2013(S1):167-169页.