CN109269499B - 一种基于相对导航的目标联合组网定位方法 - Google Patents

一种基于相对导航的目标联合组网定位方法 Download PDF

Info

Publication number
CN109269499B
CN109269499B CN201811047874.7A CN201811047874A CN109269499B CN 109269499 B CN109269499 B CN 109269499B CN 201811047874 A CN201811047874 A CN 201811047874A CN 109269499 B CN109269499 B CN 109269499B
Authority
CN
China
Prior art keywords
positioning
locator
matrix
person
slave
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
CN201811047874.7A
Other languages
English (en)
Other versions
CN109269499A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201811047874.7A priority Critical patent/CN109269499B/zh
Publication of CN109269499A publication Critical patent/CN109269499A/zh
Application granted granted Critical
Publication of CN109269499B publication Critical patent/CN109269499B/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
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/08—Position of single direction-finder fixed by determining direction of a plurality of spaced sources of known location

Landscapes

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

Abstract

本发明公开了一种基于相对导航的目标联合组网定位方法，该方法包括以下步骤：(1)计算从定位人员到主定位人员的信号方位角；(2)计算从定位人员到主定位人员的距离；(3)基于目标移动特性和联合组网定位建立运动模型和相对状态方程；(4)基于从定位人员到主定位人员的距离和信号方位角建立量测方程；(5)采用卡尔曼滤波方法计算从定位人员的位置坐标；(6)利用联合组网定位融合方法对主定位人员位置进行修正。

Description

一种基于相对导航的目标联合组网定位方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于相对导航的目标联合组网定位方法。
背景技术
在地震救援，反恐作战，野外搜救等突发事件的复杂环境下，行动人员常常需要确定自己的当前位置，并把定位信息及时传达给同伴及上级指挥者，以利于行动人员掌握自身位置状态，保障安全，也有利于指挥者全面了解所有人员的姿态，统筹下一步的行动计划。
传统的通过地图、指南针，加以观察地图环境的定位方法不那么准确，GPS模块往往也会因为没有信号无法发挥定位作用。解决方案从单纯的捷联惯导系统导航发展到捷联惯导系统加地磁信息系统，替代GPS组合的人员自主定位。随着导航地位时间增加，惯性导航系统误差将不断累积，通常通过滤波方法可以解决，或者采用联合分布式多平台惯性系统误差修正方法。
发明内容
发明目的：针对以上问题，本发明提出一种基于相对导航的目标联合组网定位方法，其目的在于不使用GPS条件下，通过组网定位的方法提高组网成员定位精度的一种途径，满足在山区、丛林、楼宇、洞穴等复杂作战环境中搜寻定位需要，为新型搜寻定位技术提供储备。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于相对导航的目标联合组网定位方法，该方法包括以下步骤：
(1)计算从定位人员到主定位人员的信号方位角；
(2)计算从定位人员到主定位人员的距离；
(3)基于目标移动特性和联合组网定位建立运动模型和相对状态方程；
(4)基于从定位人员到主定位人员的距离和信号方位角建立量测方程；
(5)采用卡尔曼滤波方法计算从定位人员的位置坐标；
(6)利用联合组网定位融合方法对主定位人员位置进行修正。
其中，步骤(1)中，计算从定位人员到主定位人员的信号方位角的方法如下：
(1.1)将N个待定位人员分为N-1个从定位人员和1个主定位人员，主定位人员ID号为m，1≤m≤N，从定位人员ID号为i，i＝1....N，且i≠m；
(1.2)tk时刻设S为从定位人员i发射装置发射的窄带信号，主定位人员m接受装置接收到数据为
Figure DEST_PATH_IMAGE002
设从定位人员i到主定位人员方位角
Figure DEST_PATH_IMAGE004
根据阵列公式得：
Figure DEST_PATH_IMAGE006
Figure DEST_PATH_IMAGE008
其中，N1、N2是两天线阵列接受的噪声，φ1为两个天线阵列相位差；
Figure DEST_PATH_IMAGE010
A为天线阵列的导向矢量阵且有：
Figure DEST_PATH_IMAGE012
Figure DEST_PATH_IMAGE014
j为虚数单位；
其中，f为电磁波频率，d为两天线阵列之间距离，c为光速；
将(1)和(2)式合并得：
Figure DEST_PATH_IMAGE016
其中，Xi为主定位人员接受从定位人员i的数据，
Figure DEST_PATH_IMAGE018
N为系统噪声；
得(4)式的协方差矩阵：
Figure DEST_PATH_IMAGE020
其中，RS为信号子空间协方差矩阵，RN为噪声协方差矩阵；
(1.3)对矩阵R进行特征分解，得到两个数据矩阵的信号子空间US1和US2；
R＝∑λieiei H≈USΣsUS H+UNΣNUN H (6)
Figure DEST_PATH_IMAGE022
其中，λi为R矩阵的特征值，ei为特征值对应的特征向量，Σs为最大特征值对应旋转不变矩阵，ΣN为噪声对应旋转不变矩阵，US为最大特征值对应的特征矢量生成的信号子空间，UN为最小特征值对应矢量生成的噪声子空间；
(1.4)求出旋转不变关系矩阵ΨLS，然后对其进行特征分解，其中最大的特征值对应的特征向量即为方向角：
Figure DEST_PATH_IMAGE024
Figure DEST_PATH_IMAGE026
其中，λi为矩阵ΨLS的特征值，γi为对应的特征向量，设最大特征值为λmax对应的特征向量为γmax，则tk时刻从定位人员i与主定位人员m之间的方位角
Figure DEST_PATH_IMAGE028
其中，步骤(2)中，计算从定位人员到主定位人员的距离方法如下：设T1时刻，从定位人员i向主定位人员m发送询问指令，T2时刻m收到询问指令，T3为m向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理计算距离
Figure DEST_PATH_IMAGE030
Figure DEST_PATH_IMAGE032
当T1＝tk时，从定位人员i和主定位人员m之间距离为
Figure DEST_PATH_IMAGE034
其中，步骤(3)中，基于目标移动特性和联合组网定位建立运动模型和相对状态方程，方法如下：记从定位人员i系统状态量Xi＝[xi,yi,vxi,vyi]T，噪声向量W(t)＝[wxi,wyi]T，其中，xi,yi为从定位人员i的二维坐标，vxi,vyi为从定位人员i的二维速度，wxi,wyi为从定位人员i二维方向上加速度噪声，设tk-1到tk时间间隔为T，则根据人员移动特性建立运动学模型为：
Figure DEST_PATH_IMAGE036
其中，xk，yk为tk时刻从定位人员的坐标，xk-1，yk-1为tk-1时刻从定位人员坐标位置，vxk，vyk为tk时刻从定位人员的速度，wx，wy为噪声加速度，vx(k-1)，vy(k-1)为tk-1时刻从定位人员的速度，写成矩阵形式：
Xk＝Fk|k-1Xk-1+Gk|k-1Wk-1 (11)
其中，Xk是tk时刻从定位人员状态向量，Wk-1为系统噪声向量；
Figure DEST_PATH_IMAGE038
从定位人员i和主定位人员m之间的相对状态方程为：
Figure DEST_PATH_IMAGE040
其中，
Figure DEST_PATH_IMAGE042
为tk时刻从定位人员i和主定位人员m之间相对状态向量，
Figure DEST_PATH_IMAGE044
为tk-1时刻从定位人员i和主定位人员m之间相对状态向量，
Figure DEST_PATH_IMAGE046
Figure DEST_PATH_IMAGE048
Figure DEST_PATH_IMAGE050
为tk时刻从定位人员i的状态向量，
Figure DEST_PATH_IMAGE052
为tk时刻主定位人员m的状态向量，
Figure DEST_PATH_IMAGE054
为tk-1时刻主定位人员m噪声矩阵，
Figure DEST_PATH_IMAGE056
为tk-1时刻从定位人员i噪声矩阵。
其中，步骤(4)中，基于从定位人员到主定位人员的距离和信号方位角建立量测方程，方法如下：
Figure DEST_PATH_IMAGE058
Figure DEST_PATH_IMAGE060
其中，
Figure DEST_PATH_IMAGE062
分别为tk时刻主定位人员m和从定位人员i之间的相对距离和方位角，
Figure DEST_PATH_IMAGE064
为tk时刻人员i相对于主定位人员的坐标差，ξr、ξθ为测距和方位的误差，将式(13)、(14)合并为式(15)：
Figure DEST_PATH_IMAGE066
其中，ΔZk为tk时刻实际量测矩阵，H1为转移矩阵，Vk为tk时刻量测噪声
且
Figure DEST_PATH_IMAGE068
其中，
Figure DEST_PATH_IMAGE070
为tk时刻主定位人员m和从定位人员i之间经量测得到的相对距离和方位角。
其中，步骤(5)中，采用卡尔曼滤波方法建立联合组网定位模型，方法如下：对(13)(14)进行一阶泰勒展开，得到转移矩阵H1为：
Figure DEST_PATH_IMAGE072
其中，
Figure DEST_PATH_IMAGE074
对于(12)(15)描述的线性离散线性系统，利用量测值ΔZk通过卡尔曼滤波融合方法对状态
Figure DEST_PATH_IMAGE076
进行估计，记初始相对状态向量为
Figure DEST_PATH_IMAGE078
P0＝I4×4为4阶单位矩阵，
Figure DEST_PATH_IMAGE080
Figure DEST_PATH_IMAGE082
为陀螺仪误差，
Figure DEST_PATH_IMAGE084
为加速度计误差，代入式(16)，(17)进行迭代：
时间更新
Figure DEST_PATH_IMAGE086
量测更新
Figure DEST_PATH_IMAGE088
其中，
Figure DEST_PATH_IMAGE090
为
Figure DEST_PATH_IMAGE092
的一步估计，I为单位矩阵，H1k为tk时刻H1阵，Pk|k-1为估计值
Figure DEST_PATH_IMAGE094
的均方误差阵，Pk为
Figure DEST_PATH_IMAGE096
均方误差阵，Pk-1为
Figure DEST_PATH_IMAGE098
均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为：
Figure DEST_PATH_IMAGE100
Figure DEST_PATH_IMAGE102
为陀螺仪误差，
Figure DEST_PATH_IMAGE104
为加速度计误差，
Figure DEST_PATH_IMAGE106
为噪声误差，经上述步骤可得tk时刻从定位人员i的相对于主定位人员m的相对状态向量
Figure DEST_PATH_IMAGE108
Figure DEST_PATH_IMAGE110
得tk时刻从定位人员i的状态向量：
Figure DEST_PATH_IMAGE112
其中，步骤(6)中，利用联合组网定位融合方法对主定位人员位置进行修正，方法如下：tk时刻主定位人员的坐标
Figure DEST_PATH_IMAGE114
为：
Figure DEST_PATH_IMAGE116
其中，
Figure DEST_PATH_IMAGE118
为tk时刻主定位人员坐标估计值，
Figure DEST_PATH_IMAGE120
为tk时刻主定位人员自身惯导系统获得坐标，
Figure DEST_PATH_IMAGE122
为tk时刻从定位人员i经过卡尔曼滤波所得坐标值，
Figure DEST_PATH_IMAGE124
为tk时刻从定位人员i相对主定位人员的位置偏移量；
Figure DEST_PATH_IMAGE126
其中，
Figure DEST_PATH_IMAGE128
分别为tk时刻主定位人员m和从定位人员i之间测量得到的相对距离和方位角度。
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
(1)利用无线通讯技术同惯导技术融合使得系统更加稳定；
(2)无线通讯中采用ESPRIT方法估计目标源方位和TOA技术测距具有较好的异步性和实用性；
(3)充分利用联合组网分布式特点增加定位精度；
(4)采用卡尔曼滤波融合方法提高了定位精度。
附图说明
图1为本发明使用的均匀线阵示意图；
图2为本发明使用的TOA测距示意图；
图3为本发明使用的方法流程图。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
将N个待定位人员分为N-1个从定位人员和1个主定位人员，各人员之间携带无线接收和发送装置，采用无线方式通讯，人员接受装置上采用ESPRIT方法估计发射信号的人员的方位角，采用TOA方法测距技术估算主从定位人员的距离，主定位人员ID号为m，1≤m≤N，从定位人员ID号为i，i＝1....N，且i≠m。
具体包括如下步骤：
一、估计从定位人员到主定位人员方位角
(1)tk时刻设S为从定位人员i发射装置发射的窄带信号，主定位人员m接受装置接收到数据为
Figure DEST_PATH_IMAGE130
设从定位人员i到主定位人员方位角
Figure DEST_PATH_IMAGE132
模型示意如图1所示。
根据阵列公式得：
Figure DEST_PATH_IMAGE134
Figure DEST_PATH_IMAGE136
式中，N1、N2是两天线阵列接受的噪声，φ1为两个天线阵列相差相位差。
Figure DEST_PATH_IMAGE138
A为天线阵列的导向矢量阵且有：
Figure DEST_PATH_IMAGE140
Figure DEST_PATH_IMAGE142
j为虚数单位。
f为电磁波频率，d为两天线阵列之间距离，c为光速。
将(1)和(2)式合并得：
Figure DEST_PATH_IMAGE144
其中，Xi为主定位人员接受从定位人员i的数据，
Figure DEST_PATH_IMAGE146
N为系统噪声。
可得(4)式的协方差矩阵：
Figure DEST_PATH_IMAGE148
其中，RS为信号子空间协方差矩阵，RN为噪声协方差矩阵。
(2)对矩阵R进行特征分解，从而得到两个数据矩阵的信号子空间US1和US2；
R＝∑λieiei H≈USΣsUS H+UNΣNUN H (6)
Figure DEST_PATH_IMAGE150
式中，λi为R矩阵的特征值，ei为特征值对应的特征向量，Σs为最大特征值对应旋转不变矩阵，ΣN为噪声对应旋转不变矩阵，US为最大特征值对应的特征矢量生成的信号子空间，UN为最小特征值对应矢量生成的噪声子空间。
(3)求出旋转不变关系矩阵ΨLS，然后对其进行特征分解，其中最大的特征值对应的特征向量即为方向角。
Figure DEST_PATH_IMAGE152
Figure DEST_PATH_IMAGE154
式中，λi为矩阵ΨLS的特征值，设最大特征值为λmax对应的特征向量为γmax，则tk时刻从定位人员i与主定位人员m之间的经测量得到的方位角
Figure DEST_PATH_IMAGE156
二、估计目标源距离
TOA具有精度高、测距耗时少、传输距离远等特点，本发明采用TOA测距技术，如图2所示。
从定位人员i和主定位人员m之间采用TOA技术相互测距，步骤如下，设T1时刻，i向m发送询问指令，T2时刻m收到询问指令，T3为m向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得，距离为
Figure DEST_PATH_IMAGE158
Figure DEST_PATH_IMAGE160
其中，C为光速，
C＝3×108m/s。
当T1＝tk时，从定位人员i和主定位人员m之间距离为
Figure DEST_PATH_IMAGE162
考虑到人员移动低速和联合组网定位具有分布式特点，建立相对状态模型和状态方程。
设定各定位人员直接可以进行相互自组网通讯，并且人员短时间按照匀速进行运动，记从定位人员i系统状态量Xi＝[xi,yi,vxi,vyi]T，噪声向量W(t)＝[wxi,wyi]T，其中，xi,yi为从定位人员i的二维坐标，vxi,vyi从定位人员i的二维速度，wxi,wyi为二维方向上加速度噪声，设tk-1到tk，时间间隔为T，则根据人员移动特性可得运动学模型为：
Figure DEST_PATH_IMAGE164
式中，xk，yk为tk时刻从定位人员的坐标，xk-1，yk-1为tk-1时刻从定位人员坐标位置，vxk，vyk为tk时刻从定位人员的速度，wx，wy为噪声加速度，vx(k-1)，vy(k-1)为tk-1时刻从定位人员的速度，写成矩阵形式：
Xk＝Fk|k-1Xk-1+Gk|k-1Wk-1 (11)
其中，Xk是tk时刻从定位人员状态向量，Wk-1为系统噪声向量。
Figure 699475DEST_PATH_IMAGE038
所以从定位人员i和主定位人员m之间的相对状态方程为：
Figure DEST_PATH_IMAGE166
其中，
Figure 616615DEST_PATH_IMAGE042
为tk时刻从定位人员i和主定位人员m之间相对状态向量，
Figure 396352DEST_PATH_IMAGE044
为tk-1时刻从定位人员i和主定位人员m之间相对状态向量，Fk|k-1，Gk|k-1如上述相同，
Figure DEST_PATH_IMAGE168
Figure DEST_PATH_IMAGE170
为tk时刻从定位人员i的状态向量，
Figure DEST_PATH_IMAGE172
为tk时刻主定位人员m的状态向量，
Figure DEST_PATH_IMAGE174
为tk-1时刻主定位人员m噪声矩阵，
Figure DEST_PATH_IMAGE176
为tk-1时刻从定位人员i噪声矩阵。
估计从定位人员i和主定位人员m之间相对坐标差，也根据相对距离和相对角度关系建立数学方程：
Figure DEST_PATH_IMAGE178
Figure DEST_PATH_IMAGE180
其中，
Figure DEST_PATH_IMAGE182
分别为tk时刻主定位人员m和从定位人员i之间的相对距离和方位角，
Figure DEST_PATH_IMAGE184
为tk时刻人员i相对于主定位人员的坐标差，ξr、ξθ为测距和方位误差。将式(13)、(14)合并为式(15)
Figure DEST_PATH_IMAGE186
其中，ΔZk为tk时刻实际量测距阵，H1为转移矩阵，Vk为tk时刻量测噪声且
Figure 791561DEST_PATH_IMAGE068
其中，
Figure DEST_PATH_IMAGE188
为tk时刻主定位人员m和从定位人员i之间经量测得到的相对距离和方位角。
为了消除定位过程中高斯白噪声影响，采用卡尔曼滤波方法建立联合组网定位模型，将距离、方位等参数传入模型，实现相对导航的目标联合组网定位，整个过程方法流程如图3所示。
式(12)、(15)即为相对组网定位状态方程和量测方程，考虑式(13)(14)非线性，对(13)(14)进行一阶泰勒展开，得到转移矩阵H1为：
Figure DEST_PATH_IMAGE190
其中，
Figure DEST_PATH_IMAGE192
对于(12)(15)描述的线性离散线性系统，可以利用量测值ΔZk通过卡尔曼滤波融合方法对状态
Figure DEST_PATH_IMAGE194
进行估计，记初始相对状态向量为
Figure DEST_PATH_IMAGE196
P0＝I4×4为4阶单位矩阵，
Figure DEST_PATH_IMAGE198
Figure DEST_PATH_IMAGE200
为陀螺仪误差，
Figure DEST_PATH_IMAGE202
为加速度计误差，代入式(16)，(17)进行迭代：
时间更新
Figure DEST_PATH_IMAGE204
量测更新
Figure DEST_PATH_IMAGE206
其中，
Figure DEST_PATH_IMAGE208
为
Figure DEST_PATH_IMAGE210
的一步估计，I为单位矩阵，H1k为tk时刻H1阵，Pk|k-1为估计值
Figure DEST_PATH_IMAGE212
的均方误差阵，Pk为
Figure DEST_PATH_IMAGE214
均方误差阵，Pk-1为
Figure DEST_PATH_IMAGE216
均方误差阵Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别：
Figure DEST_PATH_IMAGE218
其中，
Figure DEST_PATH_IMAGE220
为陀螺仪误差，
Figure DEST_PATH_IMAGE222
为加速度计误差，
Figure DEST_PATH_IMAGE224
为噪声误差。
经上述步骤可得tk时刻从定位人员i的相对于主定位人员m的准确相对状态向量
Figure DEST_PATH_IMAGE226
Figure DEST_PATH_IMAGE228
得tk时刻从定位人员i的状态向量
Figure DEST_PATH_IMAGE230
则再利用联合组网定位融合方法，对主定位人员m位置进行修正。
考虑到一套惯导多次启动，其定位误差是完全相互独立、随机的，服从正态分布当。针对联合分布式组网特点，可以通过对多套惯导位置输出采用融合的方式来提高其定位精度。
将各个从定位人员经过卡尔曼获得的位置输出信息加上相对主定位人员位置偏移量，相当于将各从定位人员惯导位置输出信息转移到主定位人员上，再与主定位人员自身惯导位置输出信息进行“加权平均”，即得到经过联合组网校正后的主定位人员m，tk时刻精确位置信息
Figure DEST_PATH_IMAGE232
Figure DEST_PATH_IMAGE234
主定位人员ID为m，从定位人员ID为i，人员数为N，其中，
Figure DEST_PATH_IMAGE236
为tk时刻主定位人员坐标估计值，
Figure DEST_PATH_IMAGE238
为tk时刻主定位人员自身惯导系统获得坐标，
Figure DEST_PATH_IMAGE240
为tk时刻从定位人员i经过卡尔曼滤波所得坐标值，
Figure DEST_PATH_IMAGE242
为tk时刻从定位人员i相对主定位人员的位置偏移量。
Figure DEST_PATH_IMAGE244
其中，
Figure DEST_PATH_IMAGE246
分别为tk时刻主定位人员m和从定位人员i之间测量得到的相对距离和方位角度。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (5)

1.一种基于相对导航的目标联合组网定位方法，其特征在于，该方法包括以下步骤：
(1)计算从定位人员到主定位人员的信号方位角；
(2)计算从定位人员到主定位人员的距离；
(3)基于目标移动特性和联合组网定位建立运动模型和相对状态方程；
(4)基于从定位人员到主定位人员的距离和信号方位角建立量测方程；
(5)采用卡尔曼滤波方法计算从定位人员的位置坐标；
(6)利用联合组网定位融合方法对主定位人员位置进行修正；
步骤(1)中，计算从定位人员到主定位人员的信号方位角的方法如下：
(1.1)将N个待定位人员分为N-1个从定位人员和1个主定位人员，主定位人员ID号为m，1≤m≤N，从定位人员ID号为i，i＝1....N，且i≠m；
(1.2)tk时刻设S为从定位人员i发射装置发射的窄带信号，主定位人员m接受装置接收到数据为
Figure FDA0003386461980000011
设从定位人员i到主定位人员m方位角
Figure FDA0003386461980000012
根据阵列公式得：
Figure FDA0003386461980000013
Figure FDA0003386461980000014
其中，N1、N2是两天线阵列接受的噪声，φ1为两个天线阵列相位差；
Figure FDA0003386461980000015
A为天线阵列的导向矢量阵且有：
Figure FDA0003386461980000016
Figure FDA0003386461980000017
j为虚数单位；
其中，f为电磁波频率，d为两天线阵列之间距离，c为光速；
将(1)和(2)式合并得：
Figure FDA0003386461980000021
其中，Xi为主定位人员接受从定位人员i的数据，
Figure FDA0003386461980000022
N为系统噪声；
(4)式的协方差矩阵：
Figure FDA0003386461980000023
其中，RS为信号子空间协方差矩阵，RN为噪声协方差矩阵；
(1.3)对矩阵R进行特征分解，得到两个数据矩阵的信号子空间US1和US2；
R＝∑λieiei H≈USΣsUS H+UNΣNUN H (6)
Figure FDA0003386461980000024
其中，λi为R矩阵的特征值，ei为特征值对应的特征向量，Σs为最大特征值对应旋转不变矩阵，ΣN为噪声对应旋转不变矩阵，US为最大特征值对应的特征矢量生成的信号子空间，UN为最小特征值对应矢量生成的噪声子空间；
(1.4)求出旋转不变关系矩阵ΨLS，然后对其进行特征分解，其中最大的特征值对应的特征向量即为方向角：
Figure FDA0003386461980000025
Figure FDA0003386461980000026
其中，λi为矩阵ΨLS的特征值，设最大特征值为λmax对应的特征向量为γmax，则tk时刻从定位人员i与主定位人员m之间的方位角
Figure FDA0003386461980000027
步骤(2)中，计算从定位人员到主定位人员的距离方法如下：设T1时刻，从定位人员i向主定位人员m发送询问指令，T2时刻m收到询问指令，T3为m向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理计算距离
Figure FDA0003386461980000028
Figure FDA0003386461980000029
当T1＝tk时，从定位人员i和主定位人员m之间距离为
Figure FDA0003386461980000031
2.根据权利要求1所述的一种基于相对导航的目标联合组网定位方法，其特征在于，步骤(3)中，基于目标移动特性和联合组网定位建立运动模型和相对状态方程，方法如下：记从定位人员i系统状态量Xi＝[xi,yi,vxi,vyi]T，噪声向量W(t)＝[wxi,wyi]T，其中，xi,yi为从定位人员i的二维坐标，vxi,vyi为从定位人员i的二维速度，wxi,wyi为从定位人员i二维方向上加速度噪声，设tk-1到tk时间间隔为T，则根据人员移动特性建立运动学模型为:
Figure FDA0003386461980000032
其中，xk，yk为tk时刻从定位人员的坐标，xk-1，yk-1为tk-1时刻从定位人员坐标位置，vxk，vyk为tk时刻从定位人员的速度，wx，wy为噪声加速度，vx(k-1)，vy(k-1)为tk-1时刻从定位人员的速度，写成矩阵形式：
Xk＝Fk|k-1Xk-1+Gk|k-1Wk-1 (11)
其中，Xk是tk时刻从定位人员状态向量，Wk-1为系统噪声向量；
Figure FDA0003386461980000033
从定位人员i和主定位人员m之间的相对状态方程为：
Figure FDA0003386461980000034
其中，
Figure FDA0003386461980000035
为tk时刻从定位人员i和主定位人员m之间相对状态向量，
Figure FDA0003386461980000036
为tk-1时刻从定位人员i和主定位人员m之间相对状态向量，
Figure FDA0003386461980000037
Figure FDA0003386461980000041
为tk时刻从定位人员i的状态向量，
Figure FDA0003386461980000042
为tk时刻主定位人员m的状态向量，
Figure FDA0003386461980000043
为tk-1时刻主定位人员m噪声矩阵，
Figure FDA0003386461980000044
为tk-1时刻从定位人员i噪声矩阵。
3.根据权利要求2所述的一种基于相对导航的目标联合组网定位方法，其特征在于，步骤(4)中，基于从定位人员到主定位人员的距离和信号方位角建立量测方程，方法如下：
Figure FDA0003386461980000045
Figure FDA0003386461980000046
其中，
Figure FDA0003386461980000047
分别为tk时刻主定位人员m和从定位人员i之间的相对距离和方位角，
Figure FDA0003386461980000048
为tk时刻从人员i相对于主定位人员m的坐标差，ξr、ξθ为测距和方位的误差，将式(13)、(14)合并为式(15)：
Figure FDA0003386461980000049
其中，ΔZk为tk时刻实际量测矩阵，H1为转移矩阵，Vk为tk时刻量测噪声且
Figure FDA00033864619800000410
其中，
Figure FDA00033864619800000411
为tk时刻主定位人员m和从定位人员i之间经量测得到的相对距离和方位角。
4.根据权利要求3所述的一种基于相对导航的目标联合组网定位方法，其特征在于，步骤(5)中，采用卡尔曼滤波方法建立联合组网定位模型，方法如下：对(13)(14)进行一阶泰勒展开，得到转移矩阵H1为：
Figure FDA00033864619800000412
其中，
Figure FDA00033864619800000413
对于(12)(15)描述的线性离散线性系统，利用量测值ΔZk通过卡尔曼滤波融合方法对状态
Figure FDA0003386461980000051
进行估计，记初始相对状态向量为
Figure FDA0003386461980000052
P0＝I4×4为4阶单位矩阵，
Figure FDA0003386461980000053
Figure FDA0003386461980000054
为陀螺仪误差，
Figure FDA0003386461980000055
为加速度计误差，代入式(16)，(17)进行迭代：
时间更新
Figure FDA0003386461980000056
量测更新
Figure FDA0003386461980000057
其中，
Figure FDA0003386461980000058
为
Figure FDA0003386461980000059
的一步估计，I为单位矩阵，H1k为tk时刻H1阵，Pk|k-1为估计值
Figure FDA00033864619800000510
的均方误差阵，Pk为
Figure FDA00033864619800000511
均方误差阵，Pk-1为
Figure FDA00033864619800000512
均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分
Figure FDA00033864619800000513
Figure FDA00033864619800000514
为陀螺仪误差，
Figure FDA00033864619800000515
为加速度计误差，
Figure FDA00033864619800000516
为噪声误差，经上述步骤可得tk时刻从定位人员i的相对于主定位人员m的相对状态向量
Figure FDA00033864619800000517
Figure FDA00033864619800000518
得tk时刻从定位人员i的状态向量：
Figure FDA00033864619800000519
5.根据权利要求4所述的一种基于相对导航的目标联合组网定位方法，其特征在于，步骤(6)中，利用联合组网定位融合方法对主定位人员位置进行修正，方法如下：tk时刻主定位人员的坐标
Figure FDA00033864619800000520
为：
Figure FDA00033864619800000521
其中，
Figure FDA00033864619800000522
为tk时刻主定位人员坐标估计值，
Figure FDA00033864619800000523
为tk时刻主定位人员自身惯导系统获得坐标，
Figure FDA0003386461980000061
为tk时刻从定位人员i经过卡尔曼滤波所得坐标值，
Figure FDA0003386461980000062
为tk时刻从定位人员i相对主定位人员的位置偏移量；
Figure FDA0003386461980000063
其中，
Figure FDA0003386461980000064
分别为tk时刻主定位人员m和从定位人员i之间测量得到的相对距离和方位角。
CN201811047874.7A 2018-09-07 2018-09-07 一种基于相对导航的目标联合组网定位方法 Active CN109269499B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811047874.7A CN109269499B (zh) 2018-09-07 2018-09-07 一种基于相对导航的目标联合组网定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811047874.7A CN109269499B (zh) 2018-09-07 2018-09-07 一种基于相对导航的目标联合组网定位方法

Publications (2)

Publication Number Publication Date
CN109269499A CN109269499A (zh) 2019-01-25
CN109269499B true CN109269499B (zh) 2022-06-17

Family

ID=65188230

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811047874.7A Active CN109269499B (zh) 2018-09-07 2018-09-07 一种基于相对导航的目标联合组网定位方法

Country Status (1)

Country Link
CN (1) CN109269499B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101001458A (zh) * 2006-06-23 2007-07-18 北京航空航天大学 基于相互测距信息的机群组网定位方法
WO2008063946A2 (en) * 2006-11-13 2008-05-29 Honeywell International Inc. Method and system for automatically estimating the spatial positions of cameras in a camera network
CN102135617A (zh) * 2011-01-06 2011-07-27 哈尔滨工程大学 双基地多输入多输出雷达多目标定位方法
CN103017771A (zh) * 2012-12-27 2013-04-03 杭州电子科技大学 一种静止传感器平台的多目标联合分配与跟踪方法
JP2014215086A (ja) * 2013-04-23 2014-11-17 株式会社豊田中央研究所 慣性航法システム、移動体端末、慣性航法装置、及びプログラム
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101001458A (zh) * 2006-06-23 2007-07-18 北京航空航天大学 基于相互测距信息的机群组网定位方法
WO2008063946A2 (en) * 2006-11-13 2008-05-29 Honeywell International Inc. Method and system for automatically estimating the spatial positions of cameras in a camera network
CN102135617A (zh) * 2011-01-06 2011-07-27 哈尔滨工程大学 双基地多输入多输出雷达多目标定位方法
CN103017771A (zh) * 2012-12-27 2013-04-03 杭州电子科技大学 一种静止传感器平台的多目标联合分配与跟踪方法
JP2014215086A (ja) * 2013-04-23 2014-11-17 株式会社豊田中央研究所 慣性航法システム、移動体端末、慣性航法装置、及びプログラム
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种机群相对定位方法;程承等;《火力与指挥控制》;20120331;第37卷(第3期);第13-15页 *

Also Published As

Publication number Publication date
CN109269499A (zh) 2019-01-25

Similar Documents

Publication Publication Date Title
AU2017233539B2 (en) Estimating locations of mobile devices in a wireless tracking system
US7821453B2 (en) Distributed iterative multimodal sensor fusion method for improved collaborative localization and navigation
CN111536967B (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
Fang et al. Graph optimization approach to range-based localization
CN107980100A (zh) 分布式定位系统和方法以及自定位设备
US11035915B2 (en) Method and system for magnetic fingerprinting
US6947880B2 (en) Method for improving accuracy of a velocity model
Xiong et al. Carrier-phase-based multi-vehicle cooperative positioning using V2V sensors
Ouyang et al. Cooperative navigation of UAVs in GNSS-denied area with colored RSSI measurements
CN109188352B (zh) 一种组合导航相对定位方法
Latif et al. Multi-robot synergistic localization in dynamic environments
Li et al. Cooperative visual-range-inertial navigation for multiple unmanned aerial vehicles
CN117320148A (zh) 多源数据融合定位方法、系统、电子设备及存储介质
CN109269499B (zh) 一种基于相对导航的目标联合组网定位方法
Xiao et al. Wision: Bolstering MAV 3d indoor state estimation by embracing multipath of wifi
Sun et al. Multi-robot range-only SLAM by active sensor nodes for urban search and rescue
CN116482735A (zh) 一种受限空间内外高精度定位方法
US10663557B2 (en) Method and system of geolocation by direct calculation of movement elements
Di Franco et al. Data fusion for relative localization of wireless mobile nodes
Schindler et al. A Relative Infrastructure-less Localization Algorithm for Decentralized and Autonomous Swarm Formation
Wang et al. Distributed collaborative navigation based on rank-defect free network
Yan et al. Cooperative navigation in unmanned surface vehicles with observability and trilateral positioning method
Yang et al. Relative navigation with displacement measurements and its absolute correction
Zhao et al. Collaborative Effects of mobile sensor network localization through distributed multimodal navigation sensor fusion
Wang et al. Maximum Correntropy Criterion-Based UKF for Loosely Coupling INS and UWB in Indoor Localization.

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