CN117146830A - 一种自适应多信标航位推算和长基线的紧组合导航方法 - Google Patents

Abstract

本发明公开一种自适应多信标航位推算和长基线的紧组合导航方法，属于组合导航测量技术领域，用于在多信标场景下进行组合导航，包括基于扩展卡尔曼滤波方法，以航速、航迹角定点运动学方程以及声速随机扰动为系统方程，使用时延作为观测量，根据接收到的随机数量的时延信息，自适应构建观测方程，以实现对信标应答信号各种接收情况的自适应处理。本发明数据利用率高，且具有更快的收敛速度和更高的定位精度，能够有效地估计海水的平均声速。

Description

一种自适应多信标航位推算和长基线的紧组合导航方法

技术领域

本发明公开一种自适应多信标航位推算和长基线的紧组合导航方法，属于组合导航测量技术领域。

背景技术

无人水下航行器UUV以其全面的水下作业能力，已然成为海洋探测、科学研究、生态保护以及国防等多个领域中最广泛应用的水下设备之一。水下导航和定位技术作为UUV能正常运行的基础，重要性日益凸显。然而，由于水下环境中无线电信号传播受限，传统的全球导航卫星系统GNSS无法直接应用于水下导航，使得水下导航定位面临着独特的挑战。

航位推算DR方法和长基线LBL水下定位方法被广泛采用。DR因其积分形式的计算方法，长时间运行会导致误差累积，可用LBL进行校正；传统长基线水下定位方法对测距数据的数量有较高的要求，但实际应用中常因海洋环境和各种噪声影响，无法接收到所需要数量信号，影响解算。基于单信标的组合导航方法，虽然每次解算只需一个测距数据与航速、航向信息组合，但因系统可观性受限，需适当的机动航行路径，且定位结果收敛较慢。另一方面，现有的水声定位算法在应用时假设声速已知，但实际中声速难以实时测量，信号传播路径的声速分布更难以精确测量，这会对距离解算造成误差，最终影响定位精度。

发明内容

本发明的目的在于提供一种自适应多信标航位推算和长基线的紧组合导航方法，以解决现有技术中，单信标的组合导航方法系统可观性受限、定位结果收敛较慢、声速难以实时测量的问题。

一种自适应多信标航位推算和长基线的紧组合导航方法，包括：

S1.构建航位推算和长基线紧组合导航模型；

S1.1. 应答器与无人水下航行器之间海域中的实际声速等效为平均声速加入系统状态中进行估计，建立无人水下航行器二维运动方程；

S1.2.建立运动学方程；

S1.3.计算系统测量输出；

S1.4.将运动学方程离散化，得离散系统方程；

S1.5.计算无人水下航行器与信标之间的距离/>；

S2.构建自适应信标数量的航位推算和长基线紧组合导航滤波方法；

S2.1.计算k时刻向k+1时刻状态一步预测；

S2.2.计算k时刻向k+1时刻状态一步预测均方误差阵；

S2.3.计算k+1时刻量测矩阵；

S2.4.计算k+1时刻滤波增益；

S3.根据信标数量进行解算。

S1.1包括：

无人水下航行器深度已知，以其位置和声速作为状态，系统状态和外部/>输入分别为：

；/>；

式中：为无人水下航行器的位置坐标，/>为无人水下航行器的速度，/>为无人水下航行器航迹角，C为平均声速。

S1.2包括：

；

式中，为状态向量，/>为速度的测量噪声，/>为航迹角的测量噪声，/>为声速随机扰动。

S1.3包括：

；

式中，为正整数，/>为无人水下航行器与各信标的应答信号传播时延：

;

式中，为信标的位置，布标完成后通过绕标获得信标的位置；/>为无人水下航行器的深度，/>由无人水下航行器携带的深度计获得。

S1.4包括：

；

式中：为k时刻状态向量；/>为k时刻量测向量；/>和/>分别为k时刻速度和航迹角的测量噪声，/>是k时刻声速随机扰动，/>为k时刻时延量测噪声向量，/>为k时刻平均声速，/>、/>、/>、/>为零均值的高斯白噪声序列，且互不相关，/>为两次运算的时间间隔。

S1.5包括：

无人水下航行器通过航速、航向信息对位置进行推算，当无人水下航行器接近信标时，航行器发送问答信号并接受信标的回信，测量在此期间信号的传播时延，结合平均声速C，计算无人水下航行器与信标/>之间的距离/>：

。

S2.1包括：

离散系统方程构成一个非线性的组合导航模型，利用扩展卡尔曼滤波方法进行计算：

k时刻向k+1时刻状态一步预测为：

；

式中，A为系数矩阵，为k时刻状态估值，/>，/>。

S2.2包括：

状态方程关于为非线性，/>，状态方程关于/>的雅克比矩阵/>为：

；

k时刻向k+1时刻状态一步预测均方误差阵为：

；

式中，为对角阵，/>，/>为航速的测量误差的标准差，/>为航向角的测量误差的标准差，/>为声速随机扰动的标准差，/>为k时刻均方误差。

S2.3包括：

k+1时刻量测矩阵为：

；

式中，为横坐标k时刻向k+1时刻的一步预测，/>为纵坐标k时刻向k+1时刻的一步预测，/>为声速k时刻向k+1时刻的一步预测，/>为中间参数；

。

S2.4包括：

k+1时刻滤波增益为：

；

式中，为系统量测噪声协方差对角矩阵，对角元素/>为时延的量测误差的标准差。

相对比现有技术，本发明具有以下有益效果：本发明能在水下航行器利用长基线进行位置校正时，自适应的处理因受海洋环境等因素影响而造成的在不测量周期中信标测距信号各种接收情况，相较于基于单信标的DR/LBL紧组合导航算法，数据利用率高、解算收敛更快，解算结果也更稳定，并且可以实时估计海水的平均声速，以应对海水声速在一定区域内测量困难的问题。

附图说明

图1是本发明实施例的真实航行轨迹图；

图2是每个测量周期接收到测距信号的数量图；

图3是与信标1的测距信号时延图；

图4是与信标2的测距信号时延图；

图5是与信标3的测距信号时延图；

图6是与信标4的测距信号时延图；

图7是位置解算结果图；

图8是定位误差对比图；

图9是平均声速估计结果图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种自适应多信标航位推算和长基线的紧组合导航方法，包括：

S1.构建航位推算和长基线紧组合导航模型；

S1.1. 应答器与无人水下航行器之间海域中的实际声速等效为平均声速加入系统状态中进行估计，建立无人水下航行器二维运动方程；

S1.2.建立运动学方程；

S1.3.计算系统测量输出；

S1.4.将运动学方程离散化，得离散系统方程；

S1.5.计算无人水下航行器与信标之间的距离/>；

S2.构建自适应信标数量的航位推算和长基线紧组合导航滤波方法；

S2.1.计算k时刻向k+1时刻状态一步预测；

S2.2.计算k时刻向k+1时刻状态一步预测均方误差阵；

S2.3.计算k+1时刻量测矩阵；

S2.4.计算k+1时刻滤波增益；

S3.根据信标数量进行解算。

S1.1包括：

无人水下航行器深度已知，以其位置和声速作为状态，系统状态和外部/>输入分别为：

；/>；

式中：为无人水下航行器的位置坐标，/>为无人水下航行器的速度，/>为无人水下航行器航迹角，C为平均声速。

S1.2包括：

；

式中，为状态向量，/>为速度的测量噪声，/>为航迹角的测量噪声，/>为声速随机扰动。

S1.3包括：

；

式中，为正整数，/>为无人水下航行器与各信标的应答信号传播时延：

;

式中，为信标的位置，布标完成后通过绕标获得信标的位置；/>为无人水下航行器的深度，/>由无人水下航行器携带的深度计获得。

S1.4包括：

；

式中：为k时刻状态向量；/>为k时刻量测向量；/>和/>分别为k时刻速度和航迹角的测量噪声，/>是k时刻声速随机扰动，/>为k时刻时延量测噪声向量，/>为k时刻平均声速，/>、/>、/>、/>为零均值的高斯白噪声序列，且互不相关，/>为两次运算的时间间隔。

S1.5包括：

无人水下航行器通过航速、航向信息对位置进行推算，当无人水下航行器接近信标时，航行器发送问答信号并接受信标的回信，测量在此期间信号的传播时延，结合平均声速C，计算无人水下航行器与信标/>之间的距离/>：

。

S2.1包括：

离散系统方程构成一个非线性的组合导航模型，利用扩展卡尔曼滤波方法进行计算：

k时刻向k+1时刻状态一步预测为：

；

式中，A为系数矩阵，为k时刻状态估值，/>，/>。

S2.2包括：

状态方程关于为非线性，/>，状态方程关于/>的雅克比矩阵/>为：

；

k时刻向k+1时刻状态一步预测均方误差阵为：

；

式中，为对角阵，/>，/>为航速的测量误差的标准差，/>为航向角的测量误差的标准差，/>为声速随机扰动的标准差，/>为k时刻均方误差。

S2.3包括：

k+1时刻量测矩阵为：

；

式中，为横坐标k时刻向k+1时刻的一步预测，/>为纵坐标k时刻向k+1时刻的一步预测，/>为声速k时刻向k+1时刻的一步预测，/>为中间参数；

。

S2.4包括：

k+1时刻滤波增益为：

；

式中，为系统量测噪声协方差对角矩阵，对角元素/>为时延的量测误差的标准差。

为了验证本发明提出的自适应多信标DR/LBL紧组合导航方法在实际应用中的效果，进行验证实验。采用实验船搭载卫星接收机和水声问答机模拟航行器的真实行驶过程。首先，将4个信标投入海中，构成边长为1km左右的矩形长基线阵，位置标定后，得到长基线阵四个信标的准确位置如表1所示。

表 1 信标位置

。

实验船携带实验设备在长基线阵中移动，水声问答机持续向各个信标发送问答信号并接收回令，记录信号传播时间；卫星接收机不断解算实验船的位置信息，利用卫星定位信息做位置差分，再根据多普勒计程仪和罗经的数据特性分别加入方差为0.2(m/s)²和0.6deg²的高斯噪声，来模拟航向和航速数据用于后续解算；同时，卫星定位数据也作为位置真值用于定位精度分析。通过声速剖面仪测得海水平均声速为1470m/s。实验船出发后，绕行“信标3”一周，然后横穿整个长基线阵。航行轨迹和信标布设位置如图1所示。数据显示，UUV在很多测量周期中都只能接收到一个或两个信标的应答信号，不利于LBL单独使用，如图2所示；通过分析UUV与各个信标单独的测距信号接收情况，发现即便已经行驶在便于对“信标3”信号收发的航线，仍有许多测量周期中，没有接收到“信标3”的测距信号，但接收到了其他信标的测距信号，不利于基于单信标的组合方法的使用，如图3、图4、图5、图6所示。

根据实测数据，人为加入了840m的初始位置偏差，以模拟水下航行器长时间航行造成的位置累积误差。分别采用DR、基于单信标的DR/LBL紧组合导航方法（固定接收信标3的测距信号）和自适应多信标DR/LBL紧组合导航方法进行解算，解算结果如图7所示，定位误差对比如图8所示，从图中可以观察到，航位推算的解算误差虽在实验期间没有明显增大，但其无法对自身累积误差进行修正，所以只适合在位置校正后的短时间内使用；基于单信标的DR/LBL紧组合导航方法的定位结果，虽然能在初始阶段缓慢收敛，但因系统可观性受限，解算误差没有收敛到可用的精度范围内；而自适应多信标DR/LBL紧组合导航方法，因数据的利用率高，系统可观性好，定位结果能很快的收敛并维持在较高的精度内。平均声速估计结果如图9所示，经过一段时间后也收敛到了真实值附近，为位置解算提供了良好的支撑。

系统量测矩阵和系统量测协方差矩阵的维度根据收到的测距信号的数量而相应改变。例如，当仅接收到“信标2”、“信标3”的应答时，量测矩阵为：

；

系统量测噪声协方差矩阵为：

；

状态估计：

；

根据每个信标的位置，提前将其在量测矩阵中所对应的公式提前写好，实际应用中，根据当前测量周期实际数据接收情况，自适应的组成对应的量测矩阵参与滤波。

以上实施例仅用于说明本发明的技术方案，而非对其限制，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，包括：

S1.构建航位推算和长基线紧组合导航模型；

S1.1. 应答器与无人水下航行器之间海域中的实际声速等效为平均声速加入系统状态中进行估计，建立无人水下航行器二维运动方程；

S1.2.建立运动学方程；

S1.3.计算系统测量输出；

S1.4.将运动学方程离散化，得离散系统方程；

S1.5.计算无人水下航行器与信标之间的距离/>；

S2.构建自适应信标数量的航位推算和长基线紧组合导航滤波方法；

S2.1.计算k时刻向k+1时刻状态一步预测；

S2.2.计算k时刻向k+1时刻状态一步预测均方误差阵；

S2.3.计算k+1时刻量测矩阵；

S2.4.计算k+1时刻滤波增益；

S3.根据信标数量进行解算。

2.根据权利要求1所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S1.1包括：

无人水下航行器深度已知，以其位置和声速作为状态，系统状态和外部/>输入分别为：

；/>；

式中：为无人水下航行器的位置坐标，/>为无人水下航行器的速度，/>为无人水下航行器航迹角，C为平均声速。

3.根据权利要求2所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S1.2包括：

；

式中，为状态向量，/>为速度的测量噪声，/>为航迹角的测量噪声，/>为声速随机扰动。

4.根据权利要求3所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S1.3包括：

；

式中，为正整数，/>为无人水下航行器与各信标的应答信号传播时延：

;

式中，为信标的位置，布标完成后通过绕标获得信标的位置；/>为无人水下航行器的深度，/>由无人水下航行器携带的深度计获得。

5.根据权利要求4所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S1.4包括：

；

式中：为k时刻状态向量；/>为k时刻量测向量；/>和/>分别为k时刻速度和航迹角的测量噪声，/>是k时刻声速随机扰动，/>为k时刻时延量测噪声向量，/>为k时刻平均声速，/>、/>、/>、/>为零均值的高斯白噪声序列，且互不相关，/>为两次运算的时间间隔。

6.根据权利要求5所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S1.5包括：

无人水下航行器通过航速、航向信息对位置进行推算，当无人水下航行器接近信标时，航行器发送问答信号并接受信标的回信，测量在此期间信号的传播时延，结合平均声速C，计算无人水下航行器与信标/>之间的距离/>：

。

7.根据权利要求6所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S2.1包括：

离散系统方程构成一个非线性的组合导航模型，利用扩展卡尔曼滤波方法进行计算：

k时刻向k+1时刻状态一步预测为：

；

式中，A为系数矩阵，为k时刻状态估值，/>，/>。

8.根据权利要求7所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S2.2包括：

状态方程关于为非线性，/>，状态方程关于/>的雅克比矩阵/>为：

；

k时刻向k+1时刻状态一步预测均方误差阵为：

；

式中，为对角阵，/>，/>为航速的测量误差的标准差，/>为航向角的测量误差的标准差，/>为声速随机扰动的标准差，/>为k时刻均方误差。

9.根据权利要求8所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S2.3包括：

k+1时刻量测矩阵为：

；

式中，为横坐标k时刻向k+1时刻的一步预测，/>为纵坐标k时刻向k+1时刻的一步预测，/>为声速k时刻向k+1时刻的一步预测，/>为中间参数；

。

10.根据权利要求9所述的自适应多信标航位推算和长基线的紧组合导航方法，其特征在于，S2.4包括：

k+1时刻滤波增益为：

；

式中，为系统量测噪声协方差对角矩阵，对角元素/>为时延的量测误差的标准差。