[发明专利]一种基于滤波的超短基线安装误差在线标定方法

权利要求书

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

(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)中建立系统的观测模型的具体步骤如下：

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

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

对其求偏微分可得：

其中，

计算已知绝对位置的应答器在船心坐标系(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是杆臂误差；(·×)是反对称矩阵的表示；由于是二阶小量，忽略它的影响；结合[x_u y_u z_u]的表达式，得到

其中

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

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

观测矩阵

因此系统量测方程为Z_k＝H_kX+V；

所述步骤(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进行可观性分解，求出各个状态量对应的奇异值，状态量对应的奇异值反映了该状态的可观测度；

所述步骤(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表示观测噪声矩阵；H_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]表示在载体坐标系下超短基线中心相对于船心的三维杆臂距离；表示安装误差角的姿态矩阵；

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