一种基于因子图的节点优选的多AUV自适应协同定位方法及系统技术方案

一种基于因子图的节点优选的多AUV自适应协同定位方法及系统，涉及多AUV协同定位技术领域，为解决现有的技术无法适用于系统拓扑结构的动态变化，且无法有效消除模糊时变噪声的干扰及系统规模较大带来的通信压力的问题。包括如下步骤：采集当前时刻动态拓扑结构信息；更新从艇及其邻居主艇信息；构建因子图模型，采用状态方程函数节点对从艇状态变量节点X

【技术实现步骤摘要】
一种基于因子图的节点优选的多AUV自适应协同定位方法及系统


[0001]本专利技术涉及多AUV协同定位
，具体而言，涉及一种基于因子图的节点优选的多AUV自适应协同定位方法及系统。

技术介绍

[0002]由于水下环境的复杂性，水下任务执行过程中多AUV协同定位系统的AUV数量可能发生改变，导致系统拓扑结构发生动态变化。在动态拓扑结构下的协同定位过程中，数据融合变成动态过程。但现有协同定位算法中系统结构往往是静态固定的，往往不是适用于动态拓扑结构下的AUV协同定位。而且由于海洋环境复杂，水温变化、介质密度不均衡、洋流干扰等多种影响，水下声速往往会发未知变化，量测信息中常常掺杂模糊时变噪声，这类噪声的统计特性未知且具有不确定的协方差矩阵，容易对协同定位算法的量测更新部分产生干扰。因子图在相关节点处传递信息具有不确定性，这种误差会随着传递而累计，使得系统定位精度下降，严重时甚至会导致系统发散。而当动态拓扑结构下系统主AUV节点数量多、系统规模较大的情况下，与所有节点进行通信会增大协同定位系统数据交互量，降低系统实时性，且相对低精度定位信息会使得本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于因子图的节点优选的多AUV自适应协同定位方法，其特征在于，包括如下步骤：S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；S2、更新从艇及其邻居主艇信息；S3、初始化主艇与从艇信息；S4、构建多AUV协同定位系统因子图模型；将从艇状态变量、主艇位置信息及主艇量测信息定义为变量节点，将状态方程和量测方程定义为函数节点，并定义自适应迭代估计函数节点及节点优选函数节点；采用状态方程函数节点对从艇状态变量节点X
k
进行传递更新，采用量测方程函数节点对主艇量测信息节点Z
k
及从艇状态变量节点X
k
进行融合更新，所述自适应迭代估计函数节点基于EM算法的自适应EKF滤波器，以估计量测噪声协方差矩阵，所述节点优选函数节点，针对系统主艇估计的位置信息节点Φ
k
及主艇量测信息节点Z
k
，分别进行克拉美罗下界CRLB
k
和测距评价因子计算，对当前时刻主艇节点进行优选；S5、基于和积算法在多AUV协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；S6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；S7、通过节点优选函数节点对主艇节点进行优选；S8、基于优选的主艇节点，将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。2.根据权利要求1所述的基于因子图的节点优选的多AUV自适应协同定位方法，其特征在于，S1中在每个采集周期T内采集系统当前动态拓扑结构信息，包括：主AUV和从AUV的数量，主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及检测目标从艇与每艘主艇之间的测距信息d。3.根据权利要求2所述的基于因子图的节点优选的多AUV自适应协同定位方法，其特征在于，S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。4.根据权利要求1所述的基于因子图的节点优选的多AUV自适应协同定位方法，其特征在于，S5包括如下过程：在第k时刻系统条件概率密度函数分解为：式中，N表示主AUV节点数量；表示第n(n＝1,2,
…
,N)艘主AUV的量测信息；X
m,n
表示第n(n＝1,2,
…
,N)艘主AUV的位置信息；f
i
表示各个AUV节点对应的概率因子，即：式中，h
i
(.)表示量测函数；z
i
表示量测真实值；∑
i
表示对应量测误差的协方差矩阵；定义系统采集到的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：
其中d
i
表示从AUV与第i艘主AUV之间的测距信息；通过状态方程函数节点f(X
k
|X
k
‑1)向变量节点X
k
传递的信息为：变量节点X
k
向状态方程函数节点f(X
k
|X
k
‑1)传递的信息为：式中，和分别代表状态变量X
k
的先验估计和方差；根据协同定位的位置方程：式中，(x
k
,y
k
)表示k时刻AUV在参考坐标系的坐标；v
k
表示k时刻AUV前向速度；θ
k
表示k时刻AUV的航向角度；Δt表示采样间隔；得到状态转换公式为：式中，Q
k
是系统过程噪声协方差矩阵，为量测噪声矩阵，的表达式为：式中，θ
k
是k时刻对应的航向角信息；将式(6)和式(7)代入式(5)，结合，得到最终信度信息：式中，S
k
的表达式为：最终，实现全局因子图节点信息传递与更新。5.根据权利要求4所述的基于因子图的节点优选的多AUV自适应协同定位方法，其特征在于，S6中根据EKF滤波算法构建从艇状态变量方程，即：式中，表示k
‑
1时刻得到的k时刻的估计值，...

【专利技术属性】
技术研发人员：罗清华，林家祺，王金龙，陈燕怡，
申请(专利权)人：哈尔滨工业大学威海，
类型：发明
国别省市：