考虑量测异常的集群式多深海潜航器的协同定位方法

技术领域

本发明属于导航技术领域，涉及了深海潜航器的导航定位方法，具体是一种考虑量测异常的集群式多深海潜航器的协同定位方法。

背景技术

威力集群多深海潜航器协同工作系统具有探测范围广、容错能力强、工作效率高的特点，能够实现单体深海潜航器难以完成的多样化、复杂化水下任务。

多深海潜航器的协同定位技术与空中多无人机、陆地多机器人协同工作系统的协同定位技术相比有着一定的特殊性，存在着复杂环境干扰、传感器限制、通信技术等固有难点。因此多深海潜航器间的信息获取和交流只能靠超声波来完成。

但由于超声波距离传感器的信号相关特性，主从深海潜航器间的相对距离信息中往往含有乘性噪声，同时水下声波波速受海水温度、盐度变化影响，其不确定性会造成主从深海潜航器的相对距离建模中的参数存在不确定性。这两种情况统称为量测异常，会造成主从潜航器协同导航系统的定位精度降低，甚至于失效。

因此在海洋复杂的环境中，考虑量测异常情况下实现主从式深海潜航器的精准协同定位是目前多深海潜航器研究的一个热门方向。

发明内容

为解决上述问题，本发明针对威力集群多深海潜航器在协同定位过程中容易出现量测异常情况，造成协同导航系统的精度和可靠性下降的问题，从而提供一种考虑量测异常的威力集群式多深海潜航器协同定位方法。

本发明通过设计新形式krein空间滤波器来有效解决量测中存在乘性噪声和参数不确定性时传统H∞滤波器难以建模的困难，降低滤波器的时空复杂度。同时，设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。最终实现威力集群多深海潜航器整体的高精度、高可靠性协同导航定位。

技术方案：为实现上述发明目的，本发明采用的技术方案，包括如下步骤：

S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程；

S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒后验滤波方程；

S3：设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。

本发明进一步改进在于：

所述步骤S1具体包括如下步骤：

S1-1:考虑一个15维系统状态量的主从潜航器协同定位离散系统：

其中，Δθ,Δφ,Δω表示从潜航器的姿态角误差，Δv

从潜航器惯导系统在当地地理坐标系下的离散时间误差方程为：

其中，

S1-2:考虑主从深海潜航器的相对距离测量信息异常的量测方程为：

其中，Z

由于水下声波波速V的不确定性问题，会导致主从深海潜航器间相对距离测量的不确定：

其中，ΔV表示水下声波波速V的不确定，τ

本发明进一步改进在于：所述步骤S2其具体如下：

S2-1：建立目标二次型

考虑乘性噪声和参数不确定性的情况下一个线性离散系统：

其中，μ

ΔHX

其中，Δ

||ξ

其中，||·||表示向量H

令v

其中，X

将能量约束SQC不等式转换为等价目标二次型：

S2-2：建立新形式krein空间系统模型和滤波方程

考虑乘性噪声和参数不确定性的情况下建立新形式krein空间系统：

其中，

且具有形式噪声方差阵：

其中，

基于新形式krein空间系统的γ水平鲁棒H∞后验滤波方程为：

其中，

本发明进一步改进在于：所述步骤S3具体包括如下步骤：

理想情况中新形式krein空间系统的γ水平鲁棒H∞后验滤波器的新息

则理论情况中

参数不确定性会反映在滤波器新息中，不确定性干扰越大，实际新息协方差与理论新息协方差越大。因此可以新息协方差入手，在线估计

即：

当且仅当适配矩阵

将正确估计的适配矩阵

正确估计的

有益效果：

本发明与现有方法相比，通过设计新形式krein空间滤波器来估计和补偿了水下声波波速受海水温度、盐度变化影响而产生的建模参数不确定性，提高了多深海潜航器协同定位的精度。其次，有效解决了量测中存在乘性噪声时传统H∞滤波器难以建模的困难，在保证多深海潜航器协同定位系统高可靠性的同时降低了滤波器的时空复杂度。

附图说明

图1、是本发明实施例中威力集群多深海潜航器系统；

图2是本发明实施例中考威力集群式多深海潜航器系统协同定位框图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。需要说明的是，下面描述中使用的词语“前”、“后”、“左”、“右”、“上”和“下”指的是附图中的方向，词语“内”和“外”分别指的是朝向或远离特定部件几何中心的方向。

如图1、2所示，本实施例提供一种考虑量测异常的集群式多深海潜航器的协同定位方法，包括如下步骤：

S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程，其具体如下：

考虑一个15维系统状态量的主从潜航器协同定位离散系统：

其中，Δθ,Δφ,Δω表示从潜航器的姿态角误差，Δv

从潜航器惯导系统在当地地理坐标系下的离散时间误差方程为：

其中，

考虑主从深海潜航器的相对距离测量信息异常的量测方程为：

其中，Z

由于水下声波波速V的不确定性问题，会导致主从深海潜航器间相对距离测量的不确定：

其中，ΔV表示水下声波波速V的不确定，τ

S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒滤波方程，其具体如下：

S2-1：建立目标二次型

考虑乘性噪声和参数不确定性的情况下一个线性离散系统：

其中，μ

ΔHX

其中，Δ

||ξ

其中，||·||表示向量H

令v

其中，X

由于能量约束SQC不等式只能给出椭圆集合，不适合描述不确定系统的结果，因此将能量约束SQC不等式转换为等价目标二次型：

S2-2：建立新形式krein空间系统模型和滤波方程

考虑乘性噪声和参数不确定性的情况下建立新形式krein空间系统：

其中，

且具有形式噪声方差阵：

其中，

基于新形式krein空间系统的γ水平鲁棒H∞后验滤波方程为：

其中，

S3：由于水下声波波速的不确定性会造成主从深海潜航器的相对距离建模中的参数存在不确定性，最优滤波器估计的状态量

理想情况中新形式krein空间系统的γ水平鲁棒H∞后验滤波器的新息

则理论情况中

参数不确定性会反映在滤波器新息中，不确定性干扰越大，实际新息协方差与理论新息协方差越大。因此可以新息协方差入手，在线估计

即：

当且仅当适配矩阵

将正确估计的适配矩阵

正确估计的

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。