[发明专利]一种基于序贯融合算法的多AUV协同定位方法

权利要求书

1.一种基于序贯融合算法的多AUV协同定位方法，包括：

步骤1：获取水声通信网络系统传感器参数，量测信息采用错时采样、错时通信的方式传输，建立主从式多AUV状态空间模型：

其中，X(t)∈Rⁿ，U(t)∈R^m分别表示水声通信系统中AUV的位置信息向量和控制输入向量，t≥0；表示第l个从AUV接收到的第l_i个主AUV的量测信息，其中k_i表示第l_i个主AUV量测信息到达的时刻，每两个AUV采样间隔为h＝k_i+1-k_i，为方便表示假设h＝1；w(t)∈R^p表示系统过程噪声，包括速度量测噪声和方位量测噪声；表示AUV之间的水声距离量测噪声；对角阵型随机变量描述量测丢失情形，其中分量服从Bernoulli分布，概率分布为意味着成功接收到量测数据，意味着未接收到量测数据，但观测中存在量测噪声项；A，B，G，H分别为恰当维数常数矩阵；由于量测信息采用错时采样、错时通信的方式传输，因此第l个从AUV在一个采样周期(k,k+1)内依次接收m个主AUV的量测信息，按照顺序编号l₁,l₂,...,l_m，在不考虑延迟的情况下，量测信息依次到达的理想离散时刻记为k₁,k₂,...,k_m，则满足关系：k＜k₁＜k₂＜…＜k_m＝k+1；

步骤2：考虑量测丢失情形，基于卡尔曼滤波算法，给出单个量测信息的局部滤波算法，计算方程为：

其中，表示状态滤波增益阵，表示滤波误差方差阵，满足如下等式：

这里，S(k_i)满足如下李雅普诺夫方程：

步骤3：基于m个主AUV的量测信息，根据步骤2的局部滤波值，k_i-1＜t≤k_i，给出第l个从AUV在一个采样周期(k,k+1)序贯融合滤波过程:

(1)基于最初到达的第l₁个主AUV量测信息可得到t＝k₁时刻的局部滤波器并基于时间段k₁＜t≤k₂局部滤波器得到t＝k₂时刻的预测值；

(2)基于k₂时刻到达的第l₂个主AUV量测信息获得局部滤波器依此类推，基于依次到达的各主AUV的量测信息可得到第l_m个滤波器

(3)当t＝k+1时，获得第l个从AUV的状态最优序贯融合滤波器为

2.根据权利要求1所述的一种基于序贯融合算法的多AUV协同定位方法，其特征在于：采用具有串行结构的序贯融合算法设计分段连续序贯融合滤波器，给出具有高精度和实时性的多AUV协同定位方法，其不仅包含采样时刻的精确估计值，还包含采样区间的精确估计值；融合过程特征为：基于最先到达的k₁时刻量测信息设计局部分段连续滤波器进一步与k₂时刻到达量测信息融合，产生局部分段连续滤波器依次类推，融合所有量测信息得到AUV位姿信息的估计。