CN106197475A - 基于序贯滤波的陀螺与磁传感器联合标定方法 - Google Patents
基于序贯滤波的陀螺与磁传感器联合标定方法 Download PDFInfo
- Publication number
- CN106197475A CN106197475A CN201610497459.6A CN201610497459A CN106197475A CN 106197475 A CN106197475 A CN 106197475A CN 201610497459 A CN201610497459 A CN 201610497459A CN 106197475 A CN106197475 A CN 106197475A
- Authority
- CN
- China
- Prior art keywords
- matrix
- represent
- magnetometer
- gyro
- state
- 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.)
- Granted
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
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供了一种基于序贯滤波的陀螺与磁传感器联合标定方法,本发明在均匀磁场中充分变化姿态,并同步采集磁力仪与陀螺的测量值,由于磁力仪和陀螺的数据共同用于本发明的磁力仪内部参数、磁力仪外部参数以及陀螺零偏的联合标定,因此标定效果佳。可以实时标定出磁力仪内、外部参数以及陀螺零偏,而且不受任何加速度干扰的影响,因此实施过程中不需要设备保持静止状态。本发明还可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。
Description
技术领域
本发明涉及传感器技术领域,具体地,涉及一种基于序贯滤波的陀螺与磁传感器联合标定方法。
背景技术
陀螺和磁传感器(后者又名磁力仪、磁强计)经常用于姿态确定或科学测量领域。陀螺敏感载体的角速度,磁力仪敏感环境磁场。当磁力仪处于铁磁材料附近时,磁力仪周围的磁场受到扭曲,不能正确测量出磁场强度。磁场干扰可分为硬磁效应和软磁效应两种。硬磁效应是由永磁铁或电流产生的加性磁干扰,而软磁效应由软磁材料诱导产生,处于背景磁场中的软磁材料会诱导产生出自身的磁场,并对背景磁场的强度和方向产生扭曲。除此之外,因制造工艺不完善,磁力仪还存在标度因数、敏感轴交叉耦合和偏置等误差,因此,在使用磁力仪之前,必须对以上误差进行标校,即进行磁力仪内部标定。
当磁力仪与陀螺一起使用时,还必须进行磁力仪外部标定,即标定磁力仪与陀螺之间的坐标系失准角。软磁效应不仅会引起磁力仪内部参数的变化,也会导致磁力仪与陀螺的坐标系失准角发生改变。因此,在使用之前,磁力仪内部标定和外部标定都需要进行。目前存在的磁力仪标定方法或要求磁力仪内部标定和外部标定先后分别进行,或要求在静态条件下采集加速度计信息。另一方面,低成本陀螺(如MEMS陀螺)的零偏误差较大,而且每次使用时都有所变化,若不做补偿直接使用陀螺测量值,将影响磁力仪外部标定的效果。
发明内容
针对现有技术中的缺陷,本发明的目的是提供一种基于序贯滤波的陀螺与磁传感器联合标定方法。
根据本发明提供的基于序贯滤波的陀螺与磁传感器联合标定方法,包括如下步骤:
步骤1:建立联合标定问题的状态空间模型;
步骤2:采用序贯滤波方法估计出状态空间模型的状态。
优选地,所述步骤1中联合标定问题的状态空间模型如下:
***方程:
其中:陀螺/加速度计的体坐标系表示为b系,惯性坐标系表示为i系,即初始时刻的b系;
式中:表示惯性坐标系相对于体坐标系的姿态矩阵,表示的时间导数,表示陀螺测量的b系下的角速度矢量,ε表示陀螺零偏,表示ε的时间导数,ng表示陀螺测量的高斯噪声,nε表示陀螺零偏的高斯噪声,矩阵S表示磁力仪参数矩阵,表示矩阵S的时间导数,向量h表示磁力仪零偏,表示h的时间导数,mi为i系下表示的恒定磁场向量,表示mi的时间导数,nmi表示因地球自转引起的磁场向量误差;其中:运算a×表示由一个三维向量a=[a1 a2 a3]T构成的叉乘矩阵,具体地,a×的展开式如下:
观测方程:
式中:ym表示磁力仪的测量值,为姿态矩阵的转置,即上标T表示向量的转置运算,nm表示磁力仪测量值的高斯噪声。
优选地,步骤1中的状态空间模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量;磁场向量mi的初始值包括:磁力仪的首次测量值;磁力仪参数矩阵S的初始值包括:单位矩阵或事先给定;陀螺零偏ε和磁力仪零偏h的初始值选为零;因状态可观性的关系,姿态的初始值确定为单位矩阵,对应的初始方差矩阵为零。
优选地,所述步骤2包括:采用序贯滤波方法估计出状态空间模型的状态,所述序贯滤波方法包括:扩展卡尔曼滤波,或者粒子滤波法;具体地,采用扩展卡尔曼滤波时,包括如下步骤:
步骤2.1:定义状态误差δx为估计值减去真值x,即δ表示对应状态的误差,而姿态估计值与姿态真值及姿态误差ψ的关系定义为
则相应的状态误差向量表示为δx≡[ψT δεT vecT(δS) δhT δmiT]T,vec(·)表示将矩阵按照列的顺序拼接起来的运算;ψT表示姿态误差ψ的转置,εT表示陀螺零偏向量ε的转置,hT、miT分别表示h、mi的转置;
状态误差的近似线性状态空间模型按如下给出
其中:噪声
式中:表示δx的时间导数,矩阵F、G、Hm分别表示***矩阵、***输入矩阵、观测矩阵;I表示单位矩阵,I的下标表示单位矩阵的阶数;0表示零矩阵,0的下标表示零矩阵的行列数;符号表示矩阵与矩阵的Kronecker乘积;
步骤2.2:不失一般性,假定当地磁场向量的大小为1,对扩展卡尔曼滤波的结果进行如下尺度调整;调整公式如下:
Srs=||mi||S,
mi rs=mi/||mi||;
式中:Srs表示尺度调整后的磁力仪矩阵,mi rs表示尺度调整后的磁场向量,||·||表示取模运算。
与现有技术相比,本发明具有如下的有益效果:
本发明中磁力仪与陀螺固定连接,在均匀磁场中充分变化姿态,并同步采集磁力仪与陀螺的测量值,由于磁力仪和陀螺的数据共同用于本发明的磁力仪内部参数、磁力仪外部参数以及陀螺零偏的联合标定,因此零标定效果佳。可以实时标定出磁力仪内、外部参数以及陀螺零偏,而且不受任何加速度干扰的影响,因此实施过程中不需要设备保持静止状态。本发明还可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改进。这些都属于本发明的保护范围。
根据本发明提供的基于序贯滤波的陀螺与磁传感器联合标定方法,包括如下步骤:
步骤1:建立联合标定问题的状态空间模型;
步骤2:采用序贯滤波方法估计出状态空间模型的状态。
所述步骤1中联合标定问题的状态空间模型如下:
***方程:
式中:表示惯性坐标系相对于体坐标系的姿态矩阵,表示的时间导数,表示陀螺测量的b系下的角速度矢量,ε表示陀螺零偏,表示ε的时间导数,ng表示陀螺测量的高斯噪声,nε表示陀螺零偏的高斯噪声,矩阵S表示磁力仪参数矩阵,表示矩阵S的时间导数,向量h表示磁力仪零偏,表示h的时间导数,mi为i系下表示的恒定磁场向量,表示mi的时间导数,nmi表示因地球自转引起的磁场向量误差;其中:运算a×表示由一个三维向量a=[a1 a2 a3]T构成的叉乘矩阵,具体地,a×的展开式如下:
观测方程:
式中:ym表示磁力仪的测量值,为姿态矩阵的转置,即上标T表示向量的转置运算,nm表示磁力仪测量值的高斯噪声。
具体地,不是一般性,假定均匀磁场的磁场强度为1,即||mi||=1。以上模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量。磁场向量mi的初始值可选为磁力仪的首次测量值,磁力仪参数矩阵S的初始值可选为单位矩阵或事先给定,陀螺零偏ε和磁力仪参数矩阵h的初始值可选为零。因状态可观性的关系,姿态的初始值确定为单位矩阵,对应的初始方差矩阵为零。
基于以上模型,可采用序贯滤波方法(如扩展卡尔曼滤波、粒子滤波等)估计出状态空间模型的状态。为了满足||mi||=1的状态约束,需要对序贯滤波的结果进行如下尺度调整:Srs=||mi||S,mi rs=mi/||mi||,其中:Srs表示尺度调整后的磁力仪矩阵,mi rs表示尺度调整后的磁场向量,||·||表示取模运算。
下面以扩展卡尔曼滤波为例扼要阐述具体实施过程:
定义状态误差δx为估计值减去真值x,即而姿态估计值与姿态真值及姿态误差ψ的关系定义为则相应的状态误差向量表示为δx=[ψT δεTvecT(δS) δhT δmiT]T,上部T表示向量的转置运算,vec(·)表示将矩阵按照列的顺序拼接起来的运算;ψT表示姿态误差ψ的转置,εT表示陀螺零偏向量ε的转置,hT、miT分别表示h、mi的装置;
状态误差的线性近似状态空间模型按如下给出
其中噪声矩阵
式中:矩阵F、G、Hm分别表示***矩阵、***输入矩阵和观测矩阵;I3表示三阶单位矩阵;符号表示矩阵与矩阵的Kronecker乘积;
对扩展卡尔曼滤波的结果进行如下尺度调整:Srs=||mi||S,mi rs=mi/||mi||。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (4)
1.一种基于序贯滤波的陀螺与磁传感器联合标定方法,其特征在于,包括如下步骤:
步骤1:建立联合标定问题的状态空间模型;
步骤2:采用序贯滤波方法估计出状态空间模型的状态。
2.根据权利要求1所述的基于序贯滤波的陀螺与磁传感器联合标定方法,其特征在于,所述步骤1中联合标定问题的状态空间模型如下:
***方程:
其中:陀螺/加速度计的体坐标系表示为b系,惯性坐标系表示为i系,即初始时刻的b系;
式中:表示惯性坐标系相对于体坐标系的姿态矩阵,表示的时间导数,表示陀螺测量的b系下的角速度矢量,ε表示陀螺零偏,表示ε的时间导数,ng表示陀螺测量的高斯噪声,nε表示陀螺零偏的高斯噪声,矩阵S表示磁力仪参数矩阵,表示矩阵S的时间导数,向量h表示磁力仪零偏,表示h的时间导数,mi为i系下表示的恒定磁场向量,表示mi的时间导数,nmi表示因地球自转引起的磁场向量误差;其中:运算a×表示由一个三维向量a=[a1 a2 a3]T构成的叉乘矩阵,具体地,a×的展开式如下:
观测方程:
式中:ym表示磁力仪的测量值,为姿态矩阵的转置,即上标T表示矩阵或向量的转置运算,nm表示磁力仪测量值的高斯噪声。
3.根据权利要求1所述的基于序贯滤波的陀螺与磁传感器联合标定方法,其特征在于,步骤1中的状态空间模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量;磁场向量mi的初始值包括:磁力仪的首次测量值;磁力仪参数矩阵S的初始值包括:单位矩阵或事先给定;陀螺零偏ε和磁力仪零偏h的初始值选为零;因状态可观性的关系,姿态的初始值确定为单位矩阵,对应的初始方差矩阵为零。
4.根据权利要求3所述的基于序贯滤波的陀螺与磁传感器联合标定方法,其特征在于,所述步骤2包括:采用序贯滤波方法估计出状态空间模型的状态,所述序贯滤波方法包括:扩展卡尔曼滤波,或者粒子滤波法;具体地,采用扩展卡尔曼滤波时,包括如下步骤:
步骤2.1:定义状态误差δx为估计值减去真值x,即δ表示对应状态的误差,而姿态估计值与姿态真值及姿态误差ψ的关系定义为则相应的状态误差向量表示为δx≡[ψT δεT vecT(δS) δhT δmiT]T,vec(·)表示将矩阵按照列的顺序拼接起来的运算;ψT表示姿态误差ψ的转置,εT表示陀螺零偏向量ε的转置,hT、miT分别表示h、mi的转置;
状态误差的近似线性状态空间模型按如下给出
其中:噪声
式中:表示δx的时间导数,矩阵F、G、Hm分别表示***矩阵、***输入矩阵和观测矩阵;I表示单位矩阵,I的下标表示单位矩阵的阶数;0表示零矩阵,0的下标表示零矩阵的行列数;符号表示矩阵与矩阵的Kronecker乘积;
步骤2.2:假定当地磁场向量的大小为1,对扩展卡尔曼滤波的结果进行如下尺度调整;调整公式如下:
Srs=||mi||S,
mi rs=mi/||mi||;
式中:Srs表示尺度调整后的磁力仪矩阵,mi rs表示尺度调整后的磁场向量,||·||表示取模运算。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610497459.6A CN106197475B (zh) | 2016-06-29 | 2016-06-29 | 基于序贯滤波的陀螺与磁传感器联合标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610497459.6A CN106197475B (zh) | 2016-06-29 | 2016-06-29 | 基于序贯滤波的陀螺与磁传感器联合标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106197475A true CN106197475A (zh) | 2016-12-07 |
CN106197475B CN106197475B (zh) | 2019-06-21 |
Family
ID=57463659
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610497459.6A Active CN106197475B (zh) | 2016-06-29 | 2016-06-29 | 基于序贯滤波的陀螺与磁传感器联合标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106197475B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107389092A (zh) * | 2017-06-27 | 2017-11-24 | 上海交通大学 | 一种基于磁传感器辅助的陀螺标定方法 |
CN110375773A (zh) * | 2019-07-29 | 2019-10-25 | 兰州交通大学 | Mems惯导***姿态初始化方法 |
CN115839726A (zh) * | 2023-02-23 | 2023-03-24 | 湖南二零八先进科技有限公司 | 磁传感器和角速度传感器联合标定的方法、***及介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP0838692A1 (fr) * | 1996-10-22 | 1998-04-29 | Sagem Sa | Terminal de téléphonie mobile cellulaire localisable |
CN1291714A (zh) * | 1999-10-11 | 2001-04-18 | 中国科学院空间科学与应用研究中心 | 地磁辅助组合导航装置 |
CN105547326A (zh) * | 2015-12-08 | 2016-05-04 | 上海交通大学 | 陀螺与磁传感器联合标定方法 |
-
2016
- 2016-06-29 CN CN201610497459.6A patent/CN106197475B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP0838692A1 (fr) * | 1996-10-22 | 1998-04-29 | Sagem Sa | Terminal de téléphonie mobile cellulaire localisable |
CN1291714A (zh) * | 1999-10-11 | 2001-04-18 | 中国科学院空间科学与应用研究中心 | 地磁辅助组合导航装置 |
CN105547326A (zh) * | 2015-12-08 | 2016-05-04 | 上海交通大学 | 陀螺与磁传感器联合标定方法 |
Non-Patent Citations (3)
Title |
---|
SEUNG-MIN OH: "Multisensor Fusion for Autonomous UAV Navigation Based on the Unscented Kalman Filter with Sequential Measurement Updates", 《2010 IEEE INTERNATIONAL CONFERENCE ON MULTISENSOR FUSION AND INTEGRATION FOR INTELLIGENT SYSTEMS》 * |
段本印: "地磁辅助惯性组合导航***技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
穆华等: "水下地磁/惯性组合导航试验分析", 《中国惯性技术学报》 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107389092A (zh) * | 2017-06-27 | 2017-11-24 | 上海交通大学 | 一种基于磁传感器辅助的陀螺标定方法 |
CN110375773A (zh) * | 2019-07-29 | 2019-10-25 | 兰州交通大学 | Mems惯导***姿态初始化方法 |
CN115839726A (zh) * | 2023-02-23 | 2023-03-24 | 湖南二零八先进科技有限公司 | 磁传感器和角速度传感器联合标定的方法、***及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN106197475B (zh) | 2019-06-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103630137B (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
Gebre-Egziabher et al. | A non-linear, two-step estimation algorithm for calibrating solid-state strapdown magnetometers | |
Sheinker et al. | Localization in 3-D using beacons of low frequency magnetic field | |
EP3006896B1 (en) | Three-axis digital compass | |
CN104931028B (zh) | 一种基于深度学习的三轴磁电子罗盘误差补偿方法 | |
Dorveaux et al. | Iterative calibration method for inertial and magnetic sensors | |
CN103027657A (zh) | 基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法 | |
CN105547326A (zh) | 陀螺与磁传感器联合标定方法 | |
Camps et al. | Numerical calibration for 3-axis accelerometers and magnetometers | |
CN103630139B (zh) | 一种基于地磁梯度张量测量的水下载体全姿态确定方法 | |
CN105891755A (zh) | 航空器挂载式磁通门磁梯度张量仪的校正方法 | |
CN106197475A (zh) | 基于序贯滤波的陀螺与磁传感器联合标定方法 | |
CN101726295A (zh) | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 | |
CN106353824A (zh) | 航空磁通门磁梯度张量仪的***校正及磁干扰补偿融合方法 | |
Hu et al. | A robust orientation estimation algorithm using MARG sensors | |
CN102540110B (zh) | 磁矩测量线圈组 | |
Sun et al. | Adaptive sensor data fusion in motion capture | |
CN103299247A (zh) | 用于磁近场的动态追踪及补偿的设备和方法 | |
US10837774B2 (en) | System and method for calibrating magnetic sensors in real and finite time | |
CN106068441A (zh) | 惯性单元的校准方法 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN106709223A (zh) | 基于惯性引导采样的视觉imu方向估计方法 | |
CN106370178A (zh) | 移动终端设备的姿态测量方法及装置 | |
Castellanos et al. | A low-cost air data attitude heading reference system for the tourism airplane applications | |
CN107860382A (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | 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 |