CN112068173A - 一种基于环路和积数据关联算法的协同导航方法 - Google Patents

一种基于环路和积数据关联算法的协同导航方法 Download PDF

Info

Publication number
CN112068173A
CN112068173A CN202010792458.0A CN202010792458A CN112068173A CN 112068173 A CN112068173 A CN 112068173A CN 202010792458 A CN202010792458 A CN 202010792458A CN 112068173 A CN112068173 A CN 112068173A
Authority
CN
China
Prior art keywords
navigation
platform
platforms
measurement
collaborative
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
CN202010792458.0A
Other languages
English (en)
Other versions
CN112068173B (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.)
Henan University of Technology
Original Assignee
Henan University of 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 Henan University of Technology filed Critical Henan University of Technology
Priority to CN202010792458.0A priority Critical patent/CN112068173B/zh
Publication of CN112068173A publication Critical patent/CN112068173A/zh
Application granted granted Critical
Publication of CN112068173B publication Critical patent/CN112068173B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

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

Abstract

本发明公开了一种基于环路和积数据关联算法的协同导航，其特征在于：协同导航***内有多个平台，平台自身用卫星（GPS）和惯导（INS）进行绝对导航，平台间用GPS和超宽带（UWB）进行相对导航，且平台间可相互通信。具体步骤为：平台自身采用惯导，卫星等传感器进行绝对导航，获取平台的位置、速度、姿态等信息；平台间采用卫星差分和超宽带的测距，测速，以及绝对导航输出结果进行相对导航，获取平台间的相对位置、速度信息；平台间互相传递绝对导航及相对导航的估出结果，构建基于环路和积数据关联算法的协同导航滤波器，估计出所有平台的位置、速度信息。本发明能够提高协同导航定位精度和可靠性，适用于工程实际应用。

Description

一种基于环路和积数据关联算法的协同导航方法
技术领域
本发明属于协同导航技术领域，具体涉及一种基于环路和积数据关联算法的协同导航。
背景技术
协同导航是协同组网编队中平台间通过导航信息交互利用，实时解算并修正自身位置、速度、姿态等导航信息，保障协同编队保持、队形重构及后续协同任务顺利完成的一种技术。
绝对导航为平台利用INS、GPS等技术，解算出自身运动状态的一种技术；相对导航为平台间通过UWB测速、GPS差分等技术，解算出平台之间的相对距离速度的一种技术。相对导航通常相对于绝对导航具有更高的定位精度，但无法获悉平台自身的运动状态。因此，融合绝对导航和相对导航，在获取平台自身运动状态的同时，也能获取到平台间的相对运动状态，且精度高于单一的绝对导航和相对导航，更有利于整个网络协同作业的运作。
发明内容
发明目的：
在协同导航网络中，通过平台间的通讯，融合基于INS/GPS的绝对导航和GPS/UWB的相对导航，基于环路和积数据关联算法进行协同导航滤波，获取整个***的导航信息。
技术方案：
一种基于环路和积数据关联算法的协同导航，其特征在于：协同导航***内有多个平台，平台自身用卫星(GPS)和惯导(INS)进行绝对导航，平台间用GPS和超宽带 (UWB)进行相对导航，且平台间可相互通信，它包括以下步骤：
步骤一：平台自身采用惯导，卫星等传感器进行绝对导航，获取平台的位置、速度、姿态等信息；
步骤二：平台间采用卫星差分和超宽带的测距测速，借助绝对导航输出结果进行相对导航，获取平台间的相对位置、速度信息；
步骤三：平台间互相传递信息，构建基于环路和积数据关联算法的协同导航滤波器，估计出所有平台的位置、速度信息；
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
本发明的优点在于
(1)通过融合相对导航，获得了相比于绝对导航精度更高的运动状态。
(2)本发明为融合绝对导航和相对导航的算法框架，并不要求固定的绝对导航和相对导航方式，具有普适性。
(3)仅通过算法融合绝对导航和相对导航，不增加传感器数量，不增加硬件成本。
本发明提出的方案通过如下仿真实验加以验证：
(1)采用三平台进行协同导航，三平台运行中除在高度有差距，其它运动状态相同，平台间都可相互通信，滤波周期Tf为200ms，仿真时间为103s；
(2)绝对导航采用惯导与卫星紧组合，惯性传感器数据采样时间为40ms，卫星伪距伪距率采样时间为200ms，陀螺仪常值漂移为0.1°/h，随机漂移为0.01°/h，加速度计常偏为0.1mg，随机为0.05mg，卫星伪距误差为5m，伪距率误差为0.1m/s；
(3)相对导航采用UWB传感器和卫星伪距伪距率双差组合，采样时间均为200ms，UWB测距误差为0.1m，测速误差为0.01m/s。
附图表说明
图1为融合绝对导航和相对导航的协同导航流程框图；
图2-图4为各平台绝对导航与协同导航误差曲线图；
图5为平台绝对导航与协同导航平均误差统计特性；
具体实施方式
下面结合附图对本发明做更进一步的解释。
本实例***内共有三个平台，平台自身用卫星(GPS)和惯导(INS)进行绝对导航，平台间用GPS和超宽带(UWB)进行相对导航，且平台间可相互通信。如图1所示，共三个步骤：
步骤一：平台自身采用惯导，卫星等传感器进行绝对导航，获取平台的位置、速度、姿态等信息；
步骤二：平台间采用卫星差分和超宽带的测距测速，借助绝对导航输出结果进行相对导航，获取平台间的相对位置、速度信息；
步骤三：平台间互相传递信息，构建基于环路和积数据关联算法的协同导航滤波器，估计出所有平台的位置、速度信息。
结算结果如图2到图4所示，为三个平台在东北天地理坐标系下三个方向的速度位置误差，图5为各个方向的平均误差统计特性，可以看到，协同导航精度高于单一的绝对导航，以下为三个步骤的详细展开：
步骤一所述的绝对导航，为INS和GPS紧组合导航，主要根据GPS接收机计算出的卫星星历数据和惯导***计算出的速度位置，解算出对应于惯性导航***的伪距和伪距率，再与GPS接收机测量到的伪距伪距率相比较，估计出惯性导航***和GPS接收机的误差量，然后将估出的参数反馈至INS内部，得到平台自身的运动状态，以平台i 为例进行说明：
绝对导航采用坐标系为东北天地理坐标系，其状态包括9维的INS误差，6维的INS元件误差，2维的GPS误差，状态方程可写作：
Figure RE-RE-RE-GDA0002766078190000031
其中：
Figure DEST_PATH_GDA0002734614370000032
代表k时刻编号为i的平台状态，
Figure DEST_PATH_GDA0002734614370000033
为东北天三个方向的平台误差角，δvE,δvN,δvU和δL,δλ,δh为三个方向的速度和位置误差，εx,εy,εz和Δx,Δy,Δz陀螺仪和加速度计在三个方向的偏置误差，δtu为时钟误差等效的测距误差，δtru为时钟频率误差等效的距离变化率,F为状态转移矩阵，G为噪声系数矩阵，W为噪声向量，上角标i为平台编号，下角标k代表时间；
设平台在地球大地坐标系下惯导输出位置为经度λI，纬度LI，高度hI，则平台的真实位置的λ,L,h可表示成如下形式：
Figure RE-RE-RE-GDA0002766078190000034
量测为GPS接收机测得的四颗卫星的伪距和伪距率，量测方程可写作：
Figure RE-RE-RE-GDA0002766078190000035
其中ρj为伪距测量，具体展开如下：
Figure RE-RE-RE-GDA0002766078190000041
式中，
Figure RE-RE-RE-GDA0002766078190000042
为卫星j在地心地球固连(ECEF)坐标系下各方向位置，下角标s代表卫星，上角标j为卫星编号，x,y,z为平台自身在ECEF坐标系下各方向位置，vρ为伪距测量噪声，ECEF坐标系与东北天地理坐标系转换关系如下：
Figure RE-RE-RE-GDA0002766078190000043
式中，RN为地球半径，f为地球的椭圆扁率；
Figure RE-RE-RE-GDA0002766078190000044
为伪距率测量，具体展开如下：
Figure RE-RE-RE-GDA0002766078190000045
式中，
Figure RE-RE-RE-GDA0002766078190000046
为平台自身在ECEF坐标系下各方向速度，
Figure RE-RE-RE-GDA0002766078190000047
为卫星在ECEF 坐标系下各方向速度，其与vE,vN,vU的转换关系如下：
Figure RE-RE-RE-GDA0002766078190000048
进一步的，步骤二所述的相对导航，任意两个有通信平台为一组，以平台i与平台j为例，它们间的相对导航具体如下：
状态变量由ECEF坐标系下双平台的相对运动状态构成，首先定义状态变量X为：
Figure RE-RE-RE-GDA0002766078190000049
式中，
Figure RE-RE-RE-GDA00027660781900000410
为平台相对位置矢量，
Figure RE-RE-RE-GDA00027660781900000411
为相对速度矢量，
Figure RE-RE-RE-GDA00027660781900000412
为相对加速度矢量，
Figure RE-RE-RE-GDA00027660781900000413
为单平台定位误差导致的相对定位误差的补偿量；
构造状态方程如下：
Figure RE-RE-RE-GDA0002766078190000051
式中，03×3为3维的全零方阵，I3×3为3维的单位矩阵，wp,wv,wa,wδp分别为相对位置、速度、加速度、时变位置误差的***噪声；
量测为两个平台间的伪距双差、双差变化率、UWB测距测速，以及两个平台的绝对导航定位数据做差，可分为以下五个部分：
第一部分，GPS的伪距双差
Figure RE-RE-RE-GDA0002766078190000052
Figure RE-RE-RE-GDA0002766078190000053
式中，
Figure RE-RE-RE-GDA0002766078190000054
分别为平台i，j中点到卫星S1、S2的方向余弦矢量，
Figure RE-RE-RE-GDA0002766078190000055
为伪距双差计算中存在的噪声；
第二部分，GPS的伪距双差变化率
Figure RE-RE-RE-GDA0002766078190000056
Figure RE-RE-RE-GDA0002766078190000057
式中，
Figure RE-RE-RE-GDA0002766078190000058
分别为平台i，j中点到卫星s1、s2的方向余弦矢量变化率，
Figure RE-RE-RE-GDA0002766078190000059
为伪距双差变化率计算中存在的噪声；
第三部分，绝对导航定位数据做差
Figure RE-RE-RE-GDA00027660781900000510
Figure RE-RE-RE-GDA00027660781900000511
式中，VΔabs为双平台绝对导航误差所引起的噪声；
第四部分，UWB测距
Figure RE-RE-RE-GDA00027660781900000512
Figure RE-RE-RE-GDA00027660781900000513
式中，||·||2代表矩阵的2范数，np_uwb为UWB测距噪声；
第五部分，UWB测速
Figure RE-RE-RE-GDA00027660781900000514
Figure RE-RE-RE-GDA0002766078190000061
式中，nv_uwb为UWB测速噪声；
综上五个部分，相对导航的量测可写作如下形式：
Figure RE-RE-RE-GDA0002766078190000062
其中，本平台采用四颗卫星，选取其中高度最高的卫星与剩下三颗卫星做差，故伪距双差测量与伪距双差变化率测量都是三维；
进一步的，步骤三所述的传递信息，其内容主要包括：
平台自身INS解算出的位置
Figure RE-RE-RE-GDA0002766078190000063
速度
Figure RE-RE-RE-GDA0002766078190000064
绝对导航估出的位置误差、速度误差，记作
Figure RE-RE-RE-GDA0002766078190000065
以及对应的协方差矩阵
Figure RE-RE-RE-GDA0002766078190000066
相对导航估出的相对位置、相对速度
Figure RE-RE-RE-GDA0002766078190000067
以及对应的协方差矩阵
Figure RE-RE-RE-GDA0002766078190000068
传递过信息后，开始构造步骤三所述的协同导航滤波器，首先构造其状态及协方差初值，构造方法如下：
Figure RE-RE-RE-GDA0002766078190000069
Figure RE-RE-RE-GDA00027660781900000610
式中，
Figure RE-RE-RE-GDA00027660781900000611
为构造的状态向量初值，
Figure RE-RE-RE-GDA00027660781900000612
为状态协方差初值，上角标i，j代表平台编号，
Figure RE-RE-RE-GDA00027660781900000613
为与平台i有量测关系的编号合集，共m个；
然后构造协同导航滤波器的量测及量测误差，构造方法如下：
Figure RE-RE-RE-GDA00027660781900000614
Figure RE-RE-RE-GDA0002766078190000071
式中：
zi为构造的量测向量，Ri为与zi对应的量测噪声；
根据绝对导航和相对导航的关系，可得出协同导航滤波器量测方程为：
Figure RE-RE-RE-GDA0002766078190000072
式中，H(·)为协同滤波器的量测方程，
Figure RE-RE-RE-GDA0002766078190000073
为
Figure RE-RE-RE-GDA0002766078190000074
和
Figure RE-RE-RE-GDA0002766078190000075
的等量关系：
Figure RE-RE-RE-GDA0002766078190000076
式中，x,y,z由INS输出位置L,λ,h以及绝对导航输出位置误差δL,δλ,δh转化而来，
Figure RE-RE-RE-GDA00027660781900000717
由INS输出速度vE,vN,vU以及绝对导航输出速度误差δvE,δvN,δvU转化而来，具体转换过程如式(5)和式(7)所示；
在构造完协同导航滤波器后，进行环路和积数据关联算法，详细计算过程如下：
第一步，对进行
Figure RE-RE-RE-GDA0002766078190000078
初始采样；
通过对
Figure RE-RE-RE-GDA0002766078190000079
利用UT变换求取积分点集
Figure RE-RE-RE-GDA00027660781900000710
其中，ξl为积分点集，ωl为对应的积分权值，n为协同滤波器的状态
Figure RE-RE-RE-GDA00027660781900000711
的维数；
第二步，观测方程传播积分点；
Figure RE-RE-RE-GDA00027660781900000712
Figure RE-RE-RE-GDA00027660781900000713
第三步，计算
Figure RE-RE-RE-GDA00027660781900000714
Figure RE-RE-RE-GDA00027660781900000715
Figure RE-RE-RE-GDA00027660781900000716
Figure RE-RE-RE-GDA0002766078190000081
Figure RE-RE-RE-GDA0002766078190000082
Figure RE-RE-RE-GDA0002766078190000083
第四步所有平台运算完毕后，进行信息交互；
首先各平台将估出的
Figure RE-RE-RE-GDA0002766078190000084
Figure RE-RE-RE-GDA0002766078190000085
按如下公式进行分割：
Figure RE-RE-RE-GDA0002766078190000086
Figure RE-RE-RE-GDA0002766078190000087
式中，
Figure RE-RE-RE-GDA0002766078190000088
与
Figure RE-RE-RE-GDA0002766078190000089
维数相同，
Figure RE-RE-RE-GDA00027660781900000810
与
Figure RE-RE-RE-GDA00027660781900000811
维数相同；
再将各平台将估出的
Figure RE-RE-RE-GDA00027660781900000812
传递给周围平台，同时交互次数加一；
第五步重新构造状态初值，再次进行滤波；
所有平台在接收到周围节点传递来的消息后，按式(31)(32)进行排列组合，生成新的
Figure RE-RE-RE-GDA00027660781900000813
与
Figure RE-RE-RE-GDA00027660781900000814
重复第一步到第四步，再次进行滤波；
Figure RE-RE-RE-GDA00027660781900000815
Figure RE-RE-RE-GDA00027660781900000816
第六步，达到设定的最大交互次数后，输出估计所有平台状态
Figure RE-RE-RE-GDA00027660781900000817
完成协同导航。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种基于环路和积数据关联算法的协同导航，其特征在于：协同导航***内有多个平台，平台自身用卫星(GPS)和惯导(INS)进行绝对导航，平台间用GPS和超宽带(UWB)进行相对导航，且平台间可相互通信，它包括以下步骤：
步骤一：平台自身采用惯导，卫星等传感器进行绝对导航，获取平台的位置、速度、姿态等信息；
步骤二：平台间采用卫星差分和超宽带的测距测速，借助绝对导航输出结果进行相对导航，获取平台间的相对位置、速度信息；
步骤三：平台间互相传递信息，构建基于环路和积数据关联算法的协同导航滤波器，估计出所有平台的位置、速度信息。
2.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤一所述的绝对导航，为INS和GPS紧组合导航，主要根据GPS接收机计算出的卫星星历数据和惯导***计算出的速度位置，解算出对应于惯性导航***的伪距和伪距率，再与GPS接收机测量到的伪距伪距率相比较，估计出惯性导航***和GPS接收机的误差量，然后将估出的参数反馈至INS内部，得到平台自身的运动状态，以平台i为例进行说明：
绝对导航采用坐标系为东北天地理坐标系，其状态包括9维的INS误差，6维的INS元件误差，2维的GPS误差，状态方程可写作：
Figure RE-FDA0002766078180000011
其中：
Figure RE-FDA0002766078180000012
代表k时刻编号为i的平台状态，
Figure RE-FDA0002766078180000013
为东北天三个方向的平台误差角，δvE,δvN,δvU和δL,δλ,δh为三个方向的速度和位置误差，εx,εy,εz和Δx,Δy,Δz陀螺仪和加速度计在三个方向的偏置误差，δtu为时钟误差等效的测距误差，δtru为时钟频率误差等效的距离变化率,F为状态转移矩阵，G为噪声系数矩阵，W为噪声向量，上角标i为平台编号，下角标k代表时间；
设平台在地球大地坐标系下惯导输出位置为经度λI，纬度LI，高度hI，则平台的真实位置的λ,L,h可表示成如下形式：
Figure RE-FDA0002766078180000021
量测为GPS接收机测得的四颗卫星的伪距和伪距率，量测方程可写作：
Figure RE-FDA0002766078180000022
其中ρj为伪距测量，具体展开如下：
Figure RE-FDA0002766078180000023
式中，
Figure RE-FDA0002766078180000024
为卫星j在地心地球固连(ECEF)坐标系下各方向位置，下角标s代表卫星，上角标j为卫星编号，x,y,z为平台自身在ECEF坐标系下各方向位置，vρ为伪距测量噪声，ECEF坐标系与东北天地理坐标系转换关系如下：
Figure RE-FDA0002766078180000025
式中，RN为地球半径，f为地球的椭圆扁率；
Figure RE-FDA0002766078180000026
为伪距率测量，具体展开如下：
Figure RE-FDA0002766078180000027
式中，
Figure RE-FDA0002766078180000028
为平台自身在ECEF坐标系下各方向速度，
Figure RE-FDA0002766078180000029
为卫星在ECEF坐标系下各方向速度，其与vE,vN,vU的转换关系如下：
Figure RE-FDA00027660781800000210
3.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤二所述的相对导航，任意两个有通信平台为一组，以平台i与平台j为例，它们间的相对导航具体如下：
状态变量由ECEF坐标系下双平台的相对运动状态构成，首先定义状态变量X为：
Figure RE-FDA0002766078180000031
式中，
Figure RE-FDA0002766078180000032
为平台相对位置矢量，
Figure RE-FDA0002766078180000033
为相对速度矢量，
Figure RE-FDA0002766078180000034
为相对加速度矢量，
Figure RE-FDA0002766078180000035
为单平台定位误差导致的相对定位误差的补偿量；
构造状态方程如下：
Figure RE-FDA0002766078180000036
式中，03×3为3维的全零方阵，I3×3为3维的单位矩阵，wp,wv,wa,wδp分别为相对位置、速度、加速度、时变位置误差的***噪声；
量测为两个平台间的伪距双差、双差变化率、UWB测距测速，以及两个平台的绝对导航定位数据做差，可分为以下五个部分：
第一部分，GPS的伪距双差
Figure RE-FDA0002766078180000037
Figure RE-FDA0002766078180000038
式中，
Figure RE-FDA0002766078180000039
分别为平台i，j中点到卫星S1、S2的方向余弦矢量，
Figure RE-FDA00027660781800000310
为伪距双差计算中存在的噪声；
第二部分，GPS的伪距双差变化率
Figure RE-FDA00027660781800000311
Figure RE-FDA00027660781800000312
式中，
Figure RE-FDA00027660781800000313
分别为平台i，j中点到卫星s1、s2的方向余弦矢量变化率，
Figure RE-FDA00027660781800000314
为伪距双差变化率计算中存在的噪声；
第三部分，绝对导航定位数据做差
Figure RE-FDA00027660781800000315
Figure RE-FDA00027660781800000316
式中，VΔabs为双平台绝对导航误差所引起的噪声；
第四部分，UWB测距
Figure RE-FDA0002766078180000041
Figure RE-FDA0002766078180000042
式中，||·||2代表矩阵的2范数，np_uwb为UWB测距噪声；
第五部分，UWB测速
Figure RE-FDA0002766078180000043
Figure RE-FDA0002766078180000044
式中，nv_uwb为UWB测速噪声；
综上五个部分，相对导航的量测可写作如下形式：
Figure RE-FDA0002766078180000045
其中，本平台采用四颗卫星，选取其中高度最高的卫星与剩下三颗卫星做差，故伪距双差测量与伪距双差变化率测量都是三维。
4.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤三所述的传递信息，其内容主要包括：
平台自身INS解算出的位置
Figure RE-FDA0002766078180000046
速度
Figure RE-FDA0002766078180000047
绝对导航估出的位置误差、速度误差，记作
Figure RE-FDA0002766078180000048
以及对应的协方差矩阵
Figure RE-FDA0002766078180000049
相对导航估出的相对位置、相对速度，记作
Figure RE-FDA00027660781800000410
以及对应的协方差矩阵
Figure RE-FDA00027660781800000411
5.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤三所述的协同导航滤波器，其状态及协方差初值构造方法如下：
Figure RE-FDA00027660781800000412
Figure RE-FDA0002766078180000051
式中，
Figure RE-FDA0002766078180000052
为构造的状态向量初值，
Figure RE-FDA0002766078180000053
为状态协方差初值，上角标i，j代表平台编号，
Figure RE-FDA0002766078180000054
为与平台i有量测关系的编号合集，共m个。
6.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤三所述的协同导航滤波器，其量测及量测误差构造方法如下：
Figure RE-FDA0002766078180000055
Figure RE-FDA0002766078180000056
式中：
zi为构造的量测向量，Ri为与zi对应的量测噪声。
7.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤三所述的协同导航滤波器，其量测方程为：
Figure RE-FDA0002766078180000057
式中，H(·)为协同滤波器的量测方程，
Figure RE-FDA0002766078180000058
为
Figure RE-FDA0002766078180000059
和
Figure RE-FDA00027660781800000510
的等量关系：
Figure RE-FDA00027660781800000511
式中，x,y,z由INS输出位置L,λ,h以及绝对导航输出位置误差δL,δλ,δh转化而来，
Figure RE-FDA00027660781800000512
由INS输出速度vE，vN，vU以及绝对导航输出速度误差δvE,δvN,δvU转化而来，具体转换过程如式(5)和式(7)所示。
8.根据权力要求1所述的基于环路和积数据关联算法的协同导航，其特征在于：步骤三所述的基于环路和积数据关联算法的协同导航滤波器，其计算过程如下：
8.1对
Figure RE-FDA0002766078180000061
进行初始采样；
通过对
Figure RE-FDA0002766078180000062
利用UT变换求取积分点集
Figure RE-FDA0002766078180000063
其中，ξl为积分点集，ωl为对应的积分权值，n为协同滤波器的状态
Figure RE-FDA0002766078180000064
的维数；
8.2观测方程传播积分点；
Figure RE-FDA0002766078180000065
Figure RE-FDA0002766078180000066
8.3计算
Figure RE-FDA0002766078180000067
Figure RE-FDA0002766078180000068
Figure RE-FDA0002766078180000069
Figure RE-FDA00027660781800000610
Figure RE-FDA00027660781800000611
Figure RE-FDA00027660781800000612
8.4所有平台运算完毕后，进行信息交互；
首先各平台将估出的
Figure RE-FDA00027660781800000613
按如下公式进行分割：
Figure RE-FDA00027660781800000614
Figure RE-FDA00027660781800000615
式中，
Figure RE-FDA00027660781800000616
与
Figure RE-FDA00027660781800000617
维数相同，
Figure RE-FDA00027660781800000618
与
Figure RE-FDA00027660781800000619
维数相同；
再将各平台将估出的
Figure RE-FDA00027660781800000620
传递给周围平台，同时交互次数加一；
8.5重新构造状态初值，再次进行滤波；
所有平台在接收到周围节点传递来的消息后，按式(31)(32)进行排列组合，生成新的
Figure RE-FDA0002766078180000071
与
Figure RE-FDA0002766078180000072
重复8.1～8.4，再次进行滤波；
Figure RE-FDA0002766078180000073
Figure RE-FDA0002766078180000074
8.6达到设定的最大交互次数后，输出估计所有平台状态
Figure RE-FDA0002766078180000075
完成协同导航。
CN202010792458.0A 2020-08-09 2020-08-09 一种基于环路和积数据关联算法的协同导航方法 Active CN112068173B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010792458.0A CN112068173B (zh) 2020-08-09 2020-08-09 一种基于环路和积数据关联算法的协同导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010792458.0A CN112068173B (zh) 2020-08-09 2020-08-09 一种基于环路和积数据关联算法的协同导航方法

Publications (2)

Publication Number Publication Date
CN112068173A true CN112068173A (zh) 2020-12-11
CN112068173B CN112068173B (zh) 2024-03-22

Family

ID=73660872

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010792458.0A Active CN112068173B (zh) 2020-08-09 2020-08-09 一种基于环路和积数据关联算法的协同导航方法

Country Status (1)

Country Link
CN (1) CN112068173B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113108785A (zh) * 2021-03-11 2021-07-13 中国电子科技集团公司第五十四研究所 一种面向同构imu的分布式协同互校准定位方法
CN114928881A (zh) * 2022-04-18 2022-08-19 上海大学 基于超宽带和视觉智能装置的协同定位***及定位方法
CN117111126A (zh) * 2023-10-20 2023-11-24 成都格理特电子技术有限公司 基于北斗的石化工作人员联合定位方法和装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2617565C1 (ru) * 2015-12-02 2017-04-25 Акционерное общество "Раменское приборостроительное конструкторское бюро" Способ оценивания ошибок инерциальной информации и её коррекции по измерениям спутниковой навигационной системы
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN111044075A (zh) * 2019-12-10 2020-04-21 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2617565C1 (ru) * 2015-12-02 2017-04-25 Акционерное общество "Раменское приборостроительное конструкторское бюро" Способ оценивания ошибок инерциальной информации и её коррекции по измерениям спутниковой навигационной системы
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN111044075A (zh) * 2019-12-10 2020-04-21 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
GIOVANNI MIRAGLIA等: "Comparison of two sensor data fusion methods in a tightly coupled UWB/IMU 3-D localization system", 《2017 INTERNATIONAL CONFERENCE ON ENGINEERING, TECHNOLOGY AND INNOVATION》 *
KANG LI等: "Self-positioning for UAV indoor navigation based on 3D laser scanner, UWB and INS", 《2016 IEEE INTERNATIONAL CONFERENCE ON INFORMATION AND AUTOMATION》 *
曾庆化;王敬贤;孟骞;熊智;钱伟行;: "基于UWB优化配置的室内行人导航方法", 中国惯性技术学报, no. 02 *
熊骏;熊智;于永军;许建新;王云舒;: "超宽带测距辅助的无人机近距离相对导航方法", 中国惯性技术学报, no. 03 *
盛利;段斌;钟小鹏;: "基于UWB技术的无人机室内飞行测试平台设计", 传感器与微***, no. 06 *
陈泽;吴美平;陈昶昊;: "基于距离信息的行人协同导航研究", 导航与控制, no. 01 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113108785A (zh) * 2021-03-11 2021-07-13 中国电子科技集团公司第五十四研究所 一种面向同构imu的分布式协同互校准定位方法
CN113108785B (zh) * 2021-03-11 2022-06-10 中国电子科技集团公司第五十四研究所 一种面向同构imu的分布式协同互校准定位方法
CN114928881A (zh) * 2022-04-18 2022-08-19 上海大学 基于超宽带和视觉智能装置的协同定位***及定位方法
CN114928881B (zh) * 2022-04-18 2024-05-14 上海大学 基于超宽带和视觉智能装置的协同定位***及定位方法
CN117111126A (zh) * 2023-10-20 2023-11-24 成都格理特电子技术有限公司 基于北斗的石化工作人员联合定位方法和装置
CN117111126B (zh) * 2023-10-20 2023-12-22 成都格理特电子技术有限公司 基于北斗的石化工作人员联合定位方法和装置

Also Published As

Publication number Publication date
CN112068173B (zh) 2024-03-22

Similar Documents

Publication Publication Date Title
CN108226985B (zh) 基于精密单点定位的列车组合导航方法
US9784582B2 (en) Method and apparatus for navigation with nonlinear models
CN107710017A (zh) 用于在实时运动模式和相对定位模式之间切换的卫星导航接收器及方法
Han et al. Accurate absolute GPS positioning through satellite clock error estimation
CN112068173A (zh) 一种基于环路和积数据关联算法的协同导航方法
CN110095800A (zh) 一种多源融合的自适应容错联邦滤波组合导航方法
CN106405592B (zh) 车载北斗载波相位周跳检测与修复方法及***
CN102436004A (zh) 定位***及定位方法
CN101743453A (zh) 任务后高精确度定位和定向***
WO2022088018A1 (zh) 定位方法、装置、设备、***以及存储介质
CN108344415A (zh) 一种组合导航信息融合方法
CN111580144B (zh) 一种mins/gps超紧组合导航***设计方法
Wendel et al. Time-differenced carrier phase measurements for tightly coupled GPS/INS integration
CN110514203A (zh) 一种基于isr-ukf的水下组合导航方法
CN103017787A (zh) 适用于摇摆晃动基座的初始对准方法
CN116047567B (zh) 基于深度学习辅助的卫惯组合定位方法及导航方法
Iqbal et al. Pseudoranges error correction in partial GPS outages for a nonlinear tightly coupled integrated system
Skaloud Reducing the GPS ambiguity search space by including inertial data
CN115932923A (zh) 一种基于v2v的共享gnss车辆增强协作定位方法
Vana et al. Low-cost, dual-frequency PPP GNSS and MEMS-IMU integration performance in obstructed environments
Yi On improving the accuracy and reliability of GPS/INS-based direct sensor georeferencing
CN115220078A (zh) 基于载波相位差分的gnss高精度定位方法及导航方法
Hu et al. Performance evaluation of stereo vision aided loosely coupled GNSS/SINS integration for land vehicle navigation in different urban environments
Vana et al. Benefits of motion constraining for robust, low-cost, dual-frequency GNSS PPP+ MEMS IMU navigation
Iiyama et al. Terrestrial GPS time-differenced carrier-phase positioning of lunar surface users

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