CN116380130A - 一种基于置信度传播的分布式协同导航定位精度评价方法 - Google Patents

一种基于置信度传播的分布式协同导航定位精度评价方法 Download PDF

Info

Publication number
CN116380130A
CN116380130A CN202310518820.9A CN202310518820A CN116380130A CN 116380130 A CN116380130 A CN 116380130A CN 202310518820 A CN202310518820 A CN 202310518820A CN 116380130 A CN116380130 A CN 116380130A
Authority
CN
China
Prior art keywords
node
state quantity
moment
positioning accuracy
navigation positioning
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.)
Pending
Application number
CN202310518820.9A
Other languages
English (en)
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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN202310518820.9A priority Critical patent/CN116380130A/zh
Publication of CN116380130A publication Critical patent/CN116380130A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computing Systems (AREA)
  • Manufacturing & Machinery (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于置信度传播的分布式协同导航定位精度评价方法，步骤为：S1、构建单个节点在当前时刻的因子图模型，S2、获得节点i在当前时刻状态量的置信度表达式，S3、确定节点i在当前时刻状态量的协方差矩阵，以得到节点i的分布式协同导航定位精度；该基于置信度传播的分布式协同导航定位精度评价方法基于置信度传播理论，并综合考虑测距误差和协同终端的位置误差对分布式协同导航定位精度的影响，解决了现有的分布式协同导航定位精度建模方法存在着未充分考虑测距误差和协同终端的位置误差的影响、建模精度差的问题；经过实验验证，该方法精度高、操作简便、具有较好的实用性。

Description

一种基于置信度传播的分布式协同导航定位精度评价方法
技术领域
本发明涉及多智能体协同导航与定位技术领域，特别涉及一种基于置信度传播的分布式协同导航定位精度评价方法。
背景技术
智能无人集群系统由无人机、无人车、机器人等多智能体通过彼此之间的信息交互构成，可以智能协同控制，完成单个无人系统无法实现的复杂工作，在军事和民用领域具有广阔应用前景。协同导航技术是一项旨在多智能体系统下提高个体定位与导航精度的关键技术。在多智能体系统中，通过系统中各个子节点之间的导航状态共享与相互观测的方式来为子节点提供更丰富的观测信息，各个节点相互协同，可实现在低成本条件下的导航定位精度提升。
目前，协同导航的方案可以分为集中式定位和分布式定位两类算法。在集中式定位中，由一个集中的计算单元收集所有终端的量测数据进行统一计算，并同时估计出多个终端的坐标，该方法存在着系统计算开销过大、稳定性差的问题。分布式定位将各个终端互相作为协同节点，在每次迭代中终端利用相邻节点的信息计算并更新自身导航参数，因此，相比于集中式定位具有较低的计算复杂度和通信负荷，是目前协同导航技术中的主流方案。
协同导航定位精度评价是设计协同导航算法以及衡量所设计的协同导航算法是否合格的重要步骤；然而，分布式协同定位算法的精度不仅与测距误差有关，还与协同终端的位置误差有关。因此，基于上述描述，分布式协同导航定位精度建模较为复杂，将现有的导航定位精度建模方法直接用于分布式协同导航定位精度建模中，必然存在着因未充分考虑测距误差和协同终端的位置误差的影响，导致的建模精度差的问题，因而现有的导航定位精度评价方法不适用于分布式协同导航定位精度的评价。
发明内容
本发明的目的是提供一种在分布式协同导航定位精度建模过程中兼具考虑测距误差和协同终端的位置误差的影响，以解决定位精度建模精度差的问题的基于置信度传播的分布式协同导航定位精度评价方法。
为此，本发明技术方案如下：
一种基于置信度传播的分布式协同导航定位精度评价方法，步骤如下：
S1、构建单个节点在当前时刻的因子图模型：对于任一节点i，设定先验因子
Figure SMS_1
和相对测距因子/>
Figure SMS_5
，并定义：节点i在初始时刻的状态量/>
Figure SMS_8
通过先验因子/>
Figure SMS_2
与当前时刻的状态量/>
Figure SMS_4
连接，从而将初始时刻状态量/>
Figure SMS_7
传递给当前时刻状态量/>
Figure SMS_10
；节点i在当前时刻的状态量/>
Figure SMS_3
与相对测距因子/>
Figure SMS_6
连接，从而将相邻节点与节点i的相对测距信息传输给节点i在当前时刻状态量/>
Figure SMS_9
；
S2、获得节点i在当前时刻状态量的置信度表达式：
Figure SMS_11
Figure SMS_12
，
式中，
Figure SMS_14
为节点在当前时刻（t=k）状态量/>
Figure SMS_17
的置信度；/>
Figure SMS_20
为先验因子/>
Figure SMS_15
的协方差矩阵；/>
Figure SMS_16
为节点i基于前一时刻的状态量/>
Figure SMS_19
获得的当前时刻的状态量的预测值；/>
Figure SMS_22
为相对测距因子/>
Figure SMS_13
的协方差矩阵；/>
Figure SMS_18
为当前时刻的相对测距信息测量值；
Figure SMS_21
为当前时刻的相对测距信息观测值；
S3、确定节点i在当前时刻状态量的协方差矩阵，以得到节点i的分布式协同导航定位精度；该步骤S3的具体步骤为：
S301、基于概率密度标准形式，定义节点i在当前时刻状态量
Figure SMS_23
的概率密度表达式为：
Figure SMS_24
，
式中，
Figure SMS_25
为节点当前状态/>
Figure SMS_26
的均值；/>
Figure SMS_27
为比例因子，其取值为常数；/>
Figure SMS_28
为节点i在当前时刻状态量/>
Figure SMS_29
的协方差矩阵；
S302、将节点i在当前时刻状态量
Figure SMS_30
的置信度表达式与概率密度表达式进行联立，即：/>
Figure SMS_31
，经整理得到节点i在当前时刻状态量的协方差矩阵/>
Figure SMS_32
，其表达式为：
Figure SMS_33
，
式中，
Figure SMS_35
，/>
Figure SMS_39
；/>
Figure SMS_43
为当前时刻下/>
Figure SMS_37
的逆矩阵；/>
Figure SMS_40
为
Figure SMS_44
的逆矩阵；/>
Figure SMS_46
的递推公式为：/>
Figure SMS_34
，其中，/>
Figure SMS_38
为当前时刻下先验因子
Figure SMS_42
的协方差矩阵，/>
Figure SMS_45
为前一时刻下先验因子/>
Figure SMS_36
的协方差矩阵；/>
Figure SMS_41
的表达式为：
Figure SMS_47
，/>
Figure SMS_48
为前一时刻的速率观测值；/>
Figure SMS_49
为航位推算解算周期；/>
Figure SMS_50
为当前时刻的航向角；/>
Figure SMS_51
的表达式为：
Figure SMS_52
，/>
Figure SMS_53
为航位推算解算周期；/>
Figure SMS_54
为陀螺仪角速率随机噪声；/>
Figure SMS_55
为UWB的相对测距误差噪声；
S303、由步骤S302计算得到的节点i在当前时刻状态量
Figure SMS_56
的协方差矩阵/>
Figure SMS_57
的模
Figure SMS_58
，即为节点i在当前时刻的分布式协同导航定位精度结果。
进一步地，在步骤S302中，
Figure SMS_59
的表达式为：/>
Figure SMS_60
)，/>
Figure SMS_61
为100 (°/h3/2)。
进一步地，在步骤S302中，
Figure SMS_62
的表达式为：/>
Figure SMS_63
)，/>
Figure SMS_64
为0.1 m2。
与现有技术相比，该基于置信度传播的分布式协同导航定位精度评价方法基于置信度传播理论，并综合考虑测距误差和协同终端的位置误差对分布式协同导航定位精度的影响，解决了现有的分布式协同导航定位精度建模方法存在着未充分考虑测距误差和协同终端的位置误差的影响、建模精度差的问题；经过实验验证，该方法精度高、操作简便、具有较好的实用性。
附图说明
图1为本发明的基于置信度传播的分布式协同导航定位精度评价方法的流程图；
图2为本发明的基于置信度传播的分布式协同导航定位精度评价方法的步骤S1中构建单个节点当前时刻的因子图模型；
图3为本发明的实施例中设置的仿真场景的示意图。
具体实施方式
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
本申请的基于置信度传播的分布式协同导航定位精度评价方法为基于图优化的通信受限环境下协同导航方法（以下简称为协同导航方法）提出导航定位精度评价方法；上述协同导航方法具体记载于已公开专利CN114838732A中。
参见图1，该基于置信度传播的分布式协同导航定位精度评价方法的具体实施步骤如下：
S1、构建单个节点在当前时刻的因子图模型：
对于任一节点i，设定先验因子
Figure SMS_65
和相对测距因子/>
Figure SMS_66
，并定义：
节点i在初始时刻（t=0）的状态量
Figure SMS_67
通过先验因子/>
Figure SMS_68
与当前时刻（t=k）的状态量/>
Figure SMS_69
连接，从而将初始时刻状态量/>
Figure SMS_70
传递给当前时刻状态量/>
Figure SMS_71
；
节点i在当前时刻（t=k）的状态量
Figure SMS_72
与相对测距因子/>
Figure SMS_73
连接，从而将相邻节点与节点i的相对测距信息传输给节点i在当前时刻状态量/>
Figure SMS_74
；
节点i的相邻节点是指处于协同导航系统中除节点i以外的全部其它节点。
如图2所示为基于节点i构建单个节点在当前时刻的因子图模型的示意图。
S2、获得节点i在当前时刻状态量的置信度表达式；
根据置信度传播理论，节点i在当前时刻状态量
Figure SMS_75
的置信度为：
Figure SMS_76
，
式中，
Figure SMS_77
为节点在当前时刻（t=k）状态量/>
Figure SMS_78
的置信度，/>
Figure SMS_79
为初始时刻状态量/>
Figure SMS_80
传递给当前时刻（t=k）状态量/>
Figure SMS_81
的置信度连接关系，/>
Figure SMS_82
为相邻节点与所构建单个节点的相对测距信息传输给当前时刻状态量/>
Figure SMS_83
的置信度连接关系；
其中，
Figure SMS_84
的展开式为：
Figure SMS_85
，
式中，
Figure SMS_86
为先验因子/>
Figure SMS_87
的协方差矩阵；/>
Figure SMS_88
为节点i基于前一时刻（t=k-1）的状态量/>
Figure SMS_89
获得的当前时刻（t=k）的状态量的预测值，该预测值的具体通过前述协同导航方法获得，即前述协同导航方法最终经优化得到的导航结果；
Figure SMS_90
的展开式为：
Figure SMS_91
，
式中，
Figure SMS_92
为相对测距因子/>
Figure SMS_93
的协方差矩阵；/>
Figure SMS_94
为当前时刻的相对测距信息测量值，由节点上配制的超宽带测距传感器获得；/>
Figure SMS_95
为当前时刻（t=k）的相对测距信息观测值，其数值基于/>
Figure SMS_96
增加噪声扰动得到；在本实施例中，为了简化计算，/>
Figure SMS_97
的取值与
Figure SMS_98
相等；
因此，获得的节点i当前时刻状态量的置信度表达式整理为：
Figure SMS_99
Figure SMS_100
，
S3、确定节点i在当前时刻状态量的协方差矩阵，以得到节点i的分布式协同导航定位精度；
具体地，该步骤S3的实施步骤如下：
S301、基于概率密度标准形式，定义节点i在当前时刻状态量
Figure SMS_101
的概率密度表达式为：
Figure SMS_102
，
式中，
Figure SMS_103
为节点当前状态/>
Figure SMS_104
的均值；/>
Figure SMS_105
为比例因子，其取值为常数；/>
Figure SMS_106
为节点i在当前时刻状态量/>
Figure SMS_107
的协方差矩阵；
S302、将节点i在当前时刻状态量
Figure SMS_108
的置信度表达式与概率密度表达式进行联立，即：/>
Figure SMS_109
，经整理得到节点i在当前时刻状态量的协方差矩阵/>
Figure SMS_110
，其表达式为：
Figure SMS_111
，
式中，
Figure SMS_112
，/>
Figure SMS_113
，/>
Figure SMS_114
为/>
Figure SMS_115
的逆矩阵，/>
Figure SMS_116
为/>
Figure SMS_117
的逆矩阵；
其中，
基于步骤S1和步骤S2，
Figure SMS_120
为先验因子/>
Figure SMS_122
的协方差矩阵，先验因子/>
Figure SMS_124
为初始时刻状态量/>
Figure SMS_119
至当前时刻状态量/>
Figure SMS_123
的连接量，因此，/>
Figure SMS_125
为一个与时间相关的量，即当前时刻的/>
Figure SMS_126
由前一时刻的/>
Figure SMS_118
递推得到；/>
Figure SMS_121
的递推公式为：
Figure SMS_127
，
式中，
Figure SMS_128
的表达式为：/>
Figure SMS_129
，其中，/>
Figure SMS_130
为前一时刻（t=k-1）的速率观测值，其具体数值由安装在节点i上的里程计获得；/>
Figure SMS_131
为航位推算解算周期，其取值与前述协同导航方法中采用的航位推算间隔时间相同；/>
Figure SMS_132
为当前时刻（t=k）的航向角；在本实施例中，/>
Figure SMS_133
为0.1s；
Figure SMS_134
的表达式为：/>
Figure SMS_135
，/>
Figure SMS_136
为航位推算解算周期，其取值与前述协同导航方法中采用的航位推算解算周期相同；在本实施例中，/>
Figure SMS_137
为0.1s；
Figure SMS_138
为陀螺仪角速率随机噪声，符合高斯噪声，/>
Figure SMS_139
的表达式为：/>
Figure SMS_140
)，其物理含义为：/>
Figure SMS_141
为均值为0，方差为/>
Figure SMS_142
高斯噪声；在本实施例中，/>
Figure SMS_143
为100 (°/h3/2)；
Figure SMS_144
为UWB的相对测距误差噪声，符合高斯噪声，/>
Figure SMS_145
的表达式为：/>
Figure SMS_146
)，其物理含义为：/>
Figure SMS_147
为均值为0，方差为/>
Figure SMS_148
高斯噪声；在本实施例中，/>
Figure SMS_149
为0.1 m2；
Figure SMS_150
为当前时刻（t=k）下先验因子/>
Figure SMS_151
的协方差矩阵，/>
Figure SMS_152
为前一时刻（t=k-1）下先验因子/>
Figure SMS_153
的协方差矩阵；初始时刻（t=0）下/>
Figure SMS_154
为0，则基于前述协同导航方法依次推算得到的各时刻下的F、G和P即可相应推算得到各时刻下的/>
Figure SMS_155
值；
S303、由步骤S302计算得到的节点i在当前时刻状态量
Figure SMS_156
的协方差矩阵/>
Figure SMS_157
的模
Figure SMS_158
，即为节点i在当前时刻的分布式协同导航定位精度结果。
进一步地，为了验证该方法在实际应用中的可行性和精度评价的准确性，利用仿真试验对结果进行验证。
如图3所示为本实施例应用的仿真场景；在该仿真场景中，初始状态下，场景分为位于房间内的室内场景和位于房间外的室外场景；室内场景中布置有一辆无人车和一名侦查员，室外场景中布置有一辆无人车；其中，室内场景布置为：房间西侧靠北设有第一进出门，房间南侧自西向东间隔地设有三个进出门，分别为第二进出门、第三进出门和第四进出门，在靠近房间东侧设有一面与第一进出门相对的遮挡墙，四个进出门为常开状态；三个通信基站分别命名为锚点1、锚点2和锚点3，锚点1和锚点2设置于室外南侧，且二者的设置位置至房间的距离一致，锚点1与第二进出门和第三进出门的连线中点相对，锚点2与第三进出门和第四进出门的连线中点相对，锚点3设置在室外西侧并与第一进出门相对；锚点1、锚点2和锚点3均可用于在通信不受阻的情况下与无人车、侦查员建立实时通信；室外的无人车命名为节点1，其初始位于第二进出门的西侧；节点1的运动轨迹设定为先从室外初始位置行驶至室内，并沿南侧墙体自西侧向东侧行驶；在整个运动过程中，节点1能够与室外南侧的两个通信基站（即锚点1和锚点2）建立通信；室内的无人车命名为节点2，其初始位于第一进出门的东侧且靠近北侧墙体处；节点2的运动轨迹设定为自室内北侧行驶至室内南侧；在整个运动过程中，节点2能够与位于室外西侧的通讯基站（即锚点3）建立通信；室内的侦查员命名为节点3，其初始位于遮挡墙西侧且靠近北侧墙体处；节点3的运动轨迹设定为自室内北侧朝南行驶一段距离至遮挡墙端头、再转弯朝向室内东侧行驶；在整个运动过程中，节点3无法与任何通讯基站建立通信。上述三个节点均配置有惯性传感器、里程计和超宽带测距传感器。
根据前述的协同导航方法可确定，三个节点的通信精度关系为：节点1＞节点2＞节点3；进而信息传输模式设定为：节点2可以接收到与节点1之间的相对距离信息，而节点3可以接收到节点1和节点2的相对距离信息。
在本次测试过程中，首先采用前述协同导航方法对导航结果进行预测，而后再采用本申请的方法对预测结果的定位精度进行仿真计算，得到定位精度仿真值；与此同时，作为本申请的对照实验，参照图3在实验室设置相同的真实场景并进行实物试验；同样地，真实场景中的各节点配置有高精度的导航设备，以获得定位结果作为基准值，通过将基准值与各节点的导航值进行对比，以计算得到定位精度实际值。需要说明的是，由于各节点一直处于运动中，因此各节点的定位精度也随时间变化；为了便于对比，将各节点分别在仿真场景和实际场景中得到的定位精度最大偏差值作为节点的定位精度。
上述仿真场景与实物场景中的各节点的测试结果具体如下表1所示。
节点名称 节点1 节点2 节点3
定位精度仿真值 0.02m 0.1m 0.4m
定位精度实际值 0.022m 0.102m 0.399m
从表1的测试结果可以明显看出，采用本申请的方法获得的各节点的定位精度分别为：节点1的定位精度为0.02m，节点2的定位精度为0.1m，节点3的定位精度0.4m；该三个定位精度结果与上述三个节点的信号接收能力是一致的，即节点1>节点2>节点3，即验证了本申请的方法能够真实地反应仿真设置的节点定位精度，即本申请的方法具有有效性；而将本申请的方法获得的定位精度与实际实物测试得到的定位精度进行对比，定位精度仿真值与实际值的最大偏差仅为2mm，精确度高，即进一步证明了本申请的方法的有效性和准确性。
综上，本申请的基于置信度传播的分布式协同导航定位精度评价方法不仅操作简便，且精度高，兼具有效性和准确性。
本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。

Claims (3)

1.一种基于置信度传播的分布式协同导航定位精度评价方法，其特征在于，步骤如下：
S1、构建单个节点在当前时刻的因子图模型：对于任一节点i，设定先验因子
Figure QLYQS_3
和相对测距因子/>
Figure QLYQS_5
，并定义：节点i在初始时刻的状态量/>
Figure QLYQS_8
通过先验因子/>
Figure QLYQS_2
与当前时刻的状态量/>
Figure QLYQS_6
连接，从而将初始时刻状态量/>
Figure QLYQS_9
传递给当前时刻状态量/>
Figure QLYQS_10
；节点i在当前时刻的状态量/>
Figure QLYQS_1
与相对测距因子/>
Figure QLYQS_4
连接，从而将相邻节点与节点i的相对测距信息传输给节点i在当前时刻状态量/>
Figure QLYQS_7
；
S2、获得节点i在当前时刻状态量的置信度表达式：
Figure QLYQS_11
Figure QLYQS_12
，
式中，
Figure QLYQS_15
为节点在当前时刻，即t=k状态量/>
Figure QLYQS_18
的置信度；/>
Figure QLYQS_21
为先验因子/>
Figure QLYQS_13
的协方差矩阵；/>
Figure QLYQS_16
为节点i基于前一时刻的状态量/>
Figure QLYQS_19
获得的当前时刻的状态量的预测值；/>
Figure QLYQS_22
为相对测距因子/>
Figure QLYQS_14
的协方差矩阵；/>
Figure QLYQS_17
为当前时刻的相对测距信息测量值；/>
Figure QLYQS_20
为当前时刻的相对测距信息观测值；
S3、确定节点i在当前时刻状态量的协方差矩阵，以得到节点i的分布式协同导航定位精度；该步骤S3的具体步骤为：
S301、基于概率密度标准形式，定义节点i在当前时刻状态量
Figure QLYQS_23
的概率密度表达式为：
Figure QLYQS_24
，
式中，
Figure QLYQS_25
为节点当前状态/>
Figure QLYQS_26
的均值；/>
Figure QLYQS_27
为比例因子，其取值为常数；/>
Figure QLYQS_28
为节点i在当前时刻状态量/>
Figure QLYQS_29
的协方差矩阵；
S302、将节点i在当前时刻状态量
Figure QLYQS_30
的置信度表达式与概率密度表达式进行联立，即：
Figure QLYQS_31
，经整理得到节点i在当前时刻状态量的协方差矩阵/>
Figure QLYQS_32
，其表达式为：
Figure QLYQS_33
，
式中，
Figure QLYQS_43
，/>
Figure QLYQS_35
；/>
Figure QLYQS_39
为当前时刻下/>
Figure QLYQS_37
的逆矩阵；/>
Figure QLYQS_38
为/>
Figure QLYQS_42
的逆矩阵；/>
Figure QLYQS_46
的递推公式为：/>
Figure QLYQS_45
，其中，/>
Figure QLYQS_51
为当前时刻下先验因子
Figure QLYQS_36
的协方差矩阵，/>
Figure QLYQS_41
为前一时刻下先验因子/>
Figure QLYQS_47
的协方差矩阵；/>
Figure QLYQS_49
的表达式为：
Figure QLYQS_50
，/>
Figure QLYQS_54
为前一时刻的速率观测值；/>
Figure QLYQS_48
为航位推算解算周期；/>
Figure QLYQS_52
为当前时刻的航向角；/>
Figure QLYQS_53
的表达式为：/>
Figure QLYQS_55
，/>
Figure QLYQS_34
为航位推算解算周期；/>
Figure QLYQS_40
为陀螺仪角速率随机噪声；/>
Figure QLYQS_44
为UWB的相对测距误差噪声；
S303、由步骤S302计算得到的节点i在当前时刻状态量
Figure QLYQS_56
的协方差矩阵/>
Figure QLYQS_57
的模/>
Figure QLYQS_58
，即为节点i在当前时刻的分布式协同导航定位精度结果。
2.根据权利要求1所述的基于置信度传播的分布式协同导航定位精度评价方法，其特征在于，在步骤S302中，
Figure QLYQS_59
的表达式为：/>
Figure QLYQS_60
)，/>
Figure QLYQS_61
为100 (°/h3/2)。
3.根据权利要求1所述的基于置信度传播的分布式协同导航定位精度评价方法，其特征在于，在步骤S302中，
Figure QLYQS_62
的表达式为：/>
Figure QLYQS_63
)，/>
Figure QLYQS_64
为0.1 m2。
CN202310518820.9A 2023-05-10 2023-05-10 一种基于置信度传播的分布式协同导航定位精度评价方法 Pending CN116380130A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310518820.9A CN116380130A (zh) 2023-05-10 2023-05-10 一种基于置信度传播的分布式协同导航定位精度评价方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310518820.9A CN116380130A (zh) 2023-05-10 2023-05-10 一种基于置信度传播的分布式协同导航定位精度评价方法

Publications (1)

Publication Number Publication Date
CN116380130A true CN116380130A (zh) 2023-07-04

Family

ID=86971191

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310518820.9A Pending CN116380130A (zh) 2023-05-10 2023-05-10 一种基于置信度传播的分布式协同导航定位精度评价方法

Country Status (1)

Country Link
CN (1) CN116380130A (zh)

Similar Documents

Publication Publication Date Title
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
Safavi et al. Distributed localization: A linear theory
US7589672B2 (en) Method for determining positional data
Chu et al. Vehicle localization via cooperative channel mapping
CN107255795A (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN112367614B (zh) 一种基于LSTM的Wi-Fi与地磁场融合的室内定位算法
CN114838732B (zh) 一种基于图优化的通信受限环境下协同导航方法
CN102325370A (zh) 一种高精度的无线传感器网络节点的三维定位器
Ouyang et al. Cooperative navigation of UAVs in GNSS-denied area with colored RSSI measurements
Si et al. An adaptive weighted Wi-Fi FTM-based positioning method in an NLOS environment
CN116222583B (zh) 一种基于滑动窗口的协同导航快速计算方法
Cheng et al. UWB/INS fusion positioning algorithm based on generalized probability data association for indoor vehicle
Latif et al. Multi-robot synergistic localization in dynamic environments
Wang et al. An indoor UWB NLOS correction positioning method based on anchor LOS/NLOS map
Sun et al. Smartphone user tracking by incorporating user orientation using a double-layer HMM
Uluskan et al. A geometrical closed form solution for RSS based far-field localization: Direction of Exponent Uncertainty
CN104535963B (zh) 基于距离测量的多移动节点实现协同目标定位的方法
CN116380130A (zh) 一种基于置信度传播的分布式协同导航定位精度评价方法
Jung et al. Scalable and modular ultra-wideband aided inertial navigation
Liu et al. An UWB/PDR fusion algorithm based on improved square root unscented Kalman filter
Martins et al. OTMCL: Orientation tracking-based Monte Carlo localization for mobile sensor networks
Ma et al. A TDOA localization method for complex environment localization
Efatmaneshnik et al. Information fusion for localization within vehicular networks
Zhao et al. A UWB/Bluetooth fusion algorithm for indoor localization
He et al. Robust Localization for Mobile Targets Along a Narrow Path With LoS/NLoS Interference

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