CN113008239B - 一种多auv协同定位鲁棒延迟滤波方法 - Google Patents

Abstract

本发明公开了一种多AUV协同定位鲁棒延迟滤波方法，包括步骤一：建立多AUV协同定位模型；步骤二：通过将延迟转换为量测偏置构建存在时变量测延迟的非线性协同定位系统模型；步骤三：构建时变延迟模型下向量与向量预测值的统计相似性度量；步骤四：将后验概率密度函数近似为高斯分布，最大化代价函数下界求得近似解；步骤五：通过求解代价函数优化解，完成辅助变量更新；步骤六：通过辅助变量对量测噪声协方差矩阵进行修正。本发明在水声通讯延迟与量测噪声异常值共同存在的情形下，可同时削弱延迟与非高斯噪声对定位精度的影响。

Description

一种多AUV协同定位鲁棒延迟滤波方法

技术领域

本发明涉及一种多AUV协同定位鲁棒延迟滤波方法，特别是一种基于统计相似度量的多AUV协同定位鲁棒延迟滤波方法，属于导航技术领域。

背景技术

协同定位是目前中间层区域多自主式水下航行器最有效的导航方法之一。在水下机器人的协同定位中，一个重要的问题是如何在非理想条件下对其位置进行准确估计。协同定位系统作为一个非线性系统，当同时存在通讯延迟和非高斯特性时，将具有更大的挑战性。目前针对导航系统延迟滤波算法的推导均是在噪声高斯的假设下进行的，因此，其对于非高斯噪声如噪声异常值等极为敏感，往往因处理不当而导致大误差的出现，不能满足对定位精度的要求。研究声学通讯延迟与非高斯特性对协同定位系统定位精度的影响，解决如何同时削弱这些非理想条件对定位精度的影响，是协同定位方向需要解决的技术问题。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种在水声通讯延迟与量测噪声异常值共同存在的情形下，可同时削弱延迟与非高斯噪声对定位精度影响的多AUV协同定位鲁棒延迟滤波方法。

为解决上述技术问题，本发明的一种多AUV协同定位鲁棒延迟滤波方法，包括以下步骤：

步骤一：建立多AUV协同定位模型；

步骤二：通过将延迟转换为量测偏置构建存在时变量测延迟的非线性协同定位系统模型；

步骤三：构建时变延迟模型下向量与向量预测值的统计相似性度量(SSM)；

步骤四：将后验概率密度函数(PDF)近似为高斯分布，最大化代价函数下界求得近似解；

步骤五：通过求解代价函数优化解，完成辅助变量更新；

步骤六：通过辅助变量对量测噪声协方差矩阵进行修正。

本发明还包括：

1.步骤一中多AUV协同定位模型具体为：

式中x_s，[k]＝[x_s,[k],y_s,[k]]^T为AUV在k时刻的位置坐标，(·)^T为转置操作，Δt为采样周期，ω_[k-1]＝[ω_x,[k-1],ω_y,[k-1]]^T为k-1时刻的过程噪声向量，

为k时刻AUV前向和右向速度，

为k时刻绝对航向；

AUV的距离量测函数为：

式中，x_r,[k]＝[x_r,[k],y_r,[k],h_r,[k]]，x_s,[k]＝[x_s,[k],y_s,[k],h_s,[k]]分别为领航者与跟随者k时刻的位置；d_[k]为使用TOA方法测得的跟随者与领航者间的相对距离；h_s,[k]和h_r,[k]分别为压力传感器测得的跟随者与领航者的深度；

将三维距离量测被转换成一个二维水平距离z_[k]，即二维量测方程表示为：

式中：δ_[k]表示量测噪声。

2.步骤二中时变量测延迟模型具体为：

式中，F为状态转移矩阵，u_[k]为k时刻的控制输入，H为观测矩阵，(·)^-1为矩阵求逆操作，且

式中，F_[k-j+1,k-j]为k-j时刻到k-j+1时刻的状态转移矩阵，

为k-i时刻的量测噪声。

3.步骤三中构建时变延迟模型下向量与向量预测值的统计相似性度量具体为：

通过最大化

以及

的SSM来实现近似后验PDFq^*(x_[k])≈p(x_[k]|z_[1:k])；

式中：

分别为预测误差协方差矩阵P_[k,k-1]和名义上的量测噪声协方差矩阵

的平方根，

x_[k,k-1]为状态预测值。

由于量测信息Z_[k-i]在滤波量测更新的过程中已知，则：

式中f_x(·)与f_z(·)分别为状态和量测相似度函数。

4.步骤四中将后验概率密度函数近似为高斯分布，最大化代价函数下界求得近似解具体为：

q(x_[k])≈N(x_[k]；μ_[k],∑_[k])

式中μ_[k]，∑_[k]分别为后验PDFq(x_[k])的均值和协方差；

最大化后验PDFq(x_[k])的问题转化为最大化均值与协方差矩阵的问题，则

式中

为优化后的均值和协方差，通过将上式代价函数近似为其下界，得到近似解。

5.步骤五中更新辅助变量具体为：

代价函数优化解

计算满足：

式中

为状态的一步预测估计值，

为增益矩阵，P_[k,k-1]为状态误差协方差矩阵，

为修正后的量测噪声协方差矩阵。

辅助参数

为

式中：

6.步骤六中通过辅助变量对量测噪声协方差矩阵进行修正具体为：

基于代价函数优化解

的求解与辅助变量

的更新，修正后的量测噪声协方差矩阵

满足：

本发明的有益效果：本发明同时考虑通讯延迟与噪声的非高斯特性，基于延迟转换为量测偏置的思路与SSM理论，对非理想条件下的协同定位问题进行了解决。现有的基于状态补偿的延迟算法、基于量测更新的延迟算法以及基于量测偏置转化的延迟算法的推导均是在噪声高斯假设下完成的，它们可以有效削弱通讯延迟对定位精度的影响，但无法有效应对非高斯噪声的干扰。

针对目前AUV协同定位延迟滤波算法不适用于非高斯噪声环境的问题，本发明首先对存在时变量测延迟的非线性协同定位系统模型进行重新推导，接着在时变延迟模型下基于SSM理论完成对量测噪声协方差矩阵的自适应修正，最终实现在通讯延迟与非高斯噪声同时存在的非理想条件下的准确定位。本发明可用于非理想条件下的多AUV协同定位领域。

本发明的主要优点体现在：

本发明基于延迟转换为量测偏置的思路，对存在时变量测延迟的非线性协同定位系统模型进行重新推导，具有更高的实用价值；

本发明在时变延迟模型下基于SSM理论完成对量测噪声协方差矩阵的自适应修正，针对实际场景中非理想条件下协同定位系统可能出现的问题有较好的应对方案；

本发明采用不动点迭代法完成求解过程，对于实际问题的求解具有更高的可实现性；

本发明也可用于其他受通讯延迟和非高斯噪声干扰的系统，如组合导航、目标跟踪系统等，是卡尔曼滤波框架下一种具有通用性和扩展性的算法。

附图说明

图1为方法流程图；

图2为协同定位系统AUV实际航行轨迹图；

图3为协同定位过程中的量测值与量测噪声图；

图4为定位误差比较图；

图5为不同概率噪声分布下的均方根误差比较图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步说明。

本发明的目的是这样实现的：

步骤一：建立多AUV协同定位模型；

步骤二：基于延迟转换为量测偏置的思路，对存在时变量测延迟的非线性协同定位系统模型进行重新推导；

步骤三：构建时变延迟模型下向量与向量预测值的统计相似性度量(SSM)；

步骤四：将后验概率密度函数(PDF)近似为高斯分布，最大化代价函数下界求得近似解；

步骤五：通过求解代价函数优化解，完成辅助变量更新；

步骤六：通过辅助变量对量测噪声协方差矩阵进行修正。

结合图1，本发明包括以下步骤：

步骤一：建立多AUV协同定位模型；

AUV的位置坐标通过如下过程方程进行推导

式中：x_s，[k]＝[x_s,[k],y_s,[k]]^T为AUV在k时刻的位置坐标，(·)^T为转置操作，Δt为采样周期，ω_[k-1]＝[ω_x,[k-1],ω_y,[k-1]]^T为k-1时刻的过程噪声向量，

为DVL测得的k时刻AUV前向和右向速度，

为罗经测得的k时刻绝对航向；

AUV的距离量测函数为：

式中:

x_r,[k]＝[x_r,[k],y_r,[k],h_r,[k]]，x_s,[k]＝[x_s,[k],y_s,[k],h_s,[k]]分别为领航者与跟随者在k时刻的位置；d_[k]为使用TOA方法测得的跟随者与领航者间的相对距离；h_s,[k]和h_r,[k]分别为压力传感器测得的跟随者与领航者的深度。

利用精确的深度信息，考虑到TOA方法的测量误差，将三维距离量测转换成一个二维水平距离z_[k]，即二维量测方程表示为：

式中：

δ_[k]表示k时刻量测噪声。

步骤二：时变量测延迟模型的推导；

设量测信息延迟时刻为i，那么在k时刻获得的是k-i时刻的量测信息：

x_[k]＝F_[k,k-i]x_[k-i]+F_[k,k-i+1]u_[k-i+1]+F_[k,k-i+1]ω_[k-i+1]+...+u_[k]+ω_[k] (5)

式中：

u_[k]为k时刻控制输入，F_[k,k-i]为多个状态转移矩阵的累积乘积，即

由式(5)得

式中(·)^-1为矩阵求逆操作。

那么

z_[k-i]-h(x_[k-i,k-i-1],x_r,[k-i],y_r,[k-i])+Hx_[k-i,k-i-1]＝Hx_[k-i]+δ_[k-i] (8)

式中：

h(·)为量测函数，且有

Z_[k-i]＝z_[k-i]-h(x_[k-i,k-i-1],x_r,[k-i],y_r,[k-i])+Hx_[k-i,k-i-1]+U_[k] (11)

式中：

为k-i时刻的量测噪声。

得到延迟系统模型

步骤三：构建时变延迟模型下的统计相似性度量；

通过最大化

以及

的SSM来实现近似后验PDFq^*(x_[k])≈p(x_[k]|z_[1:k])；

式中：

分别为预测误差协方差矩阵P_[k,k-1]和名义上的量测噪声协方差矩阵

的平方根，

x_[k,k-1]为k时刻状态预测值。

由于量测信息Z_[k-i]在滤波量测更新的过程中已知，那么

式中：f_x(·)与f_z(·)分别为状态和量测相似度函数。

步骤四：近似解求解；

将后验PDFq(x_[k])近似为高斯分布并最大化代价函数下界，从而求得q^*(x_[k])的近似解。

q(x_[k])≈N(x_[k]；μ_[k],∑_[k]) (17)

式中：μ_[k]，∑_[k]分别为后验PDFq(x_[k])的均值和协方差。

将(17)带入(16)中，最大化后验PDFq(x_[k])的问题近似转化为最大化均值与协方差矩阵的问题

式中：

为优化后的均值和协方差，将(18)中的代价函数近似为其下界，得到近似解。

步骤五：辅助变量更新；

代价函数优化解

计算如下

式中：

为状态的一步预测估计值，

为增益矩阵，P_[k,k-1]为状态误差协方差矩阵，

为修正后的量测噪声协方差矩阵。

辅助参数

为

式中：

步骤六：量测噪声协方差矩阵自适应修正。

基于代价函数优化解

的求解与辅助变量

的更新，得到

为修正后的量测噪声协方差矩阵

Claims (1)

1.一种多AUV协同定位鲁棒延迟滤波方法，其特征在于，包括以下步骤：

步骤一：建立多AUV协同定位模型，具体为：

式中x_s，[k]＝[x_s，[k]，y_s，[k]]为AUV在k时刻的位置坐标，(·)^T为转置操作，Δt为采样周期，ω_[k-1]＝[ω_x，[k-1]，ω_y，[k-1]]^T为k-1时刻的过程噪声向量，

为k时刻AUV前向和右向速度，

为k时刻绝对航向；

AUV的距离量测函数为：

式中，x_r，[k]＝[x_r，[k]，y_r，[k]，h_r，[k]]，x_s，[k]＝[x_s，[k]，y_s，[k]，h_s，[k]]分别为领航者与跟随者k时刻的位置；d_[k]为使用TOA方法测得的跟随者与领航者间的相对距离；h_s，[k]和h_r，[k]分别为压力传感器测得的跟随者与领航者的深度；

将三维距离量测被转换成一个二维水平距离z_[k]，即二维量测方程表示为：

式中：δ_[k]表示量测噪声；

步骤二：通过将延迟转换为量测偏置构建存在时变量测延迟的非线性协同定位系统模型，具体为：

式中，F为状态转移矩阵，u_[k]为k时刻的控制输入，H为观测矩阵，(·)^-1为矩阵求逆操作，且

式中，F_{[k-j+1，k-j]}为k-j时刻到k-j+1时刻的状态转移矩阵，

为k-i时刻的量测噪声；

步骤三：构建时变延迟模型下向量与向量预测值的统计相似性度量，具体为：

通过最大化

以及

的SSM来实现近似后验PDF q^*(x_[k])≈p(x_[k]|z_[1：k])；

式中，

分别为预测误差协方差矩阵P_[k，k-1]和名义上的量测噪声协方差矩阵

的平方根，

X_[k，k-1]为状态预测值；

由于量测信息Z_[k-i]在滤波量测更新的过程中己知，则：

式中f_x(·)与f_z(·)分别为状态和量测相似度函数；

步骤四：将后验概率密度函数近似为高斯分布，最大化代价函数下界求得近似解，具体为：

q(x_[k])≈N(x_[k]；μ_[k]，∑_[k])

式中，μ_[k]，∑_[k]分别为后验PDFq(x_[k])的均值和协方差；

最大化后验PDF q(x_[k])的问题转化为最大化均值与协方差矩阵的问题，则

式中，

为优化后的均值和协方差，通过将上式代价函数近似为其下界，得到近似解；

步骤五：通过求解代价函数优化解，完成辅助变量更新，具体为：

代价函数优化解

计算满足：

式中，

为状态的一步预测估计值，

为增益矩阵，P_[k，k-1]为状态误差协方差矩阵，

为修正后的量测噪声协方差矩阵；

辅助参数

为

式中：

步骤六：通过辅助变量对量测噪声协方差矩阵进行修正，具体为：

基于代价函数优化解

的求解与辅助变量

的更新，修正后的量测噪声协方差矩阵

满足：

Images