CN110174104A - 一种组合导航方法、装置、电子设备及可读存储介质 - Google Patents

一种组合导航方法、装置、电子设备及可读存储介质 Download PDF

Info

Publication number
CN110174104A
CN110174104A CN201910465502.4A CN201910465502A CN110174104A CN 110174104 A CN110174104 A CN 110174104A CN 201910465502 A CN201910465502 A CN 201910465502A CN 110174104 A CN110174104 A CN 110174104A
Authority
CN
China
Prior art keywords
current time
matrix
vector
receiver
error
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
Application number
CN201910465502.4A
Other languages
English (en)
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.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing University of Posts and Telecommunications
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 Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN201910465502.4A priority Critical patent/CN110174104A/zh
Publication of CN110174104A publication Critical patent/CN110174104A/zh
Pending legal-status Critical Current

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/003—Locating users or terminals or network equipment for network management purposes, e.g. mobility management locating network equipment

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明实施例提供了一种组合导航方法、装置、电子设备及可读存储介质，应用于无线定位技术领域，方法包括：通过INS定位方法和TC‑OFDM定位方法计算当前时刻接收机与各基站的伪距误差、载波相位误差和多普勒频移，并得到观测向量；根据前一时刻系统误差协方差矩阵的后验值、状态转移矩阵和系统噪声协方差矩阵，确定当前时刻系统误差协方差矩阵的先验值；根据伪距误差、多普勒频移和载波相位误差的观测矩阵得到最终观测矩阵；根据观测向量、系统误差协方差矩阵的先验值、最终观测矩阵、观测噪声协方差矩阵、当前时刻接收机的最终状态向量的先验值和卡尔曼滤波公式，得到当前时刻最终状态向量的后验值。本发明可提高动态场景的定位精度。

Description

一种组合导航方法、装置、电子设备及可读存储介质
技术领域
本发明涉及无线定位技术领域，特别是涉及一种组合导航方法、装置、电子设备及可读存储介质。
背景技术
在现代导航系统中，对导航信息量的要求越来越多，对导航性能(高精度、高可靠性、抗干扰及隐蔽性)的要求也越来越高。基于地面广播网络的TC-OFDM(Time&CodeDivision-Orthogonal Frequency Division Multiplexing，时分码分正交频分复用)定位技术是一种采用CDMA(Code Division Multiple Access，码分多址)扩频技术的用于解决室内位置服务的定位技术，可以使用户在室内接收到定位信号，实现高精度室内定位。
虽然TC-OFDM室内定位技术在性能指标上达到了预定的要求，但是在遮挡频繁、信号强度变化剧烈，环境和信号干扰较强的复杂室内环境以及动态场景中，由于信号强度变化剧烈，信号质量较差，甚至有可能出现由于遮挡短时间内无法接收到定位信号以及环境和其他信号干扰等问题，影响接收到的定位信号的跟踪质量以及码相位的测量值(2路码相位的到达时间差)，最终的定位结果，因此存在动态适应能力较差，易受环境和信号干扰导致定位精度和性能较差的缺点。INS(Inertial Navigation System，惯性导航系统)初始对准后可消除外界影响，但是存在陀螺仪和加速度计误差，使得位置误差会随时间逐渐增长。组合导航将二者组合在一起，发挥各自的独特优势，既能提高定位精度，又能增强接收机抗干扰能力。但是，传统的TC-OFDM/INS组合导航系统通过伪距进行定位，伪距(码相位)的测量值相对粗糙，精度较低，虽然码相位真实地反映了基站与接收机之间的距离，但是动态适应能力较弱，无法满足特定场景的需求，在动态场景和复杂环境下的定位精度较低。
发明内容
本发明实施例的目的在于提供一种组合导航方法、装置、电子设备及可读存储介质，提高动态场景和复杂环境下的定位精度和性能。具体技术方案如下：
本发明实施例提供了一种组合导航方法，所述方法包括：
通过INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过TC-OFDM定位方法测量得到当前时刻所述接收机与所述各基站的伪距测量值和载波相位测量值，根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量；
根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值、所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值；当前时刻所述接收机的最终状态向量包括：当前时刻所述接收机的初始状态向量和在当前时刻之前的N个时刻所述接收机的三维位置向量；当前时刻所述接收机的初始状态向量包括：当前时刻所述接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；
根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；所述系统噪声协方差矩阵根据所述接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；
根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵；
根据所述接收机与所述各基站的相对位置和相对运动速度、所述各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和所述接收机的跟踪误差，得到观测噪声协方差矩阵；
根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值。
可选的，所述通过惯性导航系统INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，包括：
若当前时刻i所述接收机的位置坐标为(xi,yi,zi)，第m个基站的位置坐标分别为(xm,ym,zm)，m为1～M的整数，M表示基站个数，M为大于3的整数；
根据公式：得到当前时刻i所述接收机与基站m的伪距预测值为δθ表示时钟偏差；
根据公式：
得到当前时刻i所述接收机与基站m的载波相位预测值
根据公式：得到当前时刻i所述接收机与基站m的多普勒频移
其中，λ为载波波长，分别为当前时刻i所述接收机的速度在坐标轴x、y、z的分量。
可选的，所述根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量，包括：
若当前时刻i所述接收机与基站m的伪距测量值为
根据公式：得到当前时刻i所述接收机与基站m的伪距误差
若当前时刻i所述接收机与基站m的载波相位测量值为
根据公式：得到当前时刻i所述接收机与基站m的载波相位误差
若 当前时刻的观测向量yi＝[yρi,yfi,yΦi]。
可选的，所述根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值和所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值，包括：
若当前时刻i所述接收机的初始状态向量
表示当前时刻i所述接收机的姿态误差向量，表示当前时刻i所述接收机的速度误差向量，表示当前时刻i所述接收机的位置误差向量，ba表示加速度计的零偏，bg表示陀螺仪的零偏，δθ表示时钟偏差，Δδθ表示时钟偏差率，时钟偏差率为时钟偏差的导数；
当前时刻i之前的N个时刻所述接收机的三维位置向量为xsi＝[ri-1,ri-2,…,ri-N]，ri-N表示时刻i-N所述接收机的三维位置向量；
当前时刻i所述接收机的最终状态向量Zi＝[xi,xsi]；
将所述初始状态向量xi对时间求导数，并求解所述导数的状态转移矩阵L，
根据公式：Fins＝Lτs+I，得到所述初始状态向量xi的状态转移矩阵Fins，
τs表示当前时刻和前一时刻的时间间隔，
[∧]表示求反对称矩阵，表示前一时刻i-1地心惯性坐标系下所述接收机的姿态向量，为载体坐标系中加速度计变化值；
rtb为所述接收机在发射体坐标系中的位置，为所述接收机在地心惯性坐标系的估计位置坐标，为所述接收机在地心地固坐标系的位置坐标，为估计位置坐标处的地心惯性坐标系的引力加速度；
根据公式：得到当前时刻i所述接收机的最终状态向量Zi的状态转移矩阵Φins；0N×17为N×17的零矩阵，IN为N阶单位矩阵；
获取前一时刻所述接收机的最终状态向量的后验值根据公式：
得到当前时刻i所述接收机的最终状态向量的先验值
u为高斯噪声。
可选的，所述根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值，包括：
若前一时刻i-1系统误差协方差矩阵的后验值为
若τs≤0.2s，系统噪声协方差矩阵
为所述接收机的时钟频率漂移的功率谱密度，为所述接收机的相位漂移的功率谱密度；Sra为加速度计噪声的功率谱密度，Srg为陀螺噪声的功率谱密度，τra为加速度计的测量时间间隔，τrg为陀螺仪的测量时间间隔；σra为加速度计比力测量噪声的标准差，σrg为陀螺仪角速率测量噪声的标准差；σbad为加速度计动态零偏的标准差，σbgd为陀螺仪动态零偏的标准差，τbad为加速度计动态零偏的测量时间间隔，τbgd为陀螺仪动态零偏的测量时间间隔；
根据：公式得到当前时刻i的系统误差协方差矩阵的先验值 是根据 和得到的；
其中，Pi表示当前时刻i的系统误差协方差矩阵，Pxx|i为xi的系统误差自协方差矩阵，Pxsxs|i为xsi的系统误差自协方差矩阵，Pxxs|i为xi对xsi的系统误差互协方差矩阵，Pxsx|i为xsi对xi的系统误差互协方差矩阵。
本发明实施例提供了一种组合导航装置，所述装置包括：
观测向量确定模块，用于通过INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过TC-OFDM定位方法测量得到当前时刻所述接收机与所述各基站的伪距测量值和载波相位测量值，根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量；
最终状态向量先验值确定模块，用于根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值、所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值；当前时刻所述接收机的最终状态向量包括：当前时刻所述接收机的初始状态向量和在当前时刻之前的N个时刻所述接收机的三维位置向量；当前时刻所述接收机的初始状态向量包括：当前时刻所述接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；
系统误差协方差矩阵先验值确定模块，用于根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；所述系统噪声协方差矩阵根据所述接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；
最终观测矩阵确定模块，用于根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵；
观测噪声协方差矩阵确定模块，用于根据所述接收机与所述各基站的相对位置和相对运动速度、所述各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和所述接收机的跟踪误差，得到观测噪声协方差矩阵；
最终状态向量后验值确定模块，用于根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值。
本发明实施例提供了一种电子设备，包括：处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口、所述存储器通过所述通信总线完成相互间的通信；
所述存储器，用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序时，实现上述任一所述的组合导航方法的步骤。
本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现上述任一所述的组合导航方法的步骤。
本发明实施例提供的组合导航方法、装置、电子设备及可读存储介质，将通过INS定位方法和TC-OFDM定位方法得到的伪距误差、载波相位误差以及多普勒频移加入到观测向量中，并将多个历史时刻的位置信息加入到初始状态矢量中，得到最终状态向量，构建卡尔曼滤波器。通过载波相位来约束前一时刻和当前时刻的运动距离变化量，同时利用载波相位测距的高精度优势来平滑辅助伪距，提升整体的测距精度，载波相位包含了基站信号的多普勒频移，多普勒频移反映了接收机的运动状态信息，在观测向量中引入载波相位可以更准确地得到接收机的位置速度信息。因此，本发明可以适应更高的动态场景，能够有效提升动态场景和复杂环境下的定位性能和精度，增强稳定性和抗干扰能力。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例的组合导航系统的框架图；
图2为本发明实施例的组合导航方法的流程图；
图3为本发明实施例的组合导航装置的结构图；
图4为本发明实施例的电子设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了解决现有技术中动态场景和复杂环境下的定位精度较低的问题，本发明实施例提供了一种组合导航方法、装置、电子设备及可读存储介质，可以提高动态场景和复杂环境下的定位精度。参见图1，图1为本发明实施例的组合导航系统的框架图，INS定位模块通过INS定位方法将INS传感器输出的信息(接收机的加速度、角速度和姿态角、四元数、接收机所处位置的磁场)进行解算，得到接收机的位置和运动状态信息。TC-OFDM定位接收机通过向TC-OFDM定位模块发送定位信号，TC-OFDM定位模块通过TC-OFDM定位方法产生基站信息，得到伪距误差、载波相位误差及多普勒频移。将伪距误差、载波相位误差及多普勒频移作为卡尔曼滤波器的输入，得到状态误差估计值，将状态误差估计值中的加速度计偏置和陀螺仪漂移反馈给INS传感器并对其校正，将最终状态向量的误差估计值中的时钟偏差对TC-OFDM定位接收机进行矫正，并将得到的多普勒频移用来估计TC-OFDM定位接收机接收的基站信号的多普勒频移，再将状态误差估计值中的位置和速度误差对INS定位方法解算后的位置速度信息进行修正，得到最终的定位结果。
参见图2，图2为本发明实施例的组合导航方法的流程图，包括以下步骤：
S201，通过INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过TC-OFDM定位方法测量得到当前时刻接收机与各基站的伪距测量值和载波相位测量值，根据伪距测量值与伪距预测值的伪距误差、载波相位测量值与载波相位预测值的载波相位误差，以及多普勒频移，得到观测向量。
具体的，载波相位和伪距均为接收机的距离测量值，伪距的测量值精度较低，伪距是通过码相位测量得到的，码相位真实地反映了基站与接收机之间的距离。而相对码相位来说，载波相位的测量值精度较高，载波相位的测量值含有整周模糊度，如果不借助伪距等其他距离位置信息，无法计算距离和定位结果。可以通过在组合导航系统的观测向量中引入载波相位，利用载波相位来平滑伪距，计算接收机的运动状态。由于接收机和基站之间的相对运动，会产生多普勒频移，接收到的信号频率等于信号发射频率加上多普勒频移，而多普勒频移包含了接收机的运动状态信息，反映了接收机的运动状态，可以在组合导航系统的观测向量中引入多普勒频移。
本发明实施例中，接收机通过对定位信号进行解电文得到基站信息，若当前时刻i接收机的位置坐标为(xi,yi,zi)，第m个基站的位置坐标分别为(xm,ym,zm)，m为1～M的整数，M表示基站个数，M为大于3的整数；
根据公式：得到当前时刻i接收机与基站m的伪距预测值为δθ表示时钟偏差；
根据公式：
得到当前时刻i接收机与基站m的载波相位预测值
根据公式：得到当前时刻i接收机与基站m的多普勒频移
其中，λ为载波波长，分别为当前时刻i接收机的速度在坐标轴x、y、z的分量。
若当前时刻i接收机与基站m的伪距测量值为根据公式：得到当前时刻i接收机与基站m的伪距误差
若当前时刻i接收机与基站m的载波相位测量值为
根据公式：得到当前时刻i接收机与基站m的载波相位误差
若 当前时刻的观测向量yi＝[yρi,yfi,yΦi]。
S202，根据当前时刻接收机的初始状态向量，得到接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻接收机的最终状态向量的后验值、状态转移矩阵和高斯噪声，得到当前时刻接收机的最终状态向量的先验值。
具体的，当前时刻接收机的初始状态向量包括：当前时刻接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率。若当前时刻i接收机的初始状态向量为
表示当前时刻i接收机的姿态误差向量，表示当前时刻i接收机的速度误差向量，表示当前时刻i接收机的位置误差向量，ba表示加速度计的零偏，bg表示陀螺仪的零偏，δθ表示时钟偏差，Δδθ表示时钟偏差率，时钟偏差率为时钟偏差的导数；
当前时刻i之前的N个时刻接收机的三维位置向量为xsi＝[ri-1,ri-2,…,ri-N]，ri-N表示时刻i-N接收机的三维位置向量；
当前时刻i接收机的最终状态向量Zi＝[xi,xsi]；当前时刻接收机的最终状态向量包括：当前时刻接收机的初始状态向量和在当前时刻之前的N个时刻接收机的三维位置向量。
将初始状态向量xi对时间求导数，并求解导数的状态转移矩阵L，
根据公式：Fins＝Lτs+I，得到初始状态向量xi的状态转移矩阵Fins，
τs表示当前时刻和前一时刻的时间间隔，
[∧]表示求反对称矩阵，表示前一时刻i-1地心惯性坐标系下接收机的姿态向量，为载体坐标系中加速度计变化值；
rtb为接收机在发射体坐标系中的位置，为接收机在地心惯性坐标系的估计位置坐标，为接收机在地心地固坐标系的位置坐标，为估计位置坐标处的地心惯性坐标系的引力加速度；
根据公式：得到当前时刻i接收机的最终状态向量Zi的状态转移矩阵Φins；0N×17为N×17的零矩阵，IN为N阶单位矩阵；
获取前一时刻接收机的最终状态向量的后验值根据公式：
得到当前时刻i接收机的最终状态向量的先验值u为高斯噪声。
S203，根据前一时刻系统误差协方差矩阵的后验值、状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；系统噪声协方差矩阵根据接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定。
若前一时刻i-1系统误差协方差矩阵的后验值为
若τs≤0.2s，系统噪声协方差矩阵
为接收机的时钟频率漂移的功率谱密度，为接收机的相位漂移的功率谱密度；对于温度补偿型石英晶体谐振器，通常Sra为加速度计噪声的功率谱密度，Srg为陀螺噪声的功率谱密度，τra为加速度计的测量时间间隔，τrg为陀螺仪的测量时间间隔；σra为加速度计比力测量噪声的标准差，σrg为陀螺仪角速率测量噪声的标准差；σbad为加速度计动态零偏的标准差，σbgd为陀螺仪动态零偏的标准差，τbad为加速度计动态零偏的测量时间间隔，τbgd为陀螺仪动态零偏的测量时间间隔。
根据：公式得到当前时刻i的系统误差协方差矩阵的先验值 是根据 和得到的；
其中，Pi表示当前时刻i的系统误差协方差矩阵，Pxx|i为xi的系统误差自协方差矩阵，为xsi的系统误差自协方差矩阵，为xi对xsi的系统误差互协方差矩阵，为xsi对xi的系统误差互协方差矩阵。
S204，根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、初始状态向量和三维位置向量，得到伪距误差的观测矩阵、多普勒频移的观测矩阵和载波相位误差的观测矩阵；根据伪距误差的观测矩阵、多普勒频移的观测矩阵和载波相位误差的观测矩阵，得到最终观测矩阵。
本发明实施例中，伪距误差计算公式为
将对xi求偏导数，得到xi的第一观测矩阵Hρx，
01,3为1*3的零矩阵，为当前时刻i最终状态向量中的位置误差向量，为三维向量，在坐标系三轴x、y、z的分量依次为δxi，δyi，δzi；
将对xsi求偏导数，得到xsi的第一观测矩阵Hρr，Hρr为M*3N维的矩阵，
伪距误差的观测矩阵Hρ＝[Hρx Hρr]；
其中，ri-1为最终状态向量中前一时刻i-1的位置向量，位置向量ri-1为三维向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差计算公式为
将对xi求偏导数，得到xi的第二观测矩阵HΦx，
其中，01,3为1*3的零矩阵，
将对xsi求偏导数，得到xsi的第二观测矩阵HΦr，
其中，ri-1为最终状态向量中前一时刻i-1的三维位置向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差的观测矩阵HΦ＝[HΦx HΦr]；
多普勒频移计算公式为
将对xi求偏导数，得到xi的第三观测矩阵Hfx，
将对xsi求偏导数，得到xsi的第三观测矩阵Hfr；Hfr为M*N的分块矩阵，Hfr中每一个元素为1*3的零矩阵；
多普勒频移的观测矩阵Hf＝[Hfx Hfr]，01,3为1*3的零矩阵，为观测向量中第m个基站的多普勒频移，为状态向量中当前时刻i的速度误差向量；
最终观测矩阵
S205，根据接收机与各基站的相对位置和相对运动速度、各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和接收机的跟踪误差，得到观测噪声协方差矩阵。
本发明实施例中，测量噪声协方差矩阵可以根据接收机与各基站的相对位置和相对运动速度、各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和接收机的跟踪误差确定。通常假设测量噪声协方差矩阵为常值，在理想情况下测量噪声协方差矩阵为测量的载噪比和加速度的函数；根据距离偏差的标准差的预期值，并考虑时间相关噪声影响，测量噪声协方差矩阵也可以是对多个测量量的加权。
S206，根据观测向量、系统误差协方差矩阵的先验值、最终观测矩阵、观测噪声协方差矩阵、当前时刻接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻接收机的最终状态向量的后验值。
若当前时刻i接收机的最终状态向量的先验值为当前时刻i的观测向量为yi；当前时刻i观测噪声协方差矩阵为Ri，当前时刻i系统误差协方差矩阵的先验值为当前时刻i的观测矩阵为Hi，
根据公式：得到当前时刻i接收机的最终状态向量的后验值
本发明实施例的组合导航方法，将通过INS定位方法和TC-OFDM定位方法得到的伪距误差、载波相位误差以及多普勒频移加入到观测向量中，并将多个历史时刻的位置信息加入到初始状态矢量中，得到最终状态向量，构建卡尔曼滤波器。通过载波相位来约束前一时刻和当前时刻的运动距离变化量，同时利用载波相位测距的高精度优势来平滑辅助伪距，提升整体的测距精度，载波相位包含了基站信号的多普勒频移，多普勒频移反映了接收机的运动状态信息，在观测向量中引入载波相位可以更准确地得到接收机的位置速度信息。因此，本发明可以适应更高的动态场景，能够有效提升动态场景和复杂环境下的定位性能和精度，增强稳定性和抗干扰能力。
相应于上述方法实施例，本发明实施例提供了一种组合导航装置，参见图3，图3为本发明实施例的组合导航装置的结构图，包括：
观测向量确定模块301，用于通过INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过TC-OFDM定位方法测量得到当前时刻接收机与各基站的伪距测量值和载波相位测量值，根据伪距测量值与伪距预测值的伪距误差、载波相位测量值与载波相位预测值的载波相位误差，以及多普勒频移，得到观测向量；
最终状态向量先验值确定模块302，用于根据当前时刻接收机的初始状态向量，得到接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻接收机的最终状态向量的后验值、状态转移矩阵和高斯噪声，得到当前时刻接收机的最终状态向量的先验值；当前时刻接收机的最终状态向量包括：当前时刻接收机的初始状态向量和在当前时刻之前的N个时刻接收机的三维位置向量；当前时刻接收机的初始状态向量包括：当前时刻接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；
系统误差协方差矩阵先验值确定模块303，用于根据前一时刻系统误差协方差矩阵的后验值、状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；系统噪声协方差矩阵根据接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；
最终观测矩阵确定模块304，用于根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、初始状态向量和三维位置向量，得到伪距误差的观测矩阵、多普勒频移的观测矩阵和载波相位误差的观测矩阵；根据伪距误差的观测矩阵、多普勒频移的观测矩阵和载波相位误差的观测矩阵，得到最终观测矩阵；
观测噪声协方差矩阵确定模块305，用于根据接收机与各基站的相对位置和相对运动速度、各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和接收机的跟踪误差，得到观测噪声协方差矩阵；
最终状态向量后验值确定模块306，用于根据观测向量、系统误差协方差矩阵的先验值、最终观测矩阵、观测噪声协方差矩阵、当前时刻接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻接收机的最终状态向量的后验值。
本发明实施例的组合导航装置，将通过INS定位方法和TC-OFDM定位方法得到的伪距误差、载波相位误差以及多普勒频移加入到观测向量中，并将多个历史时刻的位置信息加入到初始状态矢量中，得到最终状态向量，构建卡尔曼滤波器。通过载波相位来约束前一时刻和当前时刻的运动距离变化量，同时利用载波相位测距的高精度优势来平滑辅助伪距，提升整体的测距精度，载波相位包含了基站信号的多普勒频移，多普勒频移反映了接收机的运动状态信息，在观测向量中引入载波相位可以更准确地得到接收机的位置速度信息。因此，本发明可以适应更高的动态场景，能够有效提升动态场景和复杂环境下的定位性能和精度，增强稳定性和抗干扰能力。
可选的，观测向量确定模块，包括：
INS定位计算子模块，用于若当前时刻i接收机的位置坐标为(xi,yi,zi)，第m个基站的位置坐标分别为(xm,ym,zm)，m为1～M的整数，M表示基站个数，M为大于3的整数；
根据公式：得到当前时刻i接收机与基站m的伪距预测值为δθ表示时钟偏差；
根据公式：
得到当前时刻i接收机与基站m的载波相位预测值
根据公式：得到当前时刻i接收机与基站m的多普勒频移
其中，λ为载波波长，分别为当前时刻i接收机的速度在坐标轴x、y、z的分量。
可选的，观测向量确定模块，还包括：
观测向量确定子模块，用于若当前时刻i接收机与基站m的伪距测量值为
根据公式：得到当前时刻i接收机与基站m的伪距误差
若当前时刻i接收机与基站m的载波相位测量值为
根据公式：得到当前时刻i接收机与基站m的载波相位误差
若 当前时刻的观测向量
可选的，最终状态向量先验值确定模块，包括：
最终状态向量确定子模块，用于若当前时刻i接收机的初始状态向量
表示当前时刻i接收机的姿态误差向量，表示当前时刻i接收机的速度误差向量，表示当前时刻i接收机的位置误差向量，ba表示加速度计的零偏，bg表示陀螺仪的零偏，δθ表示时钟偏差，Δδθ表示时钟偏差率，时钟偏差率为时钟偏差的导数；
当前时刻i之前的N个时刻接收机的三维位置向量为xsi＝[ri-1,ri-2,…,ri-N]，ri-N表示时刻i-N接收机的三维位置向量；
当前时刻i接收机的最终状态向量Zi＝[xi,xsi]；
初始状态向量的状态转移矩阵计算子模块，用于将初始状态向量xi对时间求导数，并求解导数的状态转移矩阵L，
根据公式：Fins＝Lτs+I，得到初始状态向量xi的状态转移矩阵Fins，
τs表示当前时刻和前一时刻的时间间隔，
[∧]表示求反对称矩阵，表示前一时刻i-1地心惯性坐标系下接收机的姿态向量，为载体坐标系中加速度计变化值；
rtb为接收机在发射体坐标系中的位置，为接收机在地心惯性坐标系的估计位置坐标，为接收机在地心地固坐标系的位置坐标，为估计位置坐标处的地心惯性坐标系的引力加速度；
最终状态向量的状态转移矩阵计算子模块，用于根据公式：得到当前时刻i接收机的最终状态向量Zi的状态转移矩阵Φins；0N×17为N×17的零矩阵，IN为N阶单位矩阵；
最终状态向量的先验值计算子模块，用于获取前一时刻接收机的最终状态向量的后验值根据公式：
得到当前时刻i接收机的最终状态向量的先验值u为高斯噪声。
可选的，系统误差协方差矩阵先验值确定模块，具体用于若前一时刻i-1系统误差协方差矩阵的后验值为
若τs≤0.2s，系统噪声协方差矩阵
为接收机的时钟频率漂移的功率谱密度，为接收机的相位漂移的功率谱密度；Sra为加速度计噪声的功率谱密度，Srg为陀螺噪声的功率谱密度，τra为加速度计的测量时间间隔，τrg为陀螺仪的测量时间间隔；σra为加速度计比力测量噪声的标准差，σrg为陀螺仪角速率测量噪声的标准差；σbad为加速度计动态零偏的标准差，σbgd为陀螺仪动态零偏的标准差，τbad为加速度计动态零偏的测量时间间隔，τbgd为陀螺仪动态零偏的测量时间间隔；
根据：公式得到当前时刻i的系统误差协方差矩阵的先验值 是根据 和得到的；
其中，Pi表示当前时刻i的系统误差协方差矩阵，Pxx|i为xi的系统误差自协方差矩阵，为xsi的系统误差自协方差矩阵，为xi对xsi的系统误差互协方差矩阵，为xsi对xi的系统误差互协方差矩阵。
可选的，最终观测矩阵确定模块，包括：
伪距误差的观测矩阵确定子模块，用于伪距误差计算公式为
将对xi求偏导数，得到xi的第一观测矩阵Hρx，
01,3为1*3的零矩阵，为当前时刻i最终状态向量中的位置误差向量，为三维向量，在坐标系三轴x、y、z的分量依次为δxi，δyi，δzi；
将-δθ对xsi求偏导数，得到xsi的第一观测矩阵Hρr，Hρr为M*3N维的矩阵，
伪距误差的观测矩阵Hρ＝[Hρx Hρr]；
其中，ri-1为最终状态向量中前一时刻i-1的位置向量，位置向量ri-1为三维向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差的观测矩阵确定子模块，用于载波相位误差计算公式为
将对xi求偏导数，得到xi的第二观测矩阵HΦx，
其中，01,3为1*3的零矩阵，
将对xsi求偏导数，得到xsi的第二观测矩阵HΦr，
其中，ri-1为最终状态向量中前一时刻i-1的三维位置向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差的观测矩阵HΦ＝[HΦx HΦr]；
多普勒频移的观测矩阵确定子模块，用于多普勒频移计算公式为
将对xi求偏导数，得到xi的第三观测矩阵Hfx，
将对xsi求偏导数，得到xsi的第三观测矩阵Hfr；Hfr为M*N的分块矩阵，Hfr中每一个元素为1*3的零矩阵；
多普勒频移的观测矩阵Hf＝[Hfx Hfr]，01,3为1*3的零矩阵，为观测向量中第m个基站的多普勒频移，为状态向量中当前时刻i的速度误差向量；
最终观测矩阵确定子模块，用于得到最终观测矩阵
可选的，最终状态向量后验值确定模块，具体用于若当前时刻i接收机的最终状态向量的先验值为当前时刻i的观测向量为yi；当前时刻i观测噪声协方差矩阵为Ri，当前时刻i系统误差协方差矩阵的先验值为当前时刻i的观测矩阵为Hi，
根据公式：得到当前时刻i接收机的最终状态向量的后验值
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
本发明实施例还提供了一种电子设备，参见图4，图4为本发明实施例的电子设备的结构图，包括：处理器401、通信接口402、存储器403和通信总线404，其中，处理器401、通信接口402、存储器403通过通信总线404完成相互间的通信；
存储器403，用于存放计算机程序；
处理器401，用于执行存储器403上所存放的程序时，实现上述任一组合导航方法的步骤。
需要说明的是，上述电子设备提到的通信总线404可以是PCI(PeripheralComponent Interconnect，外设部件互连标准)总线或EISA(Extended Industry StandardArchitecture，扩展工业标准结构)总线等。该通信总线404可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口402用于上述电子设备与其他设备之间的通信。
存储器403可以包括RAM(Random Access Memory，随机存取存储器)，也可以包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器401可以是通用处理器，包括：CPU(Central Processing Unit，中央处理器)、NP(Network Processor，网络处理器)等；还可以是DSP(Digital SignalProcessing，数字信号处理器)、ASIC(Application Specific Integrated Circuit，专用集成电路)、FPGA(Field-Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明实施例还提供了一种计算机可读存储介质，计算机可读存储介质内存储有计算机程序，计算机程序被处理器执行时，实现上述任一组合导航方法的步骤。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置、电子设备及可读存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (10)

1.一种组合导航方法，其特征在于，所述方法包括：
通过惯性导航系统INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过时分码分正交频分复用TC-OFDM定位方法测量得到当前时刻所述接收机与所述各基站的伪距测量值和载波相位测量值，根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量；
根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值、所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值；当前时刻所述接收机的最终状态向量包括：当前时刻所述接收机的初始状态向量和在当前时刻之前的N个时刻所述接收机的三维位置向量；当前时刻所述接收机的初始状态向量包括：当前时刻所述接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；
根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；所述系统噪声协方差矩阵根据所述接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；
根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵；
根据所述接收机与所述各基站的相对位置和相对运动速度、所述各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和所述接收机的跟踪误差，得到观测噪声协方差矩阵；
根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值。
2.根据权利要求1所述的组合导航方法，其特征在于，所述通过INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，包括：
若当前时刻i所述接收机的位置坐标为(xi,yi,zi)，第m个基站的位置坐标分别为(xm,ym,zm)，m为1～M的整数，M表示基站个数，M为大于3的整数；
根据公式：得到当前时刻i所述接收机与基站m的伪距预测值为δθ表示时钟偏差；
根据公式：
得到当前时刻i所述接收机与基站m的载波相位预测值
根据公式：得到当前时刻i所述接收机与基站m的多普勒频移
其中，λ为载波波长，分别为当前时刻i所述接收机的速度在坐标轴x、y、z的分量。
3.根据权利要求1所述的组合导航方法，其特征在于，所述根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量，包括：
若当前时刻i所述接收机与基站m的伪距测量值为
根据公式：得到当前时刻i所述接收机与基站m的伪距误差
若当前时刻i所述接收机与基站m的载波相位测量值为
根据公式：得到当前时刻i所述接收机与基站m的载波相位误差
若 当前时刻的观测向量yi＝[yρi,yfi,yΦi]。
4.根据权利要求1所述的组合导航方法，其特征在于，所述根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值和所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值，包括：
若当前时刻i所述接收机的初始状态向量
表示当前时刻i所述接收机的姿态误差向量，表示当前时刻i所述接收机的速度误差向量，表示当前时刻i所述接收机的位置误差向量，ba表示加速度计的零偏，bg表示陀螺仪的零偏，δθ表示时钟偏差，Δδθ表示时钟偏差率，时钟偏差率为时钟偏差的导数；
当前时刻i之前的N个时刻所述接收机的三维位置向量为xsi＝[ri-1,ri-2,…,ri-N]，ri-N表示时刻i-N所述接收机的三维位置向量；
当前时刻i所述接收机的最终状态向量Zi＝[xi,xsi]；
将所述初始状态向量xi对时间求导数，并求解所述导数的状态转移矩阵L，
根据公式：Fins＝Lτs+I，得到所述初始状态向量xi的状态转移矩阵Fins，
τs表示当前时刻和前一时刻的时间间隔，
[∧]表示求反对称矩阵，表示前一时刻i-1地心惯性坐标系下所述接收机的姿态向量，为载体坐标系中加速度计变化值；
rtb为所述接收机在发射体坐标系中的位置，为所述接收机在地心惯性坐标系的估计位置坐标，为所述接收机在地心地固坐标系的位置坐标，为估计位置坐标处的地心惯性坐标系的引力加速度；
根据公式：得到当前时刻i所述接收机的最终状态向量Zi的状态转移矩阵Φins；0N×17为N×17的零矩阵，IN为N阶单位矩阵；
获取前一时刻所述接收机的最终状态向量的后验值根据公式：
得到当前时刻i所述接收机的最终状态向量的先验值u为高斯噪声。
5.根据权利要求1所述的组合导航方法，其特征在于，所述根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值，包括：
若前一时刻i-1系统误差协方差矩阵的后验值为
若τs≤0.2s，系统噪声协方差矩阵
为所述接收机的时钟频率漂移的功率谱密度，为所述接收机的相位漂移的功率谱密度；Sra为加速度计噪声的功率谱密度，Srg为陀螺噪声的功率谱密度，τra为加速度计的测量时间间隔，τrg为陀螺仪的测量时间间隔；σra为加速度计比力测量噪声的标准差，σrg为陀螺仪角速率测量噪声的标准差；σbad为加速度计动态零偏的标准差，σbgd为陀螺仪动态零偏的标准差，τbad为加速度计动态零偏的测量时间间隔，τbgd为陀螺仪动态零偏的测量时间间隔；
根据：公式得到当前时刻i的系统误差协方差矩阵的先验值Pi -，是根据 和得到的；
其中，Pi表示当前时刻i的系统误差协方差矩阵，Pxx|i为xi的系统误差自协方差矩阵，为xsi的系统误差自协方差矩阵，为xi对xsi的系统误差互协方差矩阵，为xsi对xi的系统误差互协方差矩阵。
6.根据权利要求1所述的组合导航方法，其特征在于，所述根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵，包括：
伪距误差计算公式为
将对xi求偏导数，得到xi的第一观测矩阵Hρx，
01,3为1*3的零矩阵，为当前时刻i最终状态向量中的位置误差向量，为三维向量，在坐标系三轴x、y、z的分量依次为δxi，δyi，δzi；
将对xsi求偏导数，得到xsi的第一观测矩阵Hρr，Hρr为M*3N维的矩阵，
所述伪距误差的观测矩阵Hρ＝[Hρx Hρr]；
其中，ri-1为最终状态向量中前一时刻i-1的位置向量，位置向量ri-1为三维向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差计算公式为
将对xi求偏导数，得到xi的第二观测矩阵HΦx，
其中，01,3为1*3的零矩阵，
将对xsi求偏导数，得到xsi的第二观测矩阵HΦr，
其中，ri-1为最终状态向量中前一时刻i-1的三维位置向量，在坐标系三轴x、y、z的分量依次为xi-1，yi-1，zi-1；
载波相位误差的观测矩阵HΦ＝[HΦx HΦr]；
多普勒频移计算公式为
将对xi求偏导数，得到xi的第三观测矩阵Hfx，
将对xsi求偏导数，得到xsi的第三观测矩阵Hfr；Hfr为M*N的分块矩阵，Hfr中每一个元素为1*3的零矩阵；
所述多普勒频移的观测矩阵Hf＝[Hfx Hfr]，01,3为1*3的零矩阵，为观测向量中第m个基站的多普勒频移，为状态向量中当前时刻i的速度误差向量；
最终观测矩阵
7.根据权利要求1所述的组合导航方法，其特征在于，所述根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值，包括：
若当前时刻i所述接收机的最终状态向量的先验值为当前时刻i的观测向量为yi；当前时刻i观测噪声协方差矩阵为Ri，当前时刻i系统误差协方差矩阵的先验值为Pi -，当前时刻i的观测矩阵为Hi，
根据公式：得到当前时刻i所述接收机的最终状态向量的后验值
8.一种组合导航装置，其特征在于，所述装置包括：
观测向量确定模块，用于通过惯性导航系统INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过时分码分正交频分复用TC-OFDM定位方法测量得到当前时刻所述接收机与所述各基站的伪距测量值和载波相位测量值，根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量；
最终状态向量先验值确定模块，用于根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值、所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值；当前时刻所述接收机的最终状态向量包括：当前时刻所述接收机的初始状态向量和在当前时刻之前的N个时刻所述接收机的三维位置向量；当前时刻所述接收机的初始状态向量包括：当前时刻所述接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；
系统误差协方差矩阵先验值确定模块，用于根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；所述系统噪声协方差矩阵根据所述接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；
最终观测矩阵确定模块，用于根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵；
观测噪声协方差矩阵确定模块，用于根据所述接收机与所述各基站的相对位置和相对运动速度、所述各基站的位置坐标和时钟噪声、INS定位方法和TC-OFDM定位方法之间同步误差残差和所述接收机的跟踪误差，得到观测噪声协方差矩阵；
最终状态向量后验值确定模块，用于根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值。
9.一种电子设备，其特征在于，包括：处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口、所述存储器通过所述通信总线完成相互间的通信；
所述存储器，用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序时，实现权利要求1～7任一所述的组合导航方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1～7任一所述的组合导航方法的步骤。
CN201910465502.4A 2019-05-30 2019-05-30 一种组合导航方法、装置、电子设备及可读存储介质 Pending CN110174104A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910465502.4A CN110174104A (zh) 2019-05-30 2019-05-30 一种组合导航方法、装置、电子设备及可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910465502.4A CN110174104A (zh) 2019-05-30 2019-05-30 一种组合导航方法、装置、电子设备及可读存储介质

Publications (1)

Publication Number Publication Date
CN110174104A true CN110174104A (zh) 2019-08-27

Family

ID=67696725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910465502.4A Pending CN110174104A (zh) 2019-05-30 2019-05-30 一种组合导航方法、装置、电子设备及可读存储介质

Country Status (1)

Country Link
CN (1) CN110174104A (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110865403A (zh) * 2019-10-18 2020-03-06 浙江天尚元科技有限公司 一种基于神经网络预学习和轮式里程计融合的定位方法
CN111007557A (zh) * 2019-12-10 2020-04-14 中国矿业大学 自适应运动学模型辅助的gnss载波相位与多普勒融合测速方法
CN111308454A (zh) * 2019-10-09 2020-06-19 中国人民解放军63921部队 一种利用测速数据提高航天器测距数据精度的方法
CN111649745A (zh) * 2020-05-18 2020-09-11 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质
CN112665586A (zh) * 2020-12-11 2021-04-16 陕西华燕航空仪表有限公司 一种提高mems陀螺仪精度的方法
CN112782732A (zh) * 2020-12-29 2021-05-11 湖北三江航天险峰电子信息有限公司 一种基于粒子群算法的导航信号解析方法及计算机存可读介质
CN112880674A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113189541A (zh) * 2021-04-23 2021-07-30 北京邮电大学 一种定位方法、装置及设备
CN113671551A (zh) * 2020-05-13 2021-11-19 千寻位置网络有限公司 Rtk定位解算方法
CN114563045A (zh) * 2022-02-28 2022-05-31 南京天溯自动化控制系统有限公司 一种医院后勤系统的检测方法、装置、电子设备及介质
CN114785439A (zh) * 2022-03-30 2022-07-22 北京邮电大学 一种提高工业物联网终端时间同步精度的方法及装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102353970A (zh) * 2011-06-10 2012-02-15 北京航空航天大学 一种高抗干扰性能gps/sins组合导航系统及实现方法
CN104133231A (zh) * 2014-07-29 2014-11-05 南京理工大学 一种基于积分多普勒平滑伪距的导航定位方法
CN104614739A (zh) * 2015-01-27 2015-05-13 南京师范大学 基于抗干扰滤波器的北斗多频接收机信号联合跟踪方法
CN106199668A (zh) * 2016-06-24 2016-12-07 南京理工大学 一种级联式gnss/sins深组合导航方法
CN107870338A (zh) * 2017-10-30 2018-04-03 湖南跨线桥航天科技有限公司 一种低更新频度的卫星导航载波跟踪方法
CN108594277A (zh) * 2018-04-27 2018-09-28 北京邮电大学 一种相位差确定方法、装置、电子设备及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102353970A (zh) * 2011-06-10 2012-02-15 北京航空航天大学 一种高抗干扰性能gps/sins组合导航系统及实现方法
CN104133231A (zh) * 2014-07-29 2014-11-05 南京理工大学 一种基于积分多普勒平滑伪距的导航定位方法
CN104614739A (zh) * 2015-01-27 2015-05-13 南京师范大学 基于抗干扰滤波器的北斗多频接收机信号联合跟踪方法
CN106199668A (zh) * 2016-06-24 2016-12-07 南京理工大学 一种级联式gnss/sins深组合导航方法
CN107870338A (zh) * 2017-10-30 2018-04-03 湖南跨线桥航天科技有限公司 一种低更新频度的卫星导航载波跟踪方法
CN108594277A (zh) * 2018-04-27 2018-09-28 北京邮电大学 一种相位差确定方法、装置、电子设备及存储介质

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
JUN MO等: "A Novel FLL-Assisted PLL With Fuzzy Control for TC-OFDM Carrier Signal Tracking", 《IEEE ACCESS》 *
罗瑞丹等: "一种基于相干叠加的复合载波导航信号捕获方法", 《宇航学报》 *
邓中亮等: "一种基于INS/TC-OFDM深组合的多径抑制算法", 《第九届中国卫星导航学术年会论文集-S10多元融合导航技术》 *
邓中亮等: "引入载波相位的多状态约束TC-OFDM/INS组合导航系统", 《第十届中国卫星导航年会论文集-S09用户终端技术》 *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111308454B (zh) * 2019-10-09 2022-02-11 中国人民解放军63921部队 一种利用测速数据提高航天器测距数据精度的方法
CN111308454A (zh) * 2019-10-09 2020-06-19 中国人民解放军63921部队 一种利用测速数据提高航天器测距数据精度的方法
CN110865403A (zh) * 2019-10-18 2020-03-06 浙江天尚元科技有限公司 一种基于神经网络预学习和轮式里程计融合的定位方法
CN110865403B (zh) * 2019-10-18 2024-03-05 尚元智行(宁波)科技有限公司 一种基于神经网络预学习和轮式里程计融合的定位方法
CN111007557A (zh) * 2019-12-10 2020-04-14 中国矿业大学 自适应运动学模型辅助的gnss载波相位与多普勒融合测速方法
CN111007557B (zh) * 2019-12-10 2022-05-13 中国矿业大学 自适应运动学模型辅助的gnss载波相位与多普勒融合测速方法
CN113671551B (zh) * 2020-05-13 2023-12-08 千寻位置网络有限公司 Rtk定位解算方法
CN113671551A (zh) * 2020-05-13 2021-11-19 千寻位置网络有限公司 Rtk定位解算方法
CN111649745A (zh) * 2020-05-18 2020-09-11 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质
CN112665586A (zh) * 2020-12-11 2021-04-16 陕西华燕航空仪表有限公司 一种提高mems陀螺仪精度的方法
CN112782732B (zh) * 2020-12-29 2023-08-29 湖北三江航天险峰电子信息有限公司 一种基于粒子群算法的导航信号解析方法及计算机存可读介质
CN112782732A (zh) * 2020-12-29 2021-05-11 湖北三江航天险峰电子信息有限公司 一种基于粒子群算法的导航信号解析方法及计算机存可读介质
CN112880674A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113189541A (zh) * 2021-04-23 2021-07-30 北京邮电大学 一种定位方法、装置及设备
CN113189541B (zh) * 2021-04-23 2023-12-12 北京邮电大学 一种定位方法、装置及设备
CN114563045A (zh) * 2022-02-28 2022-05-31 南京天溯自动化控制系统有限公司 一种医院后勤系统的检测方法、装置、电子设备及介质
CN114563045B (zh) * 2022-02-28 2023-06-27 南京天溯自动化控制系统有限公司 一种医院后勤系统的检测方法、装置、电子设备及介质
CN114785439A (zh) * 2022-03-30 2022-07-22 北京邮电大学 一种提高工业物联网终端时间同步精度的方法及装置

Similar Documents

Publication Publication Date Title
CN110174104A (zh) 一种组合导航方法、装置、电子设备及可读存储介质
CN102230971B (zh) Gps多天线测姿方法
US7171303B1 (en) Navigation method and apparatus
US7292185B2 (en) Attitude determination exploiting geometry constraints
US8711037B2 (en) Precision geolocation of moving or fixed transmitters using multiple observers
US20200333468A1 (en) Method for Tilt Measurement and Compensation of Surveying Instrument Based On GNSS Receiver and IMU Sensor
US8630804B2 (en) Method for geolocating an object by multitelemetry
CN105849589B (zh) 全球导航卫星系统、定位终端、定位方法以及记录介质
CN108919634A (zh) 一种北斗三频非差非组合观测值时间传递系统及方法
CN105425261B (zh) 基于GPS/Beidou2/INS的组合导航与定位方法
CN109459778A (zh) 基于抗差方差分量估计的码伪距/多普勒联合测速方法及其应用
CN100507594C (zh) 根据计算的时间进行位置计算的方法和系统
US20030058164A1 (en) Methods and systems for improvement of measurement efficiency in surveying
US11781868B2 (en) System and method for GNSS reflective surface mapping and position fix estimation
CN109613579B (zh) 一种基于最小二乘算法计算整周模糊度的方法和系统
CN110133700A (zh) 一种船载综合导航定位方法
US7269512B2 (en) Information processing apparatus and GPS positioning method
JP2010002258A (ja) 速度計測装置および変位計測装置
CN104252004B (zh) 利用单天线导航接收机测量自旋卫星姿态的系统及方法
US8818720B2 (en) Method and apparatus of GNSS receiver heading determination
US9052388B2 (en) Method and apparatus of GNSS receiver heading determination
CN109725332A (zh) 用于跟踪和确定物体位置的方法和系统
Tien et al. Adaptive strategy-based tightly-coupled INS/GNSS integration system aided by odometer and barometer
EP2748634B1 (en) Single receiver gps pointing vector sensing
Capuano et al. GNSS based attitude determination systems for nanosatellites

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

Application publication date: 20190827

RJ01 Rejection of invention patent application after publication