[发明专利]一种基于数学统计特征的因子图协同定位算法

说明书

本发明设计了一种基于数学统计特征的因子图协同定位算法。首先，建立基于数学统计特征的协同定位算法因子图模型，将传统的协同定位算法二维问题转化为一维问题，然后利用位置状态变量的期望、方差作为信度信息在因子图模型中的变量节点与函数节点之间进行传递，最后实现对从艇的位置状态信息进行滤波融合估计。从而实现在不改变系统中惯性器件的测量精度的情况下，通过避免对非线性距离观测方程的线性化处理，降低协同定位误差，提高协同定位系统的定位能力，同时还可以降低计算的复杂度，减小计算量。

(一)、技术领域

本发明涉及自主水下航行器(Autonomous Underwater Vehicle，AUV)协同定位技术，更确切地说，是在因子图与和积算法的基础上，通过利用相关变量的数学统计特征作为信度信息在变量节点与函数节点之间的迭代传递求解进行AUV的位置状态信息估计。利用该方法进行AUV的协同定位时，由于不需要对非线性距离观测方程进行线性化处理，避免了由线性化引起的误差，降低了协同定位误差，提高了协同定位系统的定位能力，同时由于将传统的协同定位算法二维问题转化为一维问题，降低计算的复杂度，减小计算量。

(二)、背景技术

面对日益复杂的作战环境和作战任务，单个无人作战系统的航行能力和载荷能力有限，难以完成信息采集、区域监控和多目标攻击等任务，使得多个无人作战系统的协同作战技术成为无人作战系统发展的必然选择。对于海洋领域，自主水下航行器(AutonomousUnderwater Vehicle,AUV)是一种主流发展趋势和发展方向，利用AUV进行协同作战不仅能够承担单体难以胜任的复杂任务，如情报收集、水雷探测、沿岸反潜、中继通信等，而且还具有高效率、高可靠性、高质量等优点。因此，AUV协同作战具有广阔的应用前景。

高精度的惯性导航设备虽然精度可以满足AUV水下定位需求，但其成本高昂；而低精度的惯性导航设备的误差随时间积累的速度过快，无法对AUV进行精确定位。对于声学导航设备，主要通过声学测距实现相对定位，且由于水声传播速度的影响，导致水声导航设备的更新率低，不能够实时提供给AUV位置信息。因此，多艇之间信息交互，利用少数平台搭载高精度惯性导航设备，并基于水声通讯及测距方法对配备低精度导航设备的AUV进行定位误差修正的协同定位方案有重要意义。

在系统可观测的前提下，协同定位算法是影响AUV定位精度的关键因素。然而在传统的协同定位算法中，扩展卡尔曼滤波(Extended Kalman Filter,EKF)算法虽然成功实现了协同定位但在面对非线性问题时，EKF实际上是将非线性系统取泰勒展开一阶项进行线性化，由于截断引起的高阶截断误差会对系统估计结果产生较大影响，降低协同定位精度。另外，无迹卡尔曼滤波(Unscented Kalman Filter,UKF)算法基于近似非线性函数的概率密度比近似非线性函数本身更简单的理念，采用确定性采样点得到相关统计参数，避免了非线性函数的线性化过程，因此避免了对非线性函数线性化产生的高阶截断误差，计算精度可达二阶以上。然而UKF算法的计算量较大、效率较低，限制了其在实时导航中的应用，并且对于观测噪声的鲁棒性还有待提高。

针对以上存在的问题，本发明设计了一种基于期望方差的因子图协同定位算法。本发明采用因子图进行数据融合，在协同定位系统中的噪声为高斯噪声的情况下，利用和积算法实现因子图中信息的更新，因子图和积算法将复杂的运算转变为几个简单的运算，算法中不涉及Jacobian矩阵运算，降低了计算的复杂度。因子图中不需要对距离观测方程线性化，避免了由线性化引起的误差，降低了协同定位误差，提高协同定位系统的定位能力。因子图是没有方向性的，大多数基于因子图的问题都是通过“信度信息”在变量节点和函数节点之间的迭代传递求解的，在本发明中，使用相关变量的期望和方差作为信度信息。

(三)、发明内容

本发明的目的在于设计一种基于期望方差的因子图协同定位算法，在不改变惯性器件精度的前提下，通过对协同定位算法的优化，降低算法的复杂度，提高系统的定位精度。

本发明的目的可通过以下步骤实现：