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

本发明专利技术涉及一种运动姿态传感器的数据融合与解算方法，属于多传感器数据融合技术。首先，处理器对采集到的X、Y、Z三轴角速度、加速度、复合磁场强度模拟信号进行模数转换为数字信号；其次，对三轴角速度进行积分获得初始四元数数据，该数据会有积分误差；然后，引入加速度数据和复合磁场强度数据对初始四元数进行校正，观察矩阵为重力加速度和正北方向的磁场；然后，使用基于模糊控制的自适应卡尔曼滤波器将三轴角速度、加速度和复合磁场强度进行融合并输入到四元数微分方程，得到四元数；最后，将四元数转换成欧拉角并输出。

【技术实现步骤摘要】
一种运动姿态传感器的数据融合与解算方法
本专利技术公开一种运动姿态传感器的数据融合与解算方法，具体涉及多种惯性器件数据融合技术。
技术介绍
数据融合是多元信息综合处理的一项新技术，比较确切的定义可概括为：充分利用不同的时间和空间的多传感器信息资源，采用计算机技术对按时序获得的多传感器观测信息在一定的准则下加以自动分析、综合、支配和使用，获得被测对象的一致性解释与描述，以完成所需的决策和估计任务，使系统获得比它的各个组成部分更优越的性能。数据融合从20世纪70年代末被提出，源于军事领域的C3I(command，control，communicationandintelligence)系统的需要，当时称为多源相关、多传感器混合数据融合，并于80年代建立其技术。美国是数据融合技术起步最早的国家，1983年，美国国防高级研究计划局(DARPA)推出的战略计算机计划中，将多传感器数据融合列为重大研究课题。1984年，美国国防部(DOD)成立了数据融合专家组(DFS-DataFusionSubanal)，负责指导、组织并协调这一国防关键技术的系统研究，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＝q02+q12-q22-q32R1，2＝2(q1q2+q0q3)R1，3＝2(q1q3-q2q0)R2，1＝2(q1q2-q0q3)R2，2＝q02-q12+q22-q32R2，3＝2(q3q2+q0q1)R3，1＝2(q1q3+q2q0)R3，2＝2(q3q2-q0q1)R3，3＝q02-q12-q22-q32四元数微分方程其中输入方程为角速度计输出值，宿主坐标系下真实的角速度，传感器噪声矢量，角速度漂移矢量，则有状态方程因为所以输出方程其中，为重力加速度矢量，为正北磁场的矢量线性化下的雅可比矩阵离散化模型离散时间的一阶近似Φ≌I+FTΓ≌GT滤波过程Pk|k-1＝(I+FT)Pk|k-1(I+FT)T+T2GQGTK＝Pk|k-1HT(HPk|k-1HT+R)-1Pk＝Pk|k-1-KHPk|k-1其中，为状态预测值，Pk|k-1为均方差预测，K为最优卡尔曼增益，αk为新息向量，为状态估计，Pk为均方差估计，αk又被称为残差向量，它反映了滤波模型的准确性。再次，所述的数据解算算法是将四元数根据运动姿态传感器的宿主运动特征按一定的规则进行欧拉角的计算，得到相应取值范围的姿态角数据的算法，包括如下计算方法和步骤：(1)四元数乘法设有两四元数QA和QB，其乘法关系为旋转四元数设一向量及旋转后向量四元数满足R′＝Q-1·R·Q四元数倒数(2)四元数归一当四元数归一化后|Q|＝1Q-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级本文档来自技高网...

【技术保护点】
1.一种运动姿态传感器的数据融合与解算方法，其特征在于：所述运动姿态传感器能够输出的数据包括：宿主原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据，经过融合但未经处理的宿主四元数数据，经过解算和处理的宿主姿态角数据，包括航向角、俯仰角、横滚角、倾斜角；所述原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据是运动姿态传感器的角速度计、加速度计和磁力计将采集到的X、Y、Z三个轴向的角速度、加速度、地球磁场和环境磁场之复合磁场强度等模拟数据发送给微处理器后进行模数转换后的数字化数据；所述经过融合但未经处理的宿主四元数数据是微处理器对原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据按预设好的融合算法进行数据融合而得到的超复数，它由实数加上三个虚数单位i、j、k组成，四元数一般可表示为

【技术特征摘要】
1.一种运动姿态传感器的数据融合与解算方法，其特征在于：所述运动姿态传感器能够输出的数据包括：宿主原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据，经过融合但未经处理的宿主四元数数据，经过解算和处理的宿主姿态角数据，包括航向角、俯仰角、横滚角、倾斜角；所述原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据是运动姿态传感器的角速度计、加速度计和磁力计将采集到的X、Y、Z三个轴向的角速度、加速度、地球磁场和环境磁场之复合磁场强度等模拟数据发送给微处理器后进行模数转换后的数字化数据；所述经过融合但未经处理的宿主四元数数据是微处理器对原始X、Y、Z三个轴向的角速度、加速度、复合磁场强度ADC数据按预设好的融合算法进行数据融合而得到的超复数，它由实数加上三个虚数单位i、j、k组成，四元数一般可表示为其中n为旋转方向，θ/2为转角大小，i，j，k关系为但该四元数据没有经过欧拉角解算和数据滤波、修正补偿等，四元数表示三维空间中的一个旋转；所述经过解算和处理的宿主姿态角数据是微处理器对经过融合的四元数按预设好的数据解算算法解算出初始欧拉角，即航向角、俯仰角、横滚角、倾斜角，这些姿态数据再经过预设的滤波修正算法进行处理，得到相对精确的宿主姿态角数据。2.根据权利要求1所述的一种运动姿态传感器的数据融合与解算方法，其特征在于：所述的数据融合算法是将原始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)角速度积分偏差修正，通过角速度微分方程可以得到四元数，问题是角速度计会有积分漂移，得到的角度会有误...

【专利技术属性】
技术研发人员：王立颖，路军，
申请(专利权)人：路军，
类型：发明
国别省市：辽宁,21