技术领域
[0001] 本
申请涉及
飞行器姿态估计技术领域，尤其涉及基于MEMS传感器的飞行姿态估计方法及系统。
背景技术
[0002] 飞行姿态是指飞行器的三轴在空中相对于某条参考线或某个参考平面或某固定的
坐标系统间的状态。飞行器在空中飞行时具有各种不同的飞行姿态，飞行姿态决定着飞行器的动向，既影响飞行高度，也影响飞行的方向。飞行姿态的估计在飞行器的飞行过程中有至关重要的作用，通过对飞行姿态的估计，可以保证飞行器飞行航向正确，同时保证飞行安全。
[0003] MEMS传感器是航姿参考系统(AHRS)与微惯性
导航系统(Micro-INS)的核心器件，传统的基于MEMS传感器估计飞行姿态的方法，是对MEMS传感器里的
加速度计和
陀螺仪的数据进行解算估计，并进行卡尔曼滤波
迭代，该方法通常存在以下问题，(1)MEMS传感器里的陀螺仪在长时间的运动后会出现累积误差，这是陀螺仪的器件特性并不能消除，累积误差造成漂移会影响到输出的
角度值，影响卡尔曼
滤波器系统方程的准确性；(2)该方法一般适用于稳态运动场景，包括静态模式和恒定转速运动模式，无法应用于实际场景中。在复杂运动模式下，陀螺仪累计漂移造成的误差更明显，输出结果不精确。
发明内容
[0004] 本发明的目的是要提供基于MEMS传感器的飞行姿态估计方法及系统，可以解决上述
现有技术问题中的一个或者多个。
[0005] 根据本发明的一个方面，提供基于MEMS传感器的飞行姿态估计方法，包括以下步骤：
[0006] 采集加速度计和磁
力计输出的数据，确定载体坐标系；
[0007] 将载体坐标系转
化成地面坐标系，根据对应的重力与地球
磁场的向量求误差；
[0008] 使用误差校正陀螺仪输出的数据；
[0009] 使用校正后的陀螺仪输出的数据进行四元数更新，得到新的姿态角；
[0010] 使用新的姿态角进行卡尔曼滤波得到飞行姿态估计结果。
[0011] 其有益效果为：本发明通过加速度计和磁力计的数据计算载体坐标系，再根据对应的重力与地球磁场的向量求误差，使用该误差来校正陀螺仪的输出，使陀螺仪的输出更准确，有效解决陀螺仪产生累计漂移误差的问题，增加卡尔曼滤波器输出结果的精确度；使用四元数参与计算，降低整体的运算量，降低运算成本，加快运算速度；通过解决陀螺仪产生累计漂移误差的问题，使本发明提供的方法可以应用于实际运动场景中复杂的运动模式。
[0012] 在一些实施方式中，采集加速度计和磁力计输出的数据，确定载体坐标系包括：
[0013] 根据加速度计输出的数据，确定初始
横滚角φ和初始
俯仰角θ，公式如下：
[0014]
[0015] 式中，ax、ay和az为加速度计输出的初始数据；
[0016] 载体坐标系与底面坐标系间的转换矩阵为T，当ψ＝0时，转换矩阵T的公式如下：
[0017]
[0018] 运算可得
[0019]
[0020] 式中，φ为初始横滚角，θ为初始俯仰角；
[0021] 用加速度计输出的初始数据补偿磁力计输出的初始数据，即根据初始横滚角φ、初始俯仰角θ和磁力计输出的初始数据，获取补偿后的磁力计数据，计算式如下：
[0022]
[0023] 式中，magx和magy为补偿后的磁力计输出数据，mx、my和mz为磁力计输出的初始数据；
[0024] 用补偿后的磁力计输出数据，计算初始航向角ψ，计算公式如下：
[0025]
[0026] 得到初始姿态角如下：
[0027]
[0028] 其中，φ是初始横滚角，θ是初始俯仰角，ψ是初始航向角。
[0029] 其有益效果为：用加速度计输出的初始数据补偿磁力计输出的初始数据，进一步提高初始姿态角的
精度。
[0030] 在一些实施方式中，将载体坐标系转化成地面坐标系，根据对应参考的重力向量和地磁向量求误差包括：
[0031] 根据α、γ和β获得对应的初始四元数，公式如下：
[0032]
[0033] 用初始四元数求得转换矩阵T，转换矩阵T如下：
[0034]
[0035] 对加速度计输出的初始数据进行归一化处理，处理公式如下：
[0036]
[0037] 计算重力和磁场的方向向量，公式如下：
[0038]
[0039] 使用处理后的ax′、ay′和az′与重力和磁场的方向向量计算误差，公式如下：
[0040]
[0041] 式中，ex、ey和ez为自定义误差初始值，ax′、ay′和az′为归一化处理后的加速度计输出数据，vx、vy和vz为重力和磁场的方向向量。
[0042] 一些实施方式中，使用误差校正陀螺仪输出的数据使用如下公式：
[0043]
[0044] 式中，gx、gy和gz为陀螺仪输出的初始
角速度，KP为比例调节系数，Ki为积分调节系数。
[0045] 其有益效果为：对陀螺仪输出的数据进行补偿，提高量测精度。
[0046] 在一些实施方式中，比例调节系数KP＝1且积分调节系数Ki＝0。
[0047] 在一些实施方式中，使用校正后的陀螺仪输出的数据进行四元数更新，得到新的姿态角包括：
[0048] 用校正后的角速度gx′、gy′和gz′与初始四元数q0、q1、q2和q3计算初始四元数的导数；
[0049]
[0050] 式中， 和 为初始四元数的导数；
[0051] 用初始四元数的导数求积分，得到新的四元数，公式如下：
[0052]
[0053] 式中，q0′、q1′、q2′和q3′为新的四元数；
[0054] 将新的四元数带入转换矩阵T，得到转换矩阵T如下：
[0055]
[0056] 计算姿态角，公式如下
[0057]
[0058] 式中，φ′是新的横滚角，θ′是新的俯仰角，ψ′是新的航向角。
[0059] 在一些实施方式中，使用新的姿态角进行卡尔曼滤波得到飞行姿态估计结果包括：
[0061]
[0062] 式中，Xk为k时刻的状态矢量，q为使用龙格-库塔方法求得的离散时间模型，t为
采样时间，M为运算矩阵，M的计算式如下：
[0063]
[0064] 其中，gx′、gy′和gz′为校正后的陀螺仪输出，t为采样时间；
[0065] 离散时间模型q如下：
[0066]
[0067] 式中，t为采样时间，Ωb表示航标坐标系上的角速度的分量，可以通过陀螺仪校正后的输出数据得到，Ωb矩阵类型表示为如下公式：
[0068]
[0069] 式中，gx′、gy′和gz′为校正后的陀螺仪输出；
[0070] 确定测量矩阵H(k)，测量矩阵H(k)公式如下：
[0071]
[0072] 式中，g表示
重力加速度，q0′、q1′、q2′和q3′表示新的四元数；
[0073] 进行卡尔曼滤波基本
算法编排，该算法流程如下：
[0074] 状态更新方程：
[0075]
[0076] 一步预测均方误差方程：
[0077] P(k,k-1)＝Φ(k,k-1)P(k,k-1)ΦT(k,k-1)+Q(k)
[0078] 滤波增益矩阵方程：
[0079] K(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R(k)]-1
[0080] 状态估值计算方程：
[0081]
[0082] 估计均方误差方程
[0083] P(k)＝[I-K(k)H(k)]K(k,K-1)[I-K(k)H(k)]T+K(k)R(k)KT(k)。
[0084] 第二方面，本发明
实施例提供了基于MEMS传感器的飞行姿态确定系统，包括：加速度计、磁力计、陀螺仪、处理器和滤波器，加速度计、磁力计和陀螺仪的数据输出端与处理器的输入端相连，处理器的数据输出端与滤波器的输入端相连。
[0085] 另外，在本发明技术方案中，凡未作特别说明的，均可通过采用本领域中的常规手段来实现本技术方案。
附图说明
[0086] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0087] 图1是本申请一实施例提供的基于MEMS传感器的飞行姿态估计方法的
流程图。
[0088] 图2是本申请另一实施例提供的基于MEMS传感器的飞行姿态估计系统的结构示意图。
具体实施方式
[0089] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0090] 实施例1：
[0091] 如图1所示为本发明一实施例提供的一种基于MEMS传感器的飞行姿态估计方法，包括以下步骤：
[0092] S11：采集加速度计和磁力计输出的数据，确定载体坐标系。
[0093] 在可选的实施例中，采集加速度计和磁力计输出的数据，确定载体坐标系包括：
[0094] 根据加速度计输出的数据，确定初始横滚角φ和初始俯仰角θ，公式如下：
[0095]
[0096] 式中，ax、ay和az为加速度计输出的初始数据；
[0097] 载体坐标系与底面坐标系间的转换矩阵为T，当ψ＝0时，转换矩阵T的公式如下：
[0098]
[0099] 运算可得
[0100]
[0101] 式中，φ为初始横滚角，θ为初始俯仰角；
[0102] 根据初始姿态角信息和磁力计数据，可以求得获取航向角信息所需的参数，公式如下：
[0103]
[0104] 式中，Xh、Yh和Zh的含义是，当载体坐标系与地面坐标重合时，Xh、Yh和Zh为地球磁场在载体坐标系三轴方向的分量；
[0105] 航向角ψ可以用下列公式计算得到：
[0106]
[0107] 根据Xh、Yh的变化范围，可以得到航向角ψ的一般计算式为：
[0108]
[0109] 用加速度计输出的初始数据补偿磁力计输出的初始数据，即根据初始横滚角φ、初始俯仰角θ和磁力计输出的初始数据，获取补偿后的磁力计数据，计算式如下：
[0110]
[0111] 式中，magx和magy为补偿后的磁力计输出数据，mx、my和mz为磁力计输出的初始数据；
[0112] 用补偿后的磁力计输出数据，计算初始航向角ψ，计算公式如下：
[0113]
[0114] 得到初始姿态角如下：
[0115]
[0116] 其中，φ是初始横滚角，θ是初始俯仰角，ψ是初始航向角。
[0117] S12：将载体坐标系转化成地面坐标系，根据对应的重力与地球磁场的向量求误差。
[0118] 在可选的实施例中，将载体坐标系转化成地面坐标系，根据对应参考的重力向量和地磁向量求误差包括：
[0119] 根据α、γ和β获得对应的初始四元数，公式如下：
[0120]
[0121] 用初始四元数求得转换矩阵T，转换矩阵T如下：
[0122]
[0123] 对加速度计输出的初始数据进行归一化处理，处理公式如下：
[0124]
[0125] 计算重力和磁场的方向向量，公式如下：
[0126]
[0127] 使用ax′、ay′和az′与重力和磁场的方向向量计算误差，公式如下：
[0128]
[0129] 式中，ex、ey和ez为自定义误差初始值，ax′、ay′和az′为归一化处理后的加速度计输出数据，vx、vy和vz为重力和磁场的方向向量。
[0130] S13：使用误差校正陀螺仪输出的数据。
[0131] 在可选的实施例中，使用误差校正陀螺仪输出的数据使用如下公式：
[0132]
[0133] 式中，gx、gy和gz为陀螺仪输出的初始角速度，KP为比例调节系数，Ki为积分调节系数。
[0134] 在可选的实施例中，比例调节系数KP＝1且积分调节系数Ki＝0。
[0135] S14：使用校正后的陀螺仪输出的数据进行四元数更新，得到新的姿态角。
[0136] 在可选的实施例中，使用校正后的陀螺仪输出的数据进行四元数更新，得到新的姿态角包括：
[0137] 用校正后的角速度gx′、gy′和gz′与初始四元数q0、q1、q2和q3计算初始四元数的导数；
[0138]
[0139] 式中， 和 为初始四元数的导数；
[0140] 用初始四元数的导数求积分，得到新的四元数，公式如下：
[0141]
[0142] 式中，q0′、q1′、q2′和q3′为新的四元数；
[0143] 将新的四元数带入转换矩阵T，得到转换矩阵T如下：
[0144]
[0145]
[0146] 计算姿态角，公式如下：
[0147]
[0148] 式中，φ′是新的横滚角，θ′是新的俯仰角，ψ′是新的航向角。
[0149] S15：使用新的姿态角进行卡尔曼滤波得到飞行姿态估计结果。
[0150] 卡尔曼滤波器是一种高效率的递归滤波器，会根据各测量量在不同时间下的值，考虑各时间下的联合分布再产生对未知变数的估计，因此比只以单一测量量为
基础的估计方式要更加准确。
[0151] 在可选的实施例中，使用新的姿态角进行卡尔曼滤波得到飞行姿态估计结果包括：
[0152] 确定状态方程如下：
[0153]
[0154] 式中，Xk为k时刻的状态矢量，q为使用龙格-库塔方法求得的离散时间模型，t为采样时间，M为运算矩阵，M的计算式如下：
[0155]
[0156] 其中，gx′、gy′和gz′为校正后的陀螺仪输出，t为采样时间；
[0157] 离散时间模型q如下：
[0158]
[0159] 式中，t为采样时间，Ωb表示航标坐标系上的角速度的分量，可以通过陀螺仪校正后的输出数据得到，Ωb矩阵类型表示为如下公式：
[0160]
[0161] 式中，gx′、gy′和gz′为校正后的陀螺仪输出；
[0162] 确定测量矩阵H(k)，测量矩阵H(k)公式如下：
[0163]
[0164] 式中，g表示重力加速度，q0′、q1′、q2′和q3′表示新的四元数；
[0165] 进行卡尔曼滤波基本算法编排，该算法流程如下：
[0166] 状态更新方程：
[0167]
[0168] 一步预测均方误差方程：
[0169] P(k,k-1)＝Φ(k,k-1)P(k,k-1)ΦT(k,k-1)+Q(k)
[0170] 滤波增益矩阵方程：
[0171] K(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R(k)]-1
[0172] 状态估值计算方程：
[0173]
[0174] 估计均方误差方程
[0175] P(k)＝[I-K(k)H(k)]K(k,K-1)[I-K(k)H(k)]T+K(k)R(k)KT(k)。
[0176] 本发明通过加速度计和磁力计的数据计算载体坐标系，再根据对应的重力与地球磁场的向量求误差，使用该误差来校正陀螺仪的输出，使陀螺仪的输出更准确，有效解决陀螺仪产生累计漂移误差的问题，增加卡尔曼滤波器输出结果的精确度；使用四元数参与计算，降低整体的运算量，降低运算成本，加快运算速度；通过解决陀螺仪产生累计漂移误差的问题，使本发明提供的方法可以应用于实际运动场景中复杂的运动模式。
[0177] 实施例2：
[0178] 如图2所示为本发明实施例2提供的一种基于MEMS传感器的飞行姿态确定系统，包括加速度计1、磁力计2、陀螺仪3、处理器4和滤波器5，其中，加速度计1、磁力计2和陀螺仪3的数据输出端与处理器4的输入端相连，处理器4的数据输出端与滤波器5的输入端相连。处理器4用于获取加速度计1、磁力计2和陀螺仪3测量数据并进行处理，将处理后的结果发送至滤波器5进行飞行姿态的估计。
[0179] 上述实施例2的基于MEMS传感器的飞行姿态确定系统可用于执行本发明实施例1的基于MEMS传感器的飞行姿态估计方法，并相应的达到上述本发明实施例1的基于MEMS传感器的飞行姿态估计方法所达到的技术效果，这里不再赘述。
[0180] 以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模
块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
[0181] 通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助
软件加必需的通用
硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，
服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
[0182] 最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行
修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
