CN109613520A

CN109613520A - 一种基于滤波的超短基线安装误差在线标定方法

Info

Publication number: CN109613520A
Application number: CN201811532244.9A
Authority: CN
Inventors: 张涛; 张亮; 王健; 朱永云; 翁铖铖; 王帅
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2018-12-14
Filing date: 2018-12-14
Publication date: 2019-04-12
Anticipated expiration: 2038-12-14
Also published as: CN109613520B

Abstract

本发明公开了一种基于滤波的超短基线安装误差在线标定方法，步骤为：1、建立安装角误差以及杆臂误差的状态模型；2、结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；3、通过状态模型与观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；4、用卡尔曼滤波方程对误差进行实时的在线估计安装误差；5、将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。本发明的有益效果为：基于滤波的方法可以同时在线的估计出USBL与船体之间的安装误差，方法易于实施。

Description

一种基于滤波的超短基线安装误差在线标定方法

技术领域

本发明属于导航技术领域，特别涉及一种基于滤波的超短基线安装误差在线标定方法。

背景技术

水下定位技术在海洋资源开发，海底光缆铺设等方面有着广泛的需求。水下定位精度的提高是一切海下开发活动以及海下科研工作展开的根本保障。由于水下特殊的环境，基于水声的定位技术是水下导航的主要方式之一。按照基线长短，又可以分为长基线水下定位系统(LBL,Long Base Line)，短基线水下定位系统(SBL,Short Base Line)和超短基线水下定位系统(USBL,Ultra Short Base Line)。在这三种声学系统中，USBL由于其部署方便和成本低等优点，被广泛应用于海洋领域的开发，尽管它的定位精度是这其中最低的一个。

USBL系统利用安装在船体的信号收发器以及位于海底的应答器进行定位。影响USBL系统定位精度的因素有很多，包括系统测量误差，海洋环境影响以及USBL与船体其他传感器的安装误差等。在长航时，高精度的导航中，安装误差不容忽略。通常USBL在安装时，可以通过光学仪器精确的测出它与船体之间的安装角以及杆臂误差。然而当其中一个传感器需要维护或修理时，安装位置会发生改变，因此，每一次传感器的变动都需要进行一次安装误差的标定。有效并且容易实施的标定方法对于航海中的导航至关重要。

现有的USBL安装误差海上标定需要载体执行一系列复杂的航行轨迹，而载体在海上由于环境原因无法理想的按预定轨迹航行。因此一种简便且高效的超短基线安装误差标定是提高水下定位精度的关键。

发明内容

发明目的：针对上述问题缺陷，本发明提供一种基于滤波的超短基线安装误差在线标定方法，根据船体安装的RTK以及姿态测量传感器测出的船体位置信息以及姿态信息，结合超短基线输出的斜距以及方位角信息作为观测数据，实时估计出超短基线相对于船体的安装误差。

技术方案：本发明提出一种基于滤波的超短基线安装误差在线标定方法，包括如下步骤：

步骤1：建立安装角误差以及杆臂误差的状态模型；

步骤2：结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；

步骤3：通过步骤(1)中建立的状态模型与步骤(2)中建立的观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；

步骤4：用卡尔曼滤波方程对误差进行实时的在线估计安装误差；

步骤5：将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。

所述步骤1中建立安装角误差以及杆臂误差的状态模型具体为：

选取安装角误差以及杆臂误差为系统的状态量，

X_usbl＝[δθ_x δθ_y δθ_z δL_x δL_y δL_z]

其中，δθ_x,δθ_y,δθ_z为角度安装误差，δL_x,δL_y,δL_z为杆臂安装误差。

由于安装误差为常值，其微分方程如下：

可以得到系统状态方程为：

其中F_usbl(t)＝0_6×6。

所述步骤2中，结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型推导如下：

以USBL输出的斜距和方位角，以及用已知的应答器位置与船体位置，解算应答器相对于船心之间的斜距与方位角，两者的差值作为系统的观测量。

已知斜距与方位角为[α β R]。应答器在超短基线基阵坐标系中的相对位置坐标为[x y z]。两者有如下关系：

对其求偏微分可得：

其中，

下面计算已知绝对位置的应答器在船心坐标系(o_bx_by_bz_b)中的相对位置坐标。应答器的绝对位置为p_r＝[l_r λ_r h_r]，通过长基线的方式确定。GPS测得船心的绝对位置坐标为p_c＝[l_c λ_c h_c]。其表示方法为经纬高。通过姿态测量系统得到的姿态转换矩阵为则考虑USBL安装的角度误差与杆臂误差后的应答器在基阵坐标系中的相对位置关系为：

其中，安装角误差的姿态矩阵，L_b是超短基线相对于船体在船心坐标系下的杆臂距离。Δp_e是在地球直角坐标系(o_ex_ey_ez_e)下应答器位置相对于船体的位置的坐标表示。表示为

对上述公式求微分，由于状态只有安装误差，因此可得：

其中，[Δx Δy Δz]是应答器在USBL基阵坐标系下的位置误差，δθ＝[δθ_x δθ_y δθ_z]^T是安装角误差，δL＝[δL_x δL_y δL_z]^T是杆臂误差。(g×)是反对称矩阵的表示。由于是二阶小量，忽略它的影响。结合[x_u y_u z_u]的表达式，可以得到

其中

因此，综合上述公式，系统量测方程为：

其中[α_gps β_gps R_gps]是根据已知应答器的位置坐标与GPS测得的船心坐标，计算得到的应答器相对于船心的斜距与方位角。观测噪声为V。

观测矩阵

因此系统量测方程为Z＝HX+V。

所述步骤3中，误差状态的可观性分析，其具体步骤为：

1)3.对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ₁与H₁。

2)由φ₁与H₁计算系统总的可观性矩阵M₁.

3)以此类推。依次计算r个时间段内的各状态转移矩阵φ_j与观测矩阵H_j，并求可观性矩阵

4)计算系统可观性矩阵M_s＝[M₁M₂...M_r]

5)对矩阵M_s进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度。

所述步骤4中，应用卡尔曼滤波方程进行实时估计安装误差，其具体方程为：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T。

其中，Φ_k,k-1表示k-1时刻到k时刻的状态转移矩阵，本文中Φ_k,k-1＝I_6×6，I_6×6是6维的单位矩阵。

表示k-1的滤波器的状态估计值。表示利用计算得到的对状态的一步预测。

K_k表示卡尔曼滤波器的增益。Z_k表示系统的观测值，表示在一步预测的基础上根据量测值计算得到的对k时刻系统状态的估计值。R_k表示观测噪声矩阵。

P_k|k-1为估计值的均方误差阵。P_k-1表示k-1时刻状态估计值的均方误差阵。P_k表示当前时刻状态估计值的均方误差阵。I表示单位矩阵，其维度与系统状态量相同。

所述步骤5中，将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。其具体步骤如下：

(5.1)水下潜航器装备超短基线定位系统，姿态测量传感器，并在水底安放应答器，利用长基线原理计算应答器在地理坐标系下的位置坐标

(5.2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距(R)与方位角(α,β)。

(5.3)传统的超短基线系统对潜航器的定位坐标表示如下：

其中，为姿态矩阵，由姿态传感器测得，为通过超短基线系统输出计算得到的潜航器在地理系下的位置坐标。表示应答器在超短基线水听器基阵中心坐标系下的位置，通过斜距与方位角计算得到，具体公式如下：

(5.4)由于安装误差的存在，超短基线水听器基阵中心坐标系与船体的坐标系并不重合，上述公式存在较大误差，利用上述步骤(1)-(4)中估计出的安装角度误差与杆臂误差，补偿超短基线对载体的定位结果，得到如下关系：

其中，L是估计出的杆臂误差，L＝[L_x L_y L_z]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离。表示安装误差角的姿态矩阵。

补偿安装误差后，将提高超短基线对水下载体的定位精度。

本发明采用上述技术方案，具有以下有益效果：

根据步骤1所建立的系统状态模型，本发明方案可以同时的估计出安装角度误差与杆臂误差，相比于现有的方案下先标定杆臂误差再标定角度误差，极大的提高了标定的效率。基于滤波的安装误差估计方法，可以在载体行驶过程中在线标定出安装误差并实时补偿到定位结果中，并且合理的噪声矩阵设置，可以避免现有最小二乘法对于噪声敏感的缺点，提高了估计精度。

附图说明

图1为具体实施例中USBL接收基阵安装误差角示意图。

具体实施方式

下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

一种基于滤波方法的水下超短基线安装误差在线标定方法，根据船体安装的RTK以及姿态测量传感器测出的船体位置信息以及姿态信息，结合超短基线输出的斜距以及方位角信息作为观测数据，实时估计出超短基线相对于船体的安装误差。图1是USBL接收基阵安装误差角示意图，标定过程包括如下具体步骤：

步骤1：建立安装角误差以及杆臂误差的状态模型：

选取安装角误差以及杆臂误差为系统的状态量，

X_usbl＝[δθ_x δθ_y δθ_z δL_x δL_y δL_z]

由于安装误差为常值，其微分方程如下：

可以得到系统状态方程为：

其中F_usbl(t)＝0_6×6。

步骤2结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型：

对其求偏微分可得：

其中，

对上述公式求微分，由于状态只有安装误差，因此可得：

其中

因此，综合上述公式，系统量测方程为：

观测矩阵

因此系统量测方程为Z＝HX+V。

步骤3对误差状态可观性分析，其具体步骤为：

1)对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ₁与H₁。

2)由φ₁与H₁计算系统总的可观性矩阵M₁.

4)计算系统可观性矩阵M_s＝[M₁M₂...M_r]

步骤4应用卡尔曼滤波方程进行实时估计安装误差：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T。

步骤5将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度

(1)水下潜航器装备超短基线定位系统，姿态测量传感器，并在水底安放应答器，利用长基线原理计算应答器在地理坐标系下的位置坐标

(2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距(R)与方位角(α,β)。

(3)传统的超短基线系统对潜航器的定位坐标表示如下：

(4)由于安装误差的存在，超短基线水听器基阵中心坐标系与船体的坐标系并不重合，上述公式存在较大误差，利用上述步骤(1)-(5)中估计出的安装角度误差与杆臂误差，补偿超短基线对载体的定位结果，得到如下关系：

补偿安装误差后，将提高超短基线对水下载体的定位精度。

Claims

1.一种基于滤波的超短基线安装误差在线标定方法，其特征在于，包括如下步骤：

(1)建立安装角误差以及杆臂误差的状态模型；

(2)结合RTK、姿态测量传感器的数据，以及超短基线的输出原始数据，建立系统的观测模型；

(3)通过步骤(1)中建立的状态模型与步骤(2)中建立的观测模型，对船体在水面的航行轨迹下进行误差状态的可观性分析，确保当前运动轨迹能够充分激励出误差状态；

(4)用卡尔曼滤波方程对误差进行实时的在线估计安装误差；

(5)将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度。

2.根据权利要求1所述的一种基于滤波的超短基线安装误差在线标定方法，其特征在于，所述步骤(1)中建立安装角误差以及杆臂误差的状态模型的具体步骤如下：

选取安装角误差以及杆臂误差为系统的状态量，

X_usbl＝[δθ_x δθ_y δθ_z δL_x δL_y δL_z]

其中，δθ_x,δθ_y,δθ_z为角度安装误差，δL_x,δL_y,δL_z为杆臂安装误差；

由于安装误差为常值，其微分方程如下：

可以得到系统状态方程为：

其中F_usbl(t)＝0_6×6。

3.根据权利要求1所述的一种基于滤波的超短基线安装误差在线标定方法，其特征在于，所述步骤(2)中建立系统的观测模型的具体步骤如下：

以USBL输出的斜距和方位角，以及用已知的应答器位置与船体位置，解算应答器相对于船心之间的斜距与方位角，两者的差值作为系统的观测量；

其中斜距与方位角为[α β R]；应答器在超短基线基阵坐标系中的相对位置坐标为[xy z]；两者有如下关系：

对其求偏微分可得：

其中，

计算已知绝对位置的应答器在船心坐标系(o_bx_by_bz_b)中的相对位置坐标；应答器的绝对位置为p_r＝[l_r λ_r h_r]，通过长基线的方式确定；GPS测得船心的绝对位置坐标为p_c＝[l_c λ_ch_c]；其表示方法为经纬高；通过姿态测量系统得到的姿态转换矩阵为则考虑USBL安装的角度误差与杆臂误差后的应答器在基阵坐标系中的相对位置关系为：

其中，安装角误差的姿态矩阵，L_b是超短基线相对于船体在船心坐标系下的杆臂距离；Δp_e是在地球直角坐标系(o_ex_ey_ez_e)下应答器位置相对于船体的位置的坐标表示，表示为

对上述公式求微分，由于状态只有安装误差，因此可得：

其中，[Δx Δy Δz]是应答器在USBL基阵坐标系下的位置误差，δθ＝[δθ_x δθ_y δθ_z]^T是安装角误差，δL＝[δL_x δL_y δL_z]^T是杆臂误差；(g×)是反对称矩阵的表示；由于是二阶小量，忽略它的影响；结合[x_u y_u z_u]的表达式，得到

其中

因此，综合上述公式，系统量测方程为：

其中[α_gps β_gps R_gps]是根据已知应答器的位置坐标与GPS测得的船心坐标，计算得到的应答器相对于船心的斜距与方位角；观测噪声为V；

观测矩阵

因此系统量测方程为Z＝HX+V。

4.根据权利要求1所述的一种基于滤波的超短基线安装误差在线标定方法，其特征在于，所述步骤(3)中误差状态的可观性分析，其具体步骤为：

(4.1)对待分析的导航系统进行分段处理，获取初始时间段的状态转移矩阵φ₁与H₁；

(4.2)由φ₁与H₁计算系统总的可观性矩阵M₁；

(4.3)以此类推；依次计算r个时间段内的各状态转移矩阵φ_j与观测矩阵H_j，并求可观性矩阵

(4.4)计算系统可观性矩阵M_s＝[M₁ M₂ ... M_r]；

(4.5)对矩阵M_s进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度。

5.根据权利要求1所述的一种基于滤波的超短基线安装误差在线标定方法，其特征在于，所述步骤(4)中用卡尔曼滤波方程对误差进行实时的在线估计安装误差的具体步骤方程：

状态一步预测方程：

状态估值方程：

滤波增益方程：

一步预测均方误差方程：

估计均方误差方程：P_k＝[I-K_kH_k]P_k|k-1[I-K_kH_k]^T+K_kR_kK_k ^T；

其中，Φ_k,k-1表示k-1时刻到k时刻的状态转移矩阵，本文中Φ_k,k-1＝I_6×6，I_6×6是6维的单位矩阵；

表示k-1的滤波器的状态估计值；表示利用计算得到的对状态的一步预测；

K_k表示卡尔曼滤波器的增益；Z_k表示系统的观测值，表示在一步预测的基础上根据量测值计算得到的对k时刻系统状态的估计值；R_k表示观测噪声矩阵；

P_k|k-1为估计值的均方误差阵；P_k-1表示k-1时刻状态估计值的均方误差阵；P_k表示当前时刻状态估计值的均方误差阵；I表示单位矩阵，其维度与系统状态量相同。

6.根据权利要求1所述的一种基于滤波的超短基线安装误差在线标定方法，其特征在于，所述步骤(5)中将得到的在线估计结果用来补偿超短基线对水下潜航器的定位误差，提高超短基线定位精度；具体步骤如下：

(5.2)潜航器在水下行驶过程中，超短基线系统测得并输出应答器相对于超短基线水听器基阵中心的斜距(R)与方位角(α,β)；

(5.3)传统的超短基线系统对潜航器的定位坐标表示如下：

其中，为姿态矩阵，由姿态传感器测得，为通过超短基线系统输出计算得到的潜航器在地理系下的位置坐标；表示应答器在超短基线水听器基阵中心坐标系下的位置，通过斜距与方位角计算得到，具体公式如下：

其中，L是估计出的杆臂误差，L＝[L_x L_y L_z]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离。表示安装误差角的姿态矩阵；

补偿安装误差后，将提高超短基线对水下载体的定位精度。