CN107884800B - 观测时滞***的组合导航数据解算方法、装置及导航设备 - Google Patents
观测时滞***的组合导航数据解算方法、装置及导航设备 Download PDFInfo
- Publication number
- CN107884800B CN107884800B CN201610877288.XA CN201610877288A CN107884800B CN 107884800 B CN107884800 B CN 107884800B CN 201610877288 A CN201610877288 A CN 201610877288A CN 107884800 B CN107884800 B CN 107884800B
- Authority
- CN
- China
- Prior art keywords
- time
- moment
- state parameter
- matrix
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims abstract description 193
- 238000004364 calculation method Methods 0.000 claims abstract description 90
- 230000007704 transition Effects 0.000 claims abstract description 66
- 238000005070 sampling Methods 0.000 claims description 31
- 238000012937 correction Methods 0.000 claims description 17
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000001186 cumulative effect Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 description 19
- 230000008569 process Effects 0.000 description 9
- 230000004048 modification Effects 0.000 description 8
- 238000012986 modification Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 230000003068 static effect 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/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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- 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
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种观测时滞***的组合导航数据解算方法、装置及导航设备,属于导航技术领域。所述方法包括:存储k时刻的预测状态误差协方差矩阵以及INS解算得到的k时刻的第一导航数据;在k’时刻接收到k时刻采集的GNSS数据时,根据GNSS数据解算得到的第二导航数据、第一导航数据和预测误差协方差矩阵进行组合更新解算;根据累积状态转移矩阵和组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,j时刻为完成组合更新解算的时刻;根据上一时刻的组合更新解算结果对j时刻的预测状态参数估计值进行修正;根据修正后的j时刻的预测状态参数估计值和j时刻的预测状态误差协方差矩阵修正j时刻的INS误差。
Description
技术领域
本发明涉及导航技术领域,特别涉及一种观测时滞***的组合导航数据解算方法、装置及导航设备。
背景技术
随着导航技术的不断发展,单一导航***已难以满足用户对导航精度的要求,因此,一种将至少两种导航***组合在一起的组合导航技术应用而生。
相关技术中,组合导航技术被应用在全球卫星导航***/惯性导航***(GlobalNavigation Satellite System/Inertial Navigation System,GNSS/INS)组合导航***中。相较于单一的导航***,GNSS/INS组合导航***在保持INS中信息全面性和自主性的同时,利用GNSS克服了INS中导航误差随时间累积的问题,在导航精度和导航性能上明显优于单一导航***。
然而在实时差分(Real-Time Kinematic,RTK)模式下,GNSS/INS组合导航***存在观测时滞。GNSS数据通过电台或网络传输时存在较大传输延迟,而INS数据却不存在传输延迟,导致同一时刻采集的GNSS数据和INS数据的接收时刻存在较大差异,致使进行导航数据组合解算时,GNSS数据和INS数据无法进行时间同步,严重影响组合导航的导航精度。
发明内容
为了解决相关技术中RTK模式下,GNSS数据通过电台或网络传输时存在较大传输延迟,而INS数据却不存在传输延迟,导致同一时刻采集的GNSS数据和INS数据的接收时刻存在较大差异,致使进行导航数据组合解算时,GNSS数据和INS数据无法进行时间同步,严重影响组合导航的导航精度的问题,本发明实施例提供了一种观测时滞***的组合导航数据解算方法、装置及导航设备。所述技术方案如下:
第一方面,提供了一种观测时滞***的组合导航数据解算方法,用于GNSS/INS组合导航***中,该方法包括:
存储k时刻的预测状态误差协方差矩阵以及INS解算得到的所述k时刻的第一导航数据,所述k时刻是GNSS采样时刻,所述预测状态误差协方差矩阵用于指示所述k时刻的预测状态参数估计值的误差;
在k’时刻接收到所述k时刻采集的GNSS数据时,根据所述GNSS数据解算得到的第二导航数据、所述第一导航数据和所述预测误差协方差矩阵进行组合更新解算,组合更新解算结果包括所述k时刻的状态参数估计值和状态误差协方差矩阵;
根据累积状态转移矩阵和所述组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,所述j时刻为完成组合更新解算的时刻,所述累积状态转移矩阵用于指示所述k时刻到所述j时刻的状态变化;
当k-时刻的组合更新解算发生在所述k时刻与所述k’时刻之间时,根据所述k-时刻的组合更新解算结果对所述j时刻的预测状态参数估计值进行修正,所述k-时刻为所述k时刻之前的GNSS采样时刻;
根据修正后的所述j时刻的预测状态参数估计值和所述j时刻的预测状态误差协方差矩阵修正所述j时刻的INS误差。
该实现方式中,导航设备在GNSS数据采集时刻对INS解算的INS数据进行存储,并在接收到GNSS采集的GNSS数据时对两者进行组合更新解算,从而将组合更新解算结果转换至解算完成时刻,实现GNSS数据和INS数据的时间同步;同时,当k-时刻的组合更新解算发生在k时刻与k’时刻之间,即GNSS数据的传输延迟大于GNSS的单位采样间隔时,导航设备考虑k-时刻的组合更新解算结果对状态参数估计值的影响,对解算完成时刻的预测状态参数估计值进行修正,提高了解算完成时刻的预测状态参数估计值的准确度,进而提高了组合导航的导航精度。
在一种可能的实现方式中,所述根据所述k-时刻的组合更新解算结果对所述j时刻的预测状态参数估计值进行修正,包括:
根据所述k时刻的状态参数估计值计算k-’时刻的第一预测状态参数估计值,所述k-’时刻为获取到所述k-时刻所采集的GNSS数据的时刻;
根据所述k-时刻的组合更新解算结果中包含的状态参数估计值计算所述k-’时刻的第二预测状态参数估计值;
根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正。
在一种可能的实现方式中,所述根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正,包括:
将所述第一预测状态参数估计值和所述第二预测状态参数估计值的差值确定为修正值;
根据所述修正值对所述j时刻的预测状态参数估计值进行修正;
其中,所述修为所述第一预测状态参数估计值,为所述第二预测状态参数估计值,为t1时刻到t0’时刻的状态转移矩阵,为t0时刻到t0’时刻的状态转移矩阵,为t1时刻的状态参数估计值,为t0时刻的组合更新解算结果中包含的状态参数估计值,t0时刻为所述k-时刻,t0’时刻为获取到t0时刻所采集的GNSS数据的时刻,t1时刻为所述k时刻。
该实现方式中,通过将k时刻的状态参数估计值和k-时刻的状态参数估计值状态转移至同一时刻,得到对应同一时刻的两个预测状态参数估计值,从而根据两个预测状态参数估计值的差值确定出k-时刻的组合更新解算结果对状态参数估计值所产生的影响,并应用该差值对j时刻的预计状态参数估计值进行修正,提高了解算完成时刻的预测状态参数估计值的准确度,进而提高了组合导航的导航精度。
在一种可能的实现方式中,所述根据所述GNSS数据解算得到的第二导航数据、所述第一导航数据和所述预测误差协方差矩阵进行组合更新解算,包括:
在一种可能的实现方式中,所述根据累积状态转移矩阵和所述组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,包括:
第二方面,提供了一种观测时滞***的组合导航数据解算装置,该装置包括至少一个单元,该至少一个单元用于实现上述第一方面或第一方面任意一种可能的实现方式所提供的观测时滞***的组合导航数据解算方法。
第三方面,本发明实施例提供了一种导航设备,该导航设备包括GNSS/INS组合导航***、处理器以及存储器,所述存储器用于存储一个或一个以上的指令,所述指令被指示由所述处理器执行,所述处理器用于实现上述第一方面或第一方面中任意一种可能的实现方式所提供的观测时滞***的组合导航数据解算方法。
第四方面,本发明实施例提供一种计算机可读存储介质,该计算机可读存储介质中存储有用于实现上述第一方面或第一方面中任意一种可能的实现方式所提供的观测时滞***的组合导航数据解算方法的可执行程序。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍。
图1示出了本发明各个实施例提供的观测时滞***的组合导航数据解算方法的原理示意图;
图2A示出了本发明一个实施例提供的观测时滞***的组合导航数据解算方法的流程图;
图2B是图2A所示组合导航数据解算方法的实施示意图;
图3A示出了本发明另一个实施例提供的观测时滞***的组合导航数据解算方法的流程图;
图3B是图3A所示组合导航数据解算方法的实施示意图;
图4示出了本发明一个实施例提供的导航设备的结构框图;
图5示出了本发明一个实施例提供的观测时滞***的组合导航数据解算装置的框图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明实施方式作进一步地详细描述。
INS是一种利用角速度传感器和加速度传感器进行导航的导航***。工作状态下,INS对加速度传感器采集到的加速度数据进行积分,确定物体当前的行进速度;对角速度传感器采集到的角速度数据进行积分,确定物体当前的姿态,从而根据实时的行进速度和姿态绘制出物体的行进轨迹,并根据该行进轨迹进行导航。相较于其他类型导航***,INS采集数据的频率较高(通常为100Hz或200Hz),所采集的数据全面且具有自主性;但是在进行长时间导航的情况下,连续积分所产生的累计误差将影响INS的导航准确度。而GNSS利用卫星进行导航时不存在累计误差,因此,将GNSS和INS进行组合,并利用GNSS采集的GNSS数据对同一时刻INS的INS数据进行修正(即组合更新解算过程),能够消除INS长时间导航时产生的累计误差。
但是在RTK模式下,GNSS需要通过电台或网络接收GNSS基站差分信号(即接收GNSS数据),而GNSS基站差分信号在传输过程中存在传输延迟,且该传输延迟与GNSS基站差分信号的传输距离有关。当GNSS基站差分信号的传输距离较长时,传输延迟将大于GNSS的单位采样时间间隔(即传输延迟超过1s),导致利用GNSS数据对INS数据进行修正时产生严重的时间异步问题,影响组合导航的导航精度。
而本发明各个实施例提供的导航数据解算方法中,如图1所示,在GNSS112的采样时刻,INS111通过惯性导航单元(Inertial Measurement Unit,IMU)采集的IMU数据经过IMU数据解算后存储在存储器120中(通常为寄存器)。当接收到采集的GNSS数据时,处理器即对存储器120中存储的INS数据(IMU数据经过解算后得到)以及GNSS数据(经过GNSS数据解算得到)进行组合更新解算,实现了GNSS数据与INS数据的同步化。完成组合更新解算后,处理器对组合更新解算结果进行状态转移,并在GNSS数据的传输延迟较长的情况下,利用上一次组合更新解算结果对状态转移后的结果进行修正,最终输出最优的GNSS/INS实时组合导航结果。在INS修正阶段,处理器即利用解算完成时刻的预测状态参数估计值对INS111进行修正,从而消除INS111在长时间导航过程中的累计误差和传感器误差。
本发明各个实施例适用于安装有GNSS/INS组合导航***的导航设备中,该导航设备可以是具有导航功能的移动终端或安装在车辆中的车载导航设备。GNSS/INS中的GNSS可以是全球定位***(Global Positioning System,GPS)、格洛纳斯导航***(GLONASS)、伽利略导航***(Galileo)或北斗导航***,且INS由多种IMU构成,该IMU包括加速度传感器和角速度传感器等等。下面采用示意性的实施例进行说明。
请参考图2A,其示出了本发明一个实施例提供的观测时滞***的组合导航数据解算方法的流程图。本实施例以该组合导航数据解算方法用于安装有GNSS/INS组合导航***的导航设备中来举例说明,该方法包括:
步骤201,存储k时刻的预测状态误差协方差矩阵以及INS解算得到的k时刻的第一导航数据,k时刻是GNSS采样时刻。
可选的,当在k时刻检测到采集GNSS数据时,INS对同为k时刻采集的INS数据进行解算,并对解算得到的第一导航数据进行存储。其中,解算的方法为机械编排解算,第一导航数据用于指示k时刻的行进速度、行进姿态以及行进轨迹。
在存储第一导航数据的同时,导航设备对k时刻对应的预测误差协方差矩阵进行存储,该预测状态误差协方差矩阵根据INS上一采样时刻量测后的状态误差协方差矩阵迭代计算得到,用于指示k时刻的预测状态参数估计值的误差。
比如,如图2B所示,GNSS的采样间隔大于INS的采样间隔,GNSS在t0、t1和t2时刻进行数据采样时,导航设备存储t0、t1和t2时刻INS采样并解算得到的第一导航数据。
步骤202,在k’时刻接收在k时刻采集的GNSS数据时,根据GNSS数据解算得到的第二导航数据、第一导航数据和预测误差协方差矩阵进行组合更新解算。
由于GNSS数据在传输过程中存在传输延迟,因此导航设备需要经过一段时间后才能获取k时刻采集的GNSS数据。比如,如图2B所示,导航设备在t0’时刻获取GNSS在t0时刻采集到的GNSS数据,在t1’时刻获取GNSS在t1时刻采集到的GNSS数据。
获取到GNSS数据后,导航设备对该GNSS数据进行解算,得到对应的第二导航数据,并进一步根据存储的k时刻的第一导航数据、预测状态误差协方差矩阵以及该第二导航数据进行组合更新解算,即利用k时刻的GNSS数据进行修正。其中,组合更新解算结果包括k时刻的状态参数估计值和状态误差协方差矩阵。
步骤203,根据累积状态转移矩阵和组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,j时刻为完成组合更新解算的时刻。
在j时刻,导航设备完成对k时刻的组合更新解算,并根据k时刻与j时刻之间各个采集时刻的状态转移矩阵迭代计算出k时刻到j时刻的累积状态转移矩阵。
进一步的,导航设备利用该累积状态转移矩阵对k时刻的组合更新解算结果进行状态转移,从而得到j时刻的预测状态参数估计值和预测状态误差协方差矩阵。
步骤204,当k-时刻的组合更新解算发生在k时刻与k’时刻之间时,根据k-时刻的组合更新解算结果对j时刻的预测状态参数估计值进行修正,k-时刻为k时刻之前的GNSS采样时刻。
当GNSS数据的传输延迟较短时,GNSS数据的接收以及组合更新解算过程在一个GNSS采样间隔内完成;但是当GNSS数据的传输延迟较长时,GNSS数据的接收以及组合更新解算过程可能会跨历多个GNSS采样间隔。
比如,如图2B所示,导航设备在t1时刻之后的t0’时刻接收到t0时刻采集的GNSS数据,导航设备在t2时刻之后的t1’时刻接收到t1时刻采集的GNSS数据。
当GNSS的采样时刻(k时刻)到GNSS数据接收时刻(k’时刻)之间发生了组合更新解算(k-时刻的组合更新解算)时,导航设备在进行状态转移过程中,需要考虑组合更新解算对j时刻的预测状态参数估计值的影响,并进一步对j时刻的预测状态参数估计值进行修正,从而提高j时刻的预测状态参数估计值的准确性。比如,如图2B所示,导航设备使用t0时刻的组合更新解算结果对t1’时刻的预测状态参数估计值进行修正。
步骤205,根据修正后的j时刻的预测状态参数估计值和j时刻的预测状态误差协方差矩阵修正j时刻的INS误差。
通过上述步骤201至204完成GNSS采集时刻到当前时刻(即j时刻)的状态转移后,导航设备进一步根据当前时刻的预测状态参数估计值和预测状态误差协方差矩阵修正j时刻的INS误差,从而消除长时间导航过程中产生的累计误差和传感器误差。
综上所述,本实施例中,导航设备在GNSS数据采集时刻对INS解算的INS数据进行存储,并在接收到GNSS采集的GNSS数据时对两者进行组合更新解算,从而将组合更新解算结果转换至解算完成时刻,实现GNSS数据和INS数据的时间同步;同时,当k-时刻的组合更新解算发生在k时刻与k’时刻之间,即GNSS数据的传输延迟大于GNSS的单位采样间隔时,导航设备考虑k-时刻的组合更新解算结果对状态参数估计值的影响,对解算完成时刻的预测状态参数估计值进行修正,提高了解算完成时刻的预测状态参数估计值的准确度,进而提高了组合导航的导航精度。
请参考图3A,其示出了本发明另一个实施例提供的观测时滞***的组合导航数据解算方法的流程图。本实施例以该组合导航数据解算方法用于安装有GNSS/INS组合导航***的导航设备中来举例说明,该方法包括:
步骤301,存储k时刻的预测状态误差协方差矩阵以及INS解算得到的k时刻的第一导航数据,k时刻是GNSS采样时刻。
在INS中,当前采样时刻的状态与上一采样时刻的状态有关,且当前采样时刻的状态可以根据上一采样时刻的状态和状态转移矩阵计算得到。其中,根据上一采样时刻的状态和状态转移矩阵计算当前采样时刻的状态的公式如下。
需要说明的是,k-1时刻和k时刻为相邻的INS采集时刻。
相似的,导航设备可以根据k-1时刻的状态误差协方差矩阵计算得到k时刻的预测状态误差协方差矩阵并进行存储。其中,根据计算k时刻的预测状态误差协方差矩阵的公式如下:
其中,为k时刻的预测状态误差协方差矩阵,Φk,k-1为k-1时刻至k时刻的状态转移矩阵,为k-1时刻量测更新后的状态误差协方差矩阵,为Φk,k-1的转置矩阵,Qk-1为k-1时刻经离散化后的***噪声协方差矩阵。
需要说明的是,导航设备在存储第一导航数据和预测状态误差协方差矩阵的同时,从k时刻开始累计状态转移矩阵和***噪声协方差矩阵,供后续进行状态转移时使用。
步骤302,在k’时刻接收到k时刻采集的GNSS数据时,根据GNSS数据解算得到的第二导航数据、第一导航数据和预测误差协方差矩阵进行组合更新解算。
当在k’时刻接收到k时刻采集的GNSS数据时,导航设备即通过GNSS对接收到的GNSS数据进行解算,得到指示k时刻行进方向和行进速度的第二导航数据。
完成GNSS数据解算后,导航设备获取上述步骤301存储的k时刻的第一导航数据以及预测状态误差协方差矩阵,并利用解算得到的第二导航数据对其进行修正,即根据第二导航数据、第一导航数据和预测误差协方差矩阵进行组合更新解算(卡尔曼量测更新过程),得到k时刻量测更新后的状态参数估计值以及量测更新后的状态误差协方差矩阵。
在一种可能的实施方式中,本步骤可以包括如下步骤。
可选的,在计算k时刻的状态参数估计值和状态误差协方差矩阵前,导航设备根据存储的k时刻的预计状态误差协方差矩阵和观测矩阵计算卡尔曼滤波增益矩阵。
计算得到卡尔曼滤波增益矩阵后,导航设备即通过第一公式计算量测更新后k时刻的状态参数估计值。
在计算k时刻状态参数估计值的同时,导航设备根据在k时刻预测状态误差协方差矩阵的基础上,通过第二公式计算k时刻的状态误差协方差矩阵。其中,第二公式中,Ι为单位矩阵,(Ι-KkHk)T为Ι-KkHk的转置矩阵,Rk为k时刻的观测噪声方差矩阵,为Kk的转置矩阵。
通过上述步骤,导航设备完成组合更新解算后,即获得k时刻的状态参数估计值以及相应的状态误差协方差矩阵。
步骤303,根据累积状态转移矩阵和组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,j时刻为完成组合更新解算的时刻。
经过上述步骤302可以得到k时刻的状态参数估计值以及相应的状态误差协方差矩阵,为了进一步根据k时刻的状态得到j时刻(即组合更新解算完成时刻)的状态,导航设备需要进一步根据k时刻与j时刻之间的状态转移矩阵对k时刻的状态进行状态转移。
导航设备在进行状态转移前,需要计算k时刻到j时刻的累积状态转移矩阵。在一种可能的实施方式中,导航设备将k时刻至j时刻之间的状态转移矩阵进行迭代,即可计算得到k时刻至j时刻的累积状态转移矩阵Φj/k。
进一步的,导航设备根据累计状态转移矩阵从k时刻迭代至j时刻,可以得到其中,G为***噪声驱动矩阵,w为***噪声,可见,状态转移可以由累积状态转移矩阵及累积状态噪声阵得到。结合状态转移矩阵的性质Φk+1/k-1=Φk+1/kΦk/k-1以及上述公式,可以进一步得到第三公式以及第四公式
相应的,导航设备根据k时刻的状态参数估计值以及k时刻至j时刻的累积状态转移矩阵,通过第三公式即可计算得到j时刻的预测状态参数估计值;
导航设备根据k时刻的状态误差协方差矩阵以及k时刻至j时刻的累积状态转移矩阵,通过第四公式即可计算得到j时刻的预测状态误差协方差矩阵。
步骤304,根据k时刻的状态参数估计值计算k-’时刻的第一预测状态参数估计值,k-’时刻为获取到k-时刻所采集的GNSS数据的时刻。
当GNSS数据的传输延迟较长时,GNSS数据的接收以及组合更新解算过程可能会跨历多个GNSS采样间隔。相应的,当GNSS数据的接收以及组合更新解算过程跨历多个GNSS采样间隔时,k-时刻的组合更新解算即发生在k时刻与k’时刻之间。
比如,如图3B所示,导航设备在t2时刻之后的t1’时刻接收到t1时刻采集的GNSS数据(即t1时刻GNSS数据的接收以及组合更新解算过程跨历两个GNSS采样间隔),且t0时刻的组合更新解算发生在t1时刻与t1’时刻之间。
考虑到k-时刻的组合更新解算对j时刻带来的影响,导航设备需要根据k-时刻的组合更新解算对j时刻的预测状态参数估计值进行修正,从而提高j时刻预测状态参数估计值的准确性。
在一种可能的实施方式中,如图3B所示,导航设备可以根据转移区间T1和转移区间T2内状态参数估计值的变换情况确定t0时刻的组合更新解算产生的影响。
如图3B所示,以k时刻为t1时刻,k’时刻为t1’时刻,k-时刻为t0时刻,k-’时刻为t0’时刻为例,导航设备可以根据t1时刻的状态参数估计值以及t0’时刻至t1时刻的状态转移矩阵计算t0’时刻的第一预测状态参数估计值。
步骤305,根据k-时刻的组合更新解算结果中包含的状态参数估计值计算k-’时刻的第二预测状态参数估计值。
与上述步骤304相似的,导航设备可以根据t0时刻的状态参数估计值以及t0时刻至t0’时刻的状态转移矩阵计算t0’时刻的第二预测状态参数估计值。
步骤306,根据第一预测状态参数估计值和第二预测状态参数估计值对j时刻的预测状态参数估计值进行修正。
通过上述步骤304和305,导航设备根据k时刻的状态参数估计值和k-时刻的状态参数估计值得到同一时刻(k-’时刻)的两个预测结果;进一步的,通过分析两个预测结果的差异,导航设备即可对j时刻的预测状态参数估计值进行修正。
在一种可能的实施方式中,导航设备可以将第一预测状态参数估计值和第二预测状态参数估计值的差值作为j时刻的修正值,并利用该修正值进行修正。可选的,本步骤可以包括如下步骤。
一、将第一预测状态参数估计值和第二预测状态参数估计值的差值确定为修正值。
二、根据修正值对j时刻的预测状态参数估计值进行修正。
在一种可能的实施方式中,导航设备可以根据上述步骤计算得到的修正值以及t0’时刻至t1’时刻的状态转移矩阵进行状态参数修正。比如,导航设备可以通过如下公式对j时刻的预测状态参数估计值进行修正:
步骤307,根据修正后的j时刻的预测状态参数估计值和j时刻的预测状态误差协方差矩阵修正j时刻的INS误差。
通过上述步骤301至306完成GNSS采集时刻到当前时刻(即j时刻)的状态转移以及状态参数修正后,导航设备进一步根据当前时刻的预测状态参数估计值和预测状态误差协方差矩阵修正j时刻的INS误差,从而消除长时间导航过程中产生的累计误差和传感器误差。
本实施例中,通过将k时刻的状态参数估计值和k-时刻的状态参数估计值状态转移至同一时刻,得到对应同一时刻的两个预测状态参数估计值,从而根据两个预测状态参数估计值的差值确定出k-时刻的组合更新解算结果对状态参数估计值所产生的影响,并应用该差值对j时刻的预计状态参数估计值进行修正,提高了解算完成时刻的预测状态参数估计值的准确度,进而提高了组合导航的导航精度。
以下为本发明实施例的装置实施例,对于装置实施例中未详细阐述的部分,可以参考上述方法实施例中的细节。
请参考图4,其示出了本发明一个实施例提供的观测时滞***的组合导航设备的结构框图。该导航设备包括:GNSS/INS组合导航***410、处理器420以及存储器430。
处理器420通过总线分别与GNSS/INS组合导航***410和存储器430相连。
处理器420包括一个或一个以上处理核心。处理器420通过运行操作***或应用程序模块,实现导航数据的解算及修正功能。
可选地,存储器430可存储操作***432、至少一个功能所需的应用程序模块434。可选的,应用程序模块434包括存储模块434a、解算模块434b、转换模块434c和修正模块434d。其中,存储模块434a用于实现有关数据存储的步骤;解算模块434b用于实现有关导航数据解算的步骤;转换模块434c用于实现有关状态转移的步骤;修正模块434d用于实现有关数据修正的步骤。
此外,存储器430可以由任何类型的易失性或非易失性存储设备或者它们的组合实现,如静态随机存取存储器(SRAM),电可擦除可编程只读存储器(EEPROM),可擦除可编程只读存储器(EPROM),可编程只读存储器(PROM),只读存储器(ROM),磁存储器,快闪存储器,磁盘或光盘。
本领域技术人员可以理解,图4中所示出的结构并不构成对导航设备的限定,导航设备可以包括比图示更多或更少的部件或组合某些部件,或者不同的部件布置。
请参考图5,其示出了本发明一个实施例提供的观测时滞***的组合导航数据解算装置的框图。该装置可以通过软件或硬件实现成为导航设备的全部或一部分。该装置包括:存储单元510、解算单元520、转换单元530和修正单元540。
存储单元510用于实现上述图2A或图3A实施例中的步骤201和301等步骤,以及其它隐含的由导航设备执行的存储步骤;
解算单元520用于实现上述图2A或图3A实施例中的步骤202和302等步骤,以及其它隐含的由导航设备执行的导航数据解算步骤;
转换单元530用于实现上述图2A或图3A实施例中的步骤203和303等步骤,以及其它隐含的由导航设备执行的状态转移步骤;
修正单元540用于实现上述图2A或图3A实施例中的步骤204、205、304、305、306和307等步骤,以及其它隐含的由导航设备执行的数据修正步骤。
其中,存储单元510可以由图4中的处理器420执行存储器430中的存储模块434a实现;解算单元520可以由图4中的处理器420执行存储器430中的解算模块434b实现;转换单元530可以由图4中的处理器420执行存储器430中的转换模块434c实现;修正单元540可以由图4中的处理器420执行存储器430中的修正模块434d实现。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成,也可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,上述提到的存储介质可以是只读存储器,磁盘或光盘等。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (12)
1.一种观测时滞***的组合导航数据解算方法,其特征在于,用于安装有全球卫星导航***/惯性导航***GNSS/INS组合导航***的导航设备中,所述方法包括:
存储k时刻的预测状态误差协方差矩阵以及INS解算得到的所述k时刻的第一导航数据,所述k时刻是GNSS采样时刻,所述预测状态误差协方差矩阵用于指示所述k时刻的预测状态参数估计值的误差;
在k’时刻接收到所述k时刻采集的GNSS数据时,根据所述GNSS数据解算得到的第二导航数据、所述第一导航数据和所述预测误差协方差矩阵进行组合更新解算,组合更新解算结果包括所述k时刻的状态参数估计值和状态误差协方差矩阵;
根据累积状态转移矩阵和所述组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,所述j时刻为完成组合更新解算的时刻,所述累积状态转移矩阵用于指示所述k时刻到所述j时刻的状态变化;
当k-时刻的组合更新解算发生在所述k时刻与所述k’时刻之间时,根据所述k时刻的状态参数估计值计算k-’时刻的第一预测状态参数估计值,所述k-’时刻为获取到所述k-时刻所采集的GNSS数据的时刻,根据所述k-时刻的组合更新解算结果中包含的状态参数估计值计算所述k-’时刻的第二预测状态参数估计值,根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正,所述k-时刻为所述k时刻之前的GNSS采样时刻;
根据修正后的所述j时刻的预测状态参数估计值和所述j时刻的预测状态误差协方差矩阵修正所述j时刻的INS误差。
2.根据权利要求1所述的方法,其特征在于,所述根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正,包括:
将所述第一预测状态参数估计值和所述第二预测状态参数估计值的差值确定为修正值;
根据所述修正值对所述j时刻的预测状态参数估计值进行修正;
5.一种观测时滞***的组合导航数据解算装置,其特征在于,所述装置包括:
存储单元,用于存储k时刻的预测状态误差协方差矩阵以及INS解算得到的所述k时刻的第一导航数据,所述k时刻是GNSS采样时刻,所述预测状态误差协方差矩阵用于指示所述k时刻的预测状态参数估计值的误差;
解算单元,用于在k’时刻接收到所述k时刻采集的GNSS数据时,根据所述GNSS数据解算得到的第二导航数据、所述第一导航数据和所述预测误差协方差矩阵进行组合更新解算,组合更新解算结果包括所述k时刻的状态参数估计值和状态误差协方差矩阵;
转换单元,用于根据累积状态转移矩阵和所述组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,所述j时刻为完成组合更新解算的时刻,所述累积状态转移矩阵用于指示所述k时刻到所述j时刻的状态变化;
修正单元,用于当k-时刻的组合更新解算发生在所述k时刻与所述k’时刻之间时,根据所述k时刻的状态参数估计值计算k-’时刻的第一预测状态参数估计值,所述k-’时刻为获取到所述k-时刻所采集的GNSS数据的时刻,根据所述k-时刻的组合更新解算结果中包含的状态参数估计值计算所述k-’时刻的第二预测状态参数估计值,根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正,所述k-时刻为所述k时刻之前的GNSS采样时刻;
所述修正单元,用于根据修正后的所述j时刻的预测状态参数估计值和所述j时刻的预测状态误差协方差矩阵修正所述j时刻的INS误差。
9.一种导航设备,其特征在于,所述导航设备包括:全球卫星导航***/惯性导航***GNSS/INS组合导航***、处理器以及存储器,其中,所述存储器用于存储一个或者一个以上的指令,所述处理器用于通过执行所述指令来完成下述操作;
所述存储器,用于存储k时刻的预测状态误差协方差矩阵以及INS解算得到的所述k时刻的第一导航数据,所述k时刻是GNSS采样时刻,所述预测状态误差协方差矩阵用于指示所述k时刻的预测状态参数估计值的误差;
所述处理器,用于在k’时刻接收到所述k时刻采集的GNSS数据时,根据所述GNSS数据解算得到的第二导航数据、所述第一导航数据和所述预测误差协方差矩阵进行组合更新解算,组合更新解算结果包括所述k时刻的状态参数估计值和状态误差协方差矩阵;
所述处理器,用于根据累积状态转移矩阵和所述组合更新解算结果计算j时刻的预测状态参数估计值和预测状态误差协方差矩阵,所述j时刻为完成组合更新解算的时刻,所述累积状态转移矩阵用于指示所述k时刻到所述j时刻的状态变化;
所述处理器,用于当k-时刻的组合更新解算发生在所述k时刻与所述k’时刻之间时,根据所述k时刻的状态参数估计值计算k-’时刻的第一预测状态参数估计值,所述k-’时刻为获取到所述k-时刻所采集的GNSS数据的时刻,根据所述k-时刻的组合更新解算结果中包含的状态参数估计值计算所述k-’时刻的第二预测状态参数估计值,根据所述第一预测状态参数估计值和所述第二预测状态参数估计值对所述j时刻的预测状态参数估计值进行修正,所述k-时刻为所述k时刻之前的GNSS采样时刻;
所述处理器,用于根据修正后的所述j时刻的预测状态参数估计值和所述j时刻的预测状态误差协方差矩阵修正所述j时刻的INS误差。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610877288.XA CN107884800B (zh) | 2016-09-30 | 2016-09-30 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
PCT/CN2017/104219 WO2018059532A1 (zh) | 2016-09-30 | 2017-09-29 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610877288.XA CN107884800B (zh) | 2016-09-30 | 2016-09-30 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107884800A CN107884800A (zh) | 2018-04-06 |
CN107884800B true CN107884800B (zh) | 2020-06-26 |
Family
ID=61763276
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610877288.XA Active CN107884800B (zh) | 2016-09-30 | 2016-09-30 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN107884800B (zh) |
WO (1) | WO2018059532A1 (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110207687B (zh) * | 2018-04-25 | 2022-08-23 | 腾讯科技(深圳)有限公司 | 物体姿态测量方法、测量装置、电子设备及存储介质 |
CN109724598A (zh) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | 一种gnss/ins松组合时延误差的估计及补偿方法 |
CN110006427B (zh) * | 2019-05-20 | 2020-10-27 | 中国矿业大学 | 一种低动态高振动环境下的bds/ins紧组合导航方法 |
CN113137975B (zh) * | 2020-05-28 | 2024-03-19 | 西安天和防务技术股份有限公司 | 天文惯性组合导航的惯性校正方法、装置及电子设备 |
CN112269201B (zh) * | 2020-10-23 | 2024-04-16 | 北京云恒科技研究院有限公司 | 一种gnss/ins紧耦合时间分散滤波方法 |
CN113063429B (zh) * | 2021-03-18 | 2023-10-24 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
CN112833919B (zh) * | 2021-03-25 | 2023-11-03 | 成都纵横自动化技术股份有限公司 | 一种多余度的惯性测量数据的管理方法及其*** |
CN113110548B (zh) * | 2021-04-21 | 2023-05-12 | 北京控制工程研究所 | 一种航天器椭球集合演化的设计方法 |
CN113670337B (zh) * | 2021-09-03 | 2023-05-26 | 东南大学 | 一种用于gnss/ins组合导航卫星缓变故障检测方法 |
CN113783652B (zh) * | 2021-09-13 | 2023-06-16 | 广东汇天航空航天科技有限公司 | 一种组合导航***的数据同步方法和装置 |
CN117590441B (zh) * | 2024-01-16 | 2024-04-30 | 广州导远电子科技有限公司 | 一种完好性保护水平计算方法及相关设备 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6134484A (en) * | 2000-01-28 | 2000-10-17 | Motorola, Inc. | Method and apparatus for maintaining the integrity of spacecraft based time and position using GPS |
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN105806338A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法 |
CN105866807A (zh) * | 2016-04-05 | 2016-08-17 | 南信大影像技术工程(苏州)有限公司 | 一种提高gnss实时监测数据精度的算法 |
CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
-
2016
- 2016-09-30 CN CN201610877288.XA patent/CN107884800B/zh active Active
-
2017
- 2017-09-29 WO PCT/CN2017/104219 patent/WO2018059532A1/zh active Application Filing
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6134484A (en) * | 2000-01-28 | 2000-10-17 | Motorola, Inc. | Method and apparatus for maintaining the integrity of spacecraft based time and position using GPS |
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN105806338A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法 |
CN105866807A (zh) * | 2016-04-05 | 2016-08-17 | 南信大影像技术工程(苏州)有限公司 | 一种提高gnss实时监测数据精度的算法 |
CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
Non-Patent Citations (2)
Title |
---|
"GNSS/INS紧组合算法实时性改进与嵌入式验证";章红平等;《东南大学学报(自然科学版)》;20160731;第46卷(第4期);全文 * |
张潮." GPS/SINS组合导航数据融合算法研究".《中国优秀硕士学位论文全文数据库 信息科技辑》.2015, * |
Also Published As
Publication number | Publication date |
---|---|
CN107884800A (zh) | 2018-04-06 |
WO2018059532A1 (zh) | 2018-04-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107884800B (zh) | 观测时滞***的组合导航数据解算方法、装置及导航设备 | |
US10976444B2 (en) | System and method for GNSS ambiguity resolution | |
EP2264403B1 (en) | Positioning device and positioning method | |
CN109143304B (zh) | 用于确定无人驾驶车辆位姿的方法和装置 | |
CA3003298A1 (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
US11035915B2 (en) | Method and system for magnetic fingerprinting | |
JP2009192325A (ja) | 衛星航法/推測航法統合測位装置 | |
CN113783652B (zh) | 一种组合导航***的数据同步方法和装置 | |
JP2017194456A (ja) | ナビゲーションシステム及び誤差補正のための方法 | |
CN113203418A (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** | |
CN112556696B (zh) | 一种对象定位方法、装置、计算机设备以及存储介质 | |
WO2014001320A1 (en) | Sequential estimation in a real-time positioning or navigation system using historical states | |
JP7034379B2 (ja) | 車両測位装置 | |
CN113405545A (zh) | 定位方法、装置、电子设备及计算机存储介质 | |
JP5636410B2 (ja) | 移動情報判定装置、受信機、及びそれについての方法 | |
CN113252048A (zh) | 一种导航定位方法、导航定位***及计算机可读存储介质 | |
CN113009816B (zh) | 时间同步误差的确定方法及装置、存储介质及电子装置 | |
CN112526573B (zh) | 对象定位方法和装置、存储介质及电子设备 | |
EP3631515B1 (en) | Method for estimating a position of a mobile device using gnss signals | |
US11762104B2 (en) | Method and device for locating a vehicle | |
CN115112119A (zh) | 一种基于lstm神经网络辅助的车载导航方法 | |
US20130238237A1 (en) | Multiple data sources pedestrian navigation system | |
CN117308925B (zh) | 一种频谱地图惯导组合的导航方法、装置、设备和介质 | |
CN116105731B (zh) | 稀疏测距条件下的导航方法、装置、计算机设备及介质 | |
Sirikonda et al. | Evaluation of positional performance of NavIC software receiver with inertial sensor measurements |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right |
Effective date of registration: 20211231 Address after: 450046 Floor 9, building 1, Zhengshang Boya Plaza, Longzihu wisdom Island, Zhengdong New Area, Zhengzhou City, Henan Province Patentee after: xFusion Digital Technologies Co., Ltd. Address before: 518129 Bantian HUAWEI headquarters office building, Longgang District, Guangdong, Shenzhen Patentee before: HUAWEI TECHNOLOGIES Co.,Ltd. |
|
TR01 | Transfer of patent right |