一种基于序贯融合算法的多auv协同定位方法
阅读说明：本技术 一种基于序贯融合算法的多auv协同定位方法 (Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sequential fusion algorithm ) 是由 韩春艳 王玮 李雪扬 张颂 于 2021-05-20 设计创作，主要内容包括：本发明公开了一种基于序贯融合算法的多AUV协同定位方法,包括：(1)建立多AUV协同定位系统模型,连续状态方程和离散观测方程；(2)量测信息采用错时采样、错时通信的方式；(3)采用卡尔曼滤波算法,给出单个量测信息的局部分段连续滤波器；(4)采用序贯融合方法,逐步处理采样周期内观测数据,给出当前时刻AUV的位姿信息精确估计值。本发明有益效果：量测丢失过程被描述为对角阵型随机变量,其中分量为Bernoulli随机过程,更具有普适性；采用具有串行结构的序贯融合算法设计分段连续序贯融合滤波器,给出具有高精度和实时性的多AUV协同定位方法,使其包含采样时刻和采样区间的精确估计值。本发明不仅可降低计算负担,还可以提高定位的实时性。(The invention discloses a multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on a sequential fusion algorithm, which comprises the following steps: (1) establishing a multi-AUV cooperative positioning system model, a continuous state equation and a discrete observation equation; (2) the measurement information adopts the modes of time-staggered sampling and time-staggered communication; (3) a local segmented continuous filter of single measurement information is given by adopting a Kalman filtering algorithm; (4) and (3) processing observation data in a sampling period step by adopting a sequential fusion method, and giving an accurate estimation value of the pose information of the AUV at the current moment. The invention has the beneficial effects that: the measurement loss process is described as a diagonal matrix type random variable, wherein the components are Bernoulli random processes and have universality; a sequential fusion algorithm with a serial structure is adopted to design a segmented continuous sequential fusion filter, and a multi-AUV co-location method with high precision and real-time performance is provided, so that the multi-AUV co-location filter comprises accurate estimated values of sampling time and sampling interval. The invention can not only reduce the calculation burden, but also improve the real-time performance of positioning.)
技术领域
本发明涉及网络控制技术领域，主要针对复杂水下环境影响下的水声通信系统，给出一 种带有丢失量测的水声通信系统的多AUV协同定位方法。
背景技术
随着海洋探索和资源开发力度不断增大，单水下自主作业机器人(Autonomousunderwater vehicle，AUV)越来越难满足人类对水下机器人作业系统日益复杂和多样的需求，从而使得 多AUV协同作业系统受到了国内外学者越来越多的重视和深入研究，并逐渐成为水下机器 人领域的一种主流趋势和发展方向。水下高精度的导航定位是完成水下协同作业任务的关键 一环，对于水下作业机器人的自主性任务实施，相互间高效协同作业起着决定性作用。鉴于 多AUV协同定位具有各AUV独自定位所不具有的诸多优点，近年来多AUV协同定位已逐 渐成为研究的热点。
可是，由于复杂的水下环境和水声通信网络所特有的窄带性、多径性、衰落性，水声信 号在水声通信信道内传输时会发生衰减甚至被干扰，进而导致AUV的量测定位信息延迟或 丢失等现象，这为多AUV协同导航定位的研究带来了很大困难。为了实现具有高精度、实 时性的水下定位，需要在模型中充分考虑这些随机因素的影响。然而，在现有的定位算法中， 往往选择性忽略某些随机因素的影响。因此，设计能够提高多AUV导航定位精度的协同定 位算法具有重要的理论意义和实用价值。
众所周知，协同定位问题可归结为状态估计问题，因此滤波算法成为了影响协同定位精 度和实时性的一个关键因素。现阶段滤波算法的研究已取得了丰富的成果。其中，序贯融合 滤波算法采用当前时刻传感器观测数据以序贯的处理方式逐步更新状态估值，最终得到当前 状态实时最优估计值。发明人发现，由于是将对观测值的集中扩维分散为按各局部观测的顺 序处理，从而将求解高维矩阵转化为求解低维矩阵，因此序贯融合滤波算法达到了减少计算 负担的目的。并且它由序贯的计算若干个局部卡尔曼滤波器组成，因此也可保障估计精度的 全局最优性。然而当前基于序贯融合算法设计水声通信系统多AUV协同定位算法的研究还 相对匮乏。
发明内容
为了解决上述问题，本发明提出了一种基于序贯融合算法的多AUV协同定位方法，旨 在通过设计序贯融合滤波器，针对水声通信网络中存在丢失量测的情况，给出具有较高定位 精度和实时性的多AUV协同定位方法。
为了实现上述目的，本发明采用下述技术方案。该方案具体步骤如下所示。
步骤1：获取水声通信网络系统传感器参数，量测信息采用错时采样、错时通信的方式传 输，建立主从式多AUV状态空间模型：
其中，X(t)∈Rn，U(t)∈Rm分别表示水声通信系统中AUV的位置信息向量和控制输入向量， t≥0；表示第l个从AUV接收到的第li个主AUV的量测信息，其中ki表示第li个 主AUV量测信息到达的时刻，每两个AUV采样间隔为h＝ki+1-ki，为方便表示假设h＝1； w(t)∈Rp表示系统过程噪声，包括速度量测噪声和方位量测噪声；表示AUV之间的水声距离量测噪声；对角阵型随机变量描述量测丢失情 形，其中分量服从Bernoulli分布，概率分布为i，j＝1，…m。 意味着成功接收到量测数据，意味着未接收到量测数据，但观测中存在 量测噪声项。A，B，G，H分别为恰当维数常数矩阵。由于量测信息采用错时采样、错时 通信的方式传输，因此第l个从AUV在一个采样周期(k，k+1]内依次接收m个主AUV的量测信息，按照顺序编号l1，l2，...，lm，在不考虑延迟的情况下，量测信息依次到达的理想离散时刻 记为k1，k2，...，km，则满足关系：k＜k1＜k2＜…＜km＝k+1。
步骤2：考虑量测丢失情形，基于卡尔曼滤波算法，给出单个量测信息的局部滤波 算法，计算方程为：
其中，表示状态滤波增益阵，表示滤波误差方差阵，满足如下等式：
这里，S(ki)满足如下李雅普 诺夫方程：
步骤3：基于m个主AUV的量测信息，根据步骤2的局部滤波值(ki-1＜t≤ki)，给出第l 个从AUV在一个采样周期(k，k+1]序贯融合滤波过程：
(1)基于最初到达的第l1个主AUV量测信息可得到t＝k1时刻的局部滤波器并基于时间段k1＜t≤k2局部预报器得到t＝k2时刻的预测值；
(2)基于k2时刻到达的第l2个主AUV量测信息获得局部滤波器依此 类推，基于依次到达的各主AUV的量测信息可得到第lm个滤波器
(3)当t＝k+1时，获得第l个从AUV的状态最优序贯融合滤波器为
作为优选，步骤1中，w(t)是零均值和方差阵为Q的维纳过程；是零均值和方差 阵为的高斯白噪声；系统的状态初值X(0)是均值为零和方差为V0的高斯随机变量，并且 X(0)与w(t)，以及li＝l1，...，lm相互独立。
本发明由于采用了以上技术方案，具有显著的技术效果：
(1)以被军事和民用广泛应用的水声通信网络为背景，研究水下多AUV协同定位技术， 考虑其在实际过程中存在的诸多复杂多变的不确定性，建立带丢失量测的状态空间模型，量 测丢失被描述为对角阵型随机变量，其中分量为Bernoulli随机过程，更具有普适性；
(2)基于错时采样的观测值，采用具有串行结构的序贯融合算法设计分段连续序贯融合 滤波器，给出具有高精度和实时性的多AUV协同定位方法，其不仅包含采样时刻的精确估 计值，还包含采样区间的精确估计值。与集中式融合滤波相比，有效减少计算负担，降低计 算的复杂性，更适用实际工程。
(3)本发明适用于大规模的多AUV编队系统。
附图说明
图1是本发明一种主从式多AUV协同定位模型结构示意图；
图2是本发明一种水声通信网络基本结构框图；
图3是本发明一种基于量测信息错时采样的序贯融合多AUV协同定位流程示意图；
图4是本发明实施例中多AUV协同定位系统从AUV的x轴真实状态及其估计值；
图5是本发明实施例中多AUV协同定位系统从AUV的x轴真实线速度及其估计值；
图6是本发明实施例中多AUV协同定位系统从AUV的y轴真实状态及其估计值；
图7是本发明实施例中多AUV协同定位系统从AUV的y轴真实线速度及其估计值；
具体实施方式
下面结合附图与实施例对本发明作进一步详细描述。
如图1至图7所示，一种水声通信系统多AUV协同定位方法，包括如下步骤：
主从式多AUV协同定位模式如图1所示，当定位周期开始时，从AUV向主AUV1发送通信请求信号，主AUV1采用水声Modem向从AUV广播自身的位置信息。从AUV反解码 得到主AUV位置信息以及通过测距技术获得相对距离信息。利用相同的工作原理，从AUV 可得到主AUV2的位置信息和它们之间的相对距离信息。从AUV利用这些接收到的信息， 基于相应的协同定位滤波算法实现自身位置的定位。由于复杂的水下环境，以及水声通信网 络所特有的窄带性、多径性、衰落性，水声信号在水声通信信道内传输时会发生衰减甚至被 干扰，进而导致主AUV的量测定位信息延迟或丢失等现象，水声通信网络结构如图2所示。
将实际的水声通信网络中多AUV协同导航定位根据其原理合理地抽象成一类数学模型， 需要满足如下条件：
(1)本发明考虑同一水平面的AUV系统，使得AUV装配深度压力传感器，可以独立准确测量AUV深度，可将不同深度UUV的距离投影到水平面，且不考虑两AUV在水平面重 合的情况。AUV具有良好的静态稳定性，且处于平衡状态；
(2)为了保证从AUV能够按照时间顺序将接收的信息依次排序，需要对主从AUV进行 时钟同步；
(3)由于水声Modem具有独立的测距模式，因此假设水声测距过程中获得距离相对准确；
(4)针对水声通信的窄带性特点，为了避免信道共享引发的网络堵塞，在MAC层采用时 分复用的机制共享信道，对不同的AUV分配不同的时隙，采用错时采样、错时通信的方式。 根据主从式协同定位的特点，从AUV分别接收来自不同主AUV的量测信息，按照信息发送 时间顺序依次利用主AUV的量测信息完整自身的定位。本发明忽略量测信息延迟的情形。
水声通信网络中多AUV协同定位过程可以被刻画为一个线性采样系统，即水下多AUV 协同定位系统的数学模型具体为：
其中，X(t)∈Rn，U(t)∈Rm分别表示水声通信系统中AUV的位置信息向量和控制输入向量， t≥0；表示第l个从AUV接收到的第li个主AUV的量测信息，其中ki表示第li个 主AUV量测信息到达的时刻，每两个AUV采样间隔为h＝ki+1-ki，为方便表示假设h＝1； w(t)∈Rp表示系统过程噪声，包括速度量测噪声和方位量测噪声；表示AUV之间的水声距离量测噪声；对角阵型随机变量描述量测丢失情 形，其中分量服从Bernoulli分布，概率分布为i，j＝1，…m。 意味着成功接收到量测数据，意味着未接收到量测数据，但观测中存在 量测噪声项。具体来说，量测信息采用错时采样、错时通信的方式传输，因此第l个从AUV 在一个采样周期(k，k+1]内依次接收m个主AUV的量测信息，按照顺序编号l1，...，lm，量测依 次到达的理想离散时刻记为k1，k2，...，km，则满足关系：k＜k1＜k2＜…＜km＝k+1。A，B，G， H分别为恰当维数常数矩阵。w(t)是零均值和方差阵为Q的维纳过程；是零均值和方 差阵为的高斯白噪声；系统的状态初值X(0)是均值为零和方差为V0的高斯随机变量，并 且X(0)与w(t)，以及li＝l1，...，lm相互独立。
基于上述数学模型，公开了一种基于序贯融合算法的水声通信系统的多AUV协同定位 方法，包括：
(1)将上述数学模型作为水下多AUV协同定位系统的数学模型；
(2)确定一个采样周期(k，k+1]的量测值，第l个从AUV在一个采样周期内依次接收m个 主AUV的量测信息，即k＜k1＜k2＜…＜km＝k+1；
(3)基于卡尔曼滤波算法，给出单个量测信息的局部滤波器；
(4)采用序贯融合方法，根据局部量测信息到达融合估计中心的顺序，逐步处理采样周 期内观测数据，实时给出融合估计值，最后给出当前时刻AUV的位姿信息精确估计值，如 图3所示。
具体实现方法为：
步骤1：给定时间t≥0，对于采样系统(1)，在采样周期(k，k+1]内，受加性扰动和随机丢 失观测作用下，系统可改写为
其中，满足
这里，
步骤2：基于最初到达的第l1个主AUV量测信息利用卡尔曼滤波算法，可得到 t＝k1时刻的局部滤波器和滤波误差方差阵
其中，初值为表示滤波增益阵，表示滤波误差方差阵，满足如 下等式：
这里，初值为S(k1)满足如下李雅普诺夫方程：
初始值为S(k0)＝S(k)，k0＜t≤k1。
步骤3：对于k1＜t≤k2，基于第l2个主AUV量测信息利用卡尔曼滤波算法，可 得到t＝k2时刻的局部滤波器和滤波误差方差阵局部预报器得到t＝k2时刻的 预测值；
其中，初值为t＝k1时刻的滤波值表示滤波增益阵，表示滤波误差方 差阵，满足如下等式：
这里，初值为
步骤4：依此类推，基于依次到达的各主AUV的量测信息可得到第lm个滤波器当t＝k+1时，获得第l个从AUV的状态最优序贯融合滤波器为
步骤5：令k＝k+1，返回步骤1。
由于量测信息采用错时采样、错时通信的传输方式，导致量测信息的异步性，使得传统 的集中式融合方式很难适用于实际应用中，量测扩维也带来了较大的计算负担，并且扩维后 的量测噪声与过程噪声具有相关性，增加了算法设计的复杂性。此时采用序贯融合算法，根 据局部观测数据到达融合中心的顺序，逐步处理观测数据，实时给出融合估计值，而无需等 待所有数据全部到达融合中心才执行算法，可有效减少计算负担，降低计算的复杂性。
下面结合具体实例，进一步阐述本发明。实验中，考虑水声通信网络中存在量测丢失的 情形，并采用两个主AUV轮替对从AUV进行协同定位。具体地，从实际工程中获取一组参 数如下：
本发明采用采样时间设置为T＝30s。状态初值 为
进一步地，利用所提出的基于序贯融合算法的多AUV协同定位方法，可以得到从AUV的 位置，即x轴的状态值和线速度，以及y轴的状态值和线速度，如图4-7所示。
本发明方法首先建立多AUV运动过程的连续状态方程和水声通信网络中具有量测丢失 的离散测量方程，获得系统参数；然后基于卡尔曼滤波算法获得最优的局部滤波器，滤波器 是分段连续的，不仅包含采样时刻的估计值，还包含采样区间的精确估计值；最后基于序贯 融合算法设计分段连续的序贯融合滤波器，给出具有高精度和实时性的多AUV协同定位方 法。本发明利用卡尔曼滤波算法和序贯融合方法设计了具有丢失量测的水声通信系统多AUV 协同定位方法，不仅可降低计算负担，还可以提高定位的实时性。
以上所述仅对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，本 领域技术人员应当理解，凡在本发明的技术方案的基础上，本领域技术人员不需要付出创造 性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。