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

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

Info

Publication number
CN112325880A
CN112325880A CN202110000761.7A CN202110000761A CN112325880A CN 112325880 A CN112325880 A CN 112325880A CN 202110000761 A CN202110000761 A CN 202110000761A CN 112325880 A CN112325880 A CN 112325880A
Authority
CN
China
Prior art keywords
measurement noise
motion platform
motion
uwb
measurement
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.)
Granted
Application number
CN202110000761.7A
Other languages
English (en)
Other versions
CN112325880B (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

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 170324DEST_PATH_IMAGE001
其中，
Figure 257229DEST_PATH_IMAGE002
，k表示当前时刻，
Figure 793252DEST_PATH_IMAGE003
和
Figure 700028DEST_PATH_IMAGE004
均表示IMU测量噪声平均值，
Figure 714121DEST_PATH_IMAGE005
取正整数，
Figure 921111DEST_PATH_IMAGE006
表示IMU测量噪声方差矩阵。
其中一个实施例中，对UWB测量噪声进行Allan方差分析，得到UWB测量噪声方差矩阵，包括：
计算m组UWB测量噪声中每一组的UWB测量噪声平均值。
根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 628036DEST_PATH_IMAGE007
其中，
Figure 287687DEST_PATH_IMAGE008
，k表示当前时刻，
Figure 980837DEST_PATH_IMAGE009
和
Figure 901388DEST_PATH_IMAGE010
均表示UWB测量噪声平均值，
Figure 920160DEST_PATH_IMAGE011
取正整数，
Figure 801528DEST_PATH_IMAGE012
表示UWB测量噪声方差矩阵。
其中一个实施例中，在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，包括：
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 420073DEST_PATH_IMAGE013
其中，k表示当前时刻，
Figure 70497DEST_PATH_IMAGE014
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 853645DEST_PATH_IMAGE015
表示k时刻的控制输入向量，
Figure 487889DEST_PATH_IMAGE016
表示均值为零、协方差矩阵为
Figure 647475DEST_PATH_IMAGE017
的高斯过程噪声。
其中一个实施例中，获取第一运动平台和第二运动平台的相对距离测量模型，包括：
获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 683564DEST_PATH_IMAGE018
其中，
Figure 512980DEST_PATH_IMAGE019
表示在k时刻的相对距离，
Figure 759153DEST_PATH_IMAGE020
表示第一运动平台在k时刻的状
态量，
Figure 863375DEST_PATH_IMAGE021
表示对第二运动平台在k时刻的状态量，
Figure 222813DEST_PATH_IMAGE022
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 878922DEST_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 222179DEST_PATH_IMAGE024
，包括以下步骤：
步骤202，在运动平台
Figure 864512DEST_PATH_IMAGE024
进入卫星导航拒止环境前，通过运动平台
Figure 734248DEST_PATH_IMAGE024
上的IMU传感器获取IMU测量噪声。以及，通过运动平台
Figure 171046DEST_PATH_IMAGE024
上的UWB传感器测量运动平台
Figure 1599DEST_PATH_IMAGE025
得到的UWB测量噪声。
具体地，在运动平台
Figure 572257DEST_PATH_IMAGE024
进入卫星导航拒止环境前，获取运动平台
Figure 906287DEST_PATH_IMAGE024
上的IMU传感器的IMU测量噪声，并获得该运动平台的UWB传感器测量对运动平台
Figure 779565DEST_PATH_IMAGE025
进行测量的UWB测量噪声。
步骤204，分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
步骤206，在运动平台
Figure 222047DEST_PATH_IMAGE024
（即第一运动平台）进入卫星导航拒止环境时，获取运动平台
Figure 206184DEST_PATH_IMAGE024
的运动方程，运动方程中包括零协方差矩阵。
步骤208，采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
步骤210，获取运动平台
Figure 660299DEST_PATH_IMAGE024
和运动平台
Figure 832042DEST_PATH_IMAGE025
（即第二运动平台）的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
步骤212，采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
具体地，运动平台
Figure 371608DEST_PATH_IMAGE024
的运动状态可以使用运动方程进行描述，方程的变量包括运动平台的位置、速度矢量和偏航角，以及对应的运动平台控制输入量，还包括一个运动状态误差量。运动平台
Figure 159435DEST_PATH_IMAGE024
相对于运动平台
Figure 858270DEST_PATH_IMAGE025
的距离可以使用相对距离测量模型进行描述，方程的变量包括运动平台
Figure 73350DEST_PATH_IMAGE024
和运动平台
Figure 834633DEST_PATH_IMAGE025
的位置，以及一个描述相对距离误差量。在目前的技术（如扩展卡尔曼滤波算法，即EKF算法）中，由搭载在运动平台上的IMU传感器对自身运动状态进行连续测量时，将IMU传感器的测量噪声视为高斯白噪声，因此对运动钻头的测量噪声协方差矩阵也是与时间无关的常量。与之对应，在基于运动平台的运动状态测量运动平台间的相对距离时，对应产生的相对距离测量误差矩阵也与时间无关。
Allan方差是一种时域分析技术，通过方差计算可识别误差类型，包括量化噪声、白噪声、闪烁噪声、随机游走噪声等类型，以及得到与时间具有相关性的随机误差值。步骤204分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵，步骤208和步骤212分别将与时间无关的运动状态的测量噪声协方差矩阵和相对距离的测量误差矩阵，替换为IMU测量噪声方差矩阵和UWB测量噪声方差矩阵，使得在计算运动平台的运动状态和运动平台间的相对距离时，其相应的误差值随时间变化，从而能够消除色噪声的影响。
步骤214，根据运动方程的更新结果和相对距离测量模型的更新结果，得到运动平台
Figure 550785DEST_PATH_IMAGE024
的状态更新结果。
步骤216，根据状态更新结果，确定运动平台
Figure 979492DEST_PATH_IMAGE024
的位置。
上述分布式平台相对定位方法，在采用卡尔曼滤波算法对运动方程进行更新时将零协方差矩阵替换为IMU测量噪声方差矩阵，对相对距离测量模型进行更新时将测量误差矩阵替换为UWB测量噪声方差矩阵，即对两种测量噪声进行在线参数估计和滤除，并根据运动方程和相对距离测量模型的更新结果得到第一运动平台的位置，并利用两种测量噪声的方差矩阵和Allan方差的等价关系实现噪声滤除，通过考虑测量噪声的时间相关特性滤除测量过程中的色噪声，提高对运动平台的定位结果精度。
一个实施例中，如图3所示，提供了一种分布式平台相对定位方法，包括：
步骤302，在平台
Figure 365474DEST_PATH_IMAGE024
进入卫星导航拒止环境前，通过平台
Figure 4266DEST_PATH_IMAGE024
上的IMU传感器获取IMU测量噪声；以及，通过平台
Figure 133896DEST_PATH_IMAGE024
上的UWB传感器测量平台
Figure 417110DEST_PATH_IMAGE025
得到的UWB测量噪声。
具体地，平台
Figure 98627DEST_PATH_IMAGE024
通过射频天线接收其它平台发送的平台状态的估计信息
Figure 100081DEST_PATH_IMAGE026
，k表示当前时刻，
Figure 767823DEST_PATH_IMAGE027
表示平台
Figure 30177DEST_PATH_IMAGE025
对前一时刻自身平台状态的后验估计结果。平台
Figure 23541DEST_PATH_IMAGE024
的UWB传感器根据接收的窄脉冲信号得到与平台
Figure 512291DEST_PATH_IMAGE025
之间的相对距离
Figure 842778DEST_PATH_IMAGE028
。现有技术忽略了实际测量中UWB传感器距离估计值
Figure 100584DEST_PATH_IMAGE028
中的色噪声，将测量噪声
Figure 999270DEST_PATH_IMAGE029
设为与
Figure 834371DEST_PATH_IMAGE030
无关的协方差矩阵。
步骤304，在平台
Figure 109494DEST_PATH_IMAGE024
进入卫星导航拒止环境时，获取平台
Figure 221807DEST_PATH_IMAGE024
的运动方程为：
Figure 413098DEST_PATH_IMAGE031
其中，k表示当前时刻，
Figure 610861DEST_PATH_IMAGE032
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 283151DEST_PATH_IMAGE033
表示k时刻的控制输入向量，
Figure 515549DEST_PATH_IMAGE034
表示均值为零、协方差矩阵为
Figure 756038DEST_PATH_IMAGE035
的高斯过程噪声。
步骤306，获取平台
Figure 565731DEST_PATH_IMAGE036
和平台
Figure 182657DEST_PATH_IMAGE037
的相对距离测量模型为：
Figure 3982DEST_PATH_IMAGE038
其中，
Figure 805585DEST_PATH_IMAGE039
表示在k时刻的相对距离，
Figure 712361DEST_PATH_IMAGE040
表示平台
Figure 867399DEST_PATH_IMAGE041
在k时刻的状态量，
Figure 933444DEST_PATH_IMAGE042
表示对平台
Figure 515735DEST_PATH_IMAGE043
在k时刻的状态量，
Figure 34441DEST_PATH_IMAGE044
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 727591DEST_PATH_IMAGE045
表示测量误差矩阵。
步骤308，分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。计算m组IMU测量噪声中每一组的IMU测量噪声平均值，根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 789087DEST_PATH_IMAGE046
其中，
Figure 932493DEST_PATH_IMAGE047
，k表示当前时刻，
Figure 79440DEST_PATH_IMAGE048
和
Figure 310702DEST_PATH_IMAGE049
均表示平台
Figure 351339DEST_PATH_IMAGE041
的IMU测量噪声平均值，
Figure 541012DEST_PATH_IMAGE050
取正整数，
Figure 909676DEST_PATH_IMAGE051
表示平台
Figure 337771DEST_PATH_IMAGE041
的IMU测量噪声方差矩阵。
计算m组UWB测量噪声中每一组的UWB测量噪声平均值，根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 842702DEST_PATH_IMAGE052
其中，
Figure 203276DEST_PATH_IMAGE053
，k表示当前时刻，
Figure 449449DEST_PATH_IMAGE054
和
Figure 288092DEST_PATH_IMAGE055
均表示平台
Figure 913109DEST_PATH_IMAGE041
和平台
Figure 569218DEST_PATH_IMAGE043
之间的UWB测量噪声平均值，
Figure 912475DEST_PATH_IMAGE056
取正整数，
Figure 820388DEST_PATH_IMAGE057
表示平台
Figure 424545DEST_PATH_IMAGE041
和平台
Figure 861342DEST_PATH_IMAGE043
之间的UWB测量噪声方差矩阵。
具体地，步骤304至308对色噪声的影响进行连续的、实时的Allan方差计算，以便基于滤波算法消除色噪声。平台
Figure 691895DEST_PATH_IMAGE041
进入卫星导航拒止环境的时刻为拒止时刻，此时各个平台已知拒止时刻的位置信息和后续的航迹规划，并建立平台
Figure 996974DEST_PATH_IMAGE041
状态方程
Figure 862162DEST_PATH_IMAGE058
表示平台
Figure 469861DEST_PATH_IMAGE041
导航系统的非线性状态方程，其中状态量
Figure 912344DEST_PATH_IMAGE059
，
Figure 896480DEST_PATH_IMAGE060
是平台
Figure 350595DEST_PATH_IMAGE061
的三维位置，
Figure 519408DEST_PATH_IMAGE062
是平台
Figure 324553DEST_PATH_IMAGE041
相对于正北方向的惯性速度矢量，
Figure 846802DEST_PATH_IMAGE063
为平台
Figure 545636DEST_PATH_IMAGE061
相对于正北方向的偏航角，
Figure 760717DEST_PATH_IMAGE064
；
Figure 787579DEST_PATH_IMAGE065
表示已知的平台
Figure 235222DEST_PATH_IMAGE061
的控制输入向量；
Figure 663929DEST_PATH_IMAGE066
表示均值为零、协方差矩阵为
Figure 49911DEST_PATH_IMAGE067
的高斯过程噪声。在拒止时刻，令
Figure 688703DEST_PATH_IMAGE068
，初始状态向量
Figure 818333DEST_PATH_IMAGE069
为拒止前一时刻的最终状态，其先验估计值初始化为
Figure 101547DEST_PATH_IMAGE070
，
Figure 924009DEST_PATH_IMAGE071
表示数学期望。
设
Figure 784518DEST_PATH_IMAGE072
时刻测量方程为
Figure 452260DEST_PATH_IMAGE073
，
Figure 714614DEST_PATH_IMAGE074
表示观测矩阵，其具体表达取决于IMU传感器测量方式，
Figure 707977DEST_PATH_IMAGE075
为IMU传感器对平台
Figure 931148DEST_PATH_IMAGE076
的三维速度和偏航角的测量值，
Figure 668160DEST_PATH_IMAGE077
表示IMU传感器对平台
Figure 785021DEST_PATH_IMAGE076
的三维速度及偏航角的测量噪声。对于实际的IMU模块，其测量误差是比较复杂的非平稳过程，测量噪声表现为有色噪声而非白噪声。对当前采样时刻
Figure 683707DEST_PATH_IMAGE078
、采样间隔为
Figure 659753DEST_PATH_IMAGE079
的序列分为
Figure 59510DEST_PATH_IMAGE080
组，并计算每组数据的平均值
Figure 906244DEST_PATH_IMAGE081
，
Figure 241410DEST_PATH_IMAGE082
。利用
Figure 298228DEST_PATH_IMAGE083
协方差矩阵和Allan方差的等价性，将IMU测量噪声方差矩阵
Figure 377042DEST_PATH_IMAGE084
表示为：
Figure 609440DEST_PATH_IMAGE085
将表示相对距离值
Figure 974563DEST_PATH_IMAGE086
测量噪声的UWB测量噪声方差矩阵
Figure 659622DEST_PATH_IMAGE087
表示为：
Figure 138532DEST_PATH_IMAGE088
步骤310，采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
具体地，将通过平台
Figure 959858DEST_PATH_IMAGE089
的IMU传感器得到的测量值和平台
Figure 371247DEST_PATH_IMAGE089
自身状态进行第一步扩展卡尔曼滤波。分别将状态方程和测量方程在
Figure 668237DEST_PATH_IMAGE090
处进行一阶Taylor展开，通过线性近似得到扩展卡尔曼滤波算法的预测过程如下式(3)和(4)，其中输入变量为
Figure 88854DEST_PATH_IMAGE091
时刻的后验状态估计值
Figure 30265DEST_PATH_IMAGE092
及其估计误差协方差矩阵
Figure 737190DEST_PATH_IMAGE093
、过程噪声协方差矩阵
Figure 396841DEST_PATH_IMAGE094
，输出变量为
Figure 355570DEST_PATH_IMAGE095
时刻状态预测值
Figure 276121DEST_PATH_IMAGE096
及其先验估计误差协方差矩阵
Figure 294893DEST_PATH_IMAGE097
。
Figure 176261DEST_PATH_IMAGE098
Figure 797735DEST_PATH_IMAGE099
其中，偏微分矩阵
Figure 713739DEST_PATH_IMAGE100
，在拒止时刻
Figure 903412DEST_PATH_IMAGE101
。搭载在平台
Figure 396710DEST_PATH_IMAGE102
上的IMU传感器对自身运动进行连续测量，这一测量过程称为内部测量更新，定义测量变量如下：
Figure 431662DEST_PATH_IMAGE103
利用均方误差最小原则可得k时刻的卡尔曼增益
Figure 202172DEST_PATH_IMAGE104
，可得状态估计的测量更新和估计误差的更新过程如下：
Figure 687380DEST_PATH_IMAGE105
Figure 277761DEST_PATH_IMAGE106
Figure 381984DEST_PATH_IMAGE107
Figure 397213DEST_PATH_IMAGE108
其中
Figure 663109DEST_PATH_IMAGE109
为输出的
Figure 6366DEST_PATH_IMAGE110
时刻后验状态估计值，
Figure 528262DEST_PATH_IMAGE111
表示误差协方差矩阵后验估计。在传统的EKF算法中，将IMU部件测量噪声视为高斯白噪声，测量噪声协方差矩阵
Figure 273364DEST_PATH_IMAGE112
也就是与时间无关的常量，而本实施例通过Allan方差实时分析与时间具有相关性的随机误差，通过不断更新
Figure 975741DEST_PATH_IMAGE113
消除色噪声的影响。
步骤312，采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
具体地，在多运动平台构成的分布式系统中，各平台之间的状态具有相关性，因此需要进行第二步联合滤波。将平台
Figure 665348DEST_PATH_IMAGE114
根据其对其他平台的测量数据进行的更新称为外部测量更新。首先根据平台
Figure 111373DEST_PATH_IMAGE114
和平台
Figure 976561DEST_PATH_IMAGE115
之间的测量完成外部测量更新：根据步骤306已经得到平台
Figure 974473DEST_PATH_IMAGE114
的UWB传感器获得的与平台
Figure 761163DEST_PATH_IMAGE115
之间的相对距离测量值
Figure 10879DEST_PATH_IMAGE116
，并将相对距离测量模型表示为：
Figure 589628DEST_PATH_IMAGE117
Figure 633807DEST_PATH_IMAGE118
令
Figure 173373DEST_PATH_IMAGE119
其中，测量误差
Figure 820255DEST_PATH_IMAGE120
的互协方差的计算方式与平台
Figure 394456DEST_PATH_IMAGE121
和
Figure 609537DEST_PATH_IMAGE122
之间相对距离测量值的Allan方差，即步骤308中的UWB测量噪声方差矩阵
Figure 761032DEST_PATH_IMAGE123
，具有等价性。假设各个平台都获得了对自身的平台状态的估计值，并且能够获取其他平台的这一估计值。假设
Figure 86971DEST_PATH_IMAGE124
时刻平台
Figure 781258DEST_PATH_IMAGE125
和平台
Figure 26294DEST_PATH_IMAGE126
建立了测距与通信链路，输入变量为k-1时刻平台
Figure 540452DEST_PATH_IMAGE127
对平台
Figure 670082DEST_PATH_IMAGE128
的状态后验估计值
Figure 77930DEST_PATH_IMAGE129
、互协方差矩阵
Figure 634813DEST_PATH_IMAGE130
以及偏微分矩阵
Figure 636267DEST_PATH_IMAGE131
和
Figure 431572DEST_PATH_IMAGE132
，在各个平台为同质节点（节点设备相同）的情况下，
Figure 834872DEST_PATH_IMAGE132
的计算方式与
Figure 421711DEST_PATH_IMAGE131
相同。输出变量为平台
Figure 910461DEST_PATH_IMAGE133
对平台
Figure 381894DEST_PATH_IMAGE134
的状态
Figure 764334DEST_PATH_IMAGE135
及其互协方差
Figure 663020DEST_PATH_IMAGE136
的先验估计值
Figure 639066DEST_PATH_IMAGE137
和
Figure 773244DEST_PATH_IMAGE138
：
Figure 619977DEST_PATH_IMAGE139
Figure 955144DEST_PATH_IMAGE140
其中
Figure 277541DEST_PATH_IMAGE141
表示前一时刻平台
Figure 90776DEST_PATH_IMAGE142
通过外部测量和通信得到的平台
Figure 57595DEST_PATH_IMAGE143
的互协方差后验估计值。对于任意三个运动平台
Figure 688296DEST_PATH_IMAGE144
（N为平台总数），其联合滤波过程中的互协方差后验估计方程如下：
Figure 107776DEST_PATH_IMAGE145
Figure 724702DEST_PATH_IMAGE146
Figure 936241DEST_PATH_IMAGE147
Figure 347631DEST_PATH_IMAGE148
表示当前k时刻外部测量下的平台
Figure 519986DEST_PATH_IMAGE149
互协方差后验估计值，
Figure 799658DEST_PATH_IMAGE150
表示运动平台
Figure 741069DEST_PATH_IMAGE151
对平台s的互协方差后验估计。利用上述互协方差更新过程可实时估计运动平台之间的相关性，提高定位鲁棒性。
进一步地，当
Figure 588939DEST_PATH_IMAGE152
时刻平台
Figure 104715DEST_PATH_IMAGE153
与平台
Figure 797865DEST_PATH_IMAGE154
不同时可测，如出现遮挡导致不可视情况，此时无法按照上述方式估计互协方差矩阵
Figure 983996DEST_PATH_IMAGE155
，但由于之前时刻分布式平台之间测量与状态的相关性，如果不对其进行更新可能导致估计性能的下降。不失一般性，假设此时平台
Figure 737188DEST_PATH_IMAGE156
与平台
Figure 618556DEST_PATH_IMAGE157
可测，而与平台
Figure 505610DEST_PATH_IMAGE158
不可测，此时需要在不依赖于通信链路的情况下对互协方差矩阵做出如下近似：
Figure 156034DEST_PATH_IMAGE159
由此可得：
Figure 345707DEST_PATH_IMAGE160
上式即为平台
Figure 573426DEST_PATH_IMAGE161
与
Figure 873957DEST_PATH_IMAGE162
在
Figure 769101DEST_PATH_IMAGE163
时刻不依赖平台间通信时的互协方差更新算法，在有限测量噪声情况下，
Figure 129675DEST_PATH_IMAGE164
的可逆性可以得到保证。
完成上述外部测量更新后，设k时刻平台
Figure 985636DEST_PATH_IMAGE165
中存储的所有平台状态量为
Figure 89858DEST_PATH_IMAGE166
，综合所有平台结果可得状态的后验估计更新过程如下：
Figure 839508DEST_PATH_IMAGE167
Figure 370983DEST_PATH_IMAGE168
其中，
Figure 714240DEST_PATH_IMAGE169
为k时刻的卡尔曼滤波互增益，其计算方式如下式：
Figure 215629DEST_PATH_IMAGE170
步骤314，根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。根据状态更新结果，确定第一运动平台的位置。
步骤316，根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
具体地，基于当前时刻的各个平台的平台状态更新结果
Figure 960731DEST_PATH_IMAGE171
进行碰撞检测，如果运动平台之间存在潜在碰撞可能，如平台间的碰撞概率高于预设的门限值时，则对其中一个或多个平台的航迹进行调整，以避免碰撞；否则不需要调整航迹直接进行下一时刻的运动状态估计，
Figure 522162DEST_PATH_IMAGE172
。
为了说明本申请提供的一种分布式平台相对定位方法的有效性，基于IMU传感器和UWB传感器的实际测量数据，采用上述一个实施例提供的一种分布式平台定位方法得到了对应的测量误差Allan方差分析结果。图4为一个实施例中一种分布式平台相对定位方法对传感器的测量误差分析结果，其中，a为对UWB模块进行的第一次相对距离测试数据进行的Allan方差分析结果，b为对UWB模块进行的第二次相对距离测试数据进行的Allan方差分析结果，c为对UWB模块进行的第三次相对距离测试数据进行的Allan方差分析结果，d为对IMU传感器的x方向角速率的测量数据进行的Allan方差分析结果，e为对IMU传感器的y方向角速率的测量数据进行的Allan方差分析结果，f为对IMU传感器的z方向角速率的测量数据进行的Allan方差分析结果，g为对IMU传感器的x方向加速度的测量数据进行的Allan方差分析结果，h为对IMU传感器的y方向加速度的测量数据进行的Allan方差分析结果，i为对IMU传感器的z方向加速度的测量数据进行的Allan方差分析结果。图4中a至i中，实线表示对UWB传感器或IMU传感器实测数据的Allan方差分析结果，斜率为-1的虚线代表对测试数据经噪声成分分析后对应的白噪声的Allan方差，斜率约为0的虚线表示对应的闪烁噪声的Allan方差，斜率约为+1/2的虚线表示对应的随机游走噪声的Allan方差。由a至c可以看到UWB传感器在进行相对距离测量时，其数据除白噪声外，还包含具有时间相关性的闪烁噪声和明显的随机游动噪声，而传统定位算法忽略了噪声参数与时间的相关性，未对其中的
Figure 352715DEST_PATH_IMAGE173
进行实时计算和更新。由d至f可以看到，IMU传感器测量得到的三轴角速率数据在x方向和z方向均含有色噪声成分。同理由g至i可以看到，IMU模块测试的x、y、z方向加速度的测量数据也包含色噪声。传统定位算法同样也忽略了这一色噪声的影响，未对
Figure 533160DEST_PATH_IMAGE174
进行实时计算和更新。
值得说明的是，在运动平台进入卫星导航拒止区域前已获取足够UWB和IMU传感器样本数据的条件下，可将滤波算法中的噪声协方差值初始化为Allan方差分析结果，并在后续运动过程中通过递归计算更新其噪声参数，以自适应地调整滤波算法。
根据实验结果分析现有EKF算法和本申请提供的方法的定位误差。EKF算法在色噪声的影响下产生了较大的误差，而本申请算法充分考虑并滤除了测量色噪声，算法性能在分布式平台运动过程中更加稳定，从而对运动平台的位置
Figure 260333DEST_PATH_IMAGE175
实现了更高精度的估计。在400s-600s受到色噪声影响最为显著的区间内，本申请提供的方法滤除色噪声后定位误差明显降低，而从误差的均值和标准差结果可以看出，现有EKF算法的位置估计误差约为0.63m，而本申请提供的方法估计误差在0.55m左右，精度提高了12.94%。同样，如速度估计误差和角度估计误差所示，IMU传感器对加速度和角速率的估计结果在滤除色噪声影响后，本申请提供的方法对速度
Figure 868031DEST_PATH_IMAGE176
和角度
Figure 185880DEST_PATH_IMAGE177
的估计精度分别提高了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 560230DEST_PATH_IMAGE178
其中，
Figure 748766DEST_PATH_IMAGE179
，k表示当前时刻，
Figure 792945DEST_PATH_IMAGE180
和
Figure 722724DEST_PATH_IMAGE181
均表示IMU测量噪声平均值，
Figure 244972DEST_PATH_IMAGE182
取正整数，
Figure 819173DEST_PATH_IMAGE183
表示IMU测量噪声方差矩阵。
其中一个实施例中，方差分析模块用于计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 158887DEST_PATH_IMAGE184
其中，
Figure 185749DEST_PATH_IMAGE185
Figure 511688DEST_PATH_IMAGE186
表示当前时刻，
Figure 65029DEST_PATH_IMAGE187
和
Figure 716591DEST_PATH_IMAGE188
均表示UWB测量噪声平均值，
Figure 965169DEST_PATH_IMAGE189
取正整数，
Figure 219433DEST_PATH_IMAGE190
表示UWB测量噪声方差矩阵。
其中一个实施例中，运动方程获取模块用于在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 768226DEST_PATH_IMAGE191
其中，
Figure 59530DEST_PATH_IMAGE192
表示当前时刻，
Figure 185618DEST_PATH_IMAGE193
表示
Figure 118939DEST_PATH_IMAGE192
时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 256659DEST_PATH_IMAGE194
表示
Figure 109078DEST_PATH_IMAGE192
时刻的控制输入向量，
Figure 332249DEST_PATH_IMAGE195
表示均值为零、协方差矩阵为
Figure 803681DEST_PATH_IMAGE196
的高斯过程噪声。
其中一个实施例中，相对距离测量模型获取模块用于获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 183191DEST_PATH_IMAGE197
其中，
Figure 81877DEST_PATH_IMAGE198
表示在
Figure 57923DEST_PATH_IMAGE199
时刻的相对距离，
Figure 192101DEST_PATH_IMAGE200
表示第一运动平台在
Figure 569993DEST_PATH_IMAGE199
时刻的状态量，
Figure 764214DEST_PATH_IMAGE201
表示对第二运动平台在
Figure 227557DEST_PATH_IMAGE199
时刻的状态量，
Figure 165426DEST_PATH_IMAGE202
表示根据在
Figure 132245DEST_PATH_IMAGE199
时刻的状态量计算得到的相对距离值，
Figure 638312DEST_PATH_IMAGE203
表示测量误差矩阵。
其中一个实施例中还包括航迹调整模块，用于根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
关于一种分布式平台相对定位装置的具体限定可以参见上文中对于一种分布式平台相对定位方法的限定，在此不再赘述。上述一种分布式平台相对定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种分布式平台相对定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：计算m组IMU测量噪声中每一组的IMU测量噪声平均值。根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 182426DEST_PATH_IMAGE204
其中，
Figure 799352DEST_PATH_IMAGE205
，k表示当前时刻，
Figure 886257DEST_PATH_IMAGE206
和
Figure 422280DEST_PATH_IMAGE207
均表示IMU测量噪声平均值，
Figure 594636DEST_PATH_IMAGE208
取正整数，
Figure 749674DEST_PATH_IMAGE209
表示IMU测量噪声方差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 815719DEST_PATH_IMAGE210
其中，
Figure 663589DEST_PATH_IMAGE211
，k表示当前时刻，
Figure 185225DEST_PATH_IMAGE212
和
Figure 143953DEST_PATH_IMAGE213
均表示UWB测量噪声平均值，
Figure 674292DEST_PATH_IMAGE214
取正整数，
Figure 817697DEST_PATH_IMAGE215
表示UWB测量噪声方差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 964645DEST_PATH_IMAGE216
其中，k表示当前时刻，
Figure 461485DEST_PATH_IMAGE217
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 502122DEST_PATH_IMAGE218
表示k时刻的控制输入向量，
Figure 160637DEST_PATH_IMAGE219
表示均值为零、协方差矩阵为
Figure 794880DEST_PATH_IMAGE220
的高斯过程噪声。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 954466DEST_PATH_IMAGE221
其中，
Figure 724976DEST_PATH_IMAGE222
表示在k时刻的相对距离，
Figure 944605DEST_PATH_IMAGE223
表示第一运动平台在k时刻的状态量，
Figure 800566DEST_PATH_IMAGE224
表示对第二运动平台在k时刻的状态量，
Figure 904788DEST_PATH_IMAGE225
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 654438DEST_PATH_IMAGE226
表示测量误差矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
在第一运动平台进入卫星导航拒止环境前，通过第一运动平台上的IMU传感器获取IMU测量噪声；以及，通过第一运动平台上的UWB传感器测量第二运动平台得到的UWB测量噪声。
分别对IMU测量噪声和UWB测量噪声进行Allan方差分析，得到IMU测量噪声方差矩阵和UWB测量噪声方差矩阵。
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，运动方程中包括零协方差矩阵。
采用卡尔曼滤波算法对运动方程进行更新时，将零协方差矩阵替换为IMU测量噪声方差矩阵。
获取第一运动平台和第二运动平台的相对距离测量模型，相对距离测量模型中包括测量误差矩阵。
采用卡尔曼滤波算法对相对距离测量模型进行更新时，将测量误差矩阵替换为UWB测量噪声方差矩阵。
根据运动方程的更新结果和相对距离测量模型的更新结果，得到第一运动平台的状态更新结果。
根据状态更新结果，确定第一运动平台的位置。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：分别将IMU测量噪声和UWB测量噪声以预设采样间隔划分为m组。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：计算m组IMU测量噪声中每一组的IMU测量噪声平均值。根据IMU测量噪声平均值，得到IMU测量噪声方差矩阵为：
Figure 920334DEST_PATH_IMAGE227
其中，
Figure 263591DEST_PATH_IMAGE228
，k表示当前时刻，
Figure 30559DEST_PATH_IMAGE229
和
Figure 775661DEST_PATH_IMAGE230
均表示IMU测量噪声平均值，
Figure 337092DEST_PATH_IMAGE231
取正整数，
Figure 167645DEST_PATH_IMAGE232
表示IMU测量噪声方差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：计算m组UWB测量噪声中每一组的UWB测量噪声平均值。根据UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 613670DEST_PATH_IMAGE233
其中，
Figure 69403DEST_PATH_IMAGE234
，k表示当前时刻，
Figure 942681DEST_PATH_IMAGE235
和
Figure 260530DEST_PATH_IMAGE236
均表示UWB测量噪声平均值，
Figure 103721DEST_PATH_IMAGE237
取正整数，
Figure 823415DEST_PATH_IMAGE238
表示UWB测量噪声方差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 867595DEST_PATH_IMAGE239
其中，k表示当前时刻，
Figure 531794DEST_PATH_IMAGE240
表示k时刻的状态量，状态量包括：位置量、惯性速度矢量以及偏航角。
Figure 319622DEST_PATH_IMAGE241
表示k时刻的控制输入向量，
Figure 893823DEST_PATH_IMAGE242
表示均值为零、协方差矩阵为
Figure 967958DEST_PATH_IMAGE243
的高斯过程噪声。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 994820DEST_PATH_IMAGE244
其中，
Figure 320759DEST_PATH_IMAGE245
表示在k时刻的相对距离，
Figure 874100DEST_PATH_IMAGE246
表示第一运动平台在k时刻的状态量，
Figure 260082DEST_PATH_IMAGE247
表示对第二运动平台在k时刻的状态量，
Figure 774240DEST_PATH_IMAGE248
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 28504DEST_PATH_IMAGE249
表示测量误差矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据状态更新结果得到第一运动平台和第二运动平台的碰撞概率值，当碰撞概率值高于预设值时，对第一运动平台进行航迹调整。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（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 529370DEST_PATH_IMAGE001
其中，
Figure 915352DEST_PATH_IMAGE002
，k表示当前时刻，
Figure 285634DEST_PATH_IMAGE003
和
Figure 415264DEST_PATH_IMAGE004
均表示IMU测量噪声平均值，
Figure 964057DEST_PATH_IMAGE005
取正整数，
Figure 645574DEST_PATH_IMAGE006
表示IMU测量噪声方差矩阵。
4.根据权利要求2所述的方法，其特征在于，对所述UWB测量噪声进行Allan方差分析，得到UWB测量噪声方差矩阵，包括：
计算m组UWB测量噪声中每一组的UWB测量噪声平均值；
根据所述UWB测量噪声平均值，得到UWB测量噪声方差矩阵为：
Figure 647029DEST_PATH_IMAGE007
其中，
Figure 314770DEST_PATH_IMAGE008
，k表示当前时刻，
Figure 577124DEST_PATH_IMAGE009
和
Figure 304909DEST_PATH_IMAGE010
均表示UWB测量噪声平均值，
Figure 652714DEST_PATH_IMAGE011
取正整数，
Figure 124146DEST_PATH_IMAGE012
表示UWB测量噪声方差矩阵。
5.根据权利要求1所述的方法，其特征在于，在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程，包括：
在第一运动平台进入卫星导航拒止环境时，获取第一运动平台的运动方程为：
Figure 381952DEST_PATH_IMAGE013
其中，
Figure 546217DEST_PATH_IMAGE014
表示k时刻的状态量，所述状态量包括：位置量、惯性速度矢量以及偏航角；
Figure 381318DEST_PATH_IMAGE015
表示k时刻的控制输入向量，
Figure 390862DEST_PATH_IMAGE016
表示均值为零、协方差矩阵为
Figure 768754DEST_PATH_IMAGE017
的高斯过程噪声。
6.根据权利要求1所述的方法，其特征在于，获取第一运动平台和第二运动平台的相对距离测量模型，包括：
获取第一运动平台和第二运动平台的相对距离测量模型为：
Figure 697396DEST_PATH_IMAGE018
其中，
Figure 160738DEST_PATH_IMAGE019
表示在k时刻的相对距离，
Figure 833028DEST_PATH_IMAGE020
表示第一运动平台在k时刻的状态量，
Figure 65426DEST_PATH_IMAGE021
表示对第二运动平台在k时刻的状态量，
Figure 571494DEST_PATH_IMAGE022
表示根据在k时刻的状态量计算得到的相对距离值，
Figure 115608DEST_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 true CN112325880A (zh) 2021-02-05
CN112325880B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113110589A (zh) * 2021-04-29 2021-07-13 中国人民解放军国防科技大学 面向无人机集群紧密编队构型的分布式相对定位方法及系统
CN113218388A (zh) * 2021-03-02 2021-08-06 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统

Citations (10)

* 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测距的惯导定位系统
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
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 桂林电子科技大学 一种解决分布式标签融合的多目标跟踪方法

Patent Citations (10)

* 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紧组合导航系统及方法
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
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
RAN LIU,ETC: "Cooperative relative positioning of mobile users by fusing IMU inertial and UWB ranging information", 《2017 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 *
江涛: "分布式相对定位技术的研究", 《中国博士学位论文全文数据库工程科技Ⅱ辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113218388A (zh) * 2021-03-02 2021-08-06 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113218388B (zh) * 2021-03-02 2022-07-05 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113110589A (zh) * 2021-04-29 2021-07-13 中国人民解放军国防科技大学 面向无人机集群紧密编队构型的分布式相对定位方法及系统

Also Published As

Publication number Publication date
CN112325880B (zh) 2021-03-26

Similar Documents

Publication Publication Date Title
Esfahani et al. AbolDeepIO: A novel deep inertial odometry network for autonomous vehicles
Shen et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles
Chen et al. A hybrid prediction method for bridging GPS outages in high-precision POS application
Indelman et al. Information fusion in navigation systems via factor graph based incremental smoothing
Cui et al. On sigma-point update of cubature Kalman filter for GNSS/INS under GNSS-challenged environment
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN112325880B (zh) 分布式平台相对定位方法、装置、计算机设备和存储介质
CN111707260B (zh) 一种基于频域分析及卷积神经网络的定位方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN111707294B (zh) 基于最优区间估计的行人导航零速区间检测方法和装置
CN113091738A (zh) 基于视觉惯导融合的移动机器人地图构建方法及相关设备
Srinivas et al. Overview of architecture for GPS-INS integration
Bifulco et al. Real-time smoothing of car-following data through sensor-fusion techniques
Dai et al. Robust adaptive UKF based on SVR for inertial based integrated navigation
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与系统
CN109857127B (zh) 训练神经网络模型以及飞行器姿态解算的方法、装置
El Sabbagh et al. Promoting navigation system efficiency during GPS outage via cascaded neural networks: A novel AI based approach
CN114061611A (zh) 目标对象定位方法、装置、存储介质和计算机程序产品
Zhanxin et al. Unscented Kalman filter for SINS alignment
Fariña et al. Sensor fusion algorithm selection for an autonomous wheelchair based on EKF/UKF comparison
CN109655057B (zh) 一种六推无人机加速器测量值的滤波优化方法及其系统
Ju et al. Scene-aware error modeling of lidar/visual odometry for fusion-based vehicle localization
Jaradat et al. Intelligent fault detection and fusion for INS/GPS navigation system
Colomina et al. A unified approach to static and dynamic modeling in photogrammetry and remote sensing
Buhaiov et al. Artificial Neural Networks application to MMG temperature calibration

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