买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工业大学(威海)

摘要：本发明一种动态拓扑下基于因子图的多AUV协同定位方法及系统，涉及AUV协同定位技术领域，为解决现有的技术无法应对系统拓扑结构的动态变化，无法兼顾的定位精度、定位实时性的问题。包括如下过程：采集多AUV协同定位系统当前时刻动态拓扑结构信息；更新从艇及其邻居主艇信息；构建因子图模型，将从艇状态变量及主艇量测信息定义为变量节点，采用状态方程函数节点并基于和积算法在多AUV协同定位系统因子图模型中的传递更新，采用量测方程函数节点对将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值。本发明兼顾了算法的定位效率及系统的拓展性和灵活性，保证定位结果的精确性。

主权项：1.一种动态拓扑下基于因子图的多AUV协同定位方法，其特征在于，包括如下步骤：S1、采集多AUV协同定位系统当前时刻动态拓扑结构信息；S2、更新从艇及其邻居主艇信息；S3、初始化主艇与从艇信息；S4、构建多AUV协同定位系统因子图模型；将从艇状态变量及主艇量测信息定义为变量节点，状态方程和量测方程定义为函数节点，采用状态方程函数节点对从艇状态变量节点Xk进行传递更新，采用量测方程函数节点对主艇量测信息节点Zk及从艇状态变量节点Xk进行融合更新；S5、基于和积算法在多AUV协同定位系统因子图模型中的传递更新，在因子图中的两个方向各传递一次信息，实现全局因子图节点信息传递与更新；S6、将状态变量节点与量测变量节点信息进行融合与更新，得到当前时刻从艇位置信息估计值；S5包括如下过程：在第k时刻系统条件概率密度函数分解为： 式中，N表示主AUV节点数量；表示第n艘主AUV的量测信息，n＝1,2,…,N；Xm,n表示第n艘主AUV的位置信息，n＝1,2,…,N；fi表示各个AUV节点对应的概率因子，即： 式中，hi.表示量测函数；zi表示量测真实值；∑i表示对应量测误差的协方差矩阵；定义系统采集到的测距信息d、航向角θ和从AUV的速度v均服从高斯分布： 其中di表示从AUV与第i艘主AUV之间的测距信息；通过状态方程函数节点fXk|Xk-1向变量节点Xk传递的信息为： 变量节点Xk向状态方程函数节点fXk|Xk-1传递的信息为： 式中，和分别代表状态变量Xk的先验估计和方差；根据协同定位的位置方程： 式中，xk,yk表示k时刻AUV在参考坐标系的坐标；vk表示k时刻AUV前向速度；θk表示k时刻AUV的航向角度；Δt表示采样间隔；得到状态转换公式为： 式中，Qk是系统过程噪声协方差矩阵，为量测噪声矩阵，的表达式为： 式中，θk是k时刻对应的航向角信息；将式6和式7代入式5，结合，得到最终信度信息： 式中，Sk的表达式为： 最终，实现全局因子图节点信息传递与更新；S6包括如下过程：针对k时刻系统中从AUV与第i艘主AUV之间的坐标差为和计算变量节点的信度信息为： 式中和分别代表和的标准差，表示k时刻下从AUV与第i艘主AUV之间的距离；计算变量节点传递的信度信息为： 式中代表的标准差；计算变量节点和的信度信息分别为： 计算变量节点和的信度信息分别为： 将各主艇对从艇的位置估计传递到xk，即： 式中，和为xk的方差和期望；同样的，yk的信息为： 式中，和为yk的方差和期望；对从艇的位置估计和从艇航位推算估计进行加权平均：

全文数据：

权利要求：

百度查询： 哈尔滨工业大学(威海) 一种动态拓扑下基于因子图的多AUV协同定位方法及系统