CN112197789B - 一种基于quest的ins/dvl安装误差标定方法 - Google Patents

Abstract

本发明公开一种基于QUEST的INS/DVL安装误差标定方法，针对INS/DVL安装误差标定时的大角度非线性问题及安装误差与DVL标度因数误差耦合的问题，首先进行INS体坐标系下的INS、DVL速度矢量预处理，然后基于QUEST的最优四元数求解，最后进行INS与DVL之间安装误差角计算。本发明方法不受DVL标度因数误差及安装误差角度大小影响，可以对惯导***与DVL间的安装误差进行在线标定。可以适用于各种角度情况下的DVL安装误差标定。

Description

一种基于QUEST的INS/DVL安装误差标定方法

技术领域

本发明属于误差的标定技术领域，具体涉及惯性导航***与DVL之间大角度安装误差的标定。

背景技术

惯导***与DVL安装误差的标定结果是影响SINS/DVL组合导航精度的关键因素，其直接影响SINS/DVL组合导航的定位精度。传统的最小二乘法标定及卡尔曼滤波标定，一是只能对小角度的安装误差进行标定，难以对大角度安装误差进行高精度标定；二是安装误差和DVL标度因数误差耦合在一起，误差难以分离，受载体运动环境影响较大。本文提出了一种SINS/DVL安装误差标定方法，不受DVL标度因数误差及安装误差角度大小影响，可以对惯导***与DVL间的安装误差进行在线标定。

发明内容

本发明提供了一种基于QUEST的INS/DVL安装误差标定方法，不受DVL标度因数误差及安装误差角度大小影响，可以对惯导***与DVL间的安装误差进行在线标定。

本发明一种基于QUEST的INS/DVL安装误差标定方法，包括如下步骤：

步骤一，INS体坐标系下的INS、DVL速度矢量预处理；

步骤二，基于QUEST的最优四元数求解；

步骤三，INS与DVL之间安装误差角计算。

进一步地，

所述步骤一中INS体坐标系下的INS、DVL速度矢量预处理包括：

1)根据INS的角速度和DVL相对于INS的杆臂，对DVL速度矢量进行杆臂误差的补偿，补偿方式如下：

式中：为DVL输出的原始速度矢量，/>为INS的角速度，/>为DVL相对于INS的杆臂；

2)计算INS和DVL的合速度，计算方法如下：

式中，为INS的体坐标系速度矢量，V_ins为INS的合速度，V_dvl为DVL的合速度；

3)对INS和DVL的速度进行判断，当且仅当INS合速度大于1m/s，DVL合速度大于1m/s，INS航向角速度小于1°/s，INS合速度和DVL合速度之间差值小于1m/s时，进行DVL安装误差角估计的后续步骤计算；

4)对DVL速度矢量和INS与GNSS组合导航得到的体坐标下的速度进行归一化处理，计算方法如下：

式中，为INS体坐标系速度，/>为归一化后的DVL速度矢量，/>为归一化后的INS体坐标系速度矢量。

进一步地，

所述步骤二中基于QUEST的最优四元数求解包括：

1)通过以下公式，计算各中间变量

σ＝trB (8)

S＝B+B^T (9)

其中，n表示n组速度矢量测量值；

2)根据式(6)至(9)，计算如下损失函数构造矩阵

3)λ_max为矩阵K的最大特征值，使用1作为初值，进行迭代计算求取λ_max；将λ_max,0＝1带入式(11)进行迭代，得：

λ_max，i+1＝λ_max，i-p(λ_i)/p′(λ_i)，i＝0，1，... (11)

p(λ_i)计算公式如下：

p(λ)＝λ⁴-(a+b)λ²-cλ+(ab+cσ-d) (12)

其中，a＝σ²-κ，κ为S伴随矩阵的迹；

4)根据计算出来的特征值λ_max，计算对应的K的特征向量即为对应的最优四元数。

所述步骤三中INS与DVL之间安装误差角计算包括：

1)得到最优估计的四元数后，将该四元数转化为安装误差矩阵

其中，q_i为四元数中的元素；

2)再根据安装误差矩阵即可计算得到DVL安装误差角

其中，C_ij为中第i行，第j列元素。

本发明针对INS/DVL安装误差标定时的大角度非线性问题及安装误差与DVL标度因数误差耦合的问题，提出了一种利用惯导***体坐标系速度矢量和DVL速度矢量进行安装误差估计的安装误差标定方法，可以适用于各种角度情况下的DVL安装误差标定。

附图说明

图1DVL安装误差角估计结果

图2DVL安装误差角估计结果

具体实施方式

下面结合附图对本发明作进一步详细说明。

标定DVL安装误差时，惯导***处于INS/GNSS组合导航工作模式，DVL速度不参与组合导航。在INS/GNSS组合导航过程中，当惯导***接收到DVL速度时，在该导航周期进行DVL安装误差估计的迭代计算。各周期标定算法实现过程如下：

1)根据惯导***的角速度和DVL相对于惯导***的杆臂，对DVL速度矢量进行杆臂误差的补偿。补偿方式如下：

式中：为DVL输出的原始速度矢量，/>为惯导***的角速度，/>为DVL相对于惯导***的杆臂。

2)计算惯导***和DVL的和速度，计算方法如下：

式中，为惯导***的体坐标系速度矢量，V_ins为惯导***的合速度，V_dvl为DVL的合速度。

3)对惯导***和DVL的速度进行判断，当且仅当惯导***合速度大于1m/s，DVL合速度大于1m/s，惯导***航向角速度小于1°/s，惯导***合速度和DVL合速度之间差值小于1m/s时，进行DVL安装误差角估计的后续步骤计算。

4)对DVL速度矢量和惯导***与GNSS组合导航得到的体坐标下的速度进行归一化处理。计算方法如下：

式中，为惯导***体坐标系速度，/>为归一化后的DVL速度矢量，/>为归一化后的惯导***体坐标系速度矢量。

5)取α_i为权重系数，且共n个系数，对应n组速度矢量测量值

6)通过以下公式，计算各中间变量。

S＝B+B^T (9)

7)根据式(6)至(9)，计算如下损失函数构造矩阵

8)计算K的特征值λ_i和特征向量q_i，其中i＝1,2,3,4。

9)K的所有特征值中最大的一个λ_max。λ_max理论值为1，因此可使用1作为初值，进行迭代计算求取λ_max。将λ_max,0＝1带入式(11)进行迭代，得：

λ_max,i+1＝λ_max,i-p(λ_i)/p′(λ_i),i＝0,1,... (11)

其中

10)为保证迭代收敛性，令：

[(λ+σ)I-S]^-1＝γ^-1(αI+βS+S²) (13)

可得α＝λ²-σ²+κ，β＝λ-σ，γ＝α(λ+σ)-Δ。

将α、β和γ代入式(12)可得：

p(λ)＝λ⁴-(a+b)λ²-cλ+(ab+cσ-d)＝0 (14)

其中a＝σ²-κ，

由此可得：

由此可得两速度矢量间安装误差最优解为：

其中：

11)安装误差角余弦矩阵A与四元数字的转换关系为：

其中：

为实现姿态对最优估计，定义如下损失函数：

则有：

g(A)＝1-L(A)＝tr[AB^T] (20)

为实现损失函数最小，只需满足g(A)最大即可。

将(17)带入(20)可得：

四元数的元素具有唯一约束

为求取式(21)在式(22)约束条件下对最大值，重新构建方程：

令可得

由上式分析可知，λ是K的一个特征根，则是对应的特征向量，因此(16)的结果是一种最优估计。

12)得到最优估计的四元数后，将该四元数转化为安装误差矩阵，再根据安装误差矩阵即可计算得到DVL安装误差角。

采用某次海试试验数据以此方法进行SINS/DVL标定，标定过程中安装误差角收敛过程如图1所示：

由于直线航行过程中只有前向速度，横滚角可观测度较低，需要进行转弯机动产生侧向速度，从而使估计结果收敛。图中3600s处存在转弯机动，该机动完成后，安装误差角估计值迅速收敛，得到稳定的估计结果。

在此数据基础上，将DVL原始速度加入45°航向安装误差角(DVL换能器斜45°装)，再次进行标定，两次结果比较如图2所示：

两次安装误差角估计结果统计如下：

从估计结果可以看出，此标定方法可以对DVL安装误差角进行有效标定。

上述具体实施方式仅限于解释和说明本发明的技术方案，但并不能构成对权利要求保护范围的限定。本领域技术人员应当清楚，在本发明的技术方案的基础上做任何简单的变形或替换而得到的新的技术方案，均落入本发明的保护范围内。

Claims (3)

1.一种基于QUEST的INS/DVL安装误差标定方法，其特征在于，包括如下步骤：

步骤一，INS体坐标系下的INS、DVL速度矢量预处理；

步骤二，基于QUEST的最优四元数求解；

步骤三，INS与DVL之间安装误差角计算；

所述步骤一中INS体坐标系下的INS、DVL速度矢量预处理包括：

1)根据INS的角速度和DVL相对于INS的杆臂，对DVL速度矢量进行杆臂误差的补偿，补偿方式如下：

式中：为DVL输出的原始速度矢量，/>为INS的角速度，/>为DVL相对于INS的杆臂；

2)计算INS和DVL的合速度，计算方法如下：

式中，为INS的体坐标系速度矢量，V_ins为INS的合速度，V_dvl为DVL的合速度；

3)对INS和DVL的速度进行判断，当且仅当INS合速度大于1m/s，DVL合速度大于1m/s，INS航向角速度小于1°/s，INS合速度和DVL合速度之间差值小于1m/s时，进行DVL安装误差角估计的后续步骤计算；

4)对DVL速度矢量和INS与GNSS组合导航得到的体坐标下的速度进行归一化处理，计算方法如下：

式中，为INS体坐标系速度，/>为归一化后的DVL速度矢量，/>为归一化后的INS体坐标系速度矢量。

2.根据权利要求1所述的一种基于QUEST的INS/DVL安装误差标定方法，其特征在于，

所述步骤二中基于QUEST的最优四元数求解包括：

1)通过以下公式，计算各中间变量

σ＝trB (8)

S＝B+B^T (9)

其中，n表示n组速度矢量测量值；

2)根据式(6)至(9)，计算如下损失函数构造矩阵

3)λ_max为矩阵K的最大特征值，使用1作为初值，进行迭代计算求取λ_max；将λ_max,0＝1带入式(11)进行迭代，得：

λ_max,i+1＝λ_max,i-p(λ_i)/p′(λ_i),i＝0,1,... (11)

p(λ_i)计算公式如下：

p(λ)＝λ⁴-(a+b)λ²-cλ+(ab+cσ-d) (12)

其中，a＝σ²-κ，κ为S伴随矩阵的迹；

4)根据计算出来的特征值λ_max，计算对应的K的特征向量即为对应的最优四元数。

3.根据权利要求1所述的一种基于QUEST的INS/DVL安装误差标定方法，其特征在于，

所述步骤三中INS与DVL之间安装误差角计算包括：

1)得到最优估计的四元数后，将该四元数转化为安装误差矩阵

其中，q_i为四元数中的元素；

2)再根据安装误差矩阵即可计算得到DVL安装误差角

其中，C_ij为中第i行，第j列元素。