[发明专利]一种基于组合导航的DVL误差参数在线标定方法

权利要求书

1.一种基于组合导航的DVL误差参数在线标定方法，其特征在于包括以下步骤：

步骤一：基于罗经的姿态角量测信息以及DVL的速度量测信息进行运载体航位推算，所述运载体航位推算包括坐标变换和位置解算；

步骤二：以X＝[δk″ θ′_dz δL δλ δh]^T作为状态变量、以LBL的位置量测信息和深度计的深度量测信息为观测值进行滤波，

其中，

δk″为DVL的标度因数误差的变体，θ′_dz为DVL的z轴安装角度误差的变体，δL、δλ、δh分别为运载体的纬度误差、运载体的经度误差和运载体的高度误差，δk为DVL的标度因数误差，θ_dz为DVL的z轴安装角度误差；

步骤三：利用滤波结果矫正航位推算，进而得到运载体的精确定位以及DVL误差参数；

所述滤波为卡尔曼滤波；

所述卡尔曼滤波的具体步骤为：

步骤1：将状态变量X和协方差矩阵P的初值进行设定，状态变量X和协方差矩阵P表示为：

X(0)＝[δk″(0) θ′_dz(0) δL(0) δλ(0) δh(0)]^T＝0_5×1

P(0)＝I_5×5

其中，I_5×5为5维单位矩阵；

步骤2：获取上一时刻的状态变量，上一时刻的状态变量表示为：

X(k-1)＝[δk″(k-1) θ′_dz(k-1) δL(k-1) δλ(k-1) δh(k-1)]^T

同时获取上一时刻的协方差矩阵P(k-1)；

步骤3：获取上一时刻的DVL的安装误差角和标度因数误差，并根据上一时刻的DVL的安装误差角和标度因数误差得到DVL误差矩阵，DVL误差矩阵表示为：

步骤4：由DVL得到当前的量测速度，当前的量测速度表示为：

由当前的量测速度和DVL误差矩阵得到当前的载体坐标系速度，当前的载体坐标系速度表示为：

其中，分别为k时刻载体坐标系下x轴速度、y轴速度和z轴速度，分别为k时刻DVL量测坐标系下x轴速度、y轴速度和z轴速度；

步骤5：根据当前的载体坐标系速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为

v_E(k)、v_N(k)、v_U(k)分别为k时刻导航坐标系下的东向速度、北向速度和天向速度；

步骤6：对当前的量测速度V^d(k)进行矩阵化，表示为：

然后结合当前罗经量测结果得到速度误差转移矩阵，速度误差转移矩阵表示为：

步骤7：根据速度误差转移矩阵得到当前系统矩阵F_k/k-1，当前系统矩阵F_k/k-1表示为：

其中，L为纬度，T_s为采样时间、R_n为子午圈曲率半径、R_m为卯酉圈曲率半径、secL为正割运算、L(k-1)为k-1时刻纬度，d_ij为

步骤8：根据当前系统矩阵F_k/k-1进行状态一步预测，得到状态变量的估计值，状态变量的估计值表示为：

步骤9：根据运载体上一时刻所处的经度信息得到当前噪声矩阵G_k-1，当前噪声矩阵G_k-1表示为：

其中，为导航坐标系速度误差转换矩阵，

N₁矩阵表示为：

矩阵表示为：

步骤10：根据上一时刻的协方差矩阵P(k-1)、当前噪声矩阵G_k-1以及当前系统矩阵F_k/k-1得到当前时刻的协方差矩阵估计值P_k/k-1，当前时刻的协方差矩阵估计值P_k/k-1表示为：

其中Q为DVL量测噪声协方差矩阵，σ_DVLx、σ_DVLy、σ_DVLz分别为DVL速度测量过程中的噪声标准差；

步骤11：获取当前时刻的长基线和深度计的测量值，并根据当前时刻的长基线和深度计的测量值得到当前时刻的位置量测值，当前时刻的位置量测值表示为：

其中，分别为LBL量测的纬度、LBL量测的经度和深度计量测的深度，

根据当前时刻的位置量测值和航位推算结果得到观测值，观测值表示为：

为当前的航位推算结果；

步骤12：根据当前时刻的协方差矩阵估计值P_k/k-1得到滤波增益K_k，滤波增益K_k表示为：

其中，量测矩阵H_k表示为：

量测噪声方差矩阵R表示为：

其中σ_LBLL、σ_LBLλ和σ_DEP分别为长基线的纬度量测噪声标准差、长基线的经度量测噪声标准差和深度计量测标准差；

步骤13：根据状态变量的估计值、滤波增益K_k和观测值进行状态修正，得到当前时刻的状态变量，当前时刻的状态变量表示为：

步骤14：根据滤波增益K_k、量测矩阵H_k以及当前时刻的协方差矩阵估计值P_k/k-1进行协方差阵修正，得到当前时刻的协方差矩阵，当前时刻的协方差矩阵表示为：

P_k＝(I-K_k·H_k)P_k/k-1。