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

一种基于因子图的节点优选的多auv自适应协同定位方法及系统
技术领域
1.本发明涉及多auv协同定位技术领域，具体而言，涉及一种基于因子图的节点优选的多auv自适应协同定位方法及系统。


背景技术：

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


技术实现要素：

3.本发明要解决的技术问题是：
4.现有的技术无法适用于系统拓扑结构的动态变化，且无法有效消除模糊时变噪声的干扰及系统规模较大带来的通信压力。
5.本发明为解决上述技术问题所采用的技术方案：
6.本发明提供了一种基于因子图的节点优选的多auv自适应协同定位方法，包括如下步骤：
7.s1、采集多auv协同定位系统当前时刻动态拓扑结构信息；
8.s2、更新从艇及其邻居主艇信息；
9.s3、初始化主艇与从艇信息；
10.s4、构建多auv协同定位系统因子图模型；
11.将从艇状态变量、主艇位置信息及主艇量测信息定义为变量节点，将状态方程和量测方程定义为函数节点，并定义自适应迭代估计函数节点及节点优选函数节点；采用状态方程函数节点对从艇状态变量节点xk进行传递更新，采用量测方程函数节点对主艇量测信息节点zk及从艇状态变量节点xk进行融合更新，所述自适应迭代估计函数节点基于em算法的自适应ekf滤波器，以估计量测噪声协方差矩阵，所述节点优选函数节点，针对系统主
艇估计的位置信息节点φk及主艇量测信息节点zk，分别进行克拉美罗下界crlbk和测距评价因子计算，对当前时刻主艇节点进行优选；
12.s5、基于和积算法在多auv协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；
13.s6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；
14.s7、通过节点优选函数节点对主艇节点进行优选；
15.s8、基于优选的主艇节点，将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。
16.进一步地，s1中在每个采集周期t内采集系统当前动态拓扑结构信息，包括：主auv和从auv的数量，主auv及从auv的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及检测目标从艇与每艘主艇之间的测距信息d。
17.进一步地，s3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。
18.进一步地，s5包括如下过程：
19.在第k时刻系统条件概率密度函数分解为：
[0020][0021]
式中，n表示主auv节点数量；表示第n(n＝1,2,
…
,n)艘主auv的量测信息；x
m,n
表示第n(n＝1,2,
…
,n)艘主auv的位置信息；fi表示各个auv节点对应的概率因子，即：
[0022][0023]
式中，hi(.)表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；
[0024]
定义系统采集到的测距信息d、航向角θ和从auv的速度v均服从高斯分布：
[0025][0026]
其中di表示从auv与第i艘主auv之间的测距信息；
[0027]
通过状态方程函数节点f(xk|x
k-1
)向变量节点xk传递的信息为：
[0028][0029]
变量节点xk向状态方程函数节点f(xk|x
k-1
)传递的信息为：
[0030][0031]
式中，和分别代表状态变量xk的先验估计和方差；
[0032]
根据协同定位的位置方程：
[0033][0034]
式中，(xk,yk)表示k时刻auv在参考坐标系的坐标；vk表示k时刻auv前向速度；θk表示k时刻auv的航向角度；δt表示采样间隔；
[0035]
得到状态转换公式为：
[0036][0037]
式中，qk是系统过程噪声协方差矩阵，为量测噪声矩阵，的表达式为：
[0038][0039]
式中，θk是k时刻对应的航向角信息；
[0040]
将式(6)和式(7)代入式(5)，结合，得到最终信度信息：
[0041][0042]
式中，sk的表达式为：
[0043][0044]
最终，实现全局因子图节点信息传递与更新。
[0045]
进一步地，s6中根据ekf滤波算法构建从艇状态变量方程，即：
[0046][0047]
式中，表示k-1时刻得到的k时刻的估计值，f表示状态转移矩阵，uk表示控制输入，
[0048]
构建相应的估计误差协方差矩阵，即：
[0049]
p
k|k-1
＝fp
k|k-1
ft+q
k-1
ꢀꢀꢀ
(12)
[0050]
更新从艇状态变量和估计误差协方差矩阵。
[0051]
根据em算法，首先确定初始值：
[0052][0053]
进行第l+1步滤波增益矩阵的迭代更新：
[0054][0055]
式中，为：
[0056][0057]
利用式(14)更新状态变量：
[0058][0059]
更新误差协方差矩阵：
[0060][0061]
估计量测噪声协方差矩阵rk：
[0062][0063]
迭代n次收敛后得到rk的估计结果：
[0064][0065]
进一步地，s7包括如下过程：
[0066]
分别计算各个主auv节点的克拉美罗下界crlbk和测距评价因子αi：
[0067][0068]
其中，x
k-1
＝[x
k-1
,y
k-1
]
t
表示k-1时刻从auv的状态变量；表示k时刻第i艘主auv的位置信息；r
x
表示x
k-1
的误差协方差矩阵；r
φ
表示的误差协方差矩阵；di表示从auv与第i艘主auv的测距信息；表示di的标准差；
[0069]
建立k时刻的节点优选参数矩阵：
[0070][0071]
式中，tr(
·
)表示矩阵的迹；n表示k时刻系统包含的主艇数量；
[0072]
再利用信息熵法对节点优选参数矩阵nspm中的参数进行加权处理，首先计算评价指标各自的比重pi：
[0073][0074]
式中，r1表示crlb评价参数；r2表示测距评价因子；
[0075]
计算参数的熵值：
[0076]ei
＝-p
i ln(pi)
ꢀꢀ
(22)
[0077]
计算差异性系数：
[0078]gi
＝1-eiꢀꢀ
(23)
[0079]
计算两个指标的权重：
[0080][0081]
构建权重向量ω：
[0082]
ω＝[ω
1 ω2]
ꢀꢀ
(25)
[0083]
对节点优选参数矩阵nspm进行加权处理：
[0084]hk
＝ω
·
nspmkꢀꢀ
(26)
[0085]
由式(26)得到的1
×
n矩阵hk中，每一列的数值对应系统中相应主auv的最终评价结果，筛选其中m个最小的结果，对应的主auv作为优选结果。
[0086]
进一步地，s8包括如下过程：
[0087]
针对k时刻系统中从auv与第i艘主auv之间的坐标差为和计算变量节点的信度信息为：
[0088][0089]
式中和分别代表和的标准差，表示k时刻下从auv与第i艘主auv之间的距离；
[0090]
计算变量节点传递的信度信息为：
[0091][0092]
式中代表的标准差；
[0093]
计算变量节点和的信度信息分别为：
[0094][0095][0096]
计算变量节点和的信度信息分别为：
[0097][0098][0099]
将各主艇对从艇的位置估计传递到xk，即：
[0100][0101]
式中，和为xk的方差和期望；
[0102]
同样的，yk的信息为：
[0103][0104]
式中，和为yk的方差和期望；
[0105]
对从艇的位置估计和从艇航位推算估计进行加权平均：
[0106][0107][0108]
进一步地，s8中k时刻下从auv与第i艘主auv之间的距离与和的关系为：
[0109][0110]
一种基于因子图的节点优选的多auv自适应协同定位系统，该系统具有与上述技术方案任一项技术方案的步骤对应的程序模块，运行时执行上述的基于因子图的节点优选的多auv自适应协同定位方法中的步骤。
[0111]
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述技术方案中任一项所述的基于因子图的节点优选的多auv自适应协同定位方法的步骤。
[0112]
相较于现有技术，本发明的有益效果是：
[0113]
本发明一种基于因子图的节点优选的多auv自适应协同定位方法及系统，通过实时检测系统的动态拓扑结构，增减因子图节点，将从艇状态信息、主艇位置信息以及主艇量测信息定义为变量节点，构建动态拓扑下的多auv协同定位系统因子图模型。在动态拓扑的协同定位系统基础上进一步考虑模糊时变噪声对基于因子图的协同定位干扰，通过引入最大期望算法(expectation maximization algorithm,em)的自适应ekf滤波对量测噪声协方差进行估计，去除量测噪声协方差中的不确定性。采用基于克拉美罗下界和测距评价因子的方法，筛选出系统中定位信息更准确的高质量主auv节点，有目标地增减因子图节点以减小系统数据交互量，保证定位精度并减小系统数据交互量，高效利用水下通信带宽资源。
[0114]
本发明针对系统的动态拓扑结构、主auv数量多、模糊时变噪声的干扰问题提出解决方案，兼顾了算法的定位精度、定位效率和实时性。
附图说明
[0115]
图1为本发明实施例中基于因子图的节点优选的多auv自适应协同定位方法流程图；
[0116]
图2为本发明实施例中的全局因子图模型示意图；
[0117]
图3为本发明实施例中的h(z,φ,x)的因子图模型示意图；
[0118]
图4为本发明实施例中的f(zk|xk)的因子图模型示意图；
[0119]
图5为本发明实施例中的系统结构与auv实际轨迹示意图；
[0120]
图6为本发明实施例中的动态系统结构整体变化情况示意图；
[0121]
图7为本发明实施例中的定位误差对比图；
[0122]
图8为本发明实施例中的x方向和y方向上误差对比图。
具体实施方式
[0123]
在本发明的描述中，应当说明的是，在本发明的实施例中所提到的术语“第一”、“第二”、“第三”仅用于描述目的，并不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者多个该特征。
[0124]
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
[0125]
具体实施方案一：如图1所示，本发明提供一种基于因子图的节点优选的多auv自适应协同定位方法，包括如下步骤：
[0126]
s1、采集多auv协同定位系统当前时刻动态拓扑结构信息；
[0127]
s2、更新从艇及其邻居主艇信息；
[0128]
s3、初始化主艇与从艇信息；
[0129]
s4、构建多auv协同定位系统因子图模型；
[0130]
将从艇状态变量、主艇位置信息及主艇量测信息定义为变量节点，将状态方程和量测方程定义为函数节点，并定义自适应迭代估计函数节点及节点优选函数节点；采用状态方程函数节点对从艇状态变量节点xk进行传递更新，采用量测方程函数节点对主艇量测信息节点zk及从艇状态变量节点xk进行融合更新，所述自适应迭代估计函数节点基于em算法的自适应ekf滤波器，以估计量测噪声协方差矩阵，所述节点优选函数节点，针对系统主艇估计的位置信息节点φk及主艇量测信息节点zk，分别进行克拉美罗下界crlbk和测距评价因子计算，对当前时刻主艇节点进行优选；
[0131]
s5、基于和积算法在多auv协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；
[0132]
s6、基于所述自适应迭代估计函数节点对量测噪声协方差矩阵进行迭代更新；
[0133]
s7、通过节点优选函数节点对主艇节点进行优选；
[0134]
s8、基于优选的主艇节点，将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。
[0135]
如图2所示，本实施方案中构建的系统因子图模型中k-1时刻与k时刻的从艇状态变量节点x
k-1
与xk通过状态方程函数节点f(xk|x
k-1
)相连接；主艇量测信息节点zk与从艇状态变量节点xk之间通过量测方程函数节点f(zk|xk)相连接；从艇状态变量节点x
k-1
、xk及主艇量测信息节点zk通过自适应迭代估计函数节点ik相连接；主艇量测信息节点zk、主艇位置信息节点φk和从艇的状态变量节点x
k-1
与xk通过节点优选函数节点h(z,φ,x)相连接，以对主艇节点进行优化选择。
[0136]
如图3所示，图中结构l1,l2,
…
,ln分别表示k时刻系统的n艘主auv的信息，每个结构包含主艇的位置信息节点φk和测距相关信息函数节点f和g利用主艇的信息，分别完成crlb和测距评价因子的计算，得到αk和βk，函数节点h利用变量节点αk和βk的信息，完成加权运算得到最终评价结果hk，筛选其中数值最小的m个结果其对应的主auv即为优选结果。图中的ⅲ型结构对应单个主auv节点的优化选择过程。
[0137]
如图4所示，结构ⅰ是函数节点f(zk|xk)分解后的具体结构，完成主auv与从auv的数
据融合，结构ⅱ是量测信息zk的具体结构，包括k时刻的各个主auv对应的测距信息。
[0138]
具体实施方案二：s1中在每个采集周期t内采集系统当前动态拓扑结构信息，包括：主auv和从auv的数量，主auv及从auv的位置信息、速度信息v、角速度信息及航向角信息θ，并计算检测各采集量的方差，及检测目标从艇与每艘主艇之间的测距信息d。本实施方案其它与具体实施方案一相同。
[0139]
具体实施方案三：s3中所述初始化主艇与从艇信息，包括：初始化主艇与从艇的位置信息、速度信息v、航向角信息θ及主从艇之间的测距信息d。本实施方案其它与具体实施方案二相同。
[0140]
具体实施方案四：s5包括如下过程：
[0141]
在第k时刻系统条件概率密度函数分解为：
[0142][0143]
式中，n表示主auv节点数量；表示第n(n＝1,2,
…
,n)艘主auv的量测信息；x
m,n
表示第n(n＝1,2,
…
,n)艘主auv的位置信息；fi表示各个auv节点对应的概率因子，即：
[0144][0145]
式中，hi(.)表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；
[0146]
定义系统采集到的测距信息d、航向角θ和从auv的速度v均服从高斯分布：
[0147][0148]
其中di表示从auv与第i艘主auv之间的测距信息；
[0149]
通过状态方程函数节点f(xk|x
k-1
)向变量节点xk传递的信息为：
[0150][0151]
变量节点xk向状态方程函数节点f(xk|x
k-1
)传递的信息为：
[0152][0153]
式中，和分别代表状态变量xk的先验估计和方差；
[0154]
根据协同定位的位置方程：
[0155][0156]
式中，(xk,yk)表示k时刻auv在参考坐标系的坐标；vk表示k时刻auv前向速度；θk表示k时刻auv的航向角度；δt表示采样间隔；
[0157]
得到状态转换公式为：
[0158][0159]
式中，qk是系统过程噪声协方差矩阵，为量测噪声矩阵，的表达式为：
[0160][0161]
式中，θk是k时刻对应的航向角信息；
[0162]
将式(6)和式(7)代入式(5)，结合，得到最终信度信息：
[0163][0164]
式中，sk的表达式为：
[0165][0166]
最终，实现全局因子图节点信息传递与更新。本实施方案其它与具体实施方案一相同。
[0167]
本实施方案中函数节点f(xk|x
k-1
)基于从艇的状态函数，利用上一时刻的从艇位置信息、这一时刻的从艇速度和航向角信息，推算这一时刻的从艇位置信息。
[0168]
具体实施方案五：s6中根据ekf滤波算法构建从艇状态变量方程，即：
[0169][0170]
式中，表示k-1时刻得到的k时刻的估计值，f表示状态转移矩阵，uk表示控制输入，
[0171]
构建相应的估计误差协方差矩阵，即：
[0172]
p
k|k-1
＝fp
k|k-1
ft+q
k-1
ꢀꢀꢀ
(12)
[0173]
更新从艇状态变量和估计误差协方差矩阵。
[0174]
根据em算法，首先确定初始值：
[0175][0176]
进行第l+1步滤波增益矩阵的迭代更新：
[0177][0178]
式中，为：
[0179][0180]
利用式(14)更新状态变量：
[0181][0182]
更新误差协方差矩阵：
[0183][0184]
估计量测噪声协方差矩阵rk：
[0185][0186]
迭代n次收敛后得到rk的估计结果：
[0187]
本实施方案其它与具体实施方案四相同。
[0188]
具体实施方案六：如图3所示，s7包括如下过程：
[0189]
分别计算各个主auv节点的克拉美罗下界crlbk和测距评价因子αi：
[0190][0191]
其中，x
k-1
＝[x
k-1
,y
k-1
]
t
表示k-1时刻从auv的状态变量；表示k时刻第i艘主auv的位置信息；r
x
表示x
k-1
的误差协方差矩阵；r
φ
表示的误差协方差矩阵；di表示从auv与第i艘主auv的测距信息；表示di的标准差；
[0192]
建立k时刻的节点优选参数矩阵：
[0193][0194]
式中，tr(
·
)表示矩阵的迹；n表示k时刻系统包含的主艇数量；
[0195]
再利用信息熵法对节点优选参数矩阵nspm中的参数进行加权处理，首先计算评价指标各自的比重pi：
[0196][0197]
式中，r1表示crlb评价参数；r2表示测距评价因子；
[0198]
计算参数的熵值：
[0199]ei
＝-p
i ln(pi)
ꢀꢀ
(22)
[0200]
计算差异性系数：
[0201]gi
＝1-eiꢀꢀ
(23)
[0202]
计算两个指标的权重：
[0203][0204]
构建权重向量ω：
[0205]
ω＝[ω
1 ω2]
ꢀꢀ
(25)
[0206]
对节点优选参数矩阵nspm进行加权处理：
[0207]hk
＝ω
·
nspmkꢀꢀ
(26)
[0208]
由式(26)得到的1
×
n矩阵hk中，每一列的数值对应系统中相应主auv的最终评价结果，筛选其中m个最小的结果，对应的主auv作为优选结果。本实施方案其它与具体实施方案五相同。
[0209]
具体实施方案七：如图4所示，s8包括如下过程：
[0210]
针对k时刻系统中从auv与第i艘主auv之间的坐标差为和变量节点和分别通过函数节点ci完成信息更新，计算变量节点的信度信息为：
[0211][0212]
式中和分别代表和的标准差，表示k时刻下从auv与第i艘主auv之间的距离；
[0213]
函数节点ci向变量节点传递的信息，计算变量节点传递的信度信息为：
[0214][0215]
式中代表的标准差；
[0216]
通过函数节点ai和bi进行位置信息的转换，即函数节点ai传递到变量节点和计算变量节点和的信度信息分别为：
[0217][0218][0219]
计算变量节点和的信度信息分别为：
[0220][0221][0222]
通过函数节点d和e将各主艇对从艇的位置估计与从艇位置的先验估计进行结合，得到最终位置估计：将各主艇对从艇的位置估计传递到xk，即：
[0223][0224]
式中，和为xk的方差和期望；
[0225]
同样的，yk的信息为：
[0226][0227]
式中，和为yk的方差和期望；
[0228]
对从艇的位置估计和从艇航位推算估计进行加权平均：
[0229][0230][0231]
最终，得到当前时刻从艇位置信息估计值。本实施方案其它与具体实施方案六相同。
[0232]
本实施方案中函数节点f(zk|xk)基于主艇的量测方程，对主从艇坐标差与主艇量测信息进行融合更新，再对主从艇坐标差与从艇位置估计信息进行融合更新，最后通过对各主艇对从艇的位置估计与从艇航位推算估计加权平均得到从艇位置估计。
[0233]
具体实施方案八：s8中k时刻下从auv与第i艘主auv之间的距离与和的关系为：
[0234]
本实施方案其它与具体实施方案七相同。
[0235]
一种基于因子图的节点优选的多auv自适应协同定位系统，该系统具有与上述实施方案一至八任一项实施方案的步骤对应的程序模块，运行时执行上述的基于因子图的节点优选的多auv自适应协同定位方法中的步骤。
[0236]
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现实施方案一至八中任一项所述的基于因子图的节点优选的多auv自适应协同定位方法的步骤。
[0237]
具体实施例1
[0238]
将本发明的基于因子图的节点优选的多auv自适应协同定位方法(dynamic structure-based adaptive optimized selection and factor graph,ds-aosfg)与基于本发明的未进行量测噪声协方差矩阵处理的基于节点优化选择与因子图的多auv协同定位方法(dynamic structure-based optimized selection and factor graph,ds-osfg)进行仿真分析。
[0239]
算法的基本参数设置为：设计了10艘主艇和20艘从艇的协同定位系统仿真实验。设置总仿真时长为1000s，状态更新周期δt＝1s。动态拓扑结构检测周期δt＝5s。为了满足系统可观性，设计如图5所示的轨迹图，主auv做匀速运动，均速度为vm＝2m/s，从auv做s型曲线运动，速度为vs＝6m/s；为了实现系统结构的动态拓扑，设置从auv的信息处理范围为以自身为中心的直径3000m的圆形区域，系统动态变化情况如图6所示；基于水声测距场景，设定主auv与从auv的速度和航向角的测距方差分别为和在ds-aosfg算法对比实验中，将1，3，5，7～9号共六艘主auv位置信息叠加均值为0，方差为高斯白噪声，设定量测噪声均值为0，方差为2，4，6，10号共四艘主auv节点的位置信息叠加均值为0，方差为高斯白噪声，设定量测噪声均值为1，方差为
[0240]
量测噪声均值估计初始值选取方差估计初始值选取系统状态估计初始值为：
[0241][0242]
p
0|0
＝diag[1 1 0.01]
[0243]
仿真结果与分析
[0244]
优选节点数量设置为m＝6，将ds-aosfg算法与ds-osfg算法分别应用于仿真场景中。两种算法的定位误差如图7和图8所示，ds-osfg算法的定位误差较大且波动明显，这是因为模糊时变噪声干扰了高质量auv节点的筛选，测距误差增大也影响了算法的定位效果；ds-aosfg通过自适应估计量测噪声协方差矩阵，不但提高了量测更新过程中状态变量估计精度，还能够为节点优选机制提供更精确的量测噪声方差估计结果。表1给出了两种算法的均方根误差(root mean square error，rmse)。从表中可以看出，ds-aosfg算法比ds-osfg算法的rmse减小了48.63％，这是由于ds-aosfg算法中采用了自适应ekf滤波器，在定位过程中能够保证量测噪声方差的自适应估计，减小了定位误差。实验结果验证了ds-aosfg算法在提高auv节点优选效果和定位精度方面的有效性。
[0245]
表1
[0246][0247]
经过仿真实验验证可以发现，在模糊时变噪声干扰下，ds-aosfg算法相较于ds-osfg算法能够更好地选择出高质量节点，提高系统定位精度。
[0248]
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本发明领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

技术特征：
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时刻的估计值，f表示状态转移矩阵，u
k
表示控制输
入，构建相应的估计误差协方差矩阵，即：p
k|k-1
＝fp
k|k-1
f
t
+q
k-1
ꢀꢀꢀꢀ
(12)更新从艇状态变量和估计误差协方差矩阵。根据em算法，首先确定初始值：进行第l+1步滤波增益矩阵的迭代更新：式中，为：利用式(14)更新状态变量：更新误差协方差矩阵：估计量测噪声协方差矩阵r
k
：迭代n次收敛后得到r
k
的估计结果：6.根据权利要求5所述的基于因子图的节点优选的多auv自适应协同定位方法，其特征在于，s7包括如下过程：分别计算各个主auv节点的克拉美罗下界crlb
k
和测距评价因子α
i
：其中，x
k-1
＝[x
k-1
,y
k-1
]
t
表示k-1时刻从auv的状态变量；表示k时刻第i艘主auv的位置信息；r
x
表示x
k-1
的误差协方差矩阵；r
φ
表示的误差协方差矩阵；d
i
表示从auv与第i艘主auv的测距信息；表示d
i
的标准差；建立k时刻的节点优选参数矩阵：
式中，tr(
·
)表示矩阵的迹；n表示k时刻系统包含的主艇数量；再利用信息熵法对节点优选参数矩阵nspm中的参数进行加权处理，首先计算评价指标各自的比重p
i
：式中，r1表示crlb评价参数；r2表示测距评价因子；计算参数的熵值：e
i
＝-p
i ln(p
i
)
ꢀꢀꢀꢀ
(22)计算差异性系数：g
i
＝1-e
i
ꢀꢀꢀꢀ
(23)计算两个指标的权重：构建权重向量ω：ω＝[ω
1 ω2]
ꢀꢀꢀꢀ
(25)对节点优选参数矩阵nspm进行加权处理：h
k
＝ω
·
nspm
k
ꢀꢀꢀꢀ
(26)由式(26)得到的1
×
n矩阵h
k
中，每一列的数值对应系统中相应主auv的最终评价结果，筛选其中m个最小的结果，对应的主auv作为优选结果。7.根据权利要求6所述的基于因子图的节点优选的多auv自适应协同定位方法，其特征在于，s8包括如下过程：针对k时刻系统中从auv与第i艘主auv之间的坐标差为和计算变量节点的信度信息为：式中和分别代表和的标准差，表示k时刻下从auv与第i艘主auv之间的距离；计算变量节点传递的信度信息为：式中代表的标准差；计算变量节点和的信度信息分别为：
计算变量节点和的信度信息分别为：的信度信息分别为：将各主艇对从艇的位置估计传递到x
k
，即：式中，和为x
k
的方差和期望；同样的，y
k
的信息为：式中，和为y
k
的方差和期望；对从艇的位置估计和从艇航位推算估计进行加权平均：对从艇的位置估计和从艇航位推算估计进行加权平均：8.根据权利要求7所述的基于因子图的节点优选的多auv自适应协同定位方法，其特征在于，s8中k时刻下从auv与第i艘主auv之间的距离与和的关系为：9.一种基于因子图的节点优选的多auv自适应协同定位系统，其特征在于，该系统具有与上述权利要求1～8任一项权利要求的步骤对应的程序模块，运行时执行上述的基于因子图的节点优选的多auv自适应协同定位方法中的步骤。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1～8中任一项所述的基于因子图的节点优选的多auv自适应协同定位方法的步骤。

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


技术研发人员：罗清华 林家祺 王金龙 陈燕怡
受保护的技术使用者：哈尔滨工业大学（威海）
技术研发日：2023.07.12
技术公布日：2023/9/19