CN112325880B - 分布式平台相对定位方法、装置、计算机设备和存储介质 - Google Patents

分布式平台相对定位方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN112325880B
CN112325880B CN202110000761.7A CN202110000761A CN112325880B CN 112325880 B CN112325880 B CN 112325880B CN 202110000761 A CN202110000761 A CN 202110000761A CN 112325880 B CN112325880 B CN 112325880B
Authority
CN
China
Prior art keywords
measurement noise
motion platform
motion
uwb
imu
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
Application number
CN202110000761.7A
Other languages
English (en)
Other versions
CN112325880A (zh
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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110000761.7A priority Critical patent/CN112325880B/zh
Publication of CN112325880A publication Critical patent/CN112325880A/zh
Application granted granted Critical
Publication of CN112325880B publication Critical patent/CN112325880B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

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)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本申请涉及一种分布式平台相对定位方法、装置、计算机设备和存储介质。所述方法包括：在运动平台进入卫星导航拒止环境前，获取其IMU传感器的测量噪声，其UWB传感器测量其他运动平台时的测量噪声，其运动方程，以及其与其他运动平台的相对距离测量模型。分别对两项测量噪声进行Allan方差分析得到对应的测量噪声方差矩阵。在采用卡尔曼滤波算法更新动方程时将零协方差矩阵替换为对应的测量噪声方差矩阵，更新对距离测量模型时将测量误差矩阵替换为对应的测量噪声方差矩阵，根据更新结果得到运动平台位置。上述方法考虑了测量噪声的时间相关特性并滤除测量过程中的色噪声，能够提高获得的平台位置精度。

Description

分布式平台相对定位方法、装置、计算机设备和存储介质
技术领域
本申请涉及无线电导航与定位技术领域，特别是涉及一种分布式平台相对定位方法、装置、计算机设备和存储介质。
背景技术
近年来分布式运动平台正逐渐在各个领域得到充分发展运用，如无人机蜂群、无人车系统等，这些系统已被证明比传统单平台具有更多优势。现有的分布式运动平台定位的实现方式主要依靠卫星导航和惯性导航部件（IMU），然而卫星导航在复杂地形环境下和干扰环境下，难以完成有效的定位与导航任务，而惯性导航则存在严重的误差累积问题。超宽带（UWB）传感器可以提供平台之间的相对测量获取相对距离和位置信息，其定位结果不存在累积误差，且具有定位精度高、抗多径效果好、功耗低、安全性高、系统复杂度低等优点。
目前基于IMU/UWB技术进行多运动平台相对距离估计和定位的常见实现方式如图1所示，具体是由运动平台搭载的UWB传感器接收其它平台发播的窄脉冲信号并计算两个平台之间的相对距离，利用扩展卡尔曼滤波算法融合IMU传感器测量值实现航迹递推与状态更新，最后通过平台之间的通信交互和潜在碰撞检测完成协同定位与导航。这一实现方式在基于扩展卡尔曼滤波的UWB/IMU融合算法未考虑实际应用中的测量有色噪声，且没有对噪声参数进行实时分析和在线估计，导致相对导航定位精度受限。
发明内容
基于此，有必要针对上述技术问题，提供一种能够对噪声参数进行实时分析和在线估计的分布式平台相对定位方法、装置、计算机设备和存储介质。
一种分布式平台相对定位方法，包括：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
其中一个实施例中，在对IMU测量噪声和UWB测量噪声进行Allan方差分析之前，还包括：
分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
其中一个实施例中，对IMU测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵，包括：
计算m组IMU测量噪声中每一组的IMU测量噪声平均值。
根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 504652DEST_PATH_IMAGE001
其中，
Figure 668917DEST_PATH_IMAGE002
，k表示当前时刻，
Figure 192433DEST_PATH_IMAGE003
和
Figure 467557DEST_PATH_IMAGE004
均表示IMU测量噪声平均值，
Figure 642186DEST_PATH_IMAGE005
取正整数，
Figure 774090DEST_PATH_IMAGE006
表示IMU测量噪声方差矩阵。
其中一个实施例中，对UWB测量噪声进行Allan方差分析，得到UWB测量噪声方差矩阵，包括：
计算m组UWB测量噪声中每一组的UWB测量噪声平均值。
根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 34170DEST_PATH_IMAGE007
其中，
Figure 847406DEST_PATH_IMAGE008
，k表示当前时刻，
Figure 892853DEST_PATH_IMAGE009
和
Figure 133342DEST_PATH_IMAGE010
均表示UWB测量噪声平均值，
Figure 146297DEST_PATH_IMAGE011
取正整数，
Figure 763223DEST_PATH_IMAGE012
表示UWB测量噪声方差矩阵。
其中一个实施例中，在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，包括：
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 912445DEST_PATH_IMAGE013
其中，k表示当前时刻，
Figure 386151DEST_PATH_IMAGE014
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 558507DEST_PATH_IMAGE015
表示k时刻的控制输入向量，
Figure 526594DEST_PATH_IMAGE016
表示均值为零、协方差矩阵为
Figure 733584DEST_PATH_IMAGE017
的高斯过程噪声。
其中一个实施例中，获取第一运动平台和第二运动平台的相对距离测量模型，包括：
获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 378192DEST_PATH_IMAGE018
其中，
Figure 37844DEST_PATH_IMAGE019
表示在k时刻的相对距离，
Figure 58889DEST_PATH_IMAGE020
表示第一运动平台在k时刻的状态量，
Figure 120386DEST_PATH_IMAGE021
表示第二运动平台在k时刻的状态量，
Figure 201475DEST_PATH_IMAGE022
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 348422DEST_PATH_IMAGE023
表示测量误差矩阵。
其中一个实施例中，根据状态更新结果，确定第一运动平台的位置之后，还包括：
根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
一种分布式平台相对定位装置，其特征在于，所述装置包括：
测量噪声获取模块，用于在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
方差分析模块，用于分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
运动方程获取模块，用于在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
运动方程更新模块，用于采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
相对距离测量模型获取模块，用于获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
相对距离测量模型更新模块，用于采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
第一运动平台状态更新模块，用于根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
第一运动平台位置确定模块，用于根据状态更新结果，确定第一运动平台的位置。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
上述分布式平台相对定位方法、装置、计算机设备和存储介质，在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声，并获取通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声；分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵；在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，并获取第一运动平台和第二运动平台的相对距离测量模型。在采用卡尔曼滤波算法对运动方程进行更新时将零协方差矩阵替换为IMU测量噪声方差矩阵，对相对距离测量模型进行更新时将测量误差矩阵替换为UWB测量噪声方差矩阵，对两种测量噪声进行在线参数估计和滤除，并根据运动方程和相对距离测量模型的更新结果得到第一运动平台的位置。本申请对IMU测量噪声和UWB测量噪声的影响进行连续的、实时的Allan方差计算，并利用两种测量噪声的方差矩阵和Allan方差的等价关系实现噪声滤除，通过考虑测量噪声的时变间相关特性滤除测量过程中的有色噪声，提高对运动平台的定位结果精度。
附图说明
图1为一种基于IMU/UWB技术实现多运动平台相对距离估计和定位的流程图；
图2为一个实施例中一种分布式平台相对定位方法的步骤图；
图3为另一个实施例中一种分布式平台相对定位方法的流程示意图；
图4为一个实施例中一种分布式平台相对定位方法对传感器的测量误差分析结果；
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图2所示，提供了一种分布式平台相对定位方法，可应用于卫星导航拒止条件下（即无法接收到卫星导航信号的条件下）的分布式运动平台相对定位，各运动平台均搭载了IMU和UWB传感器，其控制、通信、传感和计算任务均采用分布式处理方式，对于任一平台
Figure 658312DEST_PATH_IMAGE024
，包括以下步骤：
步骤202，在运动平台
Figure 574315DEST_PATH_IMAGE024
进入卫星导航拒止环境前，通过运动平台
Figure 826305DEST_PATH_IMAGE024
上的IMU传感器获取IMU测量噪声。以及，通过运动平台
Figure 460549DEST_PATH_IMAGE024
上的UWB传感器测量运动平台
Figure 823397DEST_PATH_IMAGE025
得到的UWB测量噪声。
具体地，在运动平台
Figure 593907DEST_PATH_IMAGE024
进入卫星导航拒止环境前，获取运动平台
Figure 16798DEST_PATH_IMAGE024
上的IMU传感器的IMU测量噪声，并获得该运动平台的UWB传感器测量对运动平台
Figure 138338DEST_PATH_IMAGE025
进行测量的UWB测量噪声。
步骤204，分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
步骤206，在运动平台
Figure 790030DEST_PATH_IMAGE024
（即第一运动平台）进入卫星导航拒止环境时，获取运动平台
Figure 680626DEST_PATH_IMAGE026
的运动方程，运动方程中包括零协方差矩阵。
步骤208，采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
步骤210，获取运动平台
Figure 274418DEST_PATH_IMAGE027
和运动平台
Figure 414413DEST_PATH_IMAGE025
（即第二运动平台）的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
步骤212，采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
具体地，运动平台
Figure 322326DEST_PATH_IMAGE024
的运动状态可以使用运动方程进行描述，方程的变量包括运动平台的位置、速度矢量和偏航角，以及对应的运动平台控制输入量，还包括一个运动状态误差量。运动平台
Figure 129745DEST_PATH_IMAGE024
相对于运动平台
Figure 832121DEST_PATH_IMAGE025
的距离可以使用相对距离测量模型进行描述，方程的变量包括运动平台
Figure 475724DEST_PATH_IMAGE024
和运动平台
Figure 656169DEST_PATH_IMAGE025
的位置，以及一个描述相对距离误差量。在目前的技术（如扩展卡尔曼滤波算法，即EKF算法）中，由搭载在运动平台上的IMU传感器对自身运动状态进行连续测量时，将IMU传感器的测量噪声视为高斯白噪声，因此对运动钻头的测量噪声协方差矩阵也是与时间无关的常量。与之对应，在基于运动平台的运动状态测量运动平台间的相对距离时，对应产生的相对距离测量误差矩阵也与时间无关。
Allan方差是一种时域分析技术，通过方差计算可识别误差类型，包括量化噪声、白噪声、闪烁噪声、随机游走噪声等类型，以及得到与时间具有相关性的随机误差值。步骤204分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵，步骤208和步骤212分别将与时间无关的运动状态的测量噪声协方差矩阵和相对距离的测量误差矩阵，替换为IMU测量噪声方差矩阵和UWB测量噪声方差矩阵，使得在计算运动平台的运动状态和运动平台间的相对距离时，其相应的误差值随时间变化，从而能够消除有色噪声的影响。
步骤214，根据运动方程的更新结果和相对距离测量模型的更新结果，得到运动平台
Figure 583674DEST_PATH_IMAGE024
的状态更新结果。
步骤216，根据状态更新结果，确定运动平台
Figure 456952DEST_PATH_IMAGE024
的位置。
上述分布式平台相对定位方法，在采用卡尔曼滤波算法对运动方程进行更新时将零协方差矩阵替换为IMU测量噪声方差矩阵，对相对距离测量模型进行更新时将测量误差矩阵替换为UWB测量噪声方差矩阵，即对两种测量噪声进行在线参数估计和滤除，并根据运动方程和相对距离测量模型的更新结果得到第一运动平台的位置，并利用两种测量噪声的方差矩阵和Allan方差的等价关系实现噪声滤除，通过考虑测量噪声的时变间相关特性滤除测量过程中的有色噪声，提高对运动平台的定位结果精度。
一个实施例中，如图3所示，提供了一种分布式平台相对定位方法，包括：
步骤302，在平台
Figure 837118DEST_PATH_IMAGE024
进入卫星导航拒止环境前，通过平台
Figure 821254DEST_PATH_IMAGE024
上的IMU传感器获取IMU测量噪声；以及，通过平台
Figure 603265DEST_PATH_IMAGE028
上的UWB传感器测量平台
Figure 647445DEST_PATH_IMAGE029
得到的UWB测量噪声。
具体地，平台
Figure 265639DEST_PATH_IMAGE024
通过射频天线接收其它平台发送的平台状态的估计信息
Figure 53467DEST_PATH_IMAGE030
，k表示当前时刻，
Figure 689984DEST_PATH_IMAGE031
表示平台
Figure 905065DEST_PATH_IMAGE025
对前一时刻自身平台状态的后验估计结果。平台
Figure 994244DEST_PATH_IMAGE024
的UWB传感器根据接收的窄脉冲信号得到与平台
Figure 585762DEST_PATH_IMAGE025
之间的相对距离
Figure 76786DEST_PATH_IMAGE032
。现有技术忽略了实际测量中UWB传感器距离估计值
Figure 462768DEST_PATH_IMAGE033
中的有色噪声，将测量噪声
Figure 789975DEST_PATH_IMAGE034
设为与
Figure 981922DEST_PATH_IMAGE035
无关的协方差矩阵。
步骤304，在平台
Figure 265136DEST_PATH_IMAGE024
进入卫星导航拒止环境时，获取平台
Figure 149916DEST_PATH_IMAGE024
的运动方程为：
Figure 151370DEST_PATH_IMAGE036
其中，k表示当前时刻，
Figure 881428DEST_PATH_IMAGE037
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 284728DEST_PATH_IMAGE038
表示k时刻的控制输入向量，
Figure 91141DEST_PATH_IMAGE039
表示均值为零、协方差矩阵为
Figure 314312DEST_PATH_IMAGE040
的高斯过程噪声。
步骤306，获取平台
Figure 113641DEST_PATH_IMAGE041
和平台
Figure 371447DEST_PATH_IMAGE042
的相对距离测量模型为：
Figure 332449DEST_PATH_IMAGE043
其中，
Figure 308496DEST_PATH_IMAGE044
表示在k时刻的相对距离，
Figure 645936DEST_PATH_IMAGE045
表示平台
Figure 758249DEST_PATH_IMAGE027
在k时刻的状态量，
Figure 640885DEST_PATH_IMAGE046
表示对平台
Figure 900965DEST_PATH_IMAGE025
在k时刻的状态量，
Figure 979779DEST_PATH_IMAGE047
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 274495DEST_PATH_IMAGE048
表示测量误差矩阵。
步骤308，分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。计算m组IMU测量噪声中每一组的IMU测量噪声平均值，根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 514983DEST_PATH_IMAGE049
其中，
Figure 527938DEST_PATH_IMAGE050
，k表示当前时刻，
Figure 144865DEST_PATH_IMAGE051
和
Figure 44819DEST_PATH_IMAGE052
均表示平台
Figure 456208DEST_PATH_IMAGE053
的IMU测量噪声平均值，
Figure 690881DEST_PATH_IMAGE054
取正整数，
Figure 111498DEST_PATH_IMAGE055
表示平台
Figure 115226DEST_PATH_IMAGE027
的IMU测量噪声方差矩阵。
计算m组UWB测量噪声中每一组的UWB测量噪声平均值，根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 963096DEST_PATH_IMAGE056
其中，
Figure 685064DEST_PATH_IMAGE057
，k表示当前时刻，
Figure 643793DEST_PATH_IMAGE058
和
Figure 530058DEST_PATH_IMAGE059
均表示平台
Figure 548830DEST_PATH_IMAGE024
和平台
Figure 492515DEST_PATH_IMAGE025
之间的UWB测量噪声平均值，
Figure 254935DEST_PATH_IMAGE060
取正整数，
Figure 233255DEST_PATH_IMAGE061
表示平台
Figure 422928DEST_PATH_IMAGE024
和平台
Figure 119488DEST_PATH_IMAGE025
之间的UWB测量噪声方差矩阵。
具体地，步骤304至308对有色噪声的影响进行连续的、实时的Allan方差计算，以便基于滤波算法消除有色噪声。平台
Figure 420020DEST_PATH_IMAGE024
进入卫星导航拒止环境的时刻为拒止时刻，此时各个平台已知拒止时刻的位置信息和后续的航迹规划，并建立平台
Figure 3579DEST_PATH_IMAGE062
状态方程
Figure 364153DEST_PATH_IMAGE063
表示平台
Figure 282430DEST_PATH_IMAGE024
导航系统的非线性状态方程，其中状态量
Figure 183390DEST_PATH_IMAGE064
，
Figure 73986DEST_PATH_IMAGE065
是平台
Figure 667778DEST_PATH_IMAGE024
的三维位置，
Figure 11035DEST_PATH_IMAGE066
是平台
Figure 466418DEST_PATH_IMAGE024
相对于正北方向的惯性速度矢量，
Figure 211520DEST_PATH_IMAGE067
为平台
Figure 976214DEST_PATH_IMAGE024
相对于正北方向的偏航角，
Figure 806767DEST_PATH_IMAGE068
；
Figure 315109DEST_PATH_IMAGE069
表示已知的平台
Figure 180296DEST_PATH_IMAGE024
的控制输入向量；
Figure 115891DEST_PATH_IMAGE070
表示均值为零、协方差矩阵为
Figure 168161DEST_PATH_IMAGE071
的高斯过程噪声。在拒止时刻，令
Figure 230926DEST_PATH_IMAGE072
，初始状态向量
Figure 950620DEST_PATH_IMAGE073
为拒止前一时刻的最终状态，其先验估计值初始化为
Figure 57117DEST_PATH_IMAGE074
，
Figure 596682DEST_PATH_IMAGE075
表示数学期望。
设
Figure 446827DEST_PATH_IMAGE076
时刻测量方程为
Figure 21028DEST_PATH_IMAGE077
，
Figure 298425DEST_PATH_IMAGE078
表示观测矩阵，其具体表达取决于IMU传感器测量方式，
Figure 590866DEST_PATH_IMAGE079
为IMU传感器对平台
Figure 464275DEST_PATH_IMAGE024
的三维速度和偏航角的测量值，
Figure 220879DEST_PATH_IMAGE080
表示IMU传感器对平台
Figure 606861DEST_PATH_IMAGE024
的三维速度及偏航角的测量噪声。对于实际的IMU模块，其测量误差是比较复杂的非平稳过程，测量噪声表现为有色噪声而非白噪声。对当前采样时刻
Figure 183336DEST_PATH_IMAGE081
、采样间隔为
Figure 312966DEST_PATH_IMAGE082
的序列分为
Figure 924076DEST_PATH_IMAGE083
组，并计算每组数据的平均值
Figure 480959DEST_PATH_IMAGE084
，
Figure 295462DEST_PATH_IMAGE085
。利用
Figure 228783DEST_PATH_IMAGE086
协方差矩阵和Allan方差的等价性，将IMU测量噪声方差矩阵
Figure 694400DEST_PATH_IMAGE087
表示为：
Figure 422184DEST_PATH_IMAGE088
将表示相对距离值
Figure 973251DEST_PATH_IMAGE089
测量噪声的UWB测量噪声方差矩阵
Figure 710263DEST_PATH_IMAGE090
表示为：
Figure 30386DEST_PATH_IMAGE091
步骤310，采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
具体地，将通过平台
Figure 476542DEST_PATH_IMAGE024
的IMU传感器得到的测量值和平台
Figure 452588DEST_PATH_IMAGE024
自身状态进行第一步扩展卡尔曼滤波。分别将状态方程和测量方程在
Figure 790029DEST_PATH_IMAGE092
处进行一阶Taylor展开，通过线性近似得到扩展卡尔曼滤波算法的预测过程如下所示，其中输入变量为
Figure 902341DEST_PATH_IMAGE093
时刻的后验状态估计值
Figure 299824DEST_PATH_IMAGE094
及其估计误差协方差矩阵
Figure 497588DEST_PATH_IMAGE095
、过程噪声协方差矩阵
Figure 638719DEST_PATH_IMAGE096
，输出变量为
Figure 605538DEST_PATH_IMAGE097
时刻状态预测值
Figure 924655DEST_PATH_IMAGE098
及其先验估计误差协方差矩阵
Figure 609714DEST_PATH_IMAGE099
。
Figure 288957DEST_PATH_IMAGE100
Figure 375862DEST_PATH_IMAGE101
其中，偏微分矩阵
Figure 115148DEST_PATH_IMAGE102
，在拒止时刻
Figure 287503DEST_PATH_IMAGE103
。搭载在平台
Figure 770437DEST_PATH_IMAGE024
上的IMU传感器对自身运动进行连续测量，这一测量过程称为内部测量更新，定义测量变量如下：
Figure 711848DEST_PATH_IMAGE104
利用均方误差最小原则可得k时刻的卡尔曼增益
Figure 107189DEST_PATH_IMAGE105
，可得状态估计的测量更新和估计误差的更新过程如下：
Figure 829157DEST_PATH_IMAGE106
Figure 787886DEST_PATH_IMAGE107
Figure 911699DEST_PATH_IMAGE108
Figure 664892DEST_PATH_IMAGE109
其中
Figure 874156DEST_PATH_IMAGE110
为输出的
Figure 636576DEST_PATH_IMAGE111
时刻后验状态估计值，
Figure 365629DEST_PATH_IMAGE112
表示误差协方差矩阵后验估计。在传统的EKF算法中，将IMU部件测量噪声视为高斯白噪声，测量噪声协方差矩阵
Figure 555302DEST_PATH_IMAGE113
也就是与时间无关的常量，而本实施例通过Allan方差实时分析与时间具有相关性的随机误差，通过不断更新
Figure 986283DEST_PATH_IMAGE114
消除有色噪声的影响。
步骤312，采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
具体地，在多运动平台构成的分布式系统中，各平台之间的状态具有相关性，因此需要进行第二步联合滤波。将平台
Figure 286814DEST_PATH_IMAGE024
根据其对其他平台的测量数据进行的更新称为外部测量更新。首先根据平台
Figure 385220DEST_PATH_IMAGE024
和平台
Figure 745794DEST_PATH_IMAGE025
之间的测量完成外部测量更新：根据步骤306已经得到平台
Figure 664072DEST_PATH_IMAGE024
的UWB传感器获得的与平台
Figure 315764DEST_PATH_IMAGE025
之间的相对距离测量值
Figure 206360DEST_PATH_IMAGE115
，并将相对距离测量模型表示为：
Figure 800152DEST_PATH_IMAGE116
Figure 143409DEST_PATH_IMAGE117
令
Figure 848060DEST_PATH_IMAGE118
其中，测量误差
Figure 593162DEST_PATH_IMAGE119
的互协方差的计算方式与平台
Figure 357855DEST_PATH_IMAGE024
和
Figure 188408DEST_PATH_IMAGE025
之间相对距离测量值的Allan方差，即步骤308中的UWB测量噪声方差矩阵
Figure 447482DEST_PATH_IMAGE120
，具有等价性。假设各个平台都获得了对自身的平台状态的估计值，并且能够获取其他平台的这一估计值。假设
Figure 47091DEST_PATH_IMAGE121
时刻平台
Figure 982686DEST_PATH_IMAGE024
和平台
Figure 300535DEST_PATH_IMAGE025
建立了测距与通信链路，输入变量为k-1时刻平台
Figure 612567DEST_PATH_IMAGE122
对平台
Figure 66683DEST_PATH_IMAGE025
的状态后验估计值
Figure 173179DEST_PATH_IMAGE123
、互协方差矩阵
Figure 978324DEST_PATH_IMAGE124
以及偏微分矩阵
Figure 579201DEST_PATH_IMAGE125
和
Figure 153401DEST_PATH_IMAGE126
，在各个平台为同质节点（节点设备相同）的情况下，
Figure 430799DEST_PATH_IMAGE127
的计算方式与
Figure 723240DEST_PATH_IMAGE128
相同。输出变量为平台
Figure 111496DEST_PATH_IMAGE024
对平台
Figure 540203DEST_PATH_IMAGE025
的状态
Figure 254081DEST_PATH_IMAGE129
及其互协方差
Figure 768239DEST_PATH_IMAGE130
的先验估计值
Figure 710919DEST_PATH_IMAGE131
和
Figure 259712DEST_PATH_IMAGE132
：
Figure 878912DEST_PATH_IMAGE133
Figure 677104DEST_PATH_IMAGE134
其中
Figure 610425DEST_PATH_IMAGE135
表示前一时刻平台
Figure 76041DEST_PATH_IMAGE024
通过外部测量和通信得到的平台
Figure 803825DEST_PATH_IMAGE025
的互协方差后验估计值。对于任意三个运动平台
Figure 105625DEST_PATH_IMAGE136
（N为平台总数），其联合滤波过程中的互协方差后验估计方程如下：
Figure 577058DEST_PATH_IMAGE137
Figure 897180DEST_PATH_IMAGE138
Figure 61446DEST_PATH_IMAGE139
Figure 99809DEST_PATH_IMAGE140
表示当前k时刻外部测量下的平台
Figure 109353DEST_PATH_IMAGE024
互协方差后验估计值，
Figure 549562DEST_PATH_IMAGE141
表示运动平台
Figure 619149DEST_PATH_IMAGE024
对平台
Figure 895541DEST_PATH_IMAGE142
的互协方差后验估计。利用上述互协方差更新过程可实时估计运动平台之间的相关性，提高定位鲁棒性。
进一步地，当
Figure 708776DEST_PATH_IMAGE143
时刻平台
Figure 3491DEST_PATH_IMAGE144
与平台
Figure 509559DEST_PATH_IMAGE145
不同时可测，如出现遮挡导致不可视情况，此时无法按照上述方式估计互协方差矩阵
Figure 256935DEST_PATH_IMAGE146
，但由于之前时刻分布式平台之间测量与状态的相关性，如果不对其进行更新可能导致估计性能的下降。不失一般性，假设此时平台
Figure 873861DEST_PATH_IMAGE147
与平台
Figure 23082DEST_PATH_IMAGE024
可测，而与平台
Figure 700051DEST_PATH_IMAGE148
不可测，此时需要在不依赖于通信链路的情况下对互协方差矩阵做出如下近似：
Figure 408158DEST_PATH_IMAGE149
由此可得：
Figure 625513DEST_PATH_IMAGE150
上式即为平台
Figure 832503DEST_PATH_IMAGE151
与
Figure 742690DEST_PATH_IMAGE148
在
Figure 402342DEST_PATH_IMAGE152
时刻不依赖平台间通信时的互协方差更新算法，在有限测量噪声情况下，
Figure 423388DEST_PATH_IMAGE153
的可逆性可以得到保证。
完成上述外部测量更新后，设k时刻平台
Figure 219305DEST_PATH_IMAGE024
中存储的所有平台状态量为
Figure 51126DEST_PATH_IMAGE154
，综合所有平台结果可得状态的后验估计更新过程如下：
Figure 198074DEST_PATH_IMAGE155
Figure 22810DEST_PATH_IMAGE156
其中，
Figure 938814DEST_PATH_IMAGE157
为k时刻的卡尔曼滤波互增益，其计算方式如下式：
Figure 925224DEST_PATH_IMAGE158
步骤314，根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。根据状态更新结果，确定第一运动平台的位置。
步骤316，根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
具体地，基于当前时刻的各个平台的平台状态更新结果
Figure 559468DEST_PATH_IMAGE159
进行碰撞检测，如果运动平台之间存在潜在碰撞可能，如平台间的碰撞概率高于预设的门限值时，则对其中一个或多个平台的航迹进行调整，以避免碰撞；否则不需要调整航迹直接进行下一时刻的运动状态估计，
Figure 922316DEST_PATH_IMAGE160
。
为了说明本申请提供的一种分布式平台相对定位方法的有效性，基于IMU传感器和UWB传感器的实际测量数据，采用上述一个实施例提供的一种分布式平台定位方法得到了对应的测量误差Allan方差分析结果（如图4所示）。图4中的实线为对UWB传感器或IMU传感器实测数据的Allan方差分析结果，虚线分别代表对测试数据经噪声成分分析后对应的白噪声（斜率为-1）、闪烁噪声（斜率约为0）和随机游走噪声（斜率约为+1/2）的Allan方差。
图4中，UWB模块测试-相对距离1、UWB模块测试-相对距离2、UWB模块测试-相对距离3为利用UWB进行多次相对距离测试所得数据的Allan方差分析结果，可以看到UWB传感器在进行相对距离测量时，其数据除白噪声外，还包含具有时间相关性的闪烁噪声和明显的随机游动噪声，而传统定位算法忽略了噪声参数与时间的相关性，未对其中的
Figure 958405DEST_PATH_IMAGE161
进行实时计算和更新。
图4中，IMU模块测试-x方向角速率、IMU模块测试-y方向角速率、IMU模块测试-z方向角速率分别为IMU传感器测量得到的三轴角速率数据的Allan方差分析结果。可以看到，x方向和z方向均含有有色噪声成分。同理，IMU模块测试-x方向加速度、IMU模块测试-y方向加速度、IMU模块测试-z方向加速度的测量数据也包含有色噪声。传统定位算法同样也忽略了这一有色噪声的影响，未对
Figure 866449DEST_PATH_IMAGE162
进行实时计算和更新。
值得说明的是，在运动平台进入卫星导航拒止区域前已获取足够UWB和IMU传感器样本数据的条件下，可将滤波算法中的噪声协方差值初始化为Allan方差分析结果，并在后续运动过程中通过递归计算更新其噪声参数，以自适应地调整滤波算法。
根据实验结果分析现有EKF算法和本申请提供的方法的定位误差。EKF算法在有色噪声的影响下产生了较大的误差，而本申请算法充分考虑并滤除了测量有色噪声，算法性能在分布式平台运动过程中更加稳定，从而对运动平台的位置
Figure 987989DEST_PATH_IMAGE163
实现了更高精度的估计。在400s-600s受到有色噪声影响最为显著的区间内，本申请提供的方法滤除有色噪声后定位误差明显降低，而从误差的均值和标准差结果可以看出，现有EKF算法的位置估计误差约为0.63m，而本申请提供的方法估计误差在0.55m左右，精度提高了12.94%。同样，如速度估计误差和角度估计误差所示，IMU传感器对加速度和角速率的估计结果在滤除有色噪声影响后，本申请提供的方法对速度
Figure 154528DEST_PATH_IMAGE164
和角度
Figure 576282DEST_PATH_IMAGE165
的估计精度分别提高了34.21%和11.95%。
应该理解的是，虽然图2-3的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2-3中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，提供了一种分布式平台相对定位装置，包括：
测量噪声获取模块，用于在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
方差分析模块，用于分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
运动方程获取模块，用于在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
运动方程更新模块，用于采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
相对距离测量模型获取模块，用于获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
相对距离测量模型更新模块，用于采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
第一运动平台状态更新模块，用于根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
第一运动平台位置确定模块，用于根据状态更新结果，确定第一运动平台的位置。
其中一个实施例中，还包括采样模块，用于分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
其中一个实施例中，方差分析模块，用于计算m组IMU测量噪声中每一组的IMU测量噪声平均值。根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 373337DEST_PATH_IMAGE166
其中，
Figure 513331DEST_PATH_IMAGE167
，k表示当前时刻，
Figure 421245DEST_PATH_IMAGE168
和
Figure 979396DEST_PATH_IMAGE169
均表示IMU测量噪声平均值，
Figure 681773DEST_PATH_IMAGE170
取正整数，
Figure 574642DEST_PATH_IMAGE171
表示IMU测量噪声方差矩阵。
其中一个实施例中，方差分析模块用于计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 20667DEST_PATH_IMAGE172
其中，
Figure 682593DEST_PATH_IMAGE173
，k表示当前时刻，
Figure 555871DEST_PATH_IMAGE174
和
Figure 936037DEST_PATH_IMAGE175
均表示UWB测量噪声平均值，
Figure 920173DEST_PATH_IMAGE176
取正整数，
Figure 452917DEST_PATH_IMAGE177
表示UWB测量噪声方差矩阵。
其中一个实施例中，运动方程获取模块用于在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 497096DEST_PATH_IMAGE178
其中，k表示当前时刻，
Figure 364558DEST_PATH_IMAGE179
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 152385DEST_PATH_IMAGE180
表示k时刻的控制输入向量，
Figure 788903DEST_PATH_IMAGE181
表示均值为零、协方差矩阵为
Figure 3984DEST_PATH_IMAGE182
的高斯过程噪声。
其中一个实施例中，相对距离测量模型获取模块用于获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 93163DEST_PATH_IMAGE183
其中，
Figure 684681DEST_PATH_IMAGE184
表示在k时刻的相对距离，
Figure 926438DEST_PATH_IMAGE185
表示第一运动平台在k时刻的状态量，
Figure 312420DEST_PATH_IMAGE186
表示第二运动平台在k时刻的状态量，
Figure 154474DEST_PATH_IMAGE187
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 284104DEST_PATH_IMAGE188
表示测量误差矩阵。
其中一个实施例中还包括航迹调整模块，用于根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
关于一种分布式平台相对定位装置的具体限定可以参见上文中对于一种分布式平台相对定位方法的限定，在此不再赘述。上述一种分布式平台相对定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种一种分布式平台相对定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：计算m组IMU测量噪声中每一组的IMU测量噪声平均值。根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 629634DEST_PATH_IMAGE189
其中，
Figure 452097DEST_PATH_IMAGE190
，k表示当前时刻，
Figure 515868DEST_PATH_IMAGE191
和
Figure 449189DEST_PATH_IMAGE192
均表示IMU测量噪声平均值，
Figure 399958DEST_PATH_IMAGE193
取正整数，
Figure 393322DEST_PATH_IMAGE194
表示IMU测量噪声方差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 944389DEST_PATH_IMAGE195
其中，
Figure 212559DEST_PATH_IMAGE196
，k表示当前时刻，
Figure 470365DEST_PATH_IMAGE197
和
Figure 696947DEST_PATH_IMAGE198
均表示UWB测量噪声平均值，
Figure 407414DEST_PATH_IMAGE199
取正整数，
Figure 495587DEST_PATH_IMAGE200
表示UWB测量噪声方差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 873479DEST_PATH_IMAGE201
其中，k表示当前时刻，
Figure 5383DEST_PATH_IMAGE202
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 468726DEST_PATH_IMAGE203
表示k时刻的控制输入向量，
Figure 344278DEST_PATH_IMAGE204
表示均值为零、协方差矩阵为
Figure 576676DEST_PATH_IMAGE205
的高斯过程噪声。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 879481DEST_PATH_IMAGE206
其中，
Figure 830120DEST_PATH_IMAGE207
表示在k时刻的相对距离，
Figure 260095DEST_PATH_IMAGE208
表示第一运动平台在k时刻的状态量，
Figure 143737DEST_PATH_IMAGE209
表示第二运动平台在k时刻的状态量，
Figure 86286DEST_PATH_IMAGE210
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 55379DEST_PATH_IMAGE211
表示测量误差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：计算m组IMU测量噪声中每一组的IMU测量噪声平均值。根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 210417DEST_PATH_IMAGE212
其中，
Figure 479724DEST_PATH_IMAGE213
，k表示当前时刻，
Figure 62015DEST_PATH_IMAGE214
和
Figure 534716DEST_PATH_IMAGE215
均表示IMU测量噪声平均值，
Figure 493444DEST_PATH_IMAGE216
取正整数，
Figure 617258DEST_PATH_IMAGE217
表示IMU测量噪声方差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 636030DEST_PATH_IMAGE218
其中，
Figure 845294DEST_PATH_IMAGE219
，k表示当前时刻，
Figure 342135DEST_PATH_IMAGE220
和
Figure 320455DEST_PATH_IMAGE221
均表示UWB测量噪声平均值，
Figure 510128DEST_PATH_IMAGE222
取正整数，
Figure 957421DEST_PATH_IMAGE223
表示UWB测量噪声方差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 257952DEST_PATH_IMAGE224
其中，k表示当前时刻，
Figure 90779DEST_PATH_IMAGE225
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 451353DEST_PATH_IMAGE226
表示k时刻的控制输入向量，
Figure 635210DEST_PATH_IMAGE227
表示均值为零、协方差矩阵为
Figure 473853DEST_PATH_IMAGE228
的高斯过程噪声。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 426765DEST_PATH_IMAGE229
其中，
Figure 958241DEST_PATH_IMAGE230
表示在k时刻的相对距离，
Figure 114547DEST_PATH_IMAGE231
表示第一运动平台在k时刻的状态量，
Figure 22460DEST_PATH_IMAGE232
表示第二运动平台在k时刻的状态量，
Figure 829879DEST_PATH_IMAGE233
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 328993DEST_PATH_IMAGE234
表示测量误差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种分布式平台相对定位方法，其特征在于，所述方法包括：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声；
分别对所述IMU测量噪声和所述UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵；
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程；所述运动方程中包括：零协方差矩阵；
采用卡尔曼滤波算法对所述运动方程进行更新时，将所述零协方差矩阵替换为所述IMU测量噪声方差矩阵；
获取第一运动平台和第二运动平台的相对距离测量模型；所述相对距离测量模型中包括：测量误差矩阵；
采用卡尔曼滤波算法对所述相对距离测量模型进行更新时，将所述测量误差矩阵替换为所述UWB测量噪声方差矩阵；
根据所述运动方程的更新结果和所述相对距离测量模型的更新结果，得到第一运动平台的状态更新结果；
根据所述状态更新结果，确定第一运动平台的位置。
2.根据权利要求1所述的方法，其特征在于，在对所述IMU测量噪声和所述UWB测量噪声进行Allan方差分析之前，所述方法还包括：
分别将所述IMU测量噪声和所述UWB测量噪声以预设采样间隔划分为m组。
3.根据权利要求2所述的方法，其特征在于，对所述IMU测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵，包括：
计算m组IMU测量噪声中每一组的IMU测量噪声平均值；
根据所述IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 404053DEST_PATH_IMAGE001
其中，
Figure 484004DEST_PATH_IMAGE002
，k表示当前时刻，
Figure 837625DEST_PATH_IMAGE003
和
Figure 370238DEST_PATH_IMAGE004
均表示IMU测量噪声平均值，
Figure 604691DEST_PATH_IMAGE005
取正整数，
Figure 273570DEST_PATH_IMAGE006
表示IMU测量噪声方差矩阵。
4.根据权利要求2所述的方法，其特征在于，对所述UWB测量噪声进行Allan方差分析，得到UWB测量噪声方差矩阵，包括：
计算m组UWB测量噪声中每一组的UWB测量噪声平均值；
根据所述UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 798092DEST_PATH_IMAGE007
其中，
Figure 818001DEST_PATH_IMAGE008
，k表示当前时刻，
Figure 820592DEST_PATH_IMAGE009
和
Figure 609556DEST_PATH_IMAGE010
均表示UWB测量噪声平均值，
Figure 304980DEST_PATH_IMAGE011
取正整数，
Figure 546605DEST_PATH_IMAGE012
表示UWB测量噪声方差矩阵。
5.根据权利要求1所述的方法，其特征在于，在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，包括：
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 352887DEST_PATH_IMAGE013
其中，
Figure 261937DEST_PATH_IMAGE014
表示k时刻的状态量，所述状态量包括：位置量、惯性速度矢量以及偏航角；
Figure 128262DEST_PATH_IMAGE015
表示k时刻的控制输入向量，
Figure 122763DEST_PATH_IMAGE016
表示均值为零、协方差矩阵为
Figure 467157DEST_PATH_IMAGE017
的高斯过程噪声。
6.根据权利要求1所述的方法，其特征在于，获取第一运动平台和第二运动平台的相对距离测量模型，包括：
获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 230713DEST_PATH_IMAGE018
其中，
Figure 503825DEST_PATH_IMAGE019
表示在k时刻的相对距离，
Figure 985622DEST_PATH_IMAGE020
表示第一运动平台在k时刻的状态量，
Figure 133707DEST_PATH_IMAGE021
表示第二运动平台在k时刻的状态量，
Figure 751770DEST_PATH_IMAGE022
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 959897DEST_PATH_IMAGE023
表示测量误差矩阵。
7.根据权利要求1至6中任意一项所述的方法，其特征在于，根据所述状态更新结果，确定第一运动平台的位置之后，所述方法还包括：
根据所述状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当所述碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
8.一种分布式平台相对定位装置，其特征在于，所述装置包括：
测量噪声获取模块，用于在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声；
方差分析模块，用于分别对所述IMU测量噪声和所述UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵；
运动方程获取模块，用于在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程；所述运动方程中包括：零协方差矩阵；
运动方程更新模块，用于采用卡尔曼滤波算法对所述运动方程进行更新时，将所述零协方差矩阵替换为所述IMU测量噪声方差矩阵；
相对距离测量模型获取模块，用于获取第一运动平台和第二运动平台的相对距离测量模型；所述相对距离测量模型中包括：测量误差矩阵；
相对距离测量模型更新模块，用于采用卡尔曼滤波算法对所述相对距离测量模型进行更新时，将所述测量误差矩阵替换为所述UWB测量噪声方差矩阵；
第一运动平台状态更新模块，用于根据所述运动方程的更新结果和所述相对距离测量模型的更新结果，得到第一运动平台的状态更新结果；
第一运动平台位置确定模块，用于根据所述状态更新结果，确定第一运动平台的位置。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202110000761.7A 2021-01-04 2021-01-04 分布式平台相对定位方法、装置、计算机设备和存储介质 Active CN112325880B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110000761.7A CN112325880B (zh) 2021-01-04 2021-01-04 分布式平台相对定位方法、装置、计算机设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110000761.7A CN112325880B (zh) 2021-01-04 2021-01-04 分布式平台相对定位方法、装置、计算机设备和存储介质

Publications (2)

Publication Number Publication Date
CN112325880A CN112325880A (zh) 2021-02-05
CN112325880B true CN112325880B (zh) 2021-03-26

Family

ID=74302014

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110000761.7A Active CN112325880B (zh) 2021-01-04 2021-01-04 分布式平台相对定位方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN112325880B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113218388B (zh) * 2021-03-02 2022-07-05 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113110589B (zh) * 2021-04-29 2023-05-16 中国人民解放军国防科技大学 面向无人机集群紧密编队构型的分布式相对定位方法及系统
CN115493535A (zh) * 2022-10-12 2022-12-20 云南电网有限责任公司红河供电局 一种基于uwb定位的弧垂在线监测方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法
CN106871893A (zh) * 2017-03-03 2017-06-20 济南大学 分布式ins/uwb紧组合导航系统及方法
CN107300385A (zh) * 2017-08-25 2017-10-27 上海瀚界科技发展有限公司 基于uwb测距的惯导定位系统
CN109916401A (zh) * 2019-04-17 2019-06-21 济南大学 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统
CN110068335A (zh) * 2019-04-23 2019-07-30 中国人民解放军国防科技大学 一种gps拒止环境下无人机集群实时定位方法及系统
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
CN110793519A (zh) * 2019-11-26 2020-02-14 河南工业大学 一种不完全测量的协同导航定位方法
CN110823215A (zh) * 2019-10-25 2020-02-21 南京航空航天大学 一种无人机相对导航信息融合方法
CN112113572A (zh) * 2020-09-18 2020-12-22 桂林电子科技大学 一种解决分布式标签融合的多目标跟踪方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10382894B2 (en) * 2017-07-28 2019-08-13 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法
CN106871893A (zh) * 2017-03-03 2017-06-20 济南大学 分布式ins/uwb紧组合导航系统及方法
CN107300385A (zh) * 2017-08-25 2017-10-27 上海瀚界科技发展有限公司 基于uwb测距的惯导定位系统
CN109916401A (zh) * 2019-04-17 2019-06-21 济南大学 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统
CN110068335A (zh) * 2019-04-23 2019-07-30 中国人民解放军国防科技大学 一种gps拒止环境下无人机集群实时定位方法及系统
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
CN110823215A (zh) * 2019-10-25 2020-02-21 南京航空航天大学 一种无人机相对导航信息融合方法
CN110793519A (zh) * 2019-11-26 2020-02-14 河南工业大学 一种不完全测量的协同导航定位方法
CN112113572A (zh) * 2020-09-18 2020-12-22 桂林电子科技大学 一种解决分布式标签融合的多目标跟踪方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Cooperative relative positioning of mobile users by fusing IMU inertial and UWB ranging information;Ran Liu,etc;《2017 IEEE International Conference on Robotics and Automation (ICRA)》;20171231;第5623-5629页 *
分布式相对定位技术的研究;江涛;《中国博士学位论文全文数据库工程科技Ⅱ辑》;20101015;C031-6 *

Also Published As

Publication number Publication date
CN112325880A (zh) 2021-02-05

Similar Documents

Publication Publication Date Title
CN112325880B (zh) 分布式平台相对定位方法、装置、计算机设备和存储介质
Shen et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles
Indelman et al. Information fusion in navigation systems via factor graph based incremental smoothing
Chen et al. A hybrid prediction method for bridging GPS outages in high-precision POS application
Zaidner et al. A novel data fusion algorithm for low-cost localisation and navigation of autonomous vineyard sprayer robots
CN110146909A (zh) 一种定位数据处理方法
CN111263900A (zh) 用于组合传感器数据的方法和系统
Huang et al. Design of a fault detection and isolation system for intelligent vehicle navigation system
Xia et al. Autonomous vehicles sideslip angle estimation: Single antenna GNSS/IMU fusion with observability analysis
CN111707294B (zh) 基于最优区间估计的行人导航零速区间检测方法和装置
Herrera et al. Improving data fusion in personal positioning systems for outdoor environments
Saadeddin et al. Optimization of intelligent approach for low-cost INS/GPS navigation system
CN113091738A (zh) 基于视觉惯导融合的移动机器人地图构建方法及相关设备
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
Lu et al. Adaptive hybrid unscented Kalman filter for aircraft sensor fault detection, isolation and reconstruction
Srinivas et al. Overview of architecture for GPS-INS integration
Dai et al. Robust adaptive UKF based on SVR for inertial based integrated navigation
Shi et al. Fault-tolerant SINS/HSB/DVL underwater integrated navigation system based on variational Bayesian robust adaptive Kalman filter and adaptive information sharing factor
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与系统
El Sabbagh et al. Promoting navigation system efficiency during GPS outage via cascaded neural networks: A novel AI based approach
Saadeddin et al. Optimization of intelligent-based approach for low-cost INS/GPS navigation system
Al Bitar et al. Neural networks aided unscented Kalman filter for integrated INS/GNSS systems
Almagbile et al. An integrated adaptive Kalman filter for improving the reliability of navigation systems
Buhaiov et al. Artificial neural networks application to MMG temperature calibration
Masrafee et al. Design of inertial measurement unit in attitude and heading reference system for real-time maneuver monitoring by using Kalman filter

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