CN102853837B - 一种mimu和gnss信息融合的方法 - Google Patents
一种mimu和gnss信息融合的方法 Download PDFInfo
- Publication number
- CN102853837B CN102853837B CN201210334728.9A CN201210334728A CN102853837B CN 102853837 B CN102853837 B CN 102853837B CN 201210334728 A CN201210334728 A CN 201210334728A CN 102853837 B CN102853837 B CN 102853837B
- Authority
- CN
- China
- Prior art keywords
- mtd
- coordinate system
- msup
- mrow
- under
- 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.)
- Expired - Fee Related
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 6
- 238000000034 method Methods 0.000 claims abstract description 18
- 239000013598 vector Substances 0.000 claims description 32
- 238000012937 correction Methods 0.000 claims description 20
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000001914 filtration Methods 0.000 abstract description 5
- 238000012545 processing Methods 0.000 abstract description 2
- 230000007547 defect Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000002194 synthesizing effect Effects 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本发明公开了MIMU和GNSS信息融合的方法,首先基于MIMU角速度和比力数据,分别在ECEF坐标系和NED坐标系下进行惯性导航解算;然后基于ECEF坐标系和NED坐标系状态方程,分别进行滤波,以达到降低滤波器状态方程维数目的;最后综合ECEF坐标系和NED坐标系下导航输出,给出最终导航解。本发明具有原理简单、处理速度快、精度高、容错性强、适用范围广等优点。
Description
技术领域
本发明主要涉及到导航***领域,特指一种基于低成本、低精度MIMU的MIMU/GNSS信息融合的方法。
背景技术
众所周知,惯性导航***以其独特的优点,在航空、航天和航海等领域得到了广泛的应用。例如,在微型飞行器、微型机器人等微小型设备等领域有着大量的应用,这也促使惯性导航***向低成本、微型化、低功耗的方向发展。近几年来,由于MIMU/GNSS组合导航***具有成本低、体积小、功耗低等优点,因而受到众多研究者的关注。MIMU(Micro-Electro-Mechanical-Systems based Inertial Measurement Unit)和GNSS(GlobalNavigation Satellite System)组合,构成MIMU/GNSS组合导航***,两者之间可以克服各自缺点、取长补短,使二者性能得到互补。MIMU/GNSS组合导航***以MIMU和GNSS输出的信息(如位置、速度、伪距或伪距率等)差值作为观测量,以MIMU线性化的误差方程作为***方程,通过Kalman滤波器对MIMU的速度、位置、姿态角以及传感器的误差进行最优估计,并根据估计结果对MIMU进行输出或者反馈校正。
对于MIMU/GNSS信息融合的方法,现有技术中有许多研究人员进行了大量的理论研究,取得了很多的进展。目前,MIMU/GNSS组合导航信息融合方法主要采用单一滤波器对MIMU的位置、速度、姿态角以及传感器的误差进行最优估计。这种方法存在滤波器状态方程维数大、计算复杂等缺点。同时,由于MEMS惯性器件性能较差,且受温度影响大等特点,在组合导航滤波器设计时需要更多的考虑惯性器件误差影响,可能会造成滤波器发散。
目前,基于低成本、低精度MIMU的MIMU/GNSS信息融合方法均采用单一滤波器,由于器件误差耦合的影响,存在计算量大、精度不高、可用性较差等不足,难以满足航空制导设备、无人飞行器、机器人和地面车辆等日益迫切的高精度要求。
发明内容
本发明要解决的技术问题就在于:针对现有技术存在的技术问题,本发明提供一种原理简单、处理速度快、精度高、容错性强、适用范围广的MIMU和GNSS信息融合的方法。
为解决上述技术问题,本发明采用以下技术方案:
一种MIMU和GNSS信息融合的方法,包括以下步骤:
(1)基于MIMU角速度和比力数据,分别在ECEF坐标系和NED坐标系下进行惯性导航解算;将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算,并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度;
(2)建立ECEF坐标系下的位置和速度误差方程,作为ECEF坐标系下的误差状态方程;建立NED坐标系下姿态角误差方程和速度误差方程,作为NED坐标系下的误差状态方程;
(3)利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程,利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程;
(4)基于ECEF坐标系下的误差状态方程,采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计;基于NED坐标系下的误差状态方程,采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计;
(5)对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正,对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正,根据估计陀螺零偏和加速度计偏置,分别对IMU输出的角速度和比力进行修正;
(6)由ECEF坐标系和NED坐标系下的惯性导航解算综合向用户输出载体的姿态角、位置、速度、角速度和加速度信息。
作为本发明的进一步改进:
所述步骤(2)中的ECEF坐标系下误差状态矢量为Xe=[δRe δVe δab]T,其中δRe、δVe分别为ECEF坐标系下的位置误差和速度误差矢量,δab为三个轴的加速度计零偏;NED坐标系下误差状态矢量Xn=[δVn ε δωb]8,其中δVn=[δVN δVE]分别为北向、东向速度误差;ε为姿态角误差;δωb为微陀螺偏差。
所述步骤(3)中ECEF坐标系下组合导航观测量采用伪距差分(星间差)和多普勒差分(星间差)来抵消接收机钟差和钟差漂移影响;NED坐标系下组合导航观测量为北向速度、东向速度。
所述步骤(4)中的卡尔曼滤波器为线性卡尔曼滤波器或扩展卡尔曼滤波器。
所述步骤(5)包括:
(5.1)对NED坐标系下姿态角对应的方向余弦阵修正为:
其中,ε×为失准角矢量的反对称矩阵,为修正前的方向余弦阵,为修正后的方向余弦阵;
(5.2)NED坐标系下的速度修正为:
其中,为NED坐标系下修正前的速度矢量,Vn为NED坐标系下修正后的速度矢量;
(5.3)ECEF坐标系下速度修正为:
其中,为修正前的位置矢量,Re为修正后的位置矢量;
(5.4)ECEF坐标系下速度修正为:
其中,为ECEF坐标系下修正前的速度矢量,Ve为ECEF坐标系下修正后的速度矢量。
与现有技术相比,本发明的优点在于:
1、本发明的MIMU和GNSS信息融合的方法,具有计算简单、精度高、容错能力强和应用领域广的优点,可广泛应用于提高航空制导设备、无人飞行器、机器人和地面车辆等用MIMU/GNSS组合导航***的导航精度。
2、本发明的MIMU和GNSS信息融合的方法针对低精度MIMU,基于误差分离原理,采用双滤波结构,使各种误差耦合误差分离;即,ECEF坐标系下导航滤波器用来估计位置、速度和加速度零偏,而NED坐标系下导航滤波器用来估计姿态角、陀螺零偏,从而提高滤波估计的精度。
3、本发明的MIMU和GNSS信息融合的方法,在ECEF坐标系下组合导航中,采用伪距单差和伪距率单差(星间单差)作为导航滤波器的观测量,这样可以抵消钟差和钟差漂移的影响,从而减少了导航滤波器的维数,提高了计算效率和导航定位的精度。
附图说明
图1是本发明方法的流程示意图。
图2为本发明方法在应用时的框架原理示意图。
具体实施方式
以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
如图1和图2所示,本发明的MIMU和GNSS信息融合的方法,其原理为:首先,基于MIMU角速度和比力数据,分别在ECEF坐标系和NED坐标系下进行惯性导航解算;然后,基于ECEF坐标系和NED坐标系状态方程,分别进行滤波,以达到降低滤波器状态方程维数目的;最后,综合ECEF坐标系和NED坐标系下导航输出,给出最终导航解。
结合具体应用实例,本发明的具体流程为:
1.采集MIMU的角速度和比力数据,分别在ECEF坐标系和NED坐标系下进行惯性导航解算,并且将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算,并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度;
2.建立ECEF坐标系下的位置和速度误差方程,作为ECEF坐标系下的误差状态方程;建立NED坐标系下姿态角误差方程和速度误差方程,作为NED坐标系下的误差状态方程。
2.1、ECEF坐标系下误差状态矢量为Xe=[δRe δVe δab]T,其中,δRe、δVe分别为ECEF坐标系下的位置误差和速度误差矢量,δab为三个轴的加速度计零偏。
误差状态方程为下式(1)
其中:
We=[Wa]T为加速度计噪声Wa向量;B=diag(βa,βa,βa)为对角阵,其元素为对应的随机过程相关时间倒数;为从载体系至ECEF坐标系的方向余弦阵;
2.2、NED坐标系下误差状态矢量Xn=[δVn ε δωb]8,其中δVn=[δVN δVE]分别为北向和东向速度误差;ε为姿态角误差;δωb为微陀螺偏差。
在北东地(NED)坐标系下的状态误差方程为下式(2):
其中, F12=-F21;
ωie为地球自转的角速率;L为纬度;fN,fE,fD为载体加速度沿北向、东向和垂直方向的分量;c11,c12,c13,…为方向余弦矩阵的元素;wgx,wgy,wgz为陀螺的噪声分量。
3.利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程,利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程;
3.1、ECEF坐标系下组合导航观测量采用伪距差分和多普勒差分(星间差)来抵消接收机钟差和钟差漂移影响,降低滤波器的维数,设通道1观测的卫星为基准卫星,则基于卫星之间单差的观测方程表示为下式(3):
式中,δρj1=δρj-δρ1,n为跟踪卫星的数目;
3.2、NED坐标系下组合导航观测量为北向速度、东向速度,测量方程为下式(4):
其中, 为MIMU导航解算的北向速度估计值,由GPS测量得到北向速度测量值; 为MIMU导航解算的东向速度估计值,由GPS测量得到东向速度测量值;v为速度观测噪声矢量。
4.基于ECEF坐标系下的误差状态方程,采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计,基于NED坐标系下的误差状态方程,采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计;
基于ECEF坐标系下的误差状态方程和观测方程,基于NED坐标系下的误差状态方程和观测方程,分别进行卡尔曼滤波:
状态一步预测方程将简化为下式(5):
XK+1/K=ΦK+1,KXK (5)
预测误差的方差阵为下式(6):
滤波增益为下式(7):
状态的最佳估计为下式(8):
XK+1/K+1=KK+1·(ZK+1-HK+1XK+1/K) (8)
估计误差的方差阵为下式(9):
PK+1/K+1=PK+1/K-KK+1HK+1PK+1/K (9)
5.对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正,对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正,根据估计陀螺零偏和加速度计偏置,分别对IMU输出的角速度和比力进行修正。
在NED坐标系下姿态角对应的方向余弦阵修正为下式(10):
其中,ε×为失准角矢量的反对称矩阵,为修正前的方向余弦阵,为修正后的方向余弦阵;
NED坐标系下速度修正为下式(11):
其中,为NED坐标系下修正前的速度矢量,Vn为NED坐标系下修正后的速度矢量。ECEF坐标系下速度修正为下式(12):
其中,为修正前的位置矢量,Re为修正后的位置矢量;
ECEF坐标系下速度修正为下式(13):
其中,为ECEF坐标系下修正前的速度矢量,Ve为ECEF坐标系下修正后的速度矢量。
以上仅是本发明的优选实施方式,本发明的保护范围并不仅局限于上述实施例,凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理前提下的若干改进和润饰,应视为本发明的保护范围。
Claims (4)
1.一种MIMU和GNSS信息融合的方法,其特征在于,包括以下步骤:
(1)基于MIMU角速度和比力数据,分别在ECEF坐标系和NED坐标系下进行惯性导航解算;将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算,并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度;
(2)建立ECEF坐标系下的位置和速度误差方程,作为ECEF坐标系下的误差状态方程;建立NED坐标系下姿态角误差方程和速度误差方程,作为NED坐标系下的误差状态方程;
(3)利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程,利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程;
(4)基于ECEF坐标系下的误差状态方程,采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计;基于NED坐标系下的误差状态方程,采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计;
(5)对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正,对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正,根据估计陀螺零偏和加速度计偏置,分别对IMU输出的角速度和比力进行修正;
所述步骤(5)包括:
(5.1)对NED坐标系下姿态角对应的方向余弦阵修正为:
其中,ε×为失准角矢量的反对称矩阵,为修正前的方向余弦阵,为修正后的方向余弦阵;
(5.2)NED坐标系下的速度修正为:
其中,为NED坐标系下修正前的速度矢量,Vn为NED坐标系下修正后的速度矢量;
(5.3)ECEF坐标系下速度修正为:
其中,为修正前的位置矢量,Re为修正后的位置矢量;
(5.4)ECEF坐标系下速度修正为:
其中,为ECEF坐标系下修正前的速度矢量,Ve为ECEF坐标系下修正后的速度矢量;
(6)由ECEF坐标系和NED坐标系下的惯性导航解算综合向用户输出载体的姿态角、位置、速度、角速度和加速度信息。
2.根据权利要求1所述MIMU和GNSS信息融合的方法,其特征在于,所述步骤(2)中的ECEF坐标系下误差状态矢量为Xe=[δRe δVe δab]T,其中δRe、δVe分别为ECEF坐标系下的位置误差和速度误差矢量,δab为三个轴的加速度计零偏;NED坐标系下误差状态矢量Xn=[δVn ε δωb]8,其中δVn=[δVN δVE]分别为北向、东向速度误差;ε为姿态角误差;δωb为微陀螺偏差。
3.根据权利要求2所述MIMU和GNSS信息融合的方法,其特征在于,所述步骤(3)中ECEF坐标系下组合导航观测量采用伪距差分和多普勒差分来抵消接收机钟差和钟差漂移的影响;NED坐标系下组合导航观测量为北向速度、东向速度。
4.根据权利要求1或2或3所述MIMU和GNSS信息融合的方法,其特征在于,所述步骤(4)中的卡尔曼滤波器为线性卡尔曼滤波器或扩展卡尔曼滤波器。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210334728.9A CN102853837B (zh) | 2012-09-11 | 2012-09-11 | 一种mimu和gnss信息融合的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210334728.9A CN102853837B (zh) | 2012-09-11 | 2012-09-11 | 一种mimu和gnss信息融合的方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102853837A CN102853837A (zh) | 2013-01-02 |
CN102853837B true CN102853837B (zh) | 2015-04-29 |
Family
ID=47400638
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201210334728.9A Expired - Fee Related CN102853837B (zh) | 2012-09-11 | 2012-09-11 | 一种mimu和gnss信息融合的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102853837B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103163508B (zh) * | 2013-02-01 | 2014-12-24 | 中国人民解放军国防科学技术大学 | 一种用于水下ins和dvl组合导航***的dvl参数标定方法 |
CN103454664B (zh) * | 2013-08-20 | 2016-02-24 | 中国人民解放军国防科学技术大学 | 一种基于陀螺测量信息约束的gnss载波相位模糊度求解方法 |
CN108810875A (zh) * | 2017-05-05 | 2018-11-13 | 中兴通讯股份有限公司 | 融合定位的方法及装置 |
CN109471102B (zh) * | 2018-10-23 | 2021-05-04 | 湖北航天技术研究院总体设计所 | 一种惯组误差修正方法 |
CN109870906B (zh) * | 2019-02-25 | 2020-06-12 | 北京航空航天大学 | 一种基于bbo优化人工势场的高速旋翼飞行器路径规划方法 |
CN110308467B (zh) * | 2019-06-21 | 2022-06-28 | 南京理工大学 | 一种基于Zynq-7020的超紧耦合微***及方法 |
CN113204042B (zh) * | 2021-05-21 | 2022-11-18 | 北京交通大学 | 一种基于精密单点定位的多星座联合列车定位方法 |
CN114545764B (zh) * | 2022-01-24 | 2022-12-20 | 西北大学 | 一种双重滤波加权平均的星站钟差数据融合方法 |
CN114967458B (zh) * | 2022-05-30 | 2023-03-10 | 江南大学 | 一种微纳机器人集群的驱动控制*** |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2113784A2 (en) * | 2008-04-30 | 2009-11-04 | Honeywell International Inc. | Systems and methods for determining location information using dual filters |
CN101706284A (zh) * | 2009-11-09 | 2010-05-12 | 哈尔滨工程大学 | 提高船用光纤陀螺捷联惯导***定位精度的方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110068973A1 (en) * | 2009-09-24 | 2011-03-24 | Coherent Navigation, Inc. | Assimilating GNSS Signals to Improve Accuracy, Robustness, and Resistance to Signal Interference |
-
2012
- 2012-09-11 CN CN201210334728.9A patent/CN102853837B/zh not_active Expired - Fee Related
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2113784A2 (en) * | 2008-04-30 | 2009-11-04 | Honeywell International Inc. | Systems and methods for determining location information using dual filters |
CN101706284A (zh) * | 2009-11-09 | 2010-05-12 | 哈尔滨工程大学 | 提高船用光纤陀螺捷联惯导***定位精度的方法 |
Non-Patent Citations (2)
Title |
---|
北斗/微惯导组合导航方法研究;何晓峰;《中国优秀博士学位论文全文数据库信息科技辑》;20110415(第4期);正文第9页,第38页,第63页,第67-69页,第96页,第118页 * |
唐康华等.MEMS-IMU/GPS组合导航中的多模态Kalman滤波器设计.《中国惯性技术学报》.2007,第15卷(第3期), * |
Also Published As
Publication number | Publication date |
---|---|
CN102853837A (zh) | 2013-01-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102853837B (zh) | 一种mimu和gnss信息融合的方法 | |
CN103917850B (zh) | 一种惯性导航***的运动对准方法 | |
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
CN104181572B (zh) | 一种弹载惯性/卫星紧组合导航方法 | |
CN106443746B (zh) | 一种低成本双天线gnss/ahrs组合测姿方法 | |
CN103217159B (zh) | 一种sins/gps/偏振光组合导航***建模及动基座初始对准方法 | |
CN102169184B (zh) | 组合导航***中测量双天线gps安装失准角的方法和装置 | |
CN106405670B (zh) | 一种适用于捷联式海洋重力仪的重力异常数据处理方法 | |
CN112378400A (zh) | 一种双天线gnss辅助的捷联惯导组合导航方法 | |
CN104316947B (zh) | Gnss/ins超紧组合导航装置及相对导航*** | |
CN103900565B (zh) | 一种基于差分gps的惯导***姿态获取方法 | |
US20050065727A1 (en) | Low cost multisensor high precision positioning and data integrated method and system thereof | |
CN103389092B (zh) | 一种系留飞艇姿态测量装置及测量方法 | |
CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** | |
CN103941274B (zh) | 一种导航方法及导航终端 | |
CN103235328A (zh) | 一种gnss与mems组合导航的方法 | |
CN104181574A (zh) | 一种捷联惯导***/全球导航卫星***组合导航滤波***及方法 | |
CN103868514A (zh) | 一种在轨飞行器自主导航*** | |
CN103217174B (zh) | 一种基于低精度微机电***的捷联惯导***初始对准方法 | |
CN102519485A (zh) | 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法 | |
CN103278165A (zh) | 基于剩磁标定的磁测及星光备份的自主导航方法 | |
CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合***进行航向校正的方法 | |
CN103454665A (zh) | 一种双差gps/sins组合导航姿态测量方法 | |
CN102393204B (zh) | 一种基于sins/cns的组合导航信息融合方法 | |
CN103575297B (zh) | 基于卫星导航接收机的gnss和mimu组合导航航向角估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20150429 Termination date: 20190911 |