CN112946571A - 基于因子图的三维协同定位方法 - Google Patents

基于因子图的三维协同定位方法 Download PDF

Info

Publication number
CN112946571A
CN112946571A CN202110174519.1A CN202110174519A CN112946571A CN 112946571 A CN112946571 A CN 112946571A CN 202110174519 A CN202110174519 A CN 202110174519A CN 112946571 A CN112946571 A CN 112946571A
Authority
CN
China
Prior art keywords
node
nodes
information
factor graph
iteration
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
CN202110174519.1A
Other languages
English (en)
Other versions
CN112946571B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110174519.1A priority Critical patent/CN112946571B/zh
Publication of CN112946571A publication Critical patent/CN112946571A/zh
Application granted granted Critical
Publication of CN112946571B publication Critical patent/CN112946571B/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
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

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

Abstract

本发明提出一种基于因子图的三维协同定位方法，首先将协同定位网络的拓扑图映射到因子图中，构建待定位节点间的因子图和内部因子图的模型，然后利用和积算法的置信度信息传递准则，推导得到了内部因子图上各个变量节点和函数节点的参数计算表达式，解决了在三维空间下待定位节点间信息传递计算推导困难的问题，提高了算法的实用性与系统的定位精度。本发明计算复杂度小、通信开销小、收敛速度快，可有效解决以无人机为代表的高动态目标定位困难的问题。

Description

基于因子图的三维协同定位方法
技术领域
本发明是一种适用于以无人机为代表的三维空间下的协同定位方法，特别涉及一种基于因子图(Factor graph)模型辅助的三维协同定位(three-dimensionalcooperative positioning)方法。
背景技术
随着信息化技术的发展，人们对于位置服务的需求越来越高。无线传感器网络(Wireless Sensor Networks,WSN)技术因成本低廉、精度高、自组网管理、适应性强等优点受到国内外学者的广泛关注。
在无线传感器网络中，节点定位技术是其不可或缺的一部分。节点定位最为简单的方案是为网络中每个节点都安装全球定位系统(Global Navigation SatelliteSystem,GNSS)设备，以实现节点自动定位功能。GNSS是当前应用最多的定位系统，通过精确的同步时钟和观测量实现定位，其突出优点是具有全球全天候导航定位的能力。但是GNSS的弊病在于当定位环境中存在众多建筑物或其它障碍物时，卫星信号容易受到遮挡而衰弱，导致定位性能降低。此外，受成本、硬件设施体积等因素限制，GNSS难以应用于无线传感器网络中所有节点的定位。
节点定位算法依据数据获取来源可分为协同定位算法与非协同定位(传统定位)算法。非协同定位算法主要针对锚节点数量充足情况下目标的高精度定位。然而在实际应用场景中，并不是所有定位环境都能够满足这一前提条件，为此，针对病态无线环境下定位困难的问题，协同定位方案成为当前研究热门。协同定位的核心思想在于引入“协作”概念，网络中所有待定位节点可与通信范围内所有节点协作通信，邻居节点包含锚节点与其它待定位节点。
目前，在二维平面内，已经提出了很多适用于复杂定位场景的协同定位算法，这些算法能够很好的解决无线环境中锚节点稀疏、存在非视距误差等病态定位条件下定位困难的问题，有的算法也引入了概率图论知识，降低了系统的通信开销。但是在实际的三维环境中，受节点部署环境的影响，无线传感器网络呈现出三维分布、链路复杂等特点。实际应用场景中网络规模、节点密度比二维平面的定位场景更为复杂，网络的拓扑结构也更加多样化，同时因为维度的增加，系统的计算变的更加复杂，功耗与通信开销与定位精度更难同时兼顾，算法的设计难度不断增大。
发明内容
本发明的目的在于提供一种基于因子图的三维协同定位方法，这种基于因子图的三维协同定位方法计算复杂度小、通信开销小、收敛速度快，可有效解决以无人机为代表的高动态目标定位困难的问题。
本发明所提出的基于因子图的三维协同定位方法结合因子图图论模型能降低系统计算量的优点和分布式协同定位方法灵活性高通信开销较小的优点，为了提升协同网络的定位性能，本发明首先将协同定位网络的拓扑图映射到因子图中，构建待定位节点间的因子图和内部因子图的模型，然后利用和积算法的置信度信息传递准则，推导得到了内部因子图上各个变量节点和函数节点的参数计算表达式，解决了在三维空间下待定位节点间信息传递计算推导困难的问题，提高了算法的实用性与系统的定位精度。
本发明的技术方案为：
所述一种基于因子图的三维协同定位方法，包括以下步骤：
步骤1：构建协同定位网络，网络中包括已知精确位置信息的锚节点和带有位置模糊度的待定位节点；
步骤2：对于每个待定位节点，执行接收通信范围内邻居节点的位置信息，测量与邻居节点间的距离，得到测距误差的统计特性；并将其通信范围内的所有邻居节点映射到因子图中，构建该待定位节点对应的内部因子图；内部因子图中包括变量节点和函数节点；
步骤3：对于具体的待定位节点mq，通过循环迭代方式得到待定位节点mq的位置坐标信息；所述循环迭代，分为上行迭代与下行迭代：
首先待定位节点mq与通信范围内邻居节点的测距信息由函数节点进入该待定位节点的内部因子图；在上行迭代迭代过程中，通过函数节点依循和积算法的置信度信息传递准则更新变量节点的置信度信息；其次进行下行迭代为下一次的迭代更新做准备，最后得到待定位节点mq的位置坐标信息；在判定达到最大迭代次数收敛时，循环结束。
进一步的，步骤3中，对于具体的待定位节点mq，通信范围内有M+N个邻居节点，其中M个锚节点，N个待定位节点；
待定位节点mq的内部因子图中包含变量节点
Figure BDA0002939406950000031
di、Δxiq、Δyiq、Δziq、xq、yq和zq，函数节点Di、Ei、Ai、Bi和Ci，i＝1,2,…,M+N；
Figure BDA0002939406950000032
表示待定位节点mq与其通信范围内第i个邻居节点的测距信息；di基于函数节点Di产生，在数值上等于
Figure BDA0002939406950000033
Δxiq、Δyiq和Δziq表示待定位节点mq与第i个邻居节点在三个坐标轴上的相对距离；xq、yq和zq表示待定位节点mq的位置估计；Di表示测距信息的统计特性，服从均值为
Figure BDA0002939406950000034
方差为
Figure BDA0002939406950000035
的高斯分布；Ei用于转换xq坐标、yq坐标与zq坐标之间的置信度信息；Ai、Bi、Ci用来描述待定位节点mq相对于第i个邻居节点在三轴上的方向和距离。
进一步的，步骤3中，对于具体的待定位节点mq，采用以下过程进行循环迭代：
在进行第k次迭代时，将待定位节点mq与通信范围内第i个邻居节点之间的测距信息
Figure BDA0002939406950000036
送入mq与第i个邻居节点的内部因子图中对应支路的函数节点
Figure BDA0002939406950000037
得到对应变量节点
Figure BDA0002939406950000038
的置信度信息，上标k表示第k次迭代；
Figure BDA0002939406950000039
其中
Figure BDA00029394069500000310
服从均值为
Figure BDA00029394069500000311
方差为
Figure BDA00029394069500000312
的高斯分布，BI()表示置信度信息，置信度信息包括均值和方差，N()表示待定位节点与第i个邻居节点的测距信息统计特性分布；
步骤3.1：第k次上行迭代：
利用函数节点
Figure BDA00029394069500000313
更新变量节点
Figure BDA00029394069500000314
和
Figure BDA00029394069500000315
的置信度信息：
Figure BDA0002939406950000041
Figure BDA0002939406950000042
Figure BDA0002939406950000043
其中，
Figure BDA0002939406950000044
和
Figure BDA0002939406950000045
分别表示变量节点
Figure BDA0002939406950000046
和
Figure BDA0002939406950000047
的均值，
Figure BDA0002939406950000048
和
Figure BDA0002939406950000049
分别表示变量节点
Figure BDA00029394069500000410
和
Figure BDA00029394069500000411
的方差；
利用函数节点
Figure BDA00029394069500000412
和
Figure BDA00029394069500000413
实现相对位置与绝对位置信息的转换，更新变量节点
Figure BDA00029394069500000414
和
Figure BDA00029394069500000415
的置信度信息；根据邻居节点类型分为两种类型，一种是第i个邻居节点为锚节点(Xi,Yi,Zi)，另一种是第i个邻居节点是存在位置模糊度的其它待定位节点
Figure BDA00029394069500000416
待定位节点mq的邻居节点为锚节点时，函数节点
Figure BDA00029394069500000417
和
Figure BDA00029394069500000418
利用公式
Figure BDA00029394069500000419
Figure BDA00029394069500000420
Figure BDA00029394069500000421
更新变量节点
Figure BDA00029394069500000422
和
Figure BDA00029394069500000423
的置信度信息；邻居节点为其它待定位节点时的置信度信息，函数节点
Figure BDA00029394069500000424
和
Figure BDA00029394069500000425
利用公式
Figure BDA00029394069500000426
Figure BDA00029394069500000427
Figure BDA00029394069500000428
更新变量节点
Figure BDA0002939406950000051
和
Figure BDA0002939406950000052
的置信度信息；
步骤3.2：第k次下行迭代：
变量节点
Figure BDA0002939406950000053
基于置信度算法计算准则，更新函数节点Ai、Bi和Ci的置信度信息：
Figure BDA0002939406950000054
Figure BDA0002939406950000055
Figure BDA0002939406950000056
其中，
Figure BDA0002939406950000057
和
Figure BDA0002939406950000058
分别是关于变量
Figure BDA0002939406950000059
的高斯分布；
待定位节点mq的邻居节点为锚节点时，函数节点
Figure BDA00029394069500000510
和
Figure BDA00029394069500000511
按照公式
Figure BDA00029394069500000512
Figure BDA00029394069500000513
Figure BDA00029394069500000514
更新变量节点
Figure BDA00029394069500000515
和
Figure BDA00029394069500000516
的置信度信息；待定位节点mq的邻居节点为其它待定位节点时，函数节点
Figure BDA00029394069500000517
和
Figure BDA00029394069500000518
按照公式
Figure BDA00029394069500000519
Figure BDA00029394069500000520
Figure BDA00029394069500000521
更新变量节点
Figure BDA00029394069500000522
和
Figure BDA00029394069500000523
的置信度信息；其中
Figure BDA00029394069500000524
和
Figure BDA00029394069500000525
为变量节点
Figure BDA00029394069500000526
和
Figure BDA00029394069500000527
的均值，
Figure BDA00029394069500000528
和
Figure BDA00029394069500000529
为变量节点
Figure BDA00029394069500000530
和
Figure BDA00029394069500000531
的方差；
步骤3.3：依据公式
Figure BDA00029394069500000532
Figure BDA0002939406950000061
Figure BDA0002939406950000062
更新待定位节点mq的置信度信息。
进一步的，在协同定位网络中所有待定位节点并行完成上行迭代和下行迭代后，判断是否达到最大迭代次数完成收敛，如果达到最大迭代次数，本次循环结束，输出所有待定位节点的位置信息，否则继续并行执行循环迭代过程。
进一步的，M+N＞＝4。
进一步的，N()取高斯分布。
有益效果
本发明通过因子图建立了待定位节点置信度传递模型，推导完善了待定位节点内部因子图模型中变量节点与函数节点的信息更新计算公式，实现三维空间下基于因子图的协同定位算法设计。
本发明选用分布式协同定位方法，在定位环境极端恶劣或锚节点数目不足的情况下，仍然可通过与通信范围内邻居节点信息交互，实现待定位目标的定位，提升了协同定位系统的稳定性。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1是本发明的因子图模型；
圆圈表示变量节点；
方框表示函数节点。
图2是基于因子图的三维协同网络拓扑图。其中包含mn个待定位节点和ap个锚节点，红色链路表示锚节点a3与待定位节点m2间存在障碍物，属于非视距(NLOS)，黄色链路表示锚节点与待定位节点间视距连接，蓝色链路表示待定位节点间通信。本发明以网络中任意待定位节点mq为例进行说明。节点mq通信范围内邻居节点的集合为
Figure BDA0002939406950000071
其中
Figure BDA0002939406950000072
表示通信范围内与节点mq相连的M个锚节点的集合，
Figure BDA0002939406950000073
表示通信范围内与mq相连的N个待定位节点集合(M+N＞＝4)。
图3是mq节点的因子图模型。
Figure BDA0002939406950000074
表示第i个邻居节点到待定位节点mq的函数节点，其中i∈Cq，
Figure BDA0002939406950000075
函数节点用于定位信息的数据处理，图3中实线表示变量节点向函数节点提供的置信度信息，虚线表示邻居节点对待定位节点提供的位置估计信息。
图4是待定位节点mq的内部因子图置信度信息传递模型；
Figure BDA0002939406950000076
为变量节点，表示待定位节点mq与其通信范围内第i个邻居节点的测距信息；
Di为函数节点，表示测距信息的统计特性，服从均值为
Figure BDA0002939406950000077
方差为
Figure BDA0002939406950000078
的高斯分布；
di为变量节点，基于函数节点Di产生，数值上等于
Figure BDA0002939406950000079
Ei为函数节点，用于转换xq坐标、yq坐标与zq坐标之间的置信度信息，xq、yq和zq为变量节点，为待定位节点mq的位置估计值；
Δxiq、Δyiq、Δziq为变量节点，表示待定位节点mq与邻居节点i在三个坐标轴上的相对距离；
Ai、Bi、Ci为函数节点，用来描述相对于第i个邻居节点的方向和距离。
具体实施方式
针对现有协同定位方法大都停留在二维平面理论研究阶段，难以实际应用于高动态复杂定位环境，本发明研究了一种基于因子图的三维协同定位方法。该方法利用因子图与置信度传递算法，构建协同定位网络中待定位节点的内部因子图模型，推导得到了因子图上各个变量节点和函数节点的参数计算表达式，有效解决了在三维空间下节点间信息传递计算推导困难的问题。此外，在待定位节点位置更新过程中，相邻节点间仅需要传递均值向量与协方差矩阵信息，具有较低的通信开销和计算复杂度，提高了算法的实用性与系统的定位精度。
下面结合理论分析，论述本发明提出的基于因子图的三维协同定位方法。
一.因子图与和积算法
在协同定位技术中，基于贝叶斯估计的协同定位算法的关键在于计算位置变量的边缘后验概率密度函数(Probability Density Function,PDF)。针对PDF计算复杂的问题，本发明引入因子图图论的相关理论知识，有效的将复杂的全局函数分解为多个局部函数的积，提高系统的定位性能。下面将进一步介绍因子图和相应的置信度传递准则。
因子图(Factor Graph,FG)是图论模型的一种，其本质是通过图形的形式表示变量和函数之间的关系，将全局函数分解为多个局部函数乘积的形式。因子图中包含三种元素，分别是变量节点、函数节点与边。以图1为例，图中圆圈表示变量节点，方框表示函数节点，边用来连接函数节点与变量节点，全局函数f(x1,x2,x3,x4)可分解为多个局部函数乘积的形式，即f(x1,x2,x3,x4)＝f1(x1)f2(x1,x2)f3(x2,x3,x4)f4(x1,x4)。
为了更好的数学衡量因子图中各个节点的信息，常利用置信度传递算法实现因子图上信息的传递计算。因子图中节点的信息传递方式可分为两类，一类是函数节点到变量节点的信息，用μf→x表示，另一类是变量节点到函数节点的信息，用μx→f表示。具体的计算准则如公式(1)和(2)所示。
1.从函数节点fi到变量节点xi的信息计算：
Figure BDA0002939406950000081
2.从变量节点xi到函数节点fi的信息计算：
Figure BDA0002939406950000082
公式(1)中符号“X”表示与函数节点fi连接的所有变量节点的集合，符号“X\xi”表示集合X中除了变量节点xi以外的集合。公式(2)中符号“N”表示与变量节点xi连接所有函数节点的集合，符号“N\fi”表示集合N中除了函数节点fi以外其它函数节点的集合。
以图1为例，图1中函数节点f3到变量节点x2的信息可表示为：
Figure BDA0002939406950000094
变量节点x1到函数节点f1的信息可表示为：
Figure BDA0002939406950000091
二.基于因子图三维协同定位方法
以图2所示协同定位网络中的任意待定位节点mq为例，将协同定位网络中所有与待定位节点mq相邻的节点映射为节点因子图，其结构如图3所示，节点因子图中包括变量节点和函数节点。图3中实线表示变量节点向函数节点所提供的定位的信息，虚线表示邻居节点对待定位节点mq提供的位置估计信息，其中邻居节点为协同定位网络中与待定位节点mq通信范围内所有节点。本发明结合置信度算法的计算准则，通过置信度信息(BeliefInformation,BI)在待定位节点的内部因子图中进行信息的迭代传递求解，进而实现待定位节点的定位。
在基于因子图的三维协同定位方法中，为降低计算复杂度将三维定位问题转化为3个支路上的一维问题，在待定位节点的内部因子图上分为x坐标组、y坐标组和z坐标组。下面以协同定位网路中任意待定位节点mq为例，构建待定位节点mq的内部因子图，如图4所示。在待定位节点mq的内部因子图中，利用欧拉公式将xq坐标、yq坐标和zq坐标三部分联系起来，即
Figure BDA0002939406950000092
其中，Δxiq、Δyiq和Δziq分别表示待定位节点mq与邻居节点i在坐标轴上的相对距离。diq表示节点间的相对距离。引入相对距离约束关系如公式(6)或(7)所示，表示为：
Figure BDA0002939406950000093
Figure BDA0002939406950000101
公式(6)和公式(7)中(Xi,Yi,Zi)表示第i邻居节点为锚节点时的坐标值，
Figure BDA0002939406950000102
表示第i个邻居节点为带有位置模糊度的其它待定位节点，(xq,yq,zq)表示待定位节点节点mq的位置估计。在本发明提出的基于因子图的三维协同方法中，“置信度信息”在待定位节点内部因子图模型中的变量节点和函数节点上迭代传递，这里置信度信息表示均值与方差信息。
在本发明提出的基于因子图的三维协同定位方法中，待定位节点的定位过程主要可分为初始化和信息循环迭代两个过程。
首先给出锚节点的精确位置坐标和待定位节点的初始坐标。
在初始化完成后在待定位节点的内部因子图上进行函数节点与变量节点的置信度信息迭代传递，分为上行迭代和下行迭代，具体操作如下：
步骤1：构建协同定位网络，网络中包括已知精确位置信息的锚节点和带有位置模糊度的待定位节点。
对于每个待定位节点，将其通信范围内的所有邻居节点映射到因子图中，构建该待定位节点对应的内部因子图；内部因子图中包括变量节点和函数节点。
以网络中任意待定位节点mq为例，初始化mq的初始位置坐标和三轴位置模糊度方差
Figure BDA0002939406950000103
和
Figure BDA0002939406950000104
并将协同定位网络中待定位节点mq通信范围内的所有节点映射到因子图中，构建待定位节点mq节点因子图和内部因子图模型，如图3和图4所示。由图4可知，待定位节点mq通信范围内有M+N个邻居节点，内部因子图模型中包含变量节点
Figure BDA0002939406950000105
di、Δxiq、Δyiq、Δziq、xq、yq和zq，函数节点Di、Ei、Ai、Bi和Ci。其中，
Figure BDA0002939406950000106
表示待定位节点mq与其通信范围内第i个邻居节点的测距信息；di基于函数节点Di产生，在数值上等于
Figure BDA0002939406950000107
Δxiq、Δyiq和Δziq表示待定位节点mq与第i个邻居节点在三个坐标轴上的相对距离；xq、yq和zq表示待定位节点mq的位置估计；Di表示测距信息的统计特性，服从均值为
Figure BDA0002939406950000111
方差为
Figure BDA0002939406950000112
的高斯分布；Ei用于转换xq坐标、yq坐标与zq坐标之间的置信度信息；Ai、Bi、Ci用来描述待定位节点mq相对于第i个邻居节点在三轴上的方向和距离。
步骤2：在进行第k次迭代时，将待定位节点mq与通信范围内第i个邻居节点之间的测距信息
Figure BDA0002939406950000113
送入mq与第i个邻居节点的内部因子图中对应支路的函数节点
Figure BDA0002939406950000114
得到对应变量节点
Figure BDA0002939406950000115
的置信度信息。
Figure BDA0002939406950000116
公式(8)中
Figure BDA0002939406950000117
服从均值为
Figure BDA0002939406950000118
方差为
Figure BDA0002939406950000119
的高斯分布，BI()表示置信度信息，置信度信息包括均值和方差，N()表示待定位节点与第i个邻居节点的测距信息统计特性分布，本实施例中取高斯分布，k表示迭代次数；
步骤3：第k次上行迭代
步骤3.1：利用函数节点
Figure BDA00029394069500001110
更新变量节点
Figure BDA00029394069500001111
和
Figure BDA00029394069500001112
的置信度信息。
Figure BDA00029394069500001113
Figure BDA00029394069500001114
Figure BDA00029394069500001115
其中，
Figure BDA00029394069500001116
和
Figure BDA00029394069500001117
分别表示变量节点
Figure BDA00029394069500001118
和
Figure BDA00029394069500001119
的均值，
Figure BDA00029394069500001120
和
Figure BDA00029394069500001121
分别表示变量节点
Figure BDA00029394069500001122
和
Figure BDA00029394069500001123
的方差；
步骤3.2：利用函数节点
Figure BDA00029394069500001124
和
Figure BDA00029394069500001125
实现相对位置与绝对位置信息的转换，更新变量节点
Figure BDA0002939406950000121
和
Figure BDA0002939406950000122
的置信度信息。根据邻居节点类型可分为两种类型，一种是第i个邻居节为锚节点(Xi,Yi,Zi)，另一种是第i个邻居节点是存在位置模糊度的其它待定位节点
Figure BDA0002939406950000123
待定位节点mq的邻居节点为锚节点时，函数节点
Figure BDA0002939406950000124
和
Figure BDA0002939406950000125
利用公式(12)(13)(14)更新变量节点
Figure BDA0002939406950000126
和
Figure BDA0002939406950000127
的置信度信息，表示为：
Figure BDA0002939406950000128
Figure BDA0002939406950000129
Figure BDA00029394069500001210
同理可得邻居节点为其它待定位节点时的置信度信息，函数节点
Figure BDA00029394069500001211
和
Figure BDA00029394069500001212
利用公式(15)(16)(17)更新变量节点
Figure BDA00029394069500001213
和
Figure BDA00029394069500001214
的置信度信息，表示为：
Figure BDA00029394069500001215
Figure BDA00029394069500001216
Figure BDA00029394069500001217
步骤4：第k次下行迭代，下行迭代的主要作用是为下一次信息的迭代更新做准备步骤4.1：变量节点
Figure BDA00029394069500001218
基于置信度算法计算准则，更新函数节点Ai、Bi和Ci的置信度信息，表示为：
Figure BDA00029394069500001219
Figure BDA00029394069500001220
Figure BDA00029394069500001221
其中，
Figure BDA00029394069500001222
和
Figure BDA00029394069500001223
分别是关于变量
Figure BDA00029394069500001224
的高斯分布。结合多个高斯分布的变量相乘后仍然服从高斯分布的特性，以某变量x'为例，x'服从均值为mu，方差为
Figure BDA0002939406950000131
的高斯分布，U个变量x'相乘时正比于均值为mΛ，方差为
Figure BDA0002939406950000132
的高斯分布，表示为：
Figure BDA0002939406950000133
Figure BDA0002939406950000134
Figure BDA0002939406950000135
步骤4.2：待定位节点mq的邻居节点为锚节点时，函数节点
Figure BDA0002939406950000136
和
Figure BDA0002939406950000137
按照公式(24)(25)(26)更新变量节点
Figure BDA0002939406950000138
和
Figure BDA0002939406950000139
的置信度信息。待定位节点mq的邻居节点为其它待定位节点时，函数节点
Figure BDA00029394069500001310
和
Figure BDA00029394069500001311
按照公式(27)(28)(29)更新变量节点
Figure BDA00029394069500001312
和
Figure BDA00029394069500001313
的置信度信息。
Figure BDA00029394069500001314
Figure BDA00029394069500001315
Figure BDA00029394069500001316
Figure BDA00029394069500001317
Figure BDA00029394069500001318
Figure BDA00029394069500001319
公式(24)至(29)中，
Figure BDA00029394069500001320
和
Figure BDA00029394069500001321
为变量节
Figure BDA00029394069500001322
和
Figure BDA00029394069500001323
的均值，
Figure BDA00029394069500001324
和
Figure BDA00029394069500001325
为变量节
Figure BDA00029394069500001326
和
Figure BDA00029394069500001327
的方差。
步骤5：依据公式(30)(31)(32)更新待定位节点mq的置信度信息
Figure BDA00029394069500001328
Figure BDA00029394069500001329
Figure BDA0002939406950000141
步骤6：在协同定位网络中所有待定位节点并行完成上行迭代和下行迭代后，判断是否达到最大迭代次数完成收敛，如果达到最大迭代次数，本次循环结束，输出所有待定位节点的位置信息，否则返回步骤2继续并行执行。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (6)

1.一种基于因子图的三维协同定位方法，其特征在于：包括以下步骤：
步骤1：构建协同定位网络，网络中包括已知精确位置信息的锚节点和带有位置模糊度的待定位节点；
步骤2：对于每个待定位节点，执行接收通信范围内邻居节点的位置信息，测量与邻居节点间的距离，得到测距误差的统计特性；并将其通信范围内的所有邻居节点映射到因子图中，构建该待定位节点对应的内部因子图；内部因子图中包括变量节点和函数节点；
步骤3：对于具体的待定位节点mq，通过循环迭代方式得到待定位节点mq的位置坐标信息；所述循环迭代，分为上行迭代与下行迭代：
首先待定位节点mq与通信范围内邻居节点的测距信息由函数节点进入该待定位节点的内部因子图；在上行迭代迭代过程中，通过函数节点依循和积算法的置信度信息传递准则更新变量节点的置信度信息；其次进行下行迭代为下一次的迭代更新做准备，最后得到待定位节点mq的位置坐标信息；在判定达到最大迭代次数收敛时，循环结束。
2.根据权利要求1所述一种基于因子图的三维协同定位方法，其特征在于：步骤3中，对于具体的待定位节点mq，通信范围内有M+N个邻居节点，其中M个锚节点，N个待定位节点；
待定位节点mq的内部因子图中包含变量节点
Figure FDA0002939406940000011
di、Δxiq、Δyiq、Δziq、xq、yq和zq，函数节点Di、Ei、Ai、Bi和Ci，i＝1,2,…,M+N；
Figure FDA0002939406940000012
表示待定位节点mq与其通信范围内第i个邻居节点的测距信息；di基于函数节点Di产生，在数值上等于
Figure FDA0002939406940000013
Δxiq、Δyiq和Δziq表示待定位节点mq与第i个邻居节点在三个坐标轴上的相对距离；xq、yq和zq表示待定位节点mq的位置估计；Di表示测距信息的统计特性，服从均值为
Figure FDA0002939406940000014
方差为
Figure FDA0002939406940000015
的高斯分布；Ei用于转换xq坐标、yq坐标与zq坐标之间的置信度信息；Ai、Bi、Ci用来描述待定位节点mq相对于第i个邻居节点在三轴上的方向和距离。
3.根据权利要求2所述一种基于因子图的三维协同定位方法，其特征在于：步骤3中，对于具体的待定位节点mq，采用以下过程进行循环迭代：
在进行第k次迭代时，将待定位节点mq与通信范围内第i个邻居节点之间的测距信息
Figure FDA0002939406940000021
送入mq与第i个邻居节点的内部因子图中对应支路的函数节点
Figure FDA0002939406940000022
得到对应变量节点
Figure FDA0002939406940000023
的置信度信息，上标k表示第k次迭代；
Figure FDA0002939406940000024
其中
Figure FDA0002939406940000025
服从均值为
Figure FDA0002939406940000026
方差为
Figure FDA0002939406940000027
的高斯分布，BI()表示置信度信息，置信度信息包括均值和方差，N()表示待定位节点与第i个邻居节点的测距信息统计特性分布；
步骤3.1：第k次上行迭代：
利用函数节点
Figure FDA0002939406940000028
更新变量节点
Figure FDA0002939406940000029
和
Figure FDA00029394069400000210
的置信度信息：
Figure FDA00029394069400000211
Figure FDA00029394069400000212
Figure FDA00029394069400000213
其中，
Figure FDA00029394069400000214
和
Figure FDA00029394069400000215
分别表示变量节点
Figure FDA00029394069400000216
和
Figure FDA00029394069400000217
的均值，
Figure FDA00029394069400000218
和
Figure FDA00029394069400000219
分别表示变量节点
Figure FDA00029394069400000220
和
Figure FDA00029394069400000221
的方差；
利用函数节点
Figure FDA00029394069400000222
和
Figure FDA00029394069400000223
实现相对位置与绝对位置信息的转换，更新变量节点
Figure FDA00029394069400000224
和
Figure FDA00029394069400000225
的置信度信息；根据邻居节点类型分为两种类型，一种是第i个邻居节点为锚节点(Xi,Yi,Zi)，另一种是第i个邻居节点是存在位置模糊度的其它待定位节点
Figure FDA00029394069400000226
待定位节点mq的邻居节点为锚节点时，函数节点
Figure FDA00029394069400000227
和
Figure FDA00029394069400000228
利用公式
Figure FDA0002939406940000031
Figure FDA0002939406940000032
Figure FDA0002939406940000033
更新变量节点
Figure FDA0002939406940000034
和
Figure FDA0002939406940000035
的置信度信息；邻居节点为其它待定位节点时的置信度信息，函数节点
Figure FDA0002939406940000036
和
Figure FDA0002939406940000037
利用公式
Figure FDA0002939406940000038
Figure FDA0002939406940000039
Figure FDA00029394069400000310
更新变量节点
Figure FDA00029394069400000311
和
Figure FDA00029394069400000312
的置信度信息；
步骤3.2：第k次下行迭代：
变量节点
Figure FDA00029394069400000313
基于置信度算法计算准则，更新函数节点Ai、Bi和Ci的置信度信息：
Figure FDA00029394069400000314
Figure FDA00029394069400000315
Figure FDA00029394069400000316
其中，
Figure FDA00029394069400000317
和
Figure FDA00029394069400000318
分别是关于变量
Figure FDA00029394069400000319
的高斯分布；
待定位节点mq的邻居节点为锚节点时，函数节点
Figure FDA00029394069400000320
和
Figure FDA00029394069400000321
按照公式
Figure FDA00029394069400000322
Figure FDA00029394069400000323
Figure FDA00029394069400000324
更新变量节点
Figure FDA0002939406940000041
和
Figure FDA0002939406940000042
的置信度信息；待定位节点mq的邻居节点为其它待定位节点时，函数节点
Figure FDA0002939406940000043
和
Figure FDA0002939406940000044
按照公式
Figure FDA0002939406940000045
Figure FDA0002939406940000046
Figure FDA0002939406940000047
更新变量节点
Figure FDA0002939406940000048
和
Figure FDA0002939406940000049
的置信度信息；其中
Figure FDA00029394069400000410
和
Figure FDA00029394069400000411
为变量节点
Figure FDA00029394069400000412
和
Figure FDA00029394069400000413
的均值，
Figure FDA00029394069400000414
和
Figure FDA00029394069400000415
为变量节点
Figure FDA00029394069400000416
和
Figure FDA00029394069400000417
的方差；
步骤3.3：依据公式
Figure FDA00029394069400000418
Figure FDA00029394069400000419
Figure FDA00029394069400000420
更新待定位节点mq的置信度信息。
4.根据权利要求3所述一种基于因子图的三维协同定位方法，其特征在于：在协同定位网络中所有待定位节点并行完成上行迭代和下行迭代后，判断是否达到最大迭代次数完成收敛，如果达到最大迭代次数，本次循环结束，输出所有待定位节点的位置信息，否则继续并行执行循环迭代过程。
5.根据权利要求3所述一种基于因子图的三维协同定位方法，其特征在于：
M+N＞＝4。
6.根据权利要求3所述一种基于因子图的三维协同定位方法，其特征在于：N()取高斯分布。
CN202110174519.1A 2021-02-08 2021-02-08 基于因子图的三维协同定位方法 Active CN112946571B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110174519.1A CN112946571B (zh) 2021-02-08 2021-02-08 基于因子图的三维协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110174519.1A CN112946571B (zh) 2021-02-08 2021-02-08 基于因子图的三维协同定位方法

Publications (2)

Publication Number Publication Date
CN112946571A true CN112946571A (zh) 2021-06-11
CN112946571B CN112946571B (zh) 2023-09-08

Family

ID=76244384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110174519.1A Active CN112946571B (zh) 2021-02-08 2021-02-08 基于因子图的三维协同定位方法

Country Status (1)

Country Link
CN (1) CN112946571B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106341886A (zh) * 2016-10-20 2017-01-18 河海大学常州校区 基于序贯一致加权和积算法的协作定位方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法
AU2020103330A4 (en) * 2020-11-09 2021-01-21 Southwest University distributed adaptive quantization estimation algorithm
CN112325885A (zh) * 2020-10-30 2021-02-05 哈尔滨工业大学 一种基于数学统计特征的因子图协同定位算法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106341886A (zh) * 2016-10-20 2017-01-18 河海大学常州校区 基于序贯一致加权和积算法的协作定位方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法
CN112325885A (zh) * 2020-10-30 2021-02-05 哈尔滨工业大学 一种基于数学统计特征的因子图协同定位算法
AU2020103330A4 (en) * 2020-11-09 2021-01-21 Southwest University distributed adaptive quantization estimation algorithm

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
崔建华;王忠勇;张传宗;张园园;: "基于因子图和联合消息传递的无线网络协作定位算法", 计算机应用, no. 05 *
张静娴;檀童和;: "结合因子图的定位算法", 数据通信, no. 02 *

Also Published As

Publication number Publication date
CN112946571B (zh) 2023-09-08

Similar Documents

Publication Publication Date Title
Zhu et al. Adapted error map based mobile robot UWB indoor positioning
CN107592671B (zh) 一种网络化多智能体主动变拓扑的自主协同定位方法
CN107315171B (zh) 一种雷达组网目标状态与系统误差联合估计算法
US7460976B2 (en) Semi-definite programming method for ad hoc network node localization
Xiong et al. Cooperative positioning for low-cost close formation flight based on relative estimation and belief propagation
CN113115205B (zh) 一种基于角度测量的分布式协作定位方法
Tang et al. GNSS/inertial navigation/wireless station fusion UAV 3-D positioning algorithm with urban canyon environment
CN111709438B (zh) 一种异构传感器信息融合方法
CN111928851B (zh) 基于tma技术的多自主水下机器人集群协同导航方法
Taghavi et al. Bias estimation for practical distributed multiradar-multitarget tracking systems
CN110779544B (zh) 一种多无人机自定位和对目标定位的双任务深度配合方法
Wang et al. On the use of calibration emitters for TDOA source localization in the presence of synchronization clock bias and sensor location errors
Zhang et al. Gaussian parameterized information aided distributed cooperative underwater positioning algorithm
CN112946571B (zh) 基于因子图的三维协同定位方法
Ferraz et al. Node localization based on distributed constrained optimization using Jacobi's method
CN114994600B (zh) 基于高度辅助的大规模矿井下用户三维实时定位方法
CN115603845B (zh) 一种基于一致性策略的最优分布式滤波器协同目标跟踪方法
CN106792982B (zh) 一种基于自适应分簇策略的多目标直接定位方法
Destiarti et al. Modified iterated extended Kalman filter for mobile cooperative tracking system
Luo et al. Distributed joint robust filter for WSNs-based target localization against non-Gaussian noises
Kang et al. Robust localization system using vector combination in wireless sensor networks
Xingxing et al. Weighted factor graph aided distributed cooperative position algorithm
Tang et al. Factor-graph-aided three-dimensional faster cooperative positioning algorithm
Fang et al. Model-free approach for sensor network localization with noisy distance measurement
Ramdani et al. Towards robust methods for indoor localization using interval data

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