CN116499442A - 电子罗盘误差补偿与解算方法、系统、介质及设备 - Google Patents
电子罗盘误差补偿与解算方法、系统、介质及设备 Download PDFInfo
- Publication number
- CN116499442A CN116499442A CN202310296712.1A CN202310296712A CN116499442A CN 116499442 A CN116499442 A CN 116499442A CN 202310296712 A CN202310296712 A CN 202310296712A CN 116499442 A CN116499442 A CN 116499442A
- Authority
- CN
- China
- Prior art keywords
- triaxial
- representing
- magnetometer
- matrix
- electronic compass
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims abstract description 75
- 238000005259 measurement Methods 0.000 claims abstract description 61
- 230000001133 acceleration Effects 0.000 claims abstract description 44
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 24
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 238000012937 correction Methods 0.000 claims abstract description 11
- 238000005457 optimization Methods 0.000 claims abstract description 7
- 238000012545 processing Methods 0.000 claims abstract description 7
- 230000005484 gravity Effects 0.000 claims description 18
- 230000005358 geomagnetic field Effects 0.000 claims description 17
- 230000009466 transformation Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 7
- 238000009795 derivation Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/38—Testing, calibrating, or compensating of compasses
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/02—Magnetic compasses
- G01C17/28—Electromagnetic compasses
- G01C17/32—Electron compasses
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Measuring Magnetic Variables (AREA)
- Navigation (AREA)
- Geophysics And Detection Of Objects (AREA)
Abstract
本发明提供了一种电子罗盘误差补偿与解算方法、系统、介质及设备，包括：步骤1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；步骤2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；步骤3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。本发明通过采用将多传感器约束引入到电子罗盘误差补偿算法中，解决了一般补偿算法估计误差参数少，导致修正后存在固定偏差问题，使得操作更简单、测量精度更高。
Description
技术领域
本发明涉及误差补偿技术领域，具体地，涉及一种电子罗盘误差补偿与解算方法、系统、介质及设备。
背景技术
如图1，为现有技术中电子罗盘的航向测量与倾角补偿原理示意图，当前电子罗盘的误差补偿算法修正后普遍存在固定偏差问题。本发明采用多传感器电子罗盘系统，通过引入多传感器约束关系实现消除固定偏差、简化操作、提升误差补偿效果，并进一步对传统航向角解算方法进行优化，提出基于双矢量定向的航向角解算方法，简化计算，提高精度。
现有技术CN108088431A，公开日2018年5月29日，公开了一种自校正电子罗盘及其校正方法。该校正方法包含有使用多颗磁力计进行磁场数据采集，但是这种方法依然是使用传统方法对测量数据进行优化和解算，精度提升有限，且仅估计9个误差参数，无法消除非对准偏差。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种电子罗盘误差补偿与解算方法、系统、介质及设备。
根据本发明提供的电子罗盘误差补偿与解算方法，包括：
步骤1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；
步骤2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；
步骤3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
优选的，在获取电子罗盘测量数据之前，先对电子罗盘的加速度计和磁力计分别进行标定；
对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，屏蔽掉外界所有磁场，测量得到磁力计本身的磁场造成的三轴零偏；
对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax，Xmin，Ymax，Ymin，Zmax，Zmin，因此计算得到x轴零偏同样计算得到Δy，Δz。
优选的，所述步骤2包括：
步骤2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，ε′为转化后的零均值高斯噪声；A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；
步骤2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值||H||，H表达式为：
其中，Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量；
对于理想无磁干扰环境下，磁力计测量值满足：
||H||2-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下相邻两磁力计测量值满足：其中，为坐标系转换矩阵，因此有：
步骤2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1，k-1)+Wk
其中变量X为：
X＝[a11，a12，a13，a21，a22，a23，a31，a32，a33，b1，b2，b3]
Wk为k时刻系统噪声，Wk～N(0，Qk)；Xk表示系统状态量；N(0，Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0，Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+1-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
其中，Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
步骤2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
优选的，所述步骤3包括：
步骤3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下满足：
其中，Gb代表加速度计实际测量值；
步骤3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，得：
即：
步骤3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
步骤3.4：根据(8)式得因此根据(7)和(8)对应关系即可计算得到航向角φ。
根据本发明提供的电子罗盘误差补偿与解算系统，包括：
模块M1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；
模块M2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；
模块M3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
优选的，在获取电子罗盘测量数据之前，先对电子罗盘的加速度计和磁力计分别进行标定；
对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，屏蔽掉外界所有磁场，测量得到磁力计本身的磁场造成的三轴零偏；
对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax，Xmin，Ymax，Ymin，Zmax，Zmin，因此计算得到x轴零偏同样计算得到Δy，Δz。
优选的，所述模块M2包括：
模块M2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，ε′为转化后的零均值高斯噪声；A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；
模块M2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值||H||，H表达式为：
其中，Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量；
对于理想无磁干扰环境下，磁力计测量值满足：
||H||2-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下相邻两磁力计测量值满足：其中，为坐标系转换矩阵，因此有：
模块M2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1，k-1)+Wk
其中变量X为：
X＝[a11，a12，a13，a21，a22，a23，a31，a32，a33，b1，b2，b3]
Wk为k时刻系统噪声，Wk～N(0，Qk)；Xk表示系统状态量；N(0，Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0，Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+1-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
其中，Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
模块M2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
优选的，所述模块M3包括：
模块M3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下满足：
其中，Gb代表加速度计实际测量值；
模块M3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，得：
即：
模块M3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
模块M3.4：根据(8)式得因此根据(7)和(8)对应关系即可计算得到航向角φ。
根据本发明提供的存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现所述的电子罗盘误差补偿与解算方法的步骤。
根据本发明提供的电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被处理器执行时实现所述的电子罗盘误差补偿与解算方法的步骤。
与现有技术相比，本发明具有如下的有益效果：
(1)本发明通过采用将多传感器约束引入到电子罗盘误差补偿算法中，解决了一般补偿算法估计误差参数少，导致修正后存在固定偏差问题，使得操作更简单、测量精度更高；
(2)本发明通过采用双矢量定向原理进行航向角解算，解决了传统解算方法计算复杂、引入误差较大等问题。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为现有技术中电子罗盘的航向测量与倾角补偿原理示意图；
图2a、图2b、图2c分别为磁力计分别在无干扰、受到硬磁干扰、受到软硬磁干扰时的测量数据分布效果图；
图3为所用硬件系统的多个传感器之间约束关系示意图；
图4为本发明电子罗盘补偿与解算方法流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
如图4，本发明提供了一种电子罗盘误差补偿与解算方法，包括如下步骤：
步骤1：获取电子罗盘测量数据，所述电子罗盘包括多个三轴磁力计和三轴加速度计，得到三轴磁场数据和三轴加速度数据；
步骤2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；
步骤3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
所述电子罗盘的多个传感器的测量坐标系之间存在固定的约束关系，通过姿态转换矩阵进行相互转换，如图3；
所述步骤2包括以下子步骤：
步骤2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，
A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；ε′为转化后的零均值高斯噪声；
步骤2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值||H||，H表达式为：
Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量，一般为北向分量、东向分量和地向分量；
如图2a～2c，对于理想无磁干扰环境下，磁力计测量值应满足：
||H||2-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下，相邻两磁力计测量值应满足：其中为坐标系转换矩阵，因此，有：
步骤2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1，k-1)+Wk
其中变量X为：
X＝[a11，a12，a13，a21，a22，a23，a31，a32，a33，b1，b2，b3]
Wk为k时刻系统噪声，Wk～N(0，Qk)；Xk表示系统状态量；N(0，Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯(正态)分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0，Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+1-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
步骤2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
所述步骤3包括以下子步骤：
步骤3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下应满足：
其中，Gb代表加速度计实际测量值；
步骤3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，可得：
即：
步骤3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
步骤3.4：根据公式(7)和(8)对应关系完成航向角解算。根据(8)式可得因此根据(7)和(8)对应关系即可计算得到航向角φ；
在步骤1之前还可以包括：对电子罗盘的加速度计和磁力计分别进行标定，对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，其能够屏蔽掉外界所有磁场，因此可以测量得到磁力计本身的磁场造成的三轴零偏；对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax，Xmin，Ymax，Ymin，Zmax，Zmin，因此计算得到x轴零偏同样计算得到Δy，Δz。
实施例2：
本发明还提供一种电子罗盘误差补偿与解算系统，所述电子罗盘误差补偿与解算系统可以通过执行所述电子罗盘误差补偿与解算方法的流程步骤予以实现，即本领域技术人员可以将所述电子罗盘误差补偿与解算方法理解为所述电子罗盘误差补偿与解算系统的优选实施方式。
根据本发明提供的电子罗盘误差补偿与解算系统，包括：模块M1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；模块M2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；模块M3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
在获取电子罗盘测量数据之前，先对电子罗盘的加速度计和磁力计分别进行标定；对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，屏蔽掉外界所有磁场，测量得到磁力计本身的磁场造成的三轴零偏；对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax，Xmin，Ymax，Ymin，Zmax，Zmin，因此计算得到x轴零偏同样计算得到Δy，Δz。
所述模块M2包括：
模块M2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，ε′为转化后的零均值高斯噪声；A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；
模块M2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值||H||，H表达式为：
其中，Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量；
对于理想无磁干扰环境下，磁力计测量值满足：
||H||2-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下相邻两磁力计测量值满足：其中，为坐标系转换矩阵，因此有：
模块M2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1，k-1)+Wk
其中变量X为：
X＝[a11，a12，a13，a21，a22，a23，a31，a32，a33，b1，b2，b3]
Wk为k时刻系统噪声，Wk～N(0，Qk)；Xk表示系统状态量；N(0，Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0，Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+1-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
其中，Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
模块M2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
所述模块M3包括：
模块M3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下满足：
其中，Gb代表加速度计实际测量值；
模块M3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，得：
即：
模块M3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
模块M3.4：根据(8)式得因此根据(7)和(8)对应关系即可计算得到航向角φ。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
Claims (10)
1.一种电子罗盘误差补偿与解算方法，其特征在于，包括：
步骤1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；
步骤2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；
步骤3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
2.根据权利要求1所述的电子罗盘误差补偿与解算方法，其特征在于，在获取电子罗盘测量数据之前，先对电子罗盘的加速度计和磁力计分别进行标定；
对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，屏蔽掉外界所有磁场，测量得到磁力计本身的磁场造成的三轴零偏；
对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax,Xmin,Ymax,Ymin,Zmax,Zmin，因此计算得到x轴零偏同样计算得到Δy,Δz。
3.根据权利要求1所述的电子罗盘误差补偿与解算方法，其特征在于，所述步骤2包括：
步骤2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，ε′为转化后的零均值高斯噪声；A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；
步骤2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值‖H‖，H表达式为：
其中，Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量；
对于理想无磁干扰环境下，磁力计测量值满足：
‖H‖3-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下相邻两磁力计测量值满足：其中，为坐标系转换矩阵，因此有：
步骤2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1,k-1)+Wk
其中变量X为：
X＝[a11,a12,a13,a21,a22,a23,a31,a32,a33,b1,b2,b3]
Wk为k时刻系统噪声，Wk～N(0,Qk)；Xk表示系统状态量；N(0,Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0,Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+`-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
其中，Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
步骤2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
4.根据权利要求1所述的电子罗盘误差补偿与解算方法，其特征在于，所述步骤3包括：
步骤3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下满足：
其中，Gb代表加速度计实际测量值；
步骤3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，得：
即：
步骤3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
步骤3.4：根据(8)式得因此根据(7)和(8)对应关系即可计算得到航向角φ。
5.一种电子罗盘误差补偿与解算系统，其特征在于，包括：
模块M1：根据三轴磁力计和三轴加速度计获取电子罗盘测量数据，得到三轴磁场数据和三轴加速度数据；
模块M2：使用扩展卡尔曼滤波算法对三轴磁场数据进行优化处理，得到误差补偿矩阵；
模块M3：根据误差补偿矩阵修正后的三轴磁场数据以及三轴加速度数据进行双矢量定向，解算得到载体航向角。
6.根据权利要求5所述的电子罗盘误差补偿与解算系统，其特征在于，在获取电子罗盘测量数据之前，先对电子罗盘的加速度计和磁力计分别进行标定；
对于磁力计，采用在磁屏蔽桶制造的无磁环境下进行标定，屏蔽掉外界所有磁场，测量得到磁力计本身的磁场造成的三轴零偏；
对于加速度计，采用在固定水平台上静止条件下，分别测量重力加速度在加速度计三轴的最大和最小分量，即Xmax,Xmin,Ymax,Ymin,Zmax,Zmin，因此计算得到x轴零偏同样计算得到Δy,Δz。
7.根据权利要求5所述的电子罗盘误差补偿与解算系统，其特征在于，所述模块M2包括：
模块M2.1：对磁力计误差进行建模，表达式为：
其中，为磁力计实际测量值；h为磁力计理想测量值；Q代表一个3*3的矩阵，包含软磁干扰、三轴不正交度、刻度因子误差；b代表零刻度漂移、硬磁干扰误差；ε代表零均值高斯噪声；
将公式(1)进行转化，得到：
其中，ε′为转化后的零均值高斯噪声；A、B为补偿矩阵；a11～a33表示包含软磁干扰、三轴不正交度、刻度因子误差在内的误差参数；b1～b3表示包含零刻度漂移、硬磁干扰误差在内的误差参数；
模块M2.2：从地磁模型中获取地理坐标系下当地地磁参考量H及其模值‖H‖，H表达式为：
其中，Hx、Hy、Hz分别表示地磁场矢量在当地地理坐标系下的三轴分量；
对于理想无磁干扰环境下，磁力计测量值满足：
‖H‖2-hTh＝0…………(4)
根据多个磁力计之间的坐标系约束关系，理想情况下相邻两磁力计测量值满足：其中，为坐标系转换矩阵，因此有：
模块M2.3：根据公式(2)、(4)和(5)利用扩展卡尔曼滤波算法进行误差补偿，并构建系统模型；
系统状态方程：
Xk＝f(Xk-1,k-1)+Wk
其中变量X为：
X＝[a11,a12,a13,a21,a22,a23,a31,a32,a33,b1,b2,b3]
Wk为k时刻系统噪声，Wk～N(0,Qk)；Xk表示系统状态量；N(0,Qk)表示均值为0、方差为Qk的正态分布；Qk表示高斯分布的方差；
系统观测方程：
其中，Vk为k时刻系统观测噪声，Vk～N(0,Rk)；Zk表示系统观测量；hk表示地磁场理论三轴测量值；Rk表示高斯噪声的方差；
模型中还包括：
Xk+1＝Xk+Kk(Zk+1-hk+1(Xk))
Pk+1＝(I-KkHk+1(Xk))Pk
其中，Kk表示滤波增益矩阵；Pk表示第k个过程的后验估计协方差矩阵；Hk+1表示观测矩阵，用对观测方程中h(Xk)求偏导后得到的雅可比矩阵表示；I表示单位矩阵；
模块M2.4：X经过所述算法若干次迭代之后，得到的值包含对磁力计误差补偿量，即得到了公式(2)中补偿矩阵A和B，从而实现对磁力计原始测量数据的校正与补偿。
8.根据权利要求5所述的电子罗盘误差补偿与解算系统，其特征在于，所述模块M3包括：
模块M3.1：获取地理坐标系下当地重力加速度参考矢量，表达式为：
用方向余弦矩阵来表示电子罗盘载体坐标系b与地理坐标系g间的方位关系，则载体坐标系下的地磁和重力加速度的测量值与地理坐标系下地磁和重力加速度参考值在理想条件下满足：
其中，Gb代表加速度计实际测量值；
模块M3.2：地磁场矢量和重力加速度矢量非共线，因此，利用双矢量定姿原理进行航向解算，构造辅助矢量为：
选择地磁场矢量作为主矢量进行单位正交化，得：
即：
模块M3.3：根据坐标系三轴旋转姿态变换矩阵计算得到
其中，α、β、代表载体姿态角，分别为俯仰角、横滚角、航向角；
模块M3.4：根据(8)式得因此根据(7)和(8)对应关系即可计算得到航向角φ。
9.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的电子罗盘误差补偿与解算方法的步骤。
10.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的电子罗盘误差补偿与解算方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310296712.1A CN116499442A (zh) | 2023-03-23 | 2023-03-23 | 电子罗盘误差补偿与解算方法、系统、介质及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310296712.1A CN116499442A (zh) | 2023-03-23 | 2023-03-23 | 电子罗盘误差补偿与解算方法、系统、介质及设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116499442A true CN116499442A (zh) | 2023-07-28 |
Family
ID=87319250
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310296712.1A Pending CN116499442A (zh) | 2023-03-23 | 2023-03-23 | 电子罗盘误差补偿与解算方法、系统、介质及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116499442A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118225032A (zh) * | 2024-05-24 | 2024-06-21 | 吉林大学 | 地面运动载体磁干扰条件下的航向角测量误差校正方法 |
-
2023
- 2023-03-23 CN CN202310296712.1A patent/CN116499442A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118225032A (zh) * | 2024-05-24 | 2024-06-21 | 吉林大学 | 地面运动载体磁干扰条件下的航向角测量误差校正方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
| EP1929246B1 (en) | Calibration of 3d field sensors | |
| CN104698485B (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
| CN109974714A (zh) | 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法 | |
| CN112945271B (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 | |
| CN110146839A (zh) | 一种移动平台磁梯度张量系统校正方法 | |
| CN103299247A (zh) | 用于磁近场的动态追踪及补偿的设备和方法 | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| CN112304337B (zh) | 基于陀螺仪和加速度计的运动角度估计方法和系统 | |
| CN108344409B (zh) | 提高卫星姿态确定精度的方法 | |
| CN116817896B (zh) | 一种基于扩展卡尔曼滤波的姿态解算方法 | |
| CN107290801A (zh) | 基于函数链接型神经网络和场模平方差的捷联三轴磁强计误差一步校正方法 | |
| CN116499442A (zh) | 电子罗盘误差补偿与解算方法、系统、介质及设备 | |
| CN112461262A (zh) | 一种校正三轴磁强计误差的装置和方法 | |
| CN107860382B (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
| Aligia et al. | An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer | |
| CN111413651A (zh) | 一种磁场总场的补偿方法、装置、系统及存储介质 | |
| CN113375693B (zh) | 一种地磁航向误差修正方法 | |
| CN108871319A (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| CN115031731A (zh) | 一种基于相对安装位置关系的多惯导协同导航方法及装置 | |
| CN113137977B (zh) | 一种sins/偏振光组合导航初始对准滤波方法 | |
| Munguia et al. | An attitude and heading reference system (AHRS) based in a dual filter | |
| CN116182871B (zh) | 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 | |
| CN108692727B (zh) | 一种带有非线性补偿滤波器的捷联惯导系统 | |
| CN110954081A (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 |