一种自主式水下航行器的对接回收方法及系统技术方案

本发明专利技术提供了一种自主式水下航行器的对接回收方法及系统，包括：获取超短基线定位系统测量的自主式水下航行器的三维位置，三维位置包括平面位置和深度；在深度不符合预设范围的情况下，将自主式水下航行器的平面位置和速度分别作为状态量和控制量，构建状态转移方程，并将自主式水下航行器与坞站之间的距离作为观测量，构建观测方程；采用带有渐消因子的无迹卡尔曼滤波算法，基于状态转移方程和观测方程生成粒子滤波算法中的建议分布；采用粒子滤波算法，基于建议分布完成自主式水下航行器的平面位置估计；控制自主式水下航行器与坞站对接

【技术实现步骤摘要】
一种自主式水下航行器的对接回收方法及系统


[0001]本专利技术属于水下航行器导航
，更具体地，涉及一种自主式水下航行器的对接回收方法及系统
。

技术介绍

[0002]受限于自身携带的能源以及水下通信速率，自主式水下航行器
(AUV)
难以长时间在水下工作，需要定期进行对接回收进行能源补充和维护
。
[0003]AUV
在水下进行远距离的导航时通常采用水声导引的方法或者采用惯性导航和多普勒计程仪等方法进行联合导航
。
然而在浅水环境下，由于传感器本身的误差和随机噪声的污染，使测量数据的可靠性降低，不利于
AUV
的准确定位
。

技术实现思路

[0004]针对现有技术的缺陷，本专利技术的目的在于提供一种自主式水下航行器的对接回收方法及系统，旨在解决现有
AUV
回收方法中
AUV
定位不准的问题
。
[0005]为实现上述目的，第一方面，本专利技术提供了一种自主式水下航行器的对接回收方法，包括以下步骤：
[0006]步骤
S101
，获取超短基线定位系统测量得到的自主式水下航行器的三维位置，以及自主式水下航行器与坞站之间的距离，并获取自主式水下航行器的速度；其中，所述超短基线定位系统测量安装在所述坞站上，所述三维位置包括平面位置和深度；
[0007]步骤
S102
，在所述深度不符合预设范围的情况下，将自主式水下航行器的平面位置和速度分别作为状态量和控制量，构建状态转移方程，并将自主式水下航行器与坞站之间的距离作为观测量，构建观测方程；
[0008]步骤
S103
，采用带有渐消因子的无迹卡尔曼滤波算法，基于状态转移方程和观测方程生成粒子滤波算法中的建议分布；
[0009]步骤
S104
，采用粒子滤波算法，基于所述建议分布完成自主式水下航行器的平面位置估计，获得自主式水下航行器的平面位置估计值；
[0010]步骤
S105
，基于所述自主式水下航行器的平面位置估计值，控制自主式水下航行器与坞站对接
。
[0011]在一个可选的示例中，步骤
S103
具体包括：
[0012]通过无迹变换的方式，获得每个粒子的
sigma
点集及对应权值；
[0013]根据所述状态转移方程，以及每个粒子的
sigma
点集及对应权值，计算每个粒子的状态预测值；
[0014]根据每个粒子的状态预测值以及渐消因子，计算每个粒子的预测协方差；
[0015]根据每个粒子的状态预测值及预测协方差，再次产生每个粒子的新
sigma
点集；
[0016]将每个粒子的新
sigma
点集代入所述观测方程，获得每个粒子的观测预测值；
[0017]根据每个粒子的新
sigma
点集及观测预测值，计算观测量的自协方差以及观测量
与状态量的互协方差，进而计算出滤波增益，再根据滤波增益计算观测更新后的状态均值及协方差；
[0018]基于所述观测更新后的状态均值及协方差，生成所述粒子滤波算法中的建议分布
。
[0019]在一个可选的示例中，所述每个粒子的预测协方差的计算公式如下：
[0020][0021]其中
λ
k
≥1
为渐消因子，
n
a
表示状态量的维度，
k
表示时刻，为协方差的权值，为
sigma
点的一步预测，为经过采样点加权和的状态预测值，定义输出残差序列
[0022][0023][0024][0025][0026]其中
tr[
·
]表示矩阵的迹，
Z
k
为观测真实值，为观测预测值，
ρ
是遗忘因子，
0<
ρ
≤1
，为状态量与观测量的互协方差，为观测量的自协方差，
R
k
为观测噪声的分布矩阵，
Q
k
‑1为过程噪声的分布矩阵，
p
k∣k
‑1为预测协方差
。
[0027]在一个可选的示例中，所述自主式水下航行器的速度是由自主式水下航行器的底部安装的多普勒计程仪测量得到的
。
[0028]第二方面，本专利技术提供了一种自主式水下航行器的对接回收系统，包括：
[0029]数据获取模块，用于获取超短基线定位系统测量得到的自主式水下航行器的三维位置，以及自主式水下航行器与坞站之间的距离，并获取自主式水下航行器的速度；其中，所述超短基线定位系统测量安装在所述坞站上，所述三维位置包括平面位置和深度；
[0030]方程构建模块，用于在所述深度不符合预设范围的情况下，将自主式水下航行器的平面位置和速度分别作为状态量和控制量，构建状态转移方程，并将自主式水下航行器与坞站之间的距离作为观测量，构建观测方程；
[0031]分布生成模块，用于采用带有渐消因子的无迹卡尔曼滤波算法，基于状态转移方程和观测方程生成粒子滤波算法中的建议分布；
[0032]位置估计模块，用于采用粒子滤波算法，基于所述建议分布完成自主式水下航行器的平面位置估计，获得自主式水下航行器的平面位置估计值；
[0033]对接回收模块，用于基于所述自主式水下航行器的平面位置估计值，控制自主式水下航行器与坞站对接
。
[0034]在一个可选的示例中，所述分布生成模块具体用于：
[0035]通过无迹变换的方式，获得每个粒子的
sigma
点集及对应权值；
[0036]根据所述状态转移方程，以及每个粒子的
sigma
点集及对应权值，计算每个粒子的状态预测值；
[0037]根据每个粒子的状态预测值以及渐消因子，计算每个粒子的预测协方差；
[0038]根据每个粒子的状态预测值及预测协方差，再次产生每个粒子的新
sigma
点集；
[0039]将每个粒子的新
sigma
点集代入所述观测方程，获得每个粒子的观测预测值；
[0040]根据每个粒子的新
sigma
点集及观测预测值，计算观测量的自协方差以及观测量与状态量的互协方差，进而计算出滤波增益，再根据滤波增益计算观测更新后的状态均值及协方差；
[0041]基于所述观测更新后的状态均值及协方差，生成所述粒子滤波算法中的建议分布
。
[0042]在一个可选的示例中，所述分布生成模块中每个粒子的预测协方差的计算公式如下：
[0043][0044]其中
λ
k
≥1
为渐消因子，
n本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种自主式水下航行器的对接回收方法，其特征在于，包括以下步骤：步骤
S101
，获取超短基线定位系统测量得到的自主式水下航行器的三维位置，以及自主式水下航行器与坞站之间的距离，并获取自主式水下航行器的速度；其中，所述超短基线定位系统测量安装在所述坞站上，所述三维位置包括平面位置和深度；步骤
S102
，在所述深度不符合预设范围的情况下，将自主式水下航行器的平面位置和速度分别作为状态量和控制量，构建状态转移方程，并将自主式水下航行器与坞站之间的距离作为观测量，构建观测方程；步骤
S103
，采用带有渐消因子的无迹卡尔曼滤波算法，基于状态转移方程和观测方程生成粒子滤波算法中的建议分布；步骤
S104
，采用粒子滤波算法，基于所述建议分布完成自主式水下航行器的平面位置估计，获得自主式水下航行器的平面位置估计值；步骤
S105
，基于所述自主式水下航行器的平面位置估计值，控制自主式水下航行器与坞站对接
。2.
根据权利要求1所述的方法，其特征在于，步骤
S103
具体包括：通过无迹变换的方式，获得每个粒子的
sigma
点集及对应权值；根据所述状态转移方程，以及每个粒子的
sigma
点集及对应权值，计算每个粒子的状态预测值；根据每个粒子的状态预测值以及渐消因子，计算每个粒子的预测协方差；根据每个粒子的状态预测值及预测协方差，再次产生每个粒子的新
sigma
点集；将每个粒子的新
sigma
点集代入所述观测方程，获得每个粒子的观测预测值；根据每个粒子的新
sigma
点集及观测预测值，计算观测量的自协方差以及观测量与状态量的互协方差，进而计算出滤波增益，再根据滤波增益计算观测更新后的状态均值及协方差；基于所述观测更新后的状态均值及协方差，生成所述粒子滤波算法中的建议分布
。3.
根据权利要求2所述的方法，其特征在于，所述每个粒子的预测协方差的计算公式如下：其中
λ
k
≥1
为渐消因子，
n
a
表示状态量的维度，
k
表示时刻，为协方差的权值，为
sigma
点的一步预测，为经过采样点加权和的状态预测值，定义输出残差序列序列序列
其中
tr[
·
]
表示矩阵的迹，
Z
k
为观测真实值，为观测预测值，
ρ
是遗忘因子，
0<≤1
，为状态量与观测量的互协方差，为观测量的自协方差，
R
k
为观测噪声的分布矩阵，
Q
k
‑1为过程噪声的分布矩阵，
p
k∣k
‑1为预测协方差
。4.
根据权利要求1至3任一项所述的方法，其特征在于，所述自主式水下航行器的速度是由自主式水下航行器的底部安装的多普勒计程仪测量得到的
。5.
一种自主式水下航行器的对接回收系统，其特征在于，包括：数据获取模块，用于获取超短基线定位系统测量得到的自主式水下航行器的三维位置，以及自主式...

【专利技术属性】
技术研发人员：付少波，关夏威，张昊，陈虹，
申请(专利权)人：中国船舶集团有限公司第七一九研究所，
类型：发明
国别省市：