CN112146653A - 一种提高组合导航解算频率的方法 - Google Patents
一种提高组合导航解算频率的方法 Download PDFInfo
- Publication number
- CN112146653A CN112146653A CN202010765967.4A CN202010765967A CN112146653A CN 112146653 A CN112146653 A CN 112146653A CN 202010765967 A CN202010765967 A CN 202010765967A CN 112146653 A CN112146653 A CN 112146653A
- Authority
- CN
- China
- Prior art keywords
- filtering
- measurement
- interruption
- updating
- navigation
- 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 22
- 238000001914 filtration Methods 0.000 claims abstract description 42
- 238000005259 measurement Methods 0.000 claims abstract description 38
- 239000011159 matrix material Substances 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000007704 transition Effects 0.000 claims abstract description 13
- 238000012937 correction Methods 0.000 claims abstract description 6
- 230000008569 process Effects 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- 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/20—Instruments for performing navigational calculations
-
- 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)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种提高组合导航解算频率的方法,首先确定组合导航的状态变量;未接收到外部传感器的量测信息时,在中断内进行IMU数据接收、纯惯性解算、一步状态转移矩阵计算和***噪声矩阵计算;定义滤波标志位默认为0,接收到外部传感器的量测信息时,在中断内再加上量测矩阵和量测向量计算,令滤波标志位为1;在中断外滤波,完成时间更新和量测更新,此时令滤波标志位为2;在中断内,当判别滤波标志位为2,根据时间、量测更新得到的状态变量对组合导航的姿态、速度和位置进行反馈校正并对相应的状态变量置零,完成一次滤波,此时滤波标志位重置为0,等待下一次滤波。本发明能够提高组合导航解算频率,提高惯导导航精度。
Description
技术领域
本发明属于惯性导航技术领域,具体涉及一种提高组合导航解算频率的方法。
背景技术
捷联惯导***(SINS)由陀螺、加速度计、导航计算机等组成可实现自主导航的***,通过其导航计算机的导航计算,能够提供包括姿态、速度和位置信息,但其误差随时间积累,不能单独长时间工作,需要利用外部传感器的观测信息通过滤波算法来修正补偿SINS***,以抑制其随时间积累的误差。
在实际基于数字信号处理(DSP)的惯导***中,导航计算机从接收惯性测量单元(IMU)数据、接收外部传感器量测信息、纯惯性解算、组合导航解算和发送导航信息都需要一定时间,DSP的中断时间需大于整个流程时间,根据实际测试发现,组合导航的卡尔曼滤波过程的时间更新和量测更新涉及到高维矩阵的多次相乘和求逆,占据了整个流程的大部分时间,抑制了导航解算频率。而通过提高组合导航解算频率,能够更真实的反映运载体的运动,提高惯导的导航精度。
发明内容
有鉴于此,本发明提供了一种提高组合导航解算频率的方法,能够提高组合导航解算频率,提高惯导导航精度。
本发明采取的技术方案如下:
一种提高组合导航解算频率的方法,所述方法步骤如下:
步骤一,选择姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为组合导航的状态变量;
步骤二,未接收到外部传感器的量测信息时,在中断内进行惯性测量单元数据接收、纯惯性解算、一步状态转移矩阵计算和***噪声矩阵计算;定义滤波标志位默认为0,接收到外部传感器的量测信息时,在中断内进行惯性测量单元数据接收、纯惯性解算、一步状态转移矩阵计算、***噪声矩阵计算以及量测矩阵计算和量测向量计算,令滤波标志位为1;在中断外滤波,完成时间更新和量测更新,此时令滤波标志位为2;
在中断内,当判别滤波标志位为2,根据时间更新和量测更新得到的状态变量对组合导航的姿态、速度和位置进行反馈校正并对相应的状态变量置零,从而完成一次滤波过程,此时滤波标志位重置为0,等待下一次滤波的到来。
进一步地,所述时间更新和量测更新在数字信号处理的主函数中进行。
有益效果:
本发明通过把计算量大的时间更新和量测更新过程转移到中断外的主函数中,能够减小中断周期,提高组合导航解算频率,能够更真实的反映运载体的角运动,进而最终提高了组合导航***的精度。
附图说明
图1为本发明的原理流程图。
具体实施方式
下面结合附图并举实施例,对本发明进行详细描述。
本发明提供了一种提高组合导航解算频率的方法,操作如下:
在精对准和组合导航中,根据捷联惯性导航***长期工作时的误差特点,选择姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为组合导航的状态变量。
其中,分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角误差,δVE、δVN、δVU分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差;δL、δλ、δh分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差;εE、εN、εU分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移;分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏。
建立卡尔曼滤波***方程
建立量测方程
Z=HX+V
其中,Z为量测向量,H为量测矩阵,V为量测噪声向量。
在没有接收到外部传感器的量测信息时,在中断内进行正常的接收IMU数据、纯惯性解算、一步状态转移矩阵φk,k-1的计算和***噪声矩阵Qk-1的计算。
其中,I为单位矩阵,Fi为每个中断内的***状态转移矩阵,Ts为中断周期,Q为***噪声方差矩阵,Gi为每个中断内的***噪声转换矩阵,i为某个中断,n为GNSS信息间隔时间内的DSP的中断个数,k为滤波次数。
定义滤波标志位默认为0。在接收到外部传感器的量测信息时,仍在中断内进行正常的接收IMU数据、纯惯性解算、一步状态转移矩阵φk,k-1的计算和***噪声矩阵Qk-1的计算即所谓中断内部的计算,同时还要计算量测矩阵Hk和量测向量Zk,此时令滤波标志位为1。滤波的过程在中断外,即在DSP的主函数中进行,当中断再次来临时,暂停滤波而继续执行中断内部的计算。经过数个中断后完成时间更新和量测更新,此时令滤波标志位为2。
其中时间更新包括:
状态一步预测向量
Xk/k-1=φk,k-1Xk-1
一步预测均方误差矩阵P
量测更新包括:
滤波增益
其中Rk为量测噪声向量的协方差矩阵。
状态估值计算
Xk=Xk/k-1+Kk(Zk-HkXk/k-1)
估计均方误差矩阵
在中断内,当惯导***判别滤波标志位为2,即时间更新和量测更新执行完毕时,根据时间更新和量测更新得到的状态变量对姿态、速度和位置进行反馈校正并对相应的状态变量置零,从而完成一次滤波过程,此时滤波标志位重置为0,等待下一次滤波的到来。其中校正方式具体如下所示:
速度误差校正:
[VE(k) VN(k) VU(k)]T=[VE(k-1) VN(k-1) VU(k-1)]T-[δVE δVN δVU]T
位置误差校正:[Lk λk hk]T=[Lk-1 λk-1 hk-1]T-[δL δλ δh]T
如图1所示,执行时,首先判断中断是否到来,若中断未到来,则判断此时的滤波标志位是否为1,若不为1则等待下一次中断;若为1则执行时间更新和量测更新,执行完毕后令滤波标志位为2;
若中断到来,接收到外部传感器的量测信息时,在中断内进行IMU数据接收、纯惯性解算、一步状态转移矩阵计算、***噪声矩阵计算以及量测矩阵计算和量测向量计算,令滤波标志位为1;未接收到外部传感器的量测信息时,在中断内进行IMU数据接收、纯惯性解算、一步状态转移矩阵计算和***噪声矩阵计算;并判断滤波标志位是否为2,若为2则进行反馈校正并对相应的状态变量置零,从而完成一次滤波过程,此时滤波标志位重置为0,发送导航信息即姿态、速度和位置信息,中断结束。若不为2则直接发送导航信息,中断结束。接着判别此时滤波标志位是否为1,若不为1则等待下一次中断;若为1则执行时间更新和量测更新,执行完毕后令滤波标志位为2。
综上所述,以上仅为本发明的较佳实施例而已,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (2)
1.一种提高组合导航解算频率的方法,其特征在于,所述方法步骤如下:
步骤一,选择姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为组合导航的状态变量;
步骤二,未接收到外部传感器的量测信息时,在中断内进行惯性测量单元数据接收、纯惯性解算、一步状态转移矩阵计算和***噪声矩阵计算;定义滤波标志位默认为0,接收到外部传感器的量测信息时,在中断内进行惯性测量单元数据接收、纯惯性解算、一步状态转移矩阵计算、***噪声矩阵计算以及量测矩阵计算和量测向量计算,令滤波标志位为1;在中断外滤波,完成时间更新和量测更新,此时令滤波标志位为2;
在中断内,当判别滤波标志位为2,根据时间更新和量测更新得到的状态变量对组合导航的姿态、速度和位置进行反馈校正并对相应的状态变量置零,从而完成一次滤波过程,此时滤波标志位重置为0,等待下一次滤波的到来。
2.如权利要求1所述的提高组合导航解算频率的方法,其特征在于,所述时间更新和量测更新在数字信号处理的主函数中进行。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010765967.4A CN112146653A (zh) | 2020-08-03 | 2020-08-03 | 一种提高组合导航解算频率的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010765967.4A CN112146653A (zh) | 2020-08-03 | 2020-08-03 | 一种提高组合导航解算频率的方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112146653A true CN112146653A (zh) | 2020-12-29 |
Family
ID=73888967
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010765967.4A Pending CN112146653A (zh) | 2020-08-03 | 2020-08-03 | 一种提高组合导航解算频率的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112146653A (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105021192A (zh) * | 2015-07-30 | 2015-11-04 | 华南理工大学 | 一种基于零速校正的组合导航***的实现方法 |
CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
CN106767900A (zh) * | 2016-11-23 | 2017-05-31 | 东南大学 | 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 |
CN109634729A (zh) * | 2018-11-20 | 2019-04-16 | 中国船舶重工集团公司第七0七研究所 | 一种捷联惯导设备多核dsp并行解算方法 |
CN109631892A (zh) * | 2019-01-22 | 2019-04-16 | 武汉大学 | 一种imu数据中断的组合导航数据处理方法 |
CN111256688A (zh) * | 2019-11-21 | 2020-06-09 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种惯性导航***的预积分算法 |
CN111351481A (zh) * | 2020-03-13 | 2020-06-30 | 南京理工大学 | 一种基于发射惯性坐标系的传递对准方法 |
-
2020
- 2020-08-03 CN CN202010765967.4A patent/CN112146653A/zh active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105021192A (zh) * | 2015-07-30 | 2015-11-04 | 华南理工大学 | 一种基于零速校正的组合导航***的实现方法 |
CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
CN106767900A (zh) * | 2016-11-23 | 2017-05-31 | 东南大学 | 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 |
CN109634729A (zh) * | 2018-11-20 | 2019-04-16 | 中国船舶重工集团公司第七0七研究所 | 一种捷联惯导设备多核dsp并行解算方法 |
CN109631892A (zh) * | 2019-01-22 | 2019-04-16 | 武汉大学 | 一种imu数据中断的组合导航数据处理方法 |
CN111256688A (zh) * | 2019-11-21 | 2020-06-09 | 中国航空工业集团公司西安飞行自动控制研究所 | 一种惯性导航***的预积分算法 |
CN111351481A (zh) * | 2020-03-13 | 2020-06-30 | 南京理工大学 | 一种基于发射惯性坐标系的传递对准方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112378400A (zh) | 一种双天线gnss辅助的捷联惯导组合导航方法 | |
US7643939B2 (en) | Methods and systems for implementing an iterated extended Kalman filter within a navigation system | |
EP1642089B1 (en) | Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients | |
CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
CN111024064A (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN108627152B (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN116007620A (zh) | 一种组合导航滤波方法、***、电子设备及存储介质 | |
CN112212862A (zh) | 一种改进粒子滤波的gps/ins组合导航方法 | |
CN112284388B (zh) | 一种无人机多源信息融合导航方法 | |
CN113009816B (zh) | 时间同步误差的确定方法及装置、存储介质及电子装置 | |
CN116718153B (zh) | 一种基于gnss和ins的形变监测方法及*** | |
CN116678406B (zh) | 组合导航姿态信息确定方法、装置、终端设备及存储介质 | |
CN103901459B (zh) | 一种mems/gps组合导航***中量测滞后的滤波方法 | |
CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
CN112146653A (zh) | 一种提高组合导航解算频率的方法 | |
CN115060275A (zh) | 一种多惯导设备导航信息择优方法 | |
Li et al. | Adaptive Kalman filter for SINS/GPS integration system with measurement noise uncertainty | |
CN114396938B (zh) | 一种舰船捷联惯导***的高精度初始对准方法 | |
CN117948986B (zh) | 一种极区因子图构建方法和极区组合导航方法 | |
CN115164886B (zh) | 车载gnss/ins组合导航***比例因子误差补偿方法 | |
CN116608852B (zh) | 一种用于农机惯导设备的陀螺仪温度漂移补偿方法 | |
CN114216480B (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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20201229 |