一种运动姿态传感器的数据融合与解算方法与流程

本发明公开一种运动姿态传感器的数据融合与解算方法，具体涉及多种惯性器件数据融合技术。

背景技术：

数据融合是多元信息综合处理的一项新技术，比较确切的定义可概括为：充分利用不同的时间和空间的多传感器信息资源，采用计算机技术对按时序获得的多传感器观测信息在一定的准则下加以自动分析、综合、支配和使用，获得被测对象的一致性解释与描述，以完成所需的决策和估计任务，使系统获得比它的各个组成部分更优越的性能。

数据融合从20世纪70年代末被提出，源于军事领域的C3I(command，control，communication and intelligence)系统的需要，当时称为多源相关、多传感器混合数据融合，并于80年代建立其技术。美国是数据融合技术起步最早的国家，1983年，美国国防高级研究计划局(DARPA)推出的战略计算机计划中，将多传感器数据融合列为重大研究课题。1984年，美国国防部(DOD)成立了数据融合专家组(DFS-Data Fusion Subanal)，负责指导、组织并协调这一国防关键技术的系统研究，1988年又将其列入国防部22项关键技术之一。同时其它西方发达国家和国际组织(如英、日、德、法及欧盟等)也积极开展了数据融合技术研究工作。

和国外相比，我国在数据融合领域的研究起步较晚。1991年海湾战争结束以后，数据融合技术引起国内有关单位和专家的高度重视。一些高校和科研院所相继对数据融合的理论、系统框架和融合算法开展了大量研究，但基本上处于理论研究的层次上，在工程化、实用化方面尚未取得有成效的突破，有许多关键技术问题尚待解决，在工程应用领域，需要开发出有重要应用价值的实用系统。近年来数据融合技术已形成研究热点，国家自然科学基金和国家863计划已将其列入重点支持项目。

技术实现要素：

本发明的目的是提供一种适用于低成本、低功耗运动姿态传感器芯片的小内核高效能多源数据融合和姿态解算的方法。

本发明解决上述问题所采用的技术方案是：

一种运动姿态传感器的数据融合与解算方法，其特征在于：

所述运动姿态传感器能够输出的数据包括：宿主原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据，经过融合但未经处理的宿主四元数数据，经过解算和处理的宿主姿态角数据，包括航向角、俯仰角、横滚角、倾斜角；

所述原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据是运动姿态传感器的角速度计、加速度计和磁力计将采集到的X、Y、Z三个轴向的角速度、加速度、地球磁场和环境磁场之复合磁场强度等模拟数据发送给微处理器后进行模数转换后的数字化数据；

所述经过融合但未经处理的宿主四元数数据是微处理器对原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据按预设好的融合算法进行数据融合而得到的超复数，它由实数加上三个虚数单位i、j、k组成，四元数一般可表示为

其中n为旋转方向，θ/2为转角大小，i，j，k关系为但该四元数据没有经过欧拉角解算和数据滤波、修正补偿等，四元数表示三维空间中的一个旋转；

所述经过解算和处理的宿主姿态角数据是微处理器对经过融合的四元数按预设好的数据解算算法解算出初始欧拉角，即航向角、俯仰角、横滚角、倾斜角，这些姿态数据再经过预设的滤波修正算法进行处理，得到相对精确的宿主姿态角数据。

其次，所述的数据融合算法是将原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据进行空间融合而形成四元数的算法，包括如下计算方法和步骤：

(1)四元数微分

已知四元数对时间微分

已知E为地理坐标系，b为宿主坐标系

因为角速度计在宿主坐标系上测得的角速度为故将转换成会比较方便

展开得

整理为

其中

(2)四元数更新

假设有一微分方程

则其解为

X(t+Δt)＝X(t)+Δt·f(X(t)，w(t))

其中Δt为取样周期，套用四元数

Q(t+Δt)＝Q(t)+Δt·Ωb·Q(t)

展开上式

其中wxwywz为宿主坐标系下的三轴角速度值；

(3)角速度积分偏差修正，

通过角速度微分方程可以得到四元数，问题是角速度计会有积分漂移，得到的角度会有误差，这时候需引入加速度计对角速度数据进行校正，以得到更准确的角度；不过这时航向也会存在误差，在误差不能忽略的使用情况下，需要用磁力计校正，观察矩阵为重力加速度和正北方向的磁场，通过这个误差向量来修正角速度数据，将融合后的角速度输入到四元数微分方程就得到四元数；

系统模型的旋转矩阵

其中

R1，1＝q0²+q1²-q2²-q3²

R1，2＝2(q1q2+q0q3)

R1，3＝2(q1q3-q2q0)

R2，1＝2(q1q2-q0q3)

R2，2＝q0²-q1²+q2²-q3²

R2，3＝2(q3q2+q0q1)

R3，1＝2(q1q3+q2q0)

R3，2＝2(q3q2-q0q1)

R3，3＝q0²-q1²-q2²-q3²

四元数微分方程

其中

输入方程

为角速度计输出值，宿主坐标系下真实的角速度，传感器噪声矢量，角速度漂移矢量，则有状态方程

因为

所以

输出方程

其中，为重力加速度矢量，为正北磁场的矢量

线性化下的雅可比矩阵

离散化模型

离散时间的一阶近似

Φ≌I+FT

Γ≌GT

滤波过程

Pk|k-1＝(I+FT)Pk|k-1(I+FT)^T+T²GQG^T

K＝Pk|k-1H^T(HPk|k-1H^T+R)^-1

Pk＝Pk|k-1-KHPk|k-1

其中，为状态预测值，Pk|k-1为均方差预测，K为最优卡尔曼增益，αk为新息向量，为状态估计，Pk为均方差估计，αk又被称为残差向量，它反映了滤波模型的准确性。

再次，所述的数据解算算法是将四元数根据运动姿态传感器的宿主运动特征按一定的规则进行欧拉角的计算，得到相应取值范围的姿态角数据的算法，包括如下计算方法和步骤：

(1)四元数乘法

设有两四元数QA和QB，其乘法关系为

旋转四元数

设一向量及旋转后向量四元数满足

R′＝Q^-1·R·Q

四元数倒数

(2)四元数归一

当四元数归一化后|Q|＝1

Q^-1＝Q*

可得到

R′＝Q*·R·Q

将其展开

另四元数旋转矩阵为Mq

(3)将四元数转换成欧拉角，

由Mq可得出

φ＝-ArcSin(M13)，

其中θ为横滚角，φ为俯仰角，为航向角

(4)倾角计算

其中θ为倾角，ax，ay，az分别为三轴重力加速度的值。

本发明的有益效果在于：

(1)提高了低成本运动姿态传感器的数据精度

基于系统级封装的运动姿态传感器芯片，由于使用MEMS惯性器件芯片，因此成本大大降低，同时精度也降低至消费级水平，本发明数据融合与解算方法的使用，可以使其精度提高至工业级水平，从而保证其可靠性。

(2)保证了低功耗运动姿态传感器的功耗水平

智能装备和智能器件的广泛应用，对基础元器件的功耗水平提出了更苛刻的要求，功耗要求越来越低，本发明的数据融合与解算方法内核小，对资源的消耗少，从而降低了软件系统对能源的消耗，保证了低功耗特点。

附图说明

图1是本发明一种运动姿态传感器数据融合与解算方法的嵌入式程序流程图

图2是本发明一种运动姿态传感器数据融合与解算方法的四元数原理图

图3是本发明一种运动姿态传感器数据融合与解算方法的基于模糊控制的自适应卡尔曼滤波器结构

具体实施方式

如图1所示，为本发明设计的运动姿态传感器数据融合与解算方法的嵌入式程序流程图。程序开始，利用一个32位处理器的内部定时器产生一个32位的级联定时器，根据处理器时钟频率，提供一个us级定时；在进行四元数算法时获取当前时间然后再减去上次解算的时间，则获得微分时间Δt；处理器获取的三轴加速度和三轴磁阻归一化；机体(宿主)坐标系磁阻矢量转换成地理坐标系下的磁阻矢量；校准磁阻矢量值，使之成为(1，0，x)^T的形式；地理坐标系统下磁阻矢量转换成机体(宿主)坐标系下的磁阻矢量；地理坐标系下加速度矢量转换成机体(宿主)坐标系下加速度矢量；加速度计测得的重力加速度向量与角速度积分后姿态推算的重力加速度向量差积；磁力计测得的磁阻向量与角速度积分后姿态推算的磁阻微量差积；角速度积分误差等于上述两组向量直接的差积和；角速度积分误差做PI修正角速度计零点偏移；四元数微分和四元数归范化；四元数转化成欧拉角；程序一个循环结束。

如图2所示，为本发明设计的运动姿态传感器数据融合与解算方法的四元数原理图。四元数都是由实数加上三个虚数单位i、j、k组成，每个四元数都是1、i、j和k的线性组合。i2＝j2＝k2＝-1，i0＝j0＝k0＝1。对于i、j、k本身的几何意义可以理解为一种旋转，其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转，k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。其中X、Y、Z为空间轴，T为时间轴。

如图3所示，为本发明设计的运动姿态传感器数据融合与解算方法的基于模糊控制的自适应卡尔曼滤波器结构图。自适应滤波基本原理是通过监测残差的量测方差与理论方差的比值来对量测方差进行实时估计。当模型准确时，比值应1附近；当观测噪声发生改变时，则其比值偏离1，需要对观测噪声进行修正。根据上述思想，设计如图3所示的自适应卡尔曼滤波器结构。目前，组合导航系统应用模糊自适应卡尔曼滤波时，由于系统的观测量之间具有相关性，因此都采用一个模糊控制模块，对观测噪声进行整体估计，然而这对于运动姿态传感器来说却是不够的。这是由于运动姿态传感器中的观测向量包含的加速度信息和磁阻信息是完全独立的，因此残差向量的量测方差Ck和理论方差Sk为：

其中CA为加速度的量测方差，SA为理论方差，CAM、CMA、SAM、SMA为加速度和磁阻的量测协方差和理论方差，CM、SM为磁阻的量测方差和理论方差。为防止加速度计和磁力计之间相互干扰，提出的自适应卡尔曼滤波在常规卡尔曼滤波器的基础上增加了两个模糊控制自适应模块(FLAS)，分别取加速度信息和磁力计信息的方差矩阵用于计算。定义qA＝tr(CA)/tr(SA)和qM＝tr(CM)/tr(SM)作为两个模糊控制模块的输入来检测观测噪声，其中tr()为矩阵的迹。

自适应模块的输出为修正权值，用来修正加速度计和磁力计的观测噪声，从而当系统的观测噪声未知时，可以对观测噪声进行实时的、精确的估计，保证了系统的稳定性。