CN116222578B - 基于自适应滤波和最优平滑的水下组合导航方法及系统 - Google Patents

Abstract

本发明属于导航定位相关技术领域，本发明提出了基于自适应滤波和最优平滑的水下组合导航方法及系统，包括：建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测，基于水下运载体当前声学量测信息对扩展卡尔曼滤波算法的预测值进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果，从而减小了组合导航的定位误差，提高了定位精度。

Description

基于自适应滤波和最优平滑的水下组合导航方法及系统

技术领域

本发明属于导航定位相关技术领域，尤其涉及基于自适应滤波和最优平滑的水下组合导航方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

水下多传感器组合导航中，惯性导航系统通过感知载体的平移和转动，由陀螺仪和加速度计测量载体的角速率和加速度，并通过航位推算方法获得载体的姿态、速度、位置、角速度和加速度等导航信息。惯性导航系统具有完全独立自主、导航信息丰富、高采样率和不受环境干扰等优势，但受其导航原理的限制，导航误差随时间不断积累变大，需要绝对定位系统对其进行校正。声学导航定位系统通过测量换能器至应答器的声波传播时间，并结合声速剖面信息获得两者之间的斜距，然后通过距离交会得到水下运载体的绝对位置坐标。声学导航定位系统能够获得高精度的绝对位置信息，但受声波传播速度的限制使得采样率低并易受到海洋环境的影响。多普勒计程仪可以提供较为准确的速度信息，磁力计输出航向信息，深度计能够提供高采样率的深度约束信息。这些传感器具有天然的互补优势。

虽然已有的方法可以精确地为水下运载体进行导航定位，但还存在各种各样的问题。单一的水下声学导航系统可以通过观测船底换能器至海底应答器的声波传播时间，并结合测量的声速剖面信息计算得到换能器至应答器的距离，然后通过滤波得到水下运载体的绝对位置坐标；但此方法受声波传播速度的限制使得采样率低，并易受到海洋环境的影响接收不到三个及以上信号，从而直接降低导航精度。水下运载体安装多普勒计程仪，并结合惯导设备为运载体提供高频的导航信息，利用多普勒计程仪测量的三维速度对惯导积分的速度进行校正，进而约束位置信息进行导航；此方法只是对惯导积分的位置误差进行约束，不能修正位置误差，长时间导航位置误差会逐渐发散。利用惯导、多普勒计程仪和压力计相结合为水下运载体提供导航服务，采用多普勒计程仪的三维速度对惯导的速度进行约束，压力计测量的深度对惯导的高度通道进行高度阻尼，从而为运载体进行导航；此方法能够抑制垂直方向的位置误差，但难以限制水平方向位置误差的发散。水下声学与惯导相结合为水下运载体提供高频的三维位置，水下声学可以提供三维绝对位置以修正惯导姿态、速度和位置误差；此方法在声学接收到信号数量较少时，难以提供准确的三维位置，量测噪声与位置误差不匹配，使得组合导航结果误差较大。

因此，虽然以上研究可以为水下运载体提供导航服务，但还存在导航结果不稳定，位置误差逐渐缓慢发散，量测噪声与观测值不匹配和导航结果不平滑等问题。

发明内容

为克服上述现有技术的不足，本发明提供了基于自适应滤波和最优平滑的水下组合导航方法及系统，利用扩展卡尔曼滤波算法将声学和惯性导航相结合，从而减小了组合导航的定位误差，提高了定位精度。

为实现上述目的，本发明的第一个方面提出基于自适应滤波和最优平滑的水下组合导航方法，包括：

建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据所述当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果。

本发明的第二个方面提出基于自适应滤波和最优平滑的水下组合导航系统，包括：

模型建立模块：建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

预测更新模块：利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

量测更新模块：获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

定位输出模块：根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果。

以上一个或多个技术方案存在以下有益效果：

在本发明中，通过建立声学观测模型和基于惯性导航的水下运载体的预测模型，利用扩展卡尔曼滤波算法将声学和惯性导航相结合，获得水下运载体的状态一步预测值和状态一步预测协方差，并利用声学的量测信息，对当前量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到当前状态的最优估计值，从而减小了组合导航的定位误差，提高了定位精度。

在本发明中，对于导航结果采用最优平滑算法，从而使导航结果更为平滑。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例一中基于自适应滤波和最优平滑的水下组合导航方法流程图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

实施例一

本实施例公开了基于自适应滤波和最优平滑的水下组合导航方法，包括：

建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据所述当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果。

如图1所示，在本实施例中，基于水下声速误差模型，构建包含声速误差的状态转移矩阵和双程传播距离的观测模型。

首先构建包含声速误差在内的状态转移矩阵。状态向量为在包含姿态误差、速度误差、位置误差、陀螺零偏和加速度零偏等15维的基础上添加声速误差，所建立的水下运载体的预测模型和状态向量分别为：

（1）

其中，为状态向量，/>为/>的微分形式，/>为姿态失准角，/>为速度误差，/>为位置误差，/>为陀螺零偏，/>为加速度计零偏，/>为水下声学误差，/>和分别为连续型状态转移矩阵和系统噪声分配矩阵，/>为零均值高斯白噪声，T表示转置。

基于声学误差特性，将声学误差建模为随机游走模型，并等效离散化为：

（2）

其中，为声速误差的激励高斯白噪声，其确定与海洋环境以及水下声学采样周期相关，本实施例中下标/>均表示第/>个历元，下标k-1表示第k-1个历元。

将上述的连续型状态转移矩阵等效离散化为：

（3）

其中，为离散化的状态转移矩阵，/>为单位阵，/>为连续时间的状态转移矩阵，/>为惯导的采样间隔。

基于水下声学往返传播的声学特性，建立水下运载体的双程传播距离的观测模型，即：

（4）

（5）

（6）

其中，为第/>个历元的量测向量；/>为第/>个历元的量测矩阵；/>为预测的状态向量；/>为第/>个历元的量测噪声向量；/>为大地坐标系到导航坐标系的方向余弦矩阵；/>为水下运载体坐标系到导航坐标系的方向余弦矩阵；/>为惯导到声学换能器的杆臂在水下运载体坐标系中的投影向量；/>为反对称阵；/>表示对角线元素，为声波双程传播时间，其余为0；/>为第/>个历元的声学双程误差方程的雅可比矩阵；/>、/>、/>为水下运载体发射时刻的水下运载体X、Y、Z轴概略坐标，/>、/>和/>为水下运载体声学信号接收时刻的X、Y、Z轴概略坐标，/>、/>和/>分别为海底基准点的X、Y、Z轴坐标，/>为运载体发射信号到应答器接收信号的概略传播距离，/>为应答器发射应答信号到运载体再接收应答信号的概略传播距离。

在本实施例中，根据前一历元的惯导的量测更新位置和所建立的预测模型得到当前历元声学信号发射时刻的惯导的位置，信号发射时刻到接收时刻的间隔预测当前历元声学信号接收时刻的惯导位置，通过杠杆臂将两个时刻惯导的位置转换到声学换能器。根据两个时刻声学换能器坐标和海底应答器计算预测发射距离和预测接收距离，从而得到预测双程距离，即：

（7）

其中，为惯导预测双程距离，/>为惯导预测发射距离，/>为惯导预测接收距离，/>，/>和/>分别为惯导预测的发射时刻声学换能器的X轴坐标、Y轴坐标、Z轴坐标，/>，/>和/>分别为惯导预测的接收时刻声学换能器X轴坐标、Y轴坐标、Z轴坐标，/>、/>和/>分别为海底基准点的X轴坐标、Y轴坐标、Z轴坐标。

在本实施例中，根据声学换能器和海底基准点的吃水深度以及声速剖面计算发射时刻和接收时刻的加权平均声速，再结合量测的声波传播时间计算得到量测的双程距离。

（8）

其中，为量测双程距离，/>为双程加权平均声速，/>为声波双程传播时间。

由惯导预测双程距离和量测双程距离得到新息向量，即：

（9）

其中，为新息向量，/>为第/>个历元的水下声学误差，/>为惯导预测双程距离，/>为量测双程距离。

在本实施例中，根据条件约束的Sage-Husa自适应滤波对声学观测值的噪声进行自适应调节，使量测噪声与观测误差相匹配，即：

（10）

（11）

（12）

其中，为第/>个历元的自适应调节后的量测噪声，/>和/>分别为设置的最大量测噪声和最小量测噪声，上标（i）表示第i个对角线元素，/>为第/>个历元的渐消系数，/>为第k-1个历元的渐消系数，/>，/>为渐消因子，取值/>，/>为新息向量，/>为第/>个历元的量测矩阵，/>为预测的方差协方差，/>为第/>个历元的新息向量的平方与误差传播后预测方差的差值。

在本实施例中，采用扩展卡尔曼滤波将水下声学观测信息和惯导信息相融合进行组合导航，扩展卡尔曼滤波的公式如下：

（13）

（14）

（15）

（16）

（17）

其中，、/>分别为预测的状态向量、预测的方差协方差，/>、/>分别为第k-1个历元量测更新的状态向量、第k-1个历元量测更新的方差协方差，/>、/>和/>分别为离散化的状态转移矩阵、第k-1个历元系统噪声分配矩阵和第k个历元量测矩阵，/>、/>分别为第k-1个历元的系统噪声矩阵、第k个历元的量测噪声矩阵，/>为第k个历元的卡尔曼增益，/>为第k个历元的量测向量，/>为单位阵，/>分别为第k个历元的量测更新的状态向量、第k个历元的量测更新的方差协方差，量测更新后的状态向量和方差协方差也被称为最优的状态向量和方差协方差，上标T表示矩阵转置。

在本实施例中，将组合导航结果，即卡尔曼滤波的最优的状态向量和协方差矩阵按由后往前的量测顺序进行最优平滑算法即RTS平滑，该方法对状态向量及其方差协方差进行反向平滑，具体为：

（18）

其中，为第k个历元后向滤波的卡尔曼增益，/>、/>分别为第k个历元前向滤波量测更新的方差协方差、前向滤波预测更新的方差协方差的逆矩阵，上标-1表示矩阵的逆矩阵，/>为后向滤波的离散化的状态转移矩阵的转置，上标T表示矩阵转置，/>、/>分别为第k个历元后向滤波量测更新的状态向量、第k个历元前向滤波量测更新的状态向量，/>为第k+1个历元后向滤波量测更新的状态向量，/>为前向滤波预测更新的状态向量，/>、/>分别为第k个历元后向滤波量测更新的方差协方差、第k个历元前向滤波量测更新的方差协方差，/>为第k+1个历元后向滤波量测更新的方差协方差，/>为前向滤波预测更新的方差协方差。

实施例二

本实施例的目的是提供基于自适应滤波和最优平滑的水下组合导航系统，包括：

模型建立模块：建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

预测更新模块：利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

量测更新模块：获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

定位输出模块：根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，包括：

建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据所述当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果；

还包括基于条件约束的自适应滤波对水声观测值噪声进行自适应调节，具体为：

根据两个时刻声学换能器坐标和海底应答器计算预测发射距离和预测接收距离，进而得到预测双程距离；

根据声学换能器和海底基准点的吃水深度以及声速剖面计算发射时刻和接收时刻的加权平均声速，再结合量测的声波传播时间计算得到量测双程距离；

根据所得到的预测双程距离和量测双程距离以及水下声学误差，得到新息向量；

根据所得到的新息向量得到自适应调整后的量测噪声，根据自适应调整后的量测噪声与预设的最大量测噪声和最小量测噪声的大小，对量测噪声进行调整，使量测噪声与观测误差相匹配；

还包括将组合导航定位结果按由后往前的量测顺序进行最优平滑算法，该方法对扩展卡尔曼滤波算法得到的最优的状态向量和最优的方差协方差进行反向平滑操作，具体为：

基于前向滤波量测更新、离散化的状态转移矩阵和预测更新的方差协方差得到后向滤波的卡尔曼增益；

根据前向滤波量测更新的状态向量、后向滤波的卡尔曼增益、前向滤波预测更新的状态向量得到后向滤波量测更新的状态向量；

根据前向滤波量测更新方差协方差、后向滤波的卡尔曼增益和预测更新的方差协方差得到后向滤波量测更新的方差协方差。

2.如权利要求1所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，根据水下运载体的姿态误差、速度误差、位置误差、陀螺零偏、加速度零偏以及声速误差建立水下运载体的状态向量；基于水下运载体的状态向量的微分形式得到水下运载体的预测模型，将水下运载体的预测模型中的连续型状态转移矩阵离散化得到离散化的状态转移矩阵。

3.如权利要求1所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，基于量测矩阵和状态向量的乘积与量测噪声向量之和，得到声学双程传播距离的观测模型的量测向量。

4.如权利要求3所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，所述量测矩阵根据大地坐标系到导航坐标系的方向余弦矩阵、水下运载体坐标系到导航坐标系的方向余弦矩阵、惯导到声学换能器的杆臂在导航坐标系中的投影向量、声波双程传播时间和声学双程误差方程的雅可比矩阵所建立。

5.如权利要求4所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，所述声学双程误差方程的雅可比矩阵根据水下运载体声学信号发射时刻坐标、水下运载体声学信号接收时刻坐标、海底基准点坐标、水下运载体发射信号到应答器接收信号的传播距离、应答器发射应答信号到水下运载体再接收应答信号的传播距离所建立。

6.如权利要求1所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，根据状态一步预测值、卡尔曼增益、量测向量和量测矩阵更新得到最优状态向量；根据状态一步预测协方差、卡尔曼增益和量测矩阵更新得到最优方差协方差。

7.如权利要求6所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，所述卡尔曼增益根据状态一步预测协方差、量测矩阵和量测噪声矩阵所确定。

8.基于自适应滤波和最优平滑的水下组合导航系统，用以实现如权利要求1-7任一项权利要求所述的基于自适应滤波和最优平滑的水下组合导航方法，其特征在于，包括：

模型建立模块：建立包含声速误差在内的水下运载体的预测模型，以及基于声学双程传播距离的观测模型；

预测更新模块：利用扩展卡尔曼滤波算法将所述预测模型以及观测模型中的相关参数作为滤波变量进行预测更新，以获得水下运载体的状态一步预测值和状态一步预测协方差；

量测更新模块：获取水下运载体当前声学量测信息，并利用扩展卡尔曼滤波算法根据当前声学量测信息对所述状态一步预测值和所述状态一步预测协方差进行更新，得到水下运载体当前状态的最优状态向量和最优方差协方差；

定位输出模块：根据当前状态的最优状态向量和最优方差协方差，得到组合导航定位结果。