CN116399335A - 基于高斯信念传播的飞行器集群分布式协同定位方法 - Google Patents
基于高斯信念传播的飞行器集群分布式协同定位方法 Download PDFInfo
- Publication number
- CN116399335A CN116399335A CN202310218483.1A CN202310218483A CN116399335A CN 116399335 A CN116399335 A CN 116399335A CN 202310218483 A CN202310218483 A CN 202310218483A CN 116399335 A CN116399335 A CN 116399335A
- Authority
- CN
- China
- Prior art keywords
- factor
- variable
- aircraft
- cluster
- state
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 230000008569 process Effects 0.000 claims abstract description 14
- 230000001133 acceleration Effects 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 7
- 230000005540 biological transmission Effects 0.000 abstract description 5
- 230000008878 coupling Effects 0.000 abstract description 4
- 238000010168 coupling process Methods 0.000 abstract description 4
- 238000005859 coupling reaction Methods 0.000 abstract description 4
- 230000007613 environmental effect Effects 0.000 abstract description 2
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000015572 biosynthetic process Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N5/00—Computing arrangements using knowledge-based models
- G06N5/04—Inference or reasoning models
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Data Mining & Analysis (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- Computational Linguistics (AREA)
- Artificial Intelligence (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于高斯信念传播的飞行器集群分布式协同定位方法,该方法包括:构建集群因子图模型,在每一时刻仅对飞行器节点的标称和误差状态进行迭代,生成惯导因子;对观测信息进行线性化,生成观测因子;当观测到来时,构建高斯信念传播过程,变量和因子之间消息多次传递迭代后进行信念更新;根据更新后的信念信息修正标称状态,输出每一时刻的状态估计。本发明的主要创新点在于:1.采用姿态独立的三自由度松耦合更新形式,相对于六自由度紧耦合可以实现更高的相对定位精度;2.实现即插即用,容许新的飞行器节点随时加入网络,失效飞行器暂时或永久退出网络,当环境干扰减弱,恢复正常的飞行器可以重新加入网络;3.据数据链拓扑结构建立概率图模型描述集群状态的联合分布,通过图模型节点间的消息传递计算每个节点状态的边缘分布,利用图模型节点与集群物理节点的对应关系,实现集群分布式推理,实时性好,表现更为稳定。
Description
技术领域
本发明属于卫星导航技术领域,具体涉及一种基于高斯信念传播的飞行器集群分布式协同定位方法。
背景技术
集群成员节点间准确的相对定位,包括相对位置和相对姿态的估计,是飞行器集群任务规划与编队控制的基础,也是集群完成协同探测、感知、搜索与打击任务的前提条件。由于纯惯导的定位误差随时间不断累积,而卫星定位信号易受屏蔽或干扰,因此基于惯导/数据链组合的集群协同相对定位逐渐成为研究的热点。现有的数据链设备兼具机间通讯和相对测距功能,已被广泛应用于飞行器集群组网。利用数据链的相对距离测量可对惯导估计结果进行修正,进而提升集群相对定位精度。由于数据链只能提供相对测量,因此惯导/数据链组合不能保证节点绝对定位精度。
在现有技术中,主流的集中式算法如扩展卡尔曼滤波(EKF),误差状态卡尔曼滤波(ESKF)需要将集群内全部节点的观测信息汇聚到中心节点集中处理,因此,集中式系统的规模和可靠性往往受到中心节点的制约,中心节点的运算和通信负担会随集群规模迅速增加,且中心节点的毁伤会导致整个系统的失效。
因此,本发明公开了一种基于高斯信念传播的飞行器集群分布式协同定位方法,根据数据链拓扑结构建立概率图模型描述集群状态的联合分布,通过图模型节点间的消息传递计算每个节点状态的边缘分布,利用图模型节点与集群物理节点的对应关系,实现集群分布式推理。高斯信念传播算法通过对观测模型线性化,建立误差状态的线性模型,在初始高斯分布假设下,误差状态分布在更新过程中始终为高斯分布,可用均值向量和协方差矩阵参数化表达。在高斯信念传播算法中,消息的更新和传递归结为均值和协方差参数的更新和传递,使得系统的计算和通信负担大为降低。其分布式系统不存在中心节点,每个节点只处理局部量测信息并与相邻节点通讯。各节点运行同样的信息处理算法和信息交互协议,可方便地增删集群节点或改变集群拓扑结构。同时,任何节点的毁伤都不会显著影响系统的性能。和集中式系统相比,分布式系统具有更高的灵活性、可扩展性和鲁棒性。
发明内容
鉴于上述的分析,本发明旨在公开一种基于高斯信念传播的飞行器集群分布式协同定位方法,用于飞行器集群长航时分布式协同定位问题。
为解决上述问题,本发明采取的解决方案如下:
步骤1,用于:根据集群内飞行器的初始状态构建因子图模型;其中,三自由度状态变量真值,定义为:xt=[pt,vt,bt],表示位置、速度、加计偏置,姿态估计依靠陀螺仪纯惯导获取;标称值定义为:x=[p,v,b],状态变量误差定义为:δx=[δp,δv,δb];
步骤2,用于:根据飞行器集群因子图模型,构建单飞行器标称状态运动方程:
构建误差状态运动方程:
其中,误差状态均值与协方差预测:
P←A·P·AT+B·∑q·BT
步骤3,用于:根据飞行器集群因子图模型,构建观测方程:
y=h(xt)+v=h(x+δx)+v
步骤4,用于:当观测到来时,构建高斯信念传播过程,通过从相邻节点发送和接收消息来迭代更新节点在本地存储的信念。其中,表示第j个因子节点向第i个变量节点发送的消息,/>表示第i个变量节点向第j个因子节点发送的消息,每次迭代包括3个阶段:
变量到因子的消息传递:
因子到变量的消息传递:
信念更新:
步骤5,用于:根据更新后各变量节点的信念状态参数η和Λ,获取相应的误差状态:
δx=μ=Λ-1η,P=∑=Λ-1
将误差状态注入当前标称状态:
误差状态重置:
步骤6,用于:输出每一时刻飞行器集群的估计状态xk
进一步,根据本发明的飞行器集群分布式协同定位方法,所述步骤2中,需要根据因子图架构和误差状态运动方程,构建惯导因子:
其中∑Pr表示误差状态的先验协方差,惯导因子规范形式为:
进一步,根据飞行器集群分布式协同定位方法,所述步骤3中,需要对观测方程进行线性化:
观测方程为:
根据观测方程以及因子图构建测距因子:
其中∑f为相对测距方差,测距因子规范形式为:
进一步,根据飞行器集群分布式协同定位方法,所述步骤4中,高斯信息具有规范形式如下:
a)变量到因子的消息传递:
b)因子到变量的消息传递:
设一个因子节点F连接了两个变量节点[x1,x2],其高斯分布形式为:
其规范型参数可以写作以下形式:
取因子分布以及与其相连的其他变量节点消息的乘积,可以得到一下参数:
为了从这个因子完成消息传递,必须边缘化除消息接受者变量x1之外的所有变量,对上一步中变量的联合高斯分布可以写作:
在对变量x2进行边缘化之后,变量x1的边缘分布如下:
c)信念更新:
节点的信念状态规范形式如下:
根据变量到因子,因子到变量消息传递过程的多次迭代(一般多于3次),各个变量的信念规范型参数可以收敛到精确的结果,最终每个变量节点的信息矢量和精度矩阵如下:
本发明的优点在于:
1.本发明采用了姿态独立的三自由度松耦合高斯信念传播过程,相较于六自由度紧耦合信念传播过程具有更高的精确度。
2.本发明通过增减因子图节点以及相应的因子描述飞行器加入及退出网络的过程,实现即插即用,提升推理架构的灵活性。其推理计算结构基本稳定,在强环境干扰下也能保证飞行器集群的定位精度。
3.本发明针对飞行器集群协同导航因子图模型拓扑结构随时间动态变化、无观测锚点的问题,提出基于高斯信念传播的分布式协同算法,通过节点间的相对距离观测来提升集群的相对定位精度。
附图说明
图1为本发明给出的飞行器集群分布式协同定位示意图;
图2为本发明给出的因子图模型示意图;
图3为本发明提供的信念传播迭代示意图;
图4为本发明提供的五台飞行器组成的集群2500s长航时仿真实验中十条数据链对应的相对位置估计误差结果图;
图5为本发明给出的集群因子图模型信念传播算法框架流程图;
具体实施方式
下面结合附图对本发明提供的基于高斯信念传播的飞行器集群分布式协同定位方法进行详细说明。
本发明给出的基于高斯信念传播的飞行器集群分布式协同定位方法的场景如图1所示,因子图架构如图2所示。下面结合具体的仿真示例对本实施例中的导航方法作出进一步的说明。系统仿真设置经度为108.93°,纬度为34.248°,高度为380m;加速度计频率为1000Hz,加计常值偏置50ug,速度随机游走陀螺仪频率为1000Hz,陀螺常值漂移0.01°/h,角度随机游走/>数据链频率为1Hz距离观测误差10m。按照具体实施步骤,可以将整个流程划分为六步骤:
步骤1,根据集群内飞行器的初始状态构建因子图模型;其中,每个飞行器节点的三自由度状态变量真值,定义为:xt=[pt,vt,bt],表示位置、速度、加计偏置;姿态估计依靠陀螺仪纯惯导获取;标称值定义为:x=[p,v,b],状态变量误差定义为:δx=[δp,δv,δb],为飞行器集群添加初始速度误差东向0.1m/s,北向0.1m/s,天向0.1m/s;初始位置误差东向1m,北向1m,天向3m;
步骤2,在每一时刻分别构建单飞行器标称状态运动方程:
在每一时刻构建误差状态运动方程:
其中,误差状态均值与协方差预测:
P←A·P·AT+B·∑q·BT
根据因子图架构和误差状态运动方程,构建惯导因子:
其中∑g表示误差状态的惯导协方差,惯导因子规范形式为:
步骤3,根据飞行器集群因子图模型,当观测来临时,构建观测方程:
y=h(xt)+v=h(x+δx)+v
观测方程进行线性化:
观测方程为:
根据观测方程以及因子图构建测距因子:
其中∑f为相对测距方差,测距因子规范形式为:
步骤4,当观测到来时,构建高斯信念传播过程,通过从相邻节点发送和接收消息来迭代更新节点在本地存储的信念。其中,表示第j个因子节点向第i个变量节点发送的消息,/>表示第i个变量节点向第j个因子节点发送的消息,高斯信息具有规范形式如下:
每次迭代包括3个阶段:
a)变量到因子的消息传递:
b)因子到变量的消息传递:
设一个因子节点F连接了两个变量节点[x1,x2],其高斯分布形式为:
其规范型参数可以写作以下形式:
取因子分布以及与其相连的其他变量节点消息的乘积,可以得到一下参数:
为了从这个因子完成消息传递,必须边缘化除消息接受者变量x1之外的所有变量,对上一步中变量的联合高斯分布可以写作:
在对变量x2进行边缘化之后,变量x1的边缘分布如下:
c)信念更新:
节点的信念状态规范形式如下:
根据变量到因子,因子到变量消息传递过程的多次迭代(一般多于3次),各个变量的信念规范型参数可以收敛到准确的结果,最终每个变量节点的信息矢量和精度矩阵如下:
步骤5,根据更新后各变量节点的信念状态参数η和Λ,获取相应的误差状态:
δx=μ=Λ-1η,P=∑=Λ-1
将误差状态注入当前标称状态进行修正:
误差状态重置:
步骤6,用于:输出每一时刻飞行器集群的估计状态xk
重复以上步骤至飞行器集群结束2500s长航时协同定位过程。其中,图4为飞行器集群内五个飞行器节点的相对位置误差估计效果。可以看到,在卫星拒止和数据链通信部分受限的情况下,分布式高斯信念传播算法可以有效地估计飞行器之间的相对位置,并将其相对位置误差基本限制在100m内,从而达到保持集群队形,提升系统综合精度的目的。可实现即插即用,容许新的飞行器节点随时加入网络,失效飞行器暂时或永久退出网络,基于因子图的推理计算结构保持稳定不变,解决了卫星导航系统的制约,在各种情况下都可以提供精确可靠的导航信息,具有灵活性,可扩展性和鲁棒性。
Claims (4)
1.基于高斯信念传播的飞行器集群分布式协同定位方法,其特征在于,该方法包括以下步骤:
步骤1,用于:根据集群内飞行器的初始状态构建因子图模型;其中,三自由度状态变量真值,定义为:xt=[pt,vt,bt],表示位置、速度、加计偏置,姿态估计依靠陀螺仪纯惯导获取;标称值定义为:x=[p,v,b],状态变量误差定义为:δx=[δp,δv,δb];
步骤2,用于:根据飞行器集群因子图模型,构建单飞行器标称状态运动方程:
构建误差状态运动方程:
其中,误差状态均值与协方差预测:
P←A·P·AT+B·Σq·BT
步骤3,用于:根据集群因子图模型,构建观测方程:
y=h(xt)+v=h(x+δx)+v
步骤4,用于:当观测到来时,构建高斯信念传播过程,通过从相邻节点发送和接收消息来迭代更新节点在本地存储的信念;其中,表示第j个因子节点向第i个变量节点发送的消息,/>表示第i个变量节点向第j个因子节点发送的消息,每次迭代包括3个阶段:
变量到因子的消息传递:
因子到变量的消息传递:
信念更新:
步骤5,用于:根据更新后各变量节点的信念状态参数η和Λ,获取相应的误差状态:
δx=μ=Λ-1η,P=∑=Λ-1
将误差状态注入当前标称状态:
误差状态重置:
步骤6,用于:输出每一时刻飞行器集群的估计状态xk。
4.如权利要求1所述的基于信念传播的长航时飞行器集群协同定位方法,其特征在于,所述步骤4中,高斯信息具有规范形式如下:
a)变量到因子的消息传递:
b)因子到变量的消息传递:
设一个因子节点F连接了两个变量节点[x1,x2],其高斯分布形式为:
其规范型参数可以写作以下形式:
取因子分布以及与其相连的其他变量节点消息的乘积,可以得到一下参数:
为了从这个因子完成消息传递,必须边缘化除消息接受者变量x1之外的所有变量,对上一步中变量的联合高斯分布可以写作:
在对变量x2进行边缘化之后,变量x1的边缘分布如下:
c)信念更新:
节点的信念状态规范形式如下:
根据变量到因子,因子到变量消息传递过程的多次迭代(一般多于3次),各个变量的信念规范型参数可以收敛到精确的结果,最终每个变量节点的信息矢量和精度矩阵如下:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310218483.1A CN116399335A (zh) | 2023-03-09 | 2023-03-09 | 基于高斯信念传播的飞行器集群分布式协同定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310218483.1A CN116399335A (zh) | 2023-03-09 | 2023-03-09 | 基于高斯信念传播的飞行器集群分布式协同定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116399335A true CN116399335A (zh) | 2023-07-07 |
Family
ID=87013241
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310218483.1A Pending CN116399335A (zh) | 2023-03-09 | 2023-03-09 | 基于高斯信念传播的飞行器集群分布式协同定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116399335A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116962176A (zh) * | 2023-09-21 | 2023-10-27 | 浪潮电子信息产业股份有限公司 | 一种分布式集群的数据处理方法、装置、系统及存储介质 |
-
2023
- 2023-03-09 CN CN202310218483.1A patent/CN116399335A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116962176A (zh) * | 2023-09-21 | 2023-10-27 | 浪潮电子信息产业股份有限公司 | 一种分布式集群的数据处理方法、装置、系统及存储介质 |
CN116962176B (zh) * | 2023-09-21 | 2024-01-23 | 浪潮电子信息产业股份有限公司 | 一种分布式集群的数据处理方法、装置、系统及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Xiong et al. | Fault-tolerant GNSS/SINS/DVL/CNS integrated navigation and positioning mechanism based on adaptive information sharing factors | |
CN113074739B (zh) | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 | |
CN113470089B (zh) | 一种基于三维点云的跨域协同定位和建图方法及系统 | |
CN113411744B (zh) | 一种高精度的室内定位追踪方法 | |
CN116399335A (zh) | 基于高斯信念传播的飞行器集群分布式协同定位方法 | |
CN110763245A (zh) | 一种基于流式计算的地图创建方法及其系统 | |
Enns et al. | Terrain-aided navigation using the Viterbi algorithm | |
Hasan et al. | Automatic estimation of inertial navigation system errors for global positioning system outage recovery | |
CN109188352B (zh) | 一种组合导航相对定位方法 | |
Goppert et al. | Invariant Kalman filter application to optical flow based visual odometry for UAVs | |
CN111190207B (zh) | 基于pstcsdref算法的无人机ins bds组合导航方法 | |
Abdolkarimi et al. | Optimization of the low-cost INS/GPS navigation system using ANFIS for high speed vehicle application | |
CN116086445A (zh) | 一种基于因子图优化的多源信息时延融合导航方法 | |
CN110213713B (zh) | 一种基于时空域联合处理的集中式协作定位系统和方法 | |
Belhajem et al. | A robust low cost approach for real time car positioning in a smart city using Extended Kalman Filter and evolutionary machine learning | |
CN115855048A (zh) | 一种多传感器融合位姿估计方法 | |
CN114440895A (zh) | 基于因子图的气压辅助Wi-Fi/PDR室内定位方法 | |
Abdolkarimi et al. | A modified neuro-fuzzy system for accuracy improvement of low-cost MEMS-Based INS/GPS navigation system | |
CN116380038A (zh) | 一种基于在线增量尺度因子图的多源导航信息融合方法 | |
CN113252039B (zh) | 面向地形辅助导航的粒子群快速匹配方法 | |
Chiang et al. | Rapid and accurate INS alignment for land applications | |
CN112683265A (zh) | 一种基于快速iss集员滤波的mimu/gps组合导航方法 | |
CN111288988A (zh) | 一种架空机器人组合定位方法 | |
Cao et al. | Prediction of SINS/GPS Navigation Information by ELM Algorithm during GPS outages | |
Xu et al. | Multi-Sensor Fusion Framework Based on GPS State Detection |
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 |