一种基于节点优化选择与因子图的多AUV协同定位方法及系统

技术领域

本发明涉及AUV协同定位技术领域，具体而言，涉及一种基于节点优化选择与因子图的多AUV协同定位方法及系统。

背景技术

由于水下环境的复杂性，水下任务执行过程中多AUV协同定位系统的AUV数量可能发生改变，导致系统拓扑结构发生动态变化。当动态拓扑结构下系统主AUV节点数量多、质量杂的情况下，与所有节点进行通信会增大协同定位系统数据交互量，降低系统实时性，且相对低精度定位信息会使得协同定位精度下降的动态拓扑系统中节点规模较大或是水下带宽有限等情况下，与系统内每个主AUV节点都进行通信必然会增大协同定位系统的数据交互量，影响系统实时性。传统协同算法中从AUV接收主AUV的位置信息来校正自身位置信息，并往往认为接收端接收的主AUV位置信息是精确的。在水下实际环境中，主AUV节点的位置信息也可能含有噪声，且不同主AUV节点信息所含有的噪声通常相互独立，传统协同定位算法对主AUV节点信息不加选择采用，将导致定位精度下降。对AUV节点进行优化选择是处理这类问题的有效方法，但目前针对主AUV节点优化选择所提出的解决方法较为缺乏，AUV节点选择机制还有待完善。因此亟需一种针对动态拓扑下主AUV节点数量众多、系统规模较大的多AUV协同定位方法。

发明内容

本发明要解决的技术问题是：

现有技术无法应对系统拓扑结构的动态变化，当系统规模较大或者系统通信带宽受限时，与系统内所有主AUV节点进行通信会带来较大的带宽压力，不但影响系统实时性，且易导致协同定位精度下降。

本发明为解决上述技术问题所采用的技术方案：

本发明提供了一种基于节点优化选择与因子图的多AUV协同定位方法，包括如下步骤：

S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；

S2、更新从艇及其邻居主艇信息；

S3、初始化主艇与从艇信息；

S4、构建多AUV协同定位系统的因子图模型；

将从艇状态变量、主艇位置信息及主艇量测信息定义为变量节点，状态方程、量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点X

S5、基于和积算法在多AUV协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；

S6、通过节点优选函数节点对主艇节点进行优选；

S7、基于优选的主艇节点，将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。

进一步地，S1中在每个采集周期T内采集系统当前动态拓扑结构信息，包括：主AUV和从AUV的数量，主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及检测目标从艇与每艘主艇之间的测距信息d。

进一步地，S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。

进一步地，S5包括如下过程：

在第k时刻系统条件概率密度函数分解为：

式中，N表示主AUV节点数量；

式中，h

定义系统的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：

其中d

通过状态方程函数节点f(X

变量节点X

式中，

根据协同定位的状态方程：

式中，(x

得到状态转换公式为：

式中，Q

式中，θ

将式(6)和式(7)代入式(5)，结合，得到：

式中，S

由此，完成了函数节点f(X

进一步地，S6包括如下过程：

分别计算各个主AUV节点的克拉美罗下界CRLB

其中，X

建立k时刻的节点优选参数矩阵：

式中，tr(·)表示矩阵的迹；N表示k时刻系统包含的主艇数量；

再利用信息熵法对节点优选参数矩阵NSPM中的参数进行加权处理，首先计算评价指标各自的比重p

式中，r

计算参数的熵值：

e

计算差异性系数：

g

计算两个指标的权重：

构建权重向量Ω：

Ω＝[ω

对节点优选参数矩阵NSPM进行加权处理：

Η

由式(18)得到的1×N矩阵Η

进一步地，S7包括如下过程：

基于优选的主艇AUV，针对k时刻系统中从AUV与第i艘主AUV之间的坐标差为

式中

计算变量节点

式中

计算变量节点

计算变量节点

将各主艇对从艇的位置估计

式中，

同样的，y

式中，

对从艇的位置估计和从艇航位推算估计进行加权平均：

进一步地，S7中k时刻下从AUV与第i艘主AUV之间的距离

一种基于节点优化选择与因子图的多AUV协同定位系统，该系统具有与上述技术方案任一项技术方案的步骤对应的程序模块，运行时执行上述的基于节点优化选择与因子图的多AUV协同定位方法中的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述技术方案中任一项所述的基于节点优化选择与因子图的多AUV协同定位方法的步骤。

相较于现有技术，本发明的有益效果是：

本发明一种基于节点优化选择与因子图的多AUV协同定位方法及系统，在传统的基于因子图的协同定位算法的基础上，通过实时检测系统的动态拓扑结构，增减因子图节点，将从艇状态信息、主艇位置信息以及主艇量测信息定义为变量节点，构建动态拓扑下协同定位因子图模型。本发明采用基于克拉美罗下界和测距评价因子的方法，筛选出系统中定位信息更准确的高质量主AUV节点，有目标地增减因子图节点以减小系统数据交互量，实现传感器的即插即用功能，保证定位精度并减小系统数据交互量，高效利用水下通信带宽资源。

本发明因子图可以对较为复杂的协同定位问题进行高效精准建模，并运用和积算法将复杂的全局问题化解成局部问题简化计算复杂度，能够大幅度提高协同定位效率，确保从AUV定位的实时性。

本发明在传统的动态拓扑下基于因子图的多AUV协同定位方法的基础上，针对系统的动态拓扑结构、主AUV数量多、质量难以保证的问题提出解决方案，兼顾了算法的定位精度、定位效率和实时性。

附图说明

图1为本发明实施例中一种基于节点优化选择与因子图的多AUV协同定位方法流程图；

图2为本发明实施例中的全局因子图模型示意图；

图3为本发明实施例中的f(Z

图4为本发明实施例中的节点优选函数局部因子图模型示意图；

图5为本发明实施例中的系统结构与AUV实际轨迹示意图；

图6为本发明实施例中的动态系统结构整体变化情况示意图；

图7为本发明实施例中的优选数量与误差RMSE关系图；

图8为本发明实施例中的定位误差对比图；

图9为本发明实施例中的X方向和Y方向上误差对比图。

具体实施方式

在本发明的描述中，应当说明的是，在本发明的实施例中所提到的术语“第一”、“第二”、“第三”仅用于描述目的，并不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者多个该特征。

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

具体实施方案一：如图1所示，本发明提供一种基于节点优化选择与因子图的多AUV协同定位方法，包括如下步骤：

S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；

S2、更新从艇及其邻居主艇信息；

S3、初始化主艇与从艇信息；

S4、构建多AUV协同定位系统的因子图模型；

将从艇状态变量、主艇位置信息及主艇量测信息定义为变量节点，状态方程、量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点X

S5、基于和积算法在多AUV协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；

S6、通过节点优选函数节点对主艇节点进行优选；

S7、基于优选的主艇节点，将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。

如图2所示，本实施方案中因子图模型中k-1时刻与k时刻的从艇状态变量节点X

如图3所示，结构Ⅰ是函数节点f(Z

如图4所示，图中结构L

具体实施方案二：S1中在每个采集周期T内采集系统当前动态拓扑结构信息，包括：主AUV和从AUV的数量，主AUV及从AUV的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及检测目标从艇与每艘主艇之间的测距信息d。本实施方案其它与具体实施方案一相同。

本实施方案中，目标从AUV的邻居主艇数量的确定方法为：设置以目标从AUV为中心、选定直径的圆形区域，确定其邻居主艇数量。根据采集信息，以从AUV为中心建立邻居节点信息，其中方差代表对应信息的不确定性。

具体实施方案三：S3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。本实施方案其它与具体实施方案二相同。

具体实施方案四：S5包括如下过程：

在第k时刻系统条件概率密度函数分解为：

式中，N表示主AUV节点数量；

式中，h

定义系统的测距信息d、航向角θ和从AUV的速度v均服从高斯分布：

其中d

通过状态方程函数节点f(X

变量节点X

式中，

根据协同定位的状态方程：

式中，(x

得到状态转换公式为：

式中，Q

式中，θ

将式(6)和式(7)代入式(5)，结合，得到：

式中，S

由此，完成了函数节点f(X

本实施方案中函数节点f(X

具体实施方案五：如图3所示，S6包括如下过程：

分别计算各个主AUV节点的克拉美罗下界CRLB

其中，X

建立k时刻的节点优选参数矩阵：

式中，tr(·)表示矩阵的迹；N表示k时刻系统包含的主艇数量；

再利用信息熵法对节点优选参数矩阵NSPM中的参数进行加权处理，首先计算评价指标各自的比重p

式中，r

计算参数的熵值：

e

计算差异性系数：

g

计算两个指标的权重：

构建权重向量Ω：

Ω＝[ω

对节点优选参数矩阵NSPM进行加权处理：

Η

由式(18)得到的1×N矩阵Η

本实施方案中，在局部因子图中，变量节点

变量节点Φ

变量节点

式中h

具体实施方案六：如图3所示，S7包括如下过程：

基于优选的主艇AUV，针对k时刻系统中从AUV与第i艘主AUV之间的坐标差为

式中

函数节点C

式中

通过函数节点A

计算变量节点

通过函数节点D和E将各主艇对从艇的位置估计与从艇位置的先验估计进行结合，得到最终位置估计：将各主艇对从艇的位置估计

式中，

同样的，y

式中，

对从艇的位置估计和从艇航位推算估计进行加权平均：

本实施方案中函数节点f(Z

具体实施方案七：S7中k时刻下从AUV与第i艘主AUV之间的距离

一种基于节点优化选择与因子图的多AUV协同定位系统，该系统具有与上述具体实施方案一至七任一项的步骤对应的程序模块，运行时执行上述的基于节点优化选择与因子图的多AUV协同定位方法中的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述具体实施方案一至七中任一项所述的基于节点优化选择与因子图的多AUV协同定位方法的步骤。

具体实施例1

为了体现本发明基于节点优化选择与因子图的多AUV协同定位方法(DS-OSFG)的性能，将本发明方法与随机选取主艇节点的基于因子图的动态拓扑下的多AUV协同定位算法(DSFG)、基于距离选取主艇节点的基于因子图的动态拓扑下的多AUV协同定位算法、基于距离的方差选取主艇节点的基于因子图的动态拓扑下的多AUV协同定位算法进行对比。

基本参数设置为：设计了10艘主艇和20艘从艇的协同定位系统仿真实验。设置总仿真时长为1000s，状态更新周期Δt＝1s。动态拓扑结构信息采集周期T＝5s。为了满足系统可观性，设计如图5所示的轨迹图，主AUV做匀速运动，均速度为v

根据动态拓扑结构系统的变化情况，将主AUV信息和主从AUV之间的测距信息进行如下设定：

第1，3，5，7，8，9号共六艘主AUV位置信息叠加均值为0，方差为

第2，4，6，10号共四艘主AUV位置信息叠加均值为0，方差为

仿真结果与分析

对于四种选取方法分别控制主AUV优选节点个数为M＝3,4,…,10，得到如图7所示的误差结果。需要说明的是，当优选节点个数多于实际节点个数时，将其调整为实际节点个数。

根据结果可以看出，本发明的DS-OSFG方法定位效果最佳，这是由于DS-OSFG方法相较于只考虑定位测距信息质量的其他算法，还考虑了主AUV节点信息质量。除了随机选取方法以外，其他三种算法的定位误差均随着节点优选数量的增加呈现出先下降后增加的趋势，因为在前期增加节点优选数量时可以通过加权减小整体定位误差，当节点优选数量较大时，质量不佳的主AUV节点信息会干扰结果，经过验证M＝6为最佳优选节点数量。

协同定位方法对比实验

固定四种节点选择方法的优选节点数量为M＝6，得到如图8与图9所示的误差结果，因为位置信息和测距信息的选取随机性，随机选取方法的定位精度最低且最不稳定。DS-OSFG算法的定位效果最好，这是由于DS-OSFG算法能够分别从节点测距信息和节点位置信息进行质量评估，在系统结构动态变化时能够更好地优选出高质量主AUV节点进行通信，保证定位精度的相对稳定。

定量分析四种算法的定位误差RMSE如表1所示，DS-OSFG算法的均方根误差相较基于距离方差、基于距离和随机选取的选取算法减少了19.75％，36.97％和58.05％。

表1

表2列出了四种算法的运行时长，因为不需要特定机制进行节点优选，随机选取法的耗时最短；DS-OSFG算法耗时最长，因为需要综合考虑节点位置信息质量和测距信息质量，计算量相较最大。

表2

经过仿真实验验证可以发现，DS-OSFG算法因为综合考虑了主节点信息质量和测距信息质量，虽然耗时略大于其他算法，但是整体定位效果优于其他算法。

虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本发明领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。