CN112945271B - 磁力计信息辅助的mems陀螺仪标定方法及标定系统 - Google Patents

磁力计信息辅助的mems陀螺仪标定方法及标定系统 Download PDF

Info

Publication number
CN112945271B
CN112945271B CN202110113501.0A CN202110113501A CN112945271B CN 112945271 B CN112945271 B CN 112945271B CN 202110113501 A CN202110113501 A CN 202110113501A CN 112945271 B CN112945271 B CN 112945271B
Authority
CN
China
Prior art keywords
magnetometer
mems
coordinate system
representing
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110113501.0A
Other languages
English (en)
Other versions
CN112945271A (zh
Inventor
徐祥
孙逸帆
李凤
陈洋豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN202110113501.0A priority Critical patent/CN112945271B/zh
Priority to PCT/CN2021/077089 priority patent/WO2022160391A1/zh
Publication of CN112945271A publication Critical patent/CN112945271A/zh
Application granted granted Critical
Publication of CN112945271B publication Critical patent/CN112945271B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种磁力计信息辅助的MEMS陀螺仪标定方法及标定系统，包括以下步骤：获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；对所述磁力计数据进行标定处理，获得标定后的磁力计数据；标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。其利用标定后的MEMS磁力计数据对MEMS陀螺仪标定，实现了低成本MEMS陀螺仪标定。

Description

磁力计信息辅助的MEMS陀螺仪标定方法及标定系统
技术领域
本发明涉及惯性导航系统技术领域，具体涉及一种磁力计信息辅助的MEMS陀螺仪标定方法及标定系统。
背景技术
为保证惯性导航系统精度，MEMS惯性器件在使用时，必须经过标定处理。对于MEMS陀螺仪标定，传统方法通常利用高精度转台对陀螺仪进行标定。但利用高精度转台对MEMS陀螺仪进行标定，存在成本过高等问题。近期，有学者提出利用MEMS加速度计数据作为辅助信息对MEMS磁力计进行标定，但在实际试验中，MEMS加速度计输出数据中包括部分外部加速度，外部加速度对标定存在干扰，降低了MEMS陀螺仪的标定精度。
发明内容
本发明要解决的技术问题是提供一种磁力计信息辅助的MEMS陀螺仪标定方法及标定系统，其利用标定后的MEMS磁力计数据对MEMS陀螺仪标定，实现了低成本MEMS陀螺仪标定。
为了解决上述技术问题，本发明提供了一种磁力计信息辅助的MEMS陀螺仪标定方法，包括以下步骤：
S1、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；
S2、对所述磁力计数据进行标定处理，获得标定后的磁力计数据；
S3、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；
S4、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。
作为优选的，在S1中，所述实时陀螺仪数据还包括静止的陀螺仪数据；所述S1与S3之间还包括：
根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小；
根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵。
作为优选的，所述S1与S3之间还包括：
根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值。
作为优选的，所述S1与S3之间还包括：
根据经验值设定误差协方差矩阵。
作为优选的，所述S3中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：
将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。
作为优选的，所述S2具体包括；
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：
Figure BDA0002919801400000031
其中，ms表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure BDA0002919801400000032
表示由导航坐标系转向载体坐标系的方向余弦矩阵。mn表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量；
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：
Figure BDA0002919801400000033
其中，k表示测量数据的标签，代表第k次的测量；
Figure BDA0002919801400000034
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure BDA0002919801400000035
表示地磁矢量在载体坐标系下第k次投影；b表示MEMS磁力计的零偏误差矢量；λk表示拉格朗日常数；
通过牛顿法对MEMS磁力计极大似然函数F进行估计：
Figure BDA0002919801400000036
其中，i表示牛顿法迭代次数，代表第i次的迭代；X(i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X(i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure BDA0002919801400000041
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure BDA0002919801400000042
代表第i次迭代中极大似然函数F的雅克比向量；
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：
mb＝Rm(ms-b)，
其中，mb表示MEMS磁力计的输出在载体坐标系下的投影；Rm代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。
作为优选的，所述S3具体包括：
由哥氏定理获得：
Figure BDA0002919801400000043
其中，
Figure BDA0002919801400000044
表示地磁矢量在导航坐标系下的投影对于时间的微分；
Figure BDA0002919801400000045
表示地磁矢量在载体坐标系下的投影对于时间的微分；
Figure BDA0002919801400000046
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；mb表示地磁矢量在载体坐标系下的投影；
由于地磁矢量在导航坐标系下的投影为常值，所以：
Figure BDA0002919801400000047
其中，
Figure BDA0002919801400000048
表示地磁矢量在导航坐标系下的投影对于时间的微分；0表示零向量。
构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：
Figure BDA0002919801400000051
其中，
Figure BDA0002919801400000052
表示地磁矢量在载体坐标系下的投影对于时间的微分；mb表示地磁矢量在载体坐标系下的投影；×表示矢量间的向量积运算；
Figure BDA0002919801400000053
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；
递推最小二乘法估计MEMS陀螺仪数据误差，包括：
Figure BDA0002919801400000054
Figure BDA0002919801400000055
Kk＝Pk|k-1Mk T(MkPk|k-1Mk T+Rk)-1，
Figure BDA0002919801400000056
Pk|k＝(I-KkMk)Pk|k-1，
其中，
Figure BDA0002919801400000057
表示k时刻递推最小二乘法一步预测；
Figure BDA0002919801400000058
表示k-1时刻递推最小二乘法最优状态估计；
Figure BDA0002919801400000059
表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；Mk表示k时刻递推最小二乘估计中观测矩阵；Kk表示k时刻的增益矩阵；Pk|k-1表示k时刻一步预测状态误差协方差矩阵；Rk表示k时刻量测噪声协方差矩阵；
Figure BDA00029198014000000510
表示k时刻递推最小二乘法最优估计状态；Pk|k表示k时刻的误差协方差矩阵；I表示单位矩阵。
作为优选的，所述S4具体包括：
MEMS陀螺仪误差标定表达式：
Figure BDA0002919801400000061
其中，
Figure BDA0002919801400000062
表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；Rw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；dw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵与零偏误差向量的积，ws表示MEMS陀螺仪的输出。
本发明公开了一种磁力计信息辅助的MEMS陀螺仪标定系统，包括：
数据采集模块，所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；
磁力计标定模块，所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据；
误差参数估值模块，所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；
陀螺仪标定模块，所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。
本发明公开了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述方法的步骤。
本发明的有益效果：
1、本发明在无高精度外部设备辅助的情况下，提出了一种利用MEMS磁力计作为辅助信息的MEMS陀螺仪标定的方法，本发明通过对MEMS磁力计数据进行标定，利用标定后的MEMS磁力计数据对MEMS陀螺仪标定，实现了低成本MEMS陀螺仪标定。
2、本发明采用基于极大似然法的全误差估计模型对MEMS磁力计进行标定，提高了MEMS磁力计标定精度，进而提高了MEMS陀螺仪的标定精度；
3、本发明采用递推最小二乘法对MEMS陀螺仪进行估计，计算精度高，计算效率高；
4、本发明采用无高精度外部辅助设备对MEMS陀螺仪进行标定，节省了标定成本，提高了MEMS陀螺仪的实用性。
附图说明
图1为磁力计信息辅助的MEMS陀螺仪标定方法的流程图；
图2为MEMS陀螺仪标定零偏误差图；
图3为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第一行向量误差图；
图4为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第二行向量误差图；
图5为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第三行向量误差图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
参照图1-图5所示，本发明的公开了一种磁力计信息辅助的MEMS陀螺仪标定方法，包括以下步骤：
步骤一、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据和静止的陀螺仪数据。
步骤二、对所述磁力计数据进行标定处理，获得标定后的磁力计数据，具体包括：
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：
Figure BDA0002919801400000081
其中，ms表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure BDA0002919801400000082
表示由导航坐标系转向载体坐标系的方向余弦矩阵。mn表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量；
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：
Figure BDA0002919801400000083
其中，k表示测量数据的标签，代表第k次的测量；
Figure BDA0002919801400000091
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure BDA0002919801400000092
表示地磁矢量在载体坐标系下第k次投影；b表示MEMS磁力计的零偏误差矢量；λk表示拉格朗日常数；
通过牛顿法对MEMS磁力计极大似然函数F进行估计：
Figure BDA0002919801400000093
其中，i表示牛顿法迭代次数，代表第i次的迭代；X(i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X(i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure BDA0002919801400000094
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure BDA0002919801400000095
代表第i次迭代中极大似然函数F的雅克比向量；
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：
mb＝Rm(ms-b)，
其中，mb表示MEMS磁力计的输出在载体坐标系下的投影；Rm代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。
之后，根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小，根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵；根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值；根据经验值设定误差协方差矩阵。
步骤三、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：
由哥氏定理获得：
Figure BDA0002919801400000101
其中，
Figure BDA0002919801400000102
表示地磁矢量在导航坐标系下的投影对于时间的微分；
Figure BDA0002919801400000103
表示地磁矢量在载体坐标系下的投影对于时间的微分；
Figure BDA0002919801400000104
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；mb表示地磁矢量在载体坐标系下的投影；
由于地磁矢量在导航坐标系下的投影为常值，所以：
Figure BDA0002919801400000105
其中，
Figure BDA0002919801400000106
表示地磁矢量在导航坐标系下的投影对于时间的微分；0表示零向量。
构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：
Figure BDA0002919801400000107
其中，
Figure BDA0002919801400000108
表示地磁矢量在载体坐标系下的投影对于时间的微分；mb表示地磁矢量在载体坐标系下的投影；×表示矢量间的向量积运算；
Figure BDA0002919801400000109
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；
递推最小二乘法估计MEMS陀螺仪数据误差，包括：
Figure BDA00029198014000001010
Figure BDA00029198014000001011
Kk＝Pk|k-1Mk T(MkPk|k-1Mk T+Rk)-1。
Figure BDA0002919801400000111
Pk|k＝(I-KkMk)Pk|k-1，
其中，
Figure BDA0002919801400000112
表示k时刻递推最小二乘法一步预测；
Figure BDA0002919801400000113
表示k-1时刻递推最小二乘法最优状态估计；
Figure BDA0002919801400000114
表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；Mk表示k时刻递推最小二乘估计中观测矩阵；Kk表示k时刻的增益矩阵；Pk|k-1表示k时刻一步预测状态误差协方差矩阵；Rk表示k时刻量测噪声协方差矩阵；
Figure BDA0002919801400000115
表示k时刻递推最小二乘法最优估计状态；Pk|k表示k时刻的误差协方差矩阵；I表示单位矩阵。
步骤四、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。
所述步骤三中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：
将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：MEMS陀螺仪误差标定表达式：
Figure BDA0002919801400000116
其中，
Figure BDA0002919801400000117
表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；Rw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；dw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵与零偏误差向量的积；ws表示MEMS陀螺仪的输出。
本发明公开了一种磁力计信息辅助的MEMS陀螺仪标定系统，包括数据采集模块、磁力计标定模块、误差参数估值模块和陀螺仪标定模块。
所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据。
所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据。
所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。
所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。
本发明中，估计的MEMS陀螺仪误差参数包括两项，一项为零偏误差，一项是标度因子误差与非正交误差的耦合项。
图2表示的是MEMS陀螺仪误差参数中，零偏误差估计值与真实值间的差值(理想结果差值为0)随数据迭代的变化，为3x1的列向量(分别是x轴,y轴,z轴方向)，图中(a),(b),(c)分别表示零偏误差估计值与真实值间的差值在三个轴x轴,y轴,z轴上随数据迭代的变化。由图2可以看出，经过递推最小二乘法得到的MEMS陀螺仪零偏误差的估计值与真实值(仿真设定)的差值小于0.01deg/s，说明我们递推最小二乘法估计得到的MEMS陀螺仪零偏误差较为准确，效果较好。
图3为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第一行向量误差图，其中，图3(a)中表示的是标度因子与非正交误差的耦合项矩阵第一行，第一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图3(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第一行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图3(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第一行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。
图4为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第二行向量误差图，其中，图4(a)中表示的是标度因子与非正交误差的耦合项矩阵第二行，第一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图4(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第二行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图4(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第二行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。
图5为MEMS陀螺仪标定标度因子与非正交误差耦合矩阵第三行向量误差图，其中，5(a)中表示的是标度因子与非正交误差的耦合项矩阵第三行，第一列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图5(b)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第三行，第二列的元素与真实值(仿真设定)间的差值随数据迭代的变化；图5(c)中表示的是表示的是标度因子与非正交误差的耦合项矩阵第三行，第三列的元素与真实值(仿真设定)间的差值随数据迭代的变化。
从图3-图5可看出，经过递推最小二乘法得到的MEMS陀螺仪标度因子和非正交误差耦合项矩阵各项与真实值(仿真设定)的差值在10的负3至10的负4次方量级，说明经过递推最小二乘法得到的误差参数估计值误差低，准确度高。
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，包括以下步骤：
S1、获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；
S2、对所述磁力计数据进行标定处理，获得标定后的磁力计数据；
S3、标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；
S4、根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理；
其中，所述S2具体包括:
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：
Figure FDA0003892413460000011
其中，ms表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure FDA0003892413460000021
表示由导航坐标系转向载体坐标系的方向余弦矩阵；mn表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量；
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：
Figure FDA0003892413460000022
其中，k表示测量数据的标签，代表第k次的测量；
Figure FDA0003892413460000023
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；
Figure FDA0003892413460000024
表示地磁矢量在载体坐标系下第k次投影；λk表示拉格朗日常数；
通过牛顿法对MEMS磁力计极大似然函数F进行估计：
Figure FDA0003892413460000025
其中，i表示牛顿法迭代次数，代表第i次的迭代；X(i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X(i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure FDA0003892413460000026
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure FDA0003892413460000027
代表第i次迭代中极大似然函数F的雅克比向量；
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：
mb＝Rm(ms-b)，
其中，mb表示MEMS磁力计的输出在载体坐标系下的投影；Rm代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量。
2.如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，在S1中，所述实时陀螺仪数据还包括静止的陀螺仪数据；所述S1与S3之间还包括：
根据静止的MEMS陀螺仪数据，获得MEMS陀螺仪随机噪声的方差大小；
根据MEMS陀螺仪随机噪声的方差大小设定递推最小二乘法初始条件中噪声协方差矩阵。
3.如权利要求2所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S1与S3之间还包括：
根据MEMS陀螺仪误差参数特性设定MEMS陀螺仪误差参数的理想值。
4.如权利要求3所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S1与S3之间还包括：
根据经验值设定误差协方差矩阵。
5.如权利要求4所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S3中利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值，具体包括：
将噪声协方差矩阵、误差协方差矩阵和MEMS陀螺仪误差估计参数的理想值作为递推最小二乘法方程的迭代初值，利用递推最小二乘法对MEMS陀螺仪的误差参数进行估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值。
6.如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S3具体包括：
由哥氏定理获得：
Figure FDA0003892413460000041
其中，
Figure FDA0003892413460000042
表示地磁矢量在导航坐标系下的投影对于时间的微分；
Figure FDA0003892413460000043
表示地磁矢量在载体坐标系下的投影对于时间的微分；
Figure FDA0003892413460000044
表示载体坐标系相对当地导航坐标系的角速度矢量在载体系轴向的分量构成的列向量；×表示矢量间的向量积运算；mb表示地磁矢量在载体坐标系下的投影；
由于地磁矢量在导航坐标系下的投影为常值，所以：
Figure FDA0003892413460000045
其中，0表示零向量；
构建MEMS磁力计数据与MEMS陀螺仪数据的关系式：
Figure FDA0003892413460000046
递推最小二乘法估计MEMS陀螺仪数据误差，包括：
Figure FDA0003892413460000047
Figure FDA0003892413460000048
Kk＝Pk|k-1Mk T(MkPk|k-1Mk T+Rk)-1，
Figure FDA0003892413460000049
Pk|k＝(I-KkMk)Pk|k-1，
其中，
Figure FDA00038924134600000410
表示k时刻递推最小二乘法一步预测；
Figure FDA00038924134600000411
表示k-1时刻递推最小二乘法最优状态估计；
Figure FDA0003892413460000051
表示k时刻经过标定后的MEMS磁力计在载体坐标系下的矢量输出对于时间的微分，表示k时刻量测；Mk表示k时刻递推最小二乘估计中观测矩阵；Kk表示k时刻的增益矩阵；Pk|k-1表示k时刻一步预测状态误差协方差矩阵；Rk表示k时刻量测噪声协方差矩阵；
Figure FDA0003892413460000052
表示k时刻递推最小二乘法最优估计状态；Pk|k表示k时刻的误差协方差矩阵；I表示单位矩阵。
7.如权利要求1所述的磁力计信息辅助的MEMS陀螺仪标定方法，其特征在于，所述S4具体包括：
MEMS陀螺仪误差标定表达式：
Figure FDA0003892413460000053
其中，
Figure FDA0003892413460000054
表示载体坐标系相对当地导航坐标系的MEMS陀螺仪的输出角速度矢量在载体系轴向的分量构成的列向量；Rw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵；dw表示MEMS陀螺仪标度因子与非正交误差耦合误差的补偿矩阵与零偏误差向量的积，ws表示MEMS陀螺仪的输出。
8.一种磁力计信息辅助的MEMS陀螺仪标定系统，其特征在于,包括：
数据采集模块，所述数据采集模块获取传感器实时数据，所述传感器实时数据包括实时陀螺仪数据和磁力计数据,所述实时陀螺仪数据包括运动的陀螺仪数据；
磁力计标定模块，所述磁力计标定模块对所述磁力计数据进行标定处理，获得标定后的磁力计数据，具体包括:
对获取的磁力计数据进行标定处理，定义标定磁力计所需参考坐标系：
b代表载体坐标系，其表示捷联惯性导航系统三轴正交坐标系，其x轴、y轴和z轴分别指向载体的右-前-上；
s代表非正交坐标系，其表示MEMS磁力计三轴非正交坐标系，表示其x轴、y轴和z轴与载体坐标系下的x轴、y轴和z轴存在一定的角度偏差；
n代表导航坐标系，其表示载体所在位置的地理坐标系，其三轴分别指向当地东向、北向和天向；
根据MEMS磁力计结构，得到MEMS磁力计在非正交坐标系下的输出模型：
Figure FDA0003892413460000061
其中，ms表示MEMS磁力计在非正交系下的矢量输出；S表示MEMS磁力计的标度因子误差矩阵；Cno表示MEMS磁力计的非正交误差矩阵；
Figure FDA0003892413460000062
表示由导航坐标系转向载体坐标系的方向余弦矩阵；mn表示地磁矢量在导航坐标系下的投影；b表示MEMS磁力计的零偏误差矢量；ε表示MEMS磁力计的随机噪声矢量；
根据MEMS磁力计在非正交坐标系下的输出，在最优估计模型下构建出MEMS磁力计极大似然函数F：
Figure FDA0003892413460000063
其中，k表示测量数据的标签，代表第k次的测量；
Figure FDA0003892413460000064
表示MEMS磁力计在非正交坐标系下第k次的矢量输出；
Figure FDA0003892413460000065
表示地磁矢量在载体坐标系下第k次投影；λk表示拉格朗日常数；
通过牛顿法对MEMS磁力计极大似然函数F进行估计：
Figure FDA0003892413460000066
其中，i表示牛顿法迭代次数，代表第i次的迭代；X(i+1)代表第i+1次迭代中的MEMS磁力计误差参数估计值；X(i)代表第i次迭代中的MEMS磁力计误差参数估计值；
Figure FDA0003892413460000071
代表第i次迭代中极大似然函数F的黑塞矩阵；
Figure FDA0003892413460000072
代表第i次迭代中极大似然函数F的雅克比向量；
通过牛顿法计算得出MEMS磁力计的误差参数值，利用MEMS磁力计标定模型进行标定：
mb＝Rm(ms-b)，
其中，mb表示MEMS磁力计的输出在载体坐标系下的投影；Rm代表通过极大似然估计法估计出MEMS磁力计的，关于标度因子与非正交误差的耦合项的误差补偿矩阵；b表示通过极大似然估计法估计出的零偏误差的补偿向量；
误差参数估值模块，所述误差参数估值模块将标定后的磁力计数据与运动的陀螺仪数据构建方程，利用递推最小二乘法对参数迭代估计直至数据终止，获得MEMS陀螺仪的误差参数的估计值；
陀螺仪标定模块，所述陀螺仪标定模块根据MEMS陀螺仪的误差参数的估计值对MEMS陀螺仪进行标定处理。
9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1到7任一项所述方法的步骤。
CN202110113501.0A 2021-01-27 2021-01-27 磁力计信息辅助的mems陀螺仪标定方法及标定系统 Active CN112945271B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110113501.0A CN112945271B (zh) 2021-01-27 2021-01-27 磁力计信息辅助的mems陀螺仪标定方法及标定系统
PCT/CN2021/077089 WO2022160391A1 (zh) 2021-01-27 2021-02-20 磁力计信息辅助的mems陀螺仪标定方法及标定系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110113501.0A CN112945271B (zh) 2021-01-27 2021-01-27 磁力计信息辅助的mems陀螺仪标定方法及标定系统

Publications (2)

Publication Number Publication Date
CN112945271A CN112945271A (zh) 2021-06-11
CN112945271B true CN112945271B (zh) 2022-12-06

Family

ID=76238152

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110113501.0A Active CN112945271B (zh) 2021-01-27 2021-01-27 磁力计信息辅助的mems陀螺仪标定方法及标定系统

Country Status (2)

Country Link
CN (1) CN112945271B (zh)
WO (1) WO2022160391A1 (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114526756B (zh) * 2022-01-04 2023-06-16 华南理工大学 一种无人机机载多传感器校正方法、装置及存储介质
CN114689901B (zh) * 2022-01-27 2023-09-05 北京邮电大学 加速度计现场标定方法和装置
CN115406467B (zh) * 2022-11-01 2023-03-21 北京开拓航宇导控科技有限公司 一种mems陀螺自动标定方法
CN115900770B (zh) * 2023-02-14 2023-05-23 北京理工大学前沿技术研究院 一种机载环境下磁传感器的在线校正方法和系统
CN115979311B (zh) * 2023-03-20 2023-07-04 伸瑞科技（北京）有限公司 Piga交叉二次项系数标定方法、系统、设备及介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103221788A (zh) * 2010-11-08 2013-07-24 阿尔派回放股份有限公司 陀螺仪传感器的标定设备和方法
CN104880201A (zh) * 2015-03-26 2015-09-02 武汉大学 Mems陀螺自动标定方法
CN105547326A (zh) * 2015-12-08 2016-05-04 上海交通大学 陀螺与磁传感器联合标定方法
CN107389092A (zh) * 2017-06-27 2017-11-24 上海交通大学 一种基于磁传感器辅助的陀螺标定方法
CN109631939A (zh) * 2018-11-08 2019-04-16 湖北三江航天红峰控制有限公司 一种基于磁强计和加速度计的快速对准方法
CN109696183A (zh) * 2019-01-28 2019-04-30 北京华捷艾米科技有限公司 惯性测量单元的标定方法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8146401B2 (en) * 2008-09-17 2012-04-03 Bae Systems Information And Electronic Systems Integration Inc. Method and apparatus for in-flight calibration of gyroscope using magnetometer reference
US8915116B2 (en) * 2013-01-23 2014-12-23 Freescale Semiconductor, Inc. Systems and method for gyroscope calibration
US20160178657A9 (en) * 2013-12-23 2016-06-23 InvenSense, Incorporated Systems and methods for sensor calibration
US11079252B2 (en) * 2017-01-06 2021-08-03 Idhl Holdings, Inc. Methods, apparatus and systems for gyroscope scale calibration
CN111121824B (zh) * 2020-01-03 2022-11-22 西北工业大学 一种mems传感器的标定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103221788A (zh) * 2010-11-08 2013-07-24 阿尔派回放股份有限公司 陀螺仪传感器的标定设备和方法
CN104880201A (zh) * 2015-03-26 2015-09-02 武汉大学 Mems陀螺自动标定方法
CN105547326A (zh) * 2015-12-08 2016-05-04 上海交通大学 陀螺与磁传感器联合标定方法
CN107389092A (zh) * 2017-06-27 2017-11-24 上海交通大学 一种基于磁传感器辅助的陀螺标定方法
CN109631939A (zh) * 2018-11-08 2019-04-16 湖北三江航天红峰控制有限公司 一种基于磁强计和加速度计的快速对准方法
CN109696183A (zh) * 2019-01-28 2019-04-30 北京华捷艾米科技有限公司 惯性测量单元的标定方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
三轴磁强计与MEMS陀螺的在轨实时标定方法;向甜等;《浙江大学学报（工学版）》;20140330;第48卷(第3期);第541-546页 *
基于自适应 UKF 算法的MEMS陀螺空中在线标定技术;秦伟等;《中国惯性技术学报》;20110430;第19卷(第2期);第170-174页 *

Also Published As

Publication number Publication date
CN112945271A (zh) 2021-06-11
WO2022160391A1 (zh) 2022-08-04

Similar Documents

Publication Publication Date Title
CN112945271B (zh) 磁力计信息辅助的mems陀螺仪标定方法及标定系统
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN110174123B (zh) 一种磁传感器实时标定方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN114485641B (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
CN112562077B (zh) 一种融合pdr和先验地图的行人室内定位方法
WO2008120145A1 (en) Method and system for orientation sensing
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN110686684B (zh) 一种小天体环绕探测器光学协同定轨方法
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
Guan et al. Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system
JP2010096647A (ja) 航法装置及び推定方法
Hemanth et al. Calibration of 3-axis magnetometers
CN114459478B (zh) 一种基于姿态运动学模型的惯性测量单元数据融合方法
CN113156167B (zh) 一种三轴加速度计的标定方法及装置
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN110733671B (zh) 一种小天体自旋角速度动力学修正方法
CN114396936B (zh) 基于多项式优化的惯性与磁传感器姿态估计方法及系统
CN111637894A (zh) 一种定常系数陆标图像导航滤波方法
CN114111770B (zh) 一种水平姿态测量方法、系统、处理设备及存储介质
CN116858286A (zh) 一种基于特殊欧式群和地球坐标系的线性传递对准方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant