CN108225378B - 一种罗盘与加速度计安装误差角的计算方法 - Google Patents

一种罗盘与加速度计安装误差角的计算方法 Download PDF

Info

Publication number
CN108225378B
CN108225378B CN201810073928.0A CN201810073928A CN108225378B CN 108225378 B CN108225378 B CN 108225378B CN 201810073928 A CN201810073928 A CN 201810073928A CN 108225378 B CN108225378 B CN 108225378B
Authority
CN
China
Prior art keywords
coordinate system
axis
vector
compass
accelerometer
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
CN201810073928.0A
Other languages
English (en)
Other versions
CN108225378A (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.)
Tudou Data Technology Group Co ltd
Original Assignee
Tudou Data Technology Group Co ltd
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 Tudou Data Technology Group Co ltd filed Critical Tudou Data Technology Group Co ltd
Priority to CN201810073928.0A priority Critical patent/CN108225378B/zh
Publication of CN108225378A publication Critical patent/CN108225378A/zh
Application granted granted Critical
Publication of CN108225378B publication Critical patent/CN108225378B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

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

Abstract

本发明公开了一种罗盘与加速度计安装误差角的计算方法，包括以下步骤：第一步，坐标定义和坐标转换关系矩阵的定义；第二步，构建电子罗盘的测量模型；第三步，重力向量在导航坐标系下的归一化描述；第四步，根据公式以及加速度计的归一化输出；第五步，导航坐标系下，当地的地磁归一化矢量B；第六步，规定载体坐标系b下的归一化地磁矢量M；第七步：加速度计和电子罗盘的安装误差角Δσ的计算。

Description

一种罗盘与加速度计安装误差角的计算方法
技术领域
本发明属于航空航天导航技术领域，涉及一种罗盘与加速度计安装误差角的计算方法。
背景技术
电子罗盘是一种指示载体机头相对于水平面的磁航向的三轴磁阻传感器。
电子罗盘作为磁阻传感器容易受到附近的磁性物质(铁、钴、镍)以及外部磁场(如电流产生的电磁场)的干扰。在干扰存在的情况下，电子罗盘无法给出准确的磁航向角。
现有技术中，不易获取加速度计和罗盘的安装误差角，从而在加速度进行辅助姿态解算的惯性测量单元(IMU)中，无法提高罗盘解算航向Yaw的精度。
发明内容
本发明的目的是为了克服现有的问题，本发明提供一种简单的罗盘与加速度计安装误差角的计算方法，进而可以提高多旋翼飞行器压航线的精度。
为实现上述目的，本发明提供的通过以下技术手段实现：
一种罗盘与加速度计安装误差角的计算方法，其特征在于，包括以下步骤：
第一步：坐标定义和坐标转换关系矩阵的定义；
采用两个坐标***分别为导航坐标系n，定义为东北天坐标系；载体坐标系b，在载体坐标系b中，xb沿多旋翼飞行器的机体指向右，yb沿多旋翼飞行器的机体指向前，zb沿多旋翼飞行器的机体垂直向上；从导航坐标系n到载体坐标系b的旋转关系矩阵
Figure GDA0003053693300000021
从载体坐标系b到导航坐标系n的旋转关系矩阵
Figure GDA0003053693300000022
旋转矩阵
Figure GDA0003053693300000023
是旋转矩阵
Figure GDA0003053693300000024
的转置和逆，数学描述公式(1)所示；
Figure GDA0003053693300000025
第二步，构建电子罗盘的测量模型如式(2)所示；
Figure GDA0003053693300000026
其中，mx为三轴电子罗盘的X轴输出数据；my为三轴电子罗盘的Y轴输出数据；mz为三轴电子罗盘的Z轴输出数据；sx为电子罗盘的X轴尺度因子；sy为电子罗盘的Y轴尺度因子；sz为电子罗盘的Z轴尺度因子；mxb为电子罗盘X轴的零位误差；myb为电子罗盘Y轴的零位误差；mzb为电子罗盘Z轴的零位误差；R为地磁场强；
第三步，重力向量在导航坐标系n下的归一化描述如公式(3)所示；
Figure GDA0003053693300000027
其中，g为导航坐标系n下的重力向量；
第四步，根据公式(1)和(3)以及加速度计的归一化输出，即载体坐标系下加速度的归一化输出，可得导航坐标系n下的归一化重力矢量和载体坐标系b下的归一化重力矢量关系如式(4)所示；
Figure GDA0003053693300000031
其中，g为导航坐标系下归一化的重力加速度常数矢量，
Figure GDA0003053693300000032
为载体坐标系向导航坐标系的旋转矩阵；a为载体坐标系下的归一化重力加速度，即加速度计的测量输出的归一化值；
第五步：导航坐标系下，当地的地磁归一化矢量B如式(5)所示；
Figure GDA0003053693300000033
其中，bx是地磁场强常矢量在导航坐标系下北向的分量；by是地磁场强常矢量在导航坐标系下东向,即y轴正向的分量；bz是地磁场强常矢量在导航坐标系下地向，即垂直于x轴和y轴指向地的z轴正向分量；
第六步，令载体坐标系b下的归一化地磁矢量M如式(6)所示；
Figure GDA0003053693300000034
当地的地磁矢量B和载体坐标系b下的归一化地磁矢量M的变换关系如式(7)所示；
Figure GDA0003053693300000035
其中
Figure GDA0003053693300000041
为载体坐标系到导航坐标系n的转换矩阵；
第七步：加速度计和电子罗盘的安装误差角Δσ的计算：
由于公式(7)中，B和g均为常数矢量，所以B和g的向量内积应为常数，如式(8)所示：
Figure GDA0003053693300000042
其中
Figure GDA0003053693300000043
为加速度计的载体坐标系b转换到导航坐标系n的转换矩阵；
Figure GDA0003053693300000044
为罗盘的载体坐标系b转换到导航坐标系n的转换矩阵；
当加速度计的载体坐标系b和罗盘的载体坐标系b存在夹角时，由式(8)可得
gTB＝|g|*|B|*cos σ＝|a|*|M|*cos(σ+Δσ) (9)
其中σ为重力归一化矢量和地磁归一化矢量在导航坐标系b下的夹角；Δσ为加速度计和罗盘的安装误差角；
第六步，求取加速度计和罗盘的安装误差角Δσ
首先，通过式(9)解算获得σ；然后，通过式(9)解算加速度计和罗盘的安装误差角Δσ。
进一步地，所述第一步中，导航坐标系n和载体坐标系b均为满足右手定则的三维坐标系，原点均选为所述多旋翼飞行器机体的重心位置。
进一步地，所述第一步中，当所述多旋翼飞行器为四旋翼飞行器时，公式(5)中的B为常数。
通过上述技术手段，本发明具有以下有益效果：
本发明能够准确地求取电子罗盘的偏移和尺度因子，提高罗盘解算的航向Yaw的精度。
本发明能够获取加速度计和罗盘的安装误差角，从而在加速度进行辅助姿态解算的惯性测量单元(IMU)中能够提高罗盘解算航向Yaw的精度。
本发明能够提高多旋翼飞行器压航线的精度。
具体实施方式
下面结合具体实施方式对本发明进行详细说明。
一种罗盘与加速度计安装误差角的计算方法，包括以下步骤：
第一步：坐标定义和坐标转换关系矩阵的定义；
采用两个坐标***分别为导航坐标系n，定义为东北天坐标系；载体坐标系b，在载体坐标系b中，xb沿多旋翼飞行器的机体指向右，yb沿多旋翼飞行器的机体指向前，zb沿多旋翼飞行器的机体垂直向上；从导航坐标系n到载体坐标系b的旋转关系矩阵
Figure GDA0003053693300000051
从载体坐标系b到导航坐标系n的旋转关系矩阵
Figure GDA0003053693300000052
旋转矩阵
Figure GDA0003053693300000053
是旋转矩阵
Figure GDA0003053693300000054
的转置和逆，数学描述公式(1)所示；
Figure GDA0003053693300000055
第二步，构建电子罗盘的测量模型如式(2)所示；
Figure GDA0003053693300000056
其中，mx为三轴电子罗盘的X轴输出数据；my为三轴电子罗盘的Y轴输出数据；mz为三轴电子罗盘的Z轴输出数据；sx为电子罗盘的X轴尺度因子；sy为电子罗盘的Y轴尺度因子；sz为电子罗盘的Z轴尺度因子；mxb为电子罗盘X轴的零位误差；myb为电子罗盘Y轴的零位误差；mzb为电子罗盘Z轴的零位误差；R为地磁场强；
第三步，重力向量在导航坐标系n下的归一化描述如公式(3)所示；
Figure GDA0003053693300000061
其中，g为导航坐标系n下的重力向量；
第四步，根据公式(1)和(3)以及加速度计的归一化输出，即载体坐标系下加速度的归一化输出，可得导航坐标系n下的归一化重力矢量和载体坐标系b下的归一化重力矢量关系如式(4)所示；
Figure GDA0003053693300000062
其中，g为导航坐标系下归一化的重力加速度常数矢量，
Figure GDA0003053693300000063
为载体坐标系向导航坐标系的旋转矩阵；a为载体坐标系下的归一化重力加速度，即加速度计的测量输出的归一化值；
第五步：导航坐标系下，当地的地磁归一化矢量B如式(5)所示；
Figure GDA0003053693300000064
其中，bx是地磁场强常矢量在导航坐标系下北向(x轴正向)的分量；by是地磁场强常矢量在导航坐标系下东向,即y轴正向的分量；bz是地磁场强常矢量在导航坐标系下地向，即垂直于x轴和y轴指向地的z轴正向分量；
第六步，令载体坐标系b下的归一化地磁矢量M如式(6)所示；
Figure GDA0003053693300000071
当地的地磁矢量B和载体坐标系b下的归一化地磁矢量M的变换关系如式(7)所示；
Figure GDA0003053693300000072
其中
Figure GDA0003053693300000073
为载体坐标系到导航坐标系n的转换矩阵；M同时还是地磁矢量在载体坐标系b下的投影，具体如式(6)所示；
第七步：加速度计和电子罗盘的安装误差角Δσ的计算：
由于公式(7)中，B和g均为常数矢量，所以B和g的向量内积应为常数，如式(8)所示：
Figure GDA0003053693300000074
其中
Figure GDA0003053693300000075
为加速度计的载体坐标系b转换到导航坐标系n的转换矩阵；
Figure GDA0003053693300000076
为罗盘的载体坐标系b转换到导航坐标系n的转换矩阵；
当加速度计的载体坐标系b和罗盘的载体坐标系b存在夹角时，由式(8)可得
gTB＝|g|*|B|*cosσ＝|a|*|M|*cos(σ+Δσ) (9)
其中σ为重力归一化矢量和地磁归一化矢量在导航坐标系b下的夹角；Δσ为加速度计和罗盘的安装误差角；
第六步，求取加速度计和罗盘的安装误差角Δσ
首先，通过式(9)解算获得σ；然后，通过式(9)解算加速度计和罗盘的安装误差角Δσ。
其中，所述第一步中，导航坐标系n和载体坐标系b均为满足右手定则的三维坐标系，原点均选为所述多旋翼飞行器机体的重心位置。
其中，所述第一步中，当所述多旋翼飞行器为四旋翼飞行器时，公式(5)中的B为常数。
本发明，通过获得三轴电子罗盘和三轴加速度计之间的轴间夹角系数之后，可以使后续的导航解算精度有所提高，特别是载体坐标系下的速度和导航坐标系下的转换更加的精确。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (3)

1.一种罗盘与加速度计安装误差角的计算方法，其特征在于，包括以下步骤：
第一步：坐标定义和坐标转换关系矩阵的定义；
采用两个坐标***分别为导航坐标系n，定义为东北天坐标系；载体坐标系b，在载体坐标系b中，xb沿多旋翼飞行器的机体指向右，yb沿多旋翼飞行器的机体指向前，zb沿多旋翼飞行器的机体垂直向上；从导航坐标系n到载体坐标系b的旋转关系矩阵
Figure FDA0003053693290000011
从载体坐标系b到导航坐标系n的旋转关系矩阵
Figure FDA0003053693290000012
旋转矩阵
Figure FDA0003053693290000013
是旋转矩阵
Figure FDA0003053693290000014
的转置和逆，数学描述公式(1)所示；
Figure FDA0003053693290000015
第二步，构建电子罗盘的测量模型如式(2)所示；
Figure FDA0003053693290000016
其中，mx为三轴电子罗盘的X轴输出数据；my为三轴电子罗盘的Y轴输出数据；mz为三轴电子罗盘的Z轴输出数据；sx为电子罗盘的X轴尺度因子；sy为电子罗盘的Y轴尺度因子；sz为电子罗盘的Z轴尺度因子；mxb为电子罗盘X轴的零位误差；myb为电子罗盘Y轴的零位误差；mzb为电子罗盘Z轴的零位误差；R为地磁场强；
第三步，重力向量在导航坐标系n下的归一化描述如公式(3)所示；
Figure FDA0003053693290000017
其中，g为导航坐标系n下的重力向量；
第四步，根据公式(1)和(3)以及加速度计的归一化输出，即载体坐标系下加速度的归一化输出，可得导航坐标系n下的归一化重力矢量和载体坐标系b下的归一化重力矢量关系如式(4)所示；
Figure FDA0003053693290000021
其中，g为导航坐标系下归一化的重力加速度常数矢量，
Figure FDA0003053693290000022
为载体坐标系向导航坐标系的旋转矩阵；a为载体坐标系下的归一化重力加速度，即加速度计的测量输出的归一化值；
第五步：导航坐标系下，当地的地磁矢量B如式(5)所示；
Figure FDA0003053693290000023
其中，bx是地磁场强常矢量在导航坐标系下北向的分量；by是地磁场强常矢量在导航坐标系下东向，即y轴正向的分量；bz是地磁场强常矢量在导航坐标系下地向，即垂直于x轴和y轴指向地的z轴正向分量；
第六步，令载体坐标系b下的归一化地磁矢量M如式(6)所示；
Figure FDA0003053693290000024
当地的地磁矢量B和载体坐标系b下的归一化地磁矢量M的变换关系如式(7)所示；
Figure FDA0003053693290000031
其中
Figure FDA0003053693290000032
为载体坐标系到导航坐标系n的转换矩阵；
第七步：加速度计和电子罗盘的安装误差角Δσ的计算：
由于公式(7)中，B和g均为常数矢量，所以B和g的向量内积应为常数，如式(8)所示：
Figure FDA0003053693290000033
其中
Figure FDA0003053693290000034
为加速度计的载体坐标系b转换到导航坐标系n的转换矩阵；
Figure FDA0003053693290000035
为罗盘的载体坐标系b转换到导航坐标系n的转换矩阵；
当加速度计的载体坐标系b和罗盘的载体坐标系b存在夹角时，由式(8)可得
gTB＝|g|*|B|*cosσ＝|a|*|M|*cos(σ+Δσ) (9)
其中σ为重力归一化矢量和地磁归一化矢量在导航坐标系b下的夹角；Δσ为加速度计和罗盘的安装误差角；
第六步，求取加速度计和罗盘的安装误差角Δσ
首先，通过式(9)解算获得σ；然后，通过式(9)解算加速度计和罗盘的安装误差角Δσ。
2.根据权利要求1所述的一种罗盘与加速度计安装误差角的计算方法，其特征在于，所述第一步中，导航坐标系n和载体坐标系b均为满足右手定则的三维坐标系，原点均选为所述多旋翼飞行器机体的重心位置。
3.根据权利要求1所述的一种罗盘与加速度计安装误差角的计算方法，其特征在于，
所述第一步中，当所述多旋翼飞行器为四旋翼飞行器时，公式(5)中的B为常数。
CN201810073928.0A 2018-01-25 2018-01-25 一种罗盘与加速度计安装误差角的计算方法 Active CN108225378B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810073928.0A CN108225378B (zh) 2018-01-25 2018-01-25 一种罗盘与加速度计安装误差角的计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810073928.0A CN108225378B (zh) 2018-01-25 2018-01-25 一种罗盘与加速度计安装误差角的计算方法

Publications (2)

Publication Number Publication Date
CN108225378A CN108225378A (zh) 2018-06-29
CN108225378B true CN108225378B (zh) 2021-07-02

Family

ID=62667319

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810073928.0A Active CN108225378B (zh) 2018-01-25 2018-01-25 一种罗盘与加速度计安装误差角的计算方法

Country Status (1)

Country Link
CN (1) CN108225378B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112461224B (zh) * 2020-11-10 2021-09-14 武汉大学 一种基于已知姿态角的磁力计标定方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102252689A (zh) * 2010-05-19 2011-11-23 北京国浩传感器技术研究院(普通合伙) 一种基于磁传感器的电子罗盘校准方法
CN104880192A (zh) * 2015-06-12 2015-09-02 北京航空航天大学 一种基于偏振罗盘的载体航向角计算方法
CN105180968A (zh) * 2015-09-02 2015-12-23 北京天航华创科技股份有限公司 一种imu/磁强计安装失准角在线滤波标定方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7057173B2 (en) * 2004-01-05 2006-06-06 Laser Technology, Inc. Magnetoresistive (MR) sensor temperature compensation and magnetic cross-term reduction techniques utilizing selective set and reset gain measurements

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102252689A (zh) * 2010-05-19 2011-11-23 北京国浩传感器技术研究院(普通合伙) 一种基于磁传感器的电子罗盘校准方法
CN104880192A (zh) * 2015-06-12 2015-09-02 北京航空航天大学 一种基于偏振罗盘的载体航向角计算方法
CN105180968A (zh) * 2015-09-02 2015-12-23 北京天航华创科技股份有限公司 一种imu/磁强计安装失准角在线滤波标定方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
电子罗盘安装误差标定与补偿方法研究;范成叶等;《传感技术学报》;20130531;第26卷(第5期);第622-626页 *

Also Published As

Publication number Publication date
CN108225378A (zh) 2018-06-29

Similar Documents

Publication Publication Date Title
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
US10215564B2 (en) Automatic compass calibration systems and methods
CN107272069B (zh) 基于磁异常梯度的磁性目标追踪方法
CN104698485B (zh) 基于bd、gps及mems的组合导航***及导航方法
Li et al. Magnetic sensors for navigation applications: an overview
CN103591949B (zh) 三轴姿态测量***非正交性误差的正交补偿方法
US4930085A (en) Method for determining the heading of an aircraft
CN109550219B (zh) 一种运动信息的确定方法、***及移动设备
CN106353824A (zh) 航空磁通门磁梯度张量仪的***校正及磁干扰补偿融合方法
CN103439727B (zh) 一种地面坐标的测量方法
CN104880192A (zh) 一种基于偏振罗盘的载体航向角计算方法
CN110530397A (zh) 一种基于九轴imu的磁场自动标定方法
CN104166169A (zh) 一种捷联于水下潜器的三轴磁场传感器误差补偿方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN112556724A (zh) 动态环境下的微型飞行器低成本导航***初始粗对准方法
CN108225378B (zh) 一种罗盘与加速度计安装误差角的计算方法
CN109000639A (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
CN105606093A (zh) 基于重力实时补偿的惯性导航方法及装置
CN104809352A (zh) 基于正演的拖曳式航磁全张量梯度数据软补偿方法
CN107860382A (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
CN105277210A (zh) 一种任意安装多轴陀螺组合标定方法
CN108318017B (zh) 一种差分磁罗盘消除随机磁干扰的数据处理方法
CN108710145A (zh) 一种无人机定位***及方法
Cui et al. Three-axis magnetometer calibration based on optimal ellipsoidal fitting under constraint condition for pedestrian positioning system using foot-mounted inertial sensor/magnetometer

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
CB02 Change of applicant information

Address after: Room 504, Block E, HUanpu science and Technology Industrial Park, 211 tianguba Road, high tech Zone, Xi'an City, Shaanxi Province, 710000

Applicant after: Tudou Data Technology Group Co.,Ltd.

Address before: Room 504, Block E, HUanpu science and Technology Industrial Park, 211 tianguba Road, high tech Zone, Xi'an City, Shaanxi Province, 710000

Applicant before: SHAANXI TUDOU DATA TECHNOLOGY Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant