一种组合导航方法、装置、电子设备及可读存储介质制造方法及图纸

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

An Integrated Navigation Method, Device, Electronic Equipment and Readable Storage Media

【技术实现步骤摘要】
一种组合导航方法、装置、电子设备及可读存储介质
本专利技术涉及无线定位
，特别是涉及一种组合导航方法、装置、电子设备及可读存储介质。
技术介绍
在现代导航系统中，对导航信息量的要求越来越多，对导航性能(高精度、高可靠性、抗干扰及隐蔽性)的要求也越来越高。基于地面广播网络的TC-OFDM(Time&CodeDivision-OrthogonalFrequencyDivisionMultiplexing，时分码分正交频分复用)定位技术是一种采用CDMA(CodeDivisionMultipleAccess，码分多址)扩频技术的用于解决室内位置服务的定位技术，可以使用户在室内接收到定位信号，实现高精度室内定位。虽然TC-OFDM室内定位技术在性能指标上达到了预定的要求，但是在遮挡频繁、信号强度变化剧烈，环境和信号干扰较强的复杂室内环境以及动态场景中，由于信号强度变化剧烈，信号质量较差，甚至有可能出现由于遮挡短时间内无法接收到定位信号以及环境和其他信号干扰等问题，影响接收到的定位信号的跟踪质量以及码相位的测量值(2路码相位的到达时间差)，最终的定位结果，因此存在动态适应能力较差，易受环境和信号干扰导致定位精度和性能较差的缺点。INS(InertialNavigationSystem，惯性导航系统)初始对准后可消除外界影响，但是存在陀螺仪和加速度计误差，使得位置误差会随时间逐渐增长。组合导航将二者组合在一起，发挥各自的独特优势，既能提高定位精度，又能增强接收机抗干扰能力。但是，传统的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为高斯噪声。可选的，所述根据前一时刻系统误差本文档来自技高网...

【技术保护点】
1.一种组合导航方法，其特征在于，所述方法包括：通过惯性导航系统INS定位方法计算当前时刻接收机与各基站的伪距预测值、载波相位预测值和多普勒频移，通过时分码分正交频分复用TC‑OFDM定位方法测量得到当前时刻所述接收机与所述各基站的伪距测量值和载波相位测量值，根据所述伪距测量值与所述伪距预测值的伪距误差、所述载波相位测量值与所述载波相位预测值的载波相位误差，以及所述多普勒频移，得到观测向量；根据当前时刻所述接收机的初始状态向量，得到所述接收机的最终状态向量的状态转移矩阵，获取并根据前一时刻所述接收机的最终状态向量的后验值、所述状态转移矩阵和高斯噪声，得到当前时刻所述接收机的最终状态向量的先验值；当前时刻所述接收机的最终状态向量包括：当前时刻所述接收机的初始状态向量和在当前时刻之前的N个时刻所述接收机的三维位置向量；当前时刻所述接收机的初始状态向量包括：当前时刻所述接收机的姿态误差向量、速度误差向量、位置误差向量、加速度计的零偏、陀螺仪的零偏、时钟偏差和时钟偏差率；根据前一时刻系统误差协方差矩阵的后验值、所述状态转移矩阵和系统噪声协方差矩阵，确定当前时刻的系统误差协方差矩阵的先验值；所述系统噪声协方差矩阵根据所述接收机的时钟频率漂移的功率谱密度、相位漂移的功率谱密度、加速度计噪声的功率谱密度、陀螺仪噪声的功率谱密度、加速度计动态零偏、陀螺仪动态零偏、当前时刻和前一时刻的时间间隔、加速度计的测量时间间隔、陀螺仪的测量时间间隔、加速度计动态零偏的测量时间间隔和陀螺仪动态零偏的测量时间间隔确定；根据伪距误差计算公式、载波相位误差计算公式、多普勒频移计算公式、所述初始状态向量和所述三维位置向量，得到所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵；根据所述伪距误差的观测矩阵、所述多普勒频移的观测矩阵和所述载波相位误差的观测矩阵，得到最终观测矩阵；根据所述接收机与所述各基站的相对位置和相对运动速度、所述各基站的位置坐标和时钟噪声、INS定位方法和TC‑OFDM定位方法之间同步误差残差和所述接收机的跟踪误差，得到观测噪声协方差矩阵；根据所述观测向量、所述系统误差协方差矩阵的先验值、所述最终观测矩阵、所述观测噪声协方差矩阵、当前时刻所述接收机的最终状态向量的先验值、以及卡尔曼滤波公式，得到当前时刻所述接收机的最终状态向量的后验值。...

【技术特征摘要】
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为加...

【专利技术属性】
技术研发人员：邓中亮，谢磊，范时伟，贾步云，莫君，罗牧星，汪浩，
申请(专利权)人：北京邮电大学，
类型：发明
国别省市：北京,11