CN103389115A - Sins/dvl组合导航系统一体化误差标定方法 - Google Patents

Abstract

本发明涉及一种SINS/DVL组合导航系统一体化误差标定方法，包括：实时获取载体相对DVL仪器坐标系的第一航行距离分量和GPS位置信息相对载体系的第二航行距离分量；将所述第一航行距离分量和第二航行距离分量进行基于SVD的最小二乘拟合，从而得到SINS/DVL一体化转换矩阵；判断步骤2估计到的所述转换矩阵是否满足精度要求，如果不满足则重复步骤1至2，直至拟合精度达到导航的要求。本发明提出的基于奇异值分解（SVD）的最小二乘安装偏差角在线估计方法，通过采用GPS测量航迹与DVL推位航迹拟合的方法对DVL/IMU安装偏差角进行估计，不仅解决了常规最小二乘稳定性差的问题，同时对GPS的量测精度要求不高，方案实现简单，工程实用性强。

Description

SINS/DVL组合导航系统一体化误差标定方法

技术领域

本发明涉及导航领域，特别是涉及一种SINS/DVL组合导航系统一体化误差标定方法。

背景技术

惯性导航系统（INS）的定位误差具有随时间累积的特性，对于舰船等需要长时间导航的运载体来说，往往辅以其它导航设备进行组合导航，以提高导航系统的定位精度。多普勒计程仪（DVL）以其测速精度高，误差不随时间累积的特点，成为INS一种重要的辅助导航设备，在船用导航领域得到了广泛的应用，尤其对于无线电信号无法有效传播的水下环境，INS/DVL组合导航系统发挥着举足轻重的作用。由多普勒计程仪测速原理可知，计程仪输出的速度是沿DVL仪器坐标系上的，使用DVL测量速度进行组合导航时，必须要经过坐标转换，将其由仪器坐标系转换到导航坐标系上，这一过程通常是用INS航姿信息或者罗经的航向信息进行坐标系的转换。由于该过程是在假定DVL仪器坐标系与IMU（惯性测量单元）姿态测量系之间不存在安装偏差角的前提下进行的，因此，如果二者之间存在安装偏差角（安装过程中不可避免），尤其是艏向偏差角时，便会产生速度转换误差，进而引起载体的航迹向与航向偏差，最终直接影响系统的导航精度。考虑到这部分误差的重要性，在利用DVL速度信息进行辅助导航之前，必须要对DVL与IMU之间的安装偏差角进行校准。

目前，常用通过高精度GPS提供的载体速度信息与DVL输出的速度信息进行最小二乘拟合或者滤波估计的方法对安装偏差角进行估计，方案对GPS的测速精度要求较高，实现条件较为苛刻，同时受量测噪声的影响导致方案稳定性较差。

发明内容

本发明的目的是提供一种实现简单、稳定性好、精度高的SINS/DVL组合导航系统一体化误差标定方法。

为解决上述技术问题，作为本发明的一个方面，提供了一种SINS/DVL组合导航系统一体化误差标定方法，其特征在于，包括：步骤1，实时获取载体相对DVL仪器坐标系的第一航行距离分量和GPS位置信息相对载体系的第二航行距离分量；步骤2，将所述第一航行距离分量和第二航行距离分量进行基于SVD的最小二乘拟合，从而得到SINS/DVL一体化转换矩阵；步骤3，判断步骤2估计到的所述转换矩阵是否满足精度要求，如果不满足则重复步骤1至2，直至拟合精度达到导航的要求。

进一步地，所述步骤1具体包括：使所述载体保持固定航向航行，同时采集DVL的速度输出信息、GPS的位置输出信息、SINS输出的航向信息和SINS输出的姿态信息；利用所述速度输出信息进行航位推算从而得到所述第一航行距离分量；根据所述位置输出信息实时解算出所述载体东北向航行距离分量，并利用捷联矩阵将其转化为相对载体系的所述第二航行距离分量。

进一步地，所述航位推算根据下式公式（1）推算：

$r_d^i\left(t\right)=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\·}{r}}_d^i\left(t\right)\·\mathrm{\Δt}=\left[\begin{array}{c}{x}_d^i\left(t\right)\\ y_d^i\left(t\right)\end{array}\right]$ 公式(1)

其中，i表示惯性系，d表示DVL，N表示第N时刻，t表示时间；

是所述速度输出信息，

是所述第一航行距离分量，

分别为t时刻载体航行距离在DVL仪器坐标系x、y轴上的距离分量。

进一步地，所述第二航行距离分量通过下式公式（2）获得：

$r_g^b\left(t\right)=C_n^b\left(t\right)\·\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}{x}_g^b\left(t\right)\\ y_g^b\left(t\right)\end{array}\right]$ 公式(2)

其中，b表示载体系，n表示导航系，g表示GPS；

为所述第二航行距离信息；

为SINS随时间变化捷联矩阵；

x_g、y_g为载体相对初始时刻的东北向距离误差；

为GPS测得的载体航行距离在载体系x、y轴分量。

进一步地，在所述公式（2）中：

$\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\δ\λ}\left(t\right)}{R_1}\·\sec L\left(t\right)\\ \frac{\mathrm{\δL}\left(t\right)}{R_2}\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ\λ}\left(t\right)\\ \mathrm{\δL}\left(t\right)\end{array}\right]=p_g^n\left(t\right)-p_g^n\left(0\right)$

$C_n^b\left(t\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]$

其中，t表示时间；

δλ、δL为载体相对初始时刻的经纬度误差；

R₁、R₂分别为地球子午面内的曲率半径和卯酉圈平面内的曲率半径；

ψ为载体航向角；

为GPS获得的载体初始时刻的位置坐标；

为GPS的位置输出信息。

进一步地，所述步骤2中的最小二乘拟合具体包括以下步骤：

（1）根据t=1…N时刻求得的

计算二者相应的均值

${\stackrel{\‾}{r}}_d^i=\frac{1}{N}\underset{t=1}{\overset{n}{\mathrm{\Σ}}}{r}_d^i\left(t\right)$

${\stackrel{\‾}{r}}_g^b=\frac{1}{N}\underset{t=1}{\overset{\mathrm{tN}}{\mathrm{\Σ}}}{r}_g^b\left(t\right)$

（2）计算

相对于各自均值的偏差

和

$q_d^i\left(t\right)=r_d^i\left(t\right)-{\stackrel{\‾}{r}}_d^i$

$q_g^b\left(t\right)=r_g^b\left(t\right)-{\stackrel{\‾}{r}}_g^b$

（3）计算3×3矩阵H

$H=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{q}_d^i\left(t\right)q_g^b{\left(t\right)}^T$

其中，T表示矩阵转置。

（4）对H矩阵进行奇异值分解

H=UΛV^T

（5）计算矩阵X

X=VU^T

（6）计算X的绝对值det(x)

如果det(x)=1，则

如果det(x)=-1，则需重新进行拟合估计

其中，X为所述一体化转换矩阵。

本发明提出的基于奇异值分解（SVD）的最小二乘安装偏差角在线估计方法，通过采用GPS测量航迹与DVL推位航迹拟合的方法对DVL/IMU安装偏差角进行估计，不仅解决了常规最小二乘稳定性差的问题，同时对GPS的量测精度要求不高，方案实现简单，工程实用性强。

附图说明

图1示意性示出了本发明的流程图；

图2示意性示出了基于SVD分解最小二乘拟合方法流程图；以及

图3示意性示出了仿真效果对比图。

具体实施方式

以下对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。

本发明涉及的坐标系有：i——多普勒计程仪（DVL）仪器坐标系；b——机体坐标系；n——导航坐标系。

设多普勒计程仪测量速度

${\stackrel{\·}{r}}_d^i\left(t\right)=\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]$ 式(1)

其中，

表示DVL在t时刻测得的速度，上标i表示测量所在坐标系为i系，下标d表示测量信息是通过DVL获得，下文表示亦如此；v_x、v_y分别表示测量速度在DVL仪器坐标系x、y轴上分量，这里根据实际应用情况不考虑计程仪垂向速度。

多普勒计程仪测量速度经过一系列坐标变换转换到地理坐标系上，表示如下

${\stackrel{.}{r}}_d^n\left(t\right)=C_b^n\left(t\right)\·C_i^b\·{\stackrel{.}{r}}_d^i\left(t\right)$ 式（2）

其中，

为DVL测量速度在地理系上分量，为SINS随时间变化捷联矩阵（这里假设IMU惯性测量系与载体系完全重合）；

为由多普勒计程仪测速坐标系到IMU姿态坐标系的坐标转换矩阵，在这里即为所要求的安装偏差角构成的常值方向余弦矩阵，表示如下：

$C_i^b=\left[\begin{array}{cc}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]$

其中θ为DVL仪器坐标系与IMU姿态测量系安装偏差角。

对上式进行积分即可得到相应的位置信息

${\hat{r}}_d^n\left(t\right)={\hat{r}}_d^n\left(t\right)+{\∫}_{t_0}^t{C}_b^n\left(\mathrm{\τ}\right){\·C}_i^b\·{\stackrel{.}{r}}_d^i\left(\mathrm{\τ}\right)\·\mathrm{d\τ}$ 式（3）

其中，

为在时刻t估计得到的位置距离，

为初始时刻给定的初始位置距离信息。

根据性质 ${\left(\mathrm{ABC}\right)}^S=(C^T\⊗A)B^S$ 式（4）

其中，

表示克罗内克积，()^S表示矩阵的向量形式

上式（4）可以表示为

${\hat{r}}_d^n\left(t\right)={\∫}_{t_0}^t[{\stackrel{\·}{r}}_d^{\mathrm{iT}}\left(\mathrm{\τ}\right)\⊗C_b^n\left(\mathrm{\τ}\right)]\·\mathrm{d\τ}{\left(C_i^b\right)}^S$ 式（5）

通过上式（5）可以看出，用常规最小二乘法即可求解出

考虑到常规最小二乘涉及矩阵正交性问题，这在实际中很难保证。

为了避免该问题的出现，本发明采用基于SVD的最小二乘方法对偏差矩阵进行估计，其原理图如下：

式（2）可以表示为

$C_n^b\left(t\right)\·{\stackrel{\·}{r}}_d^n\left(t\right)=C_i^b\·{\stackrel{\·}{r}}_d^i\left(t\right)$ 式(6)

理论上我们可以直接通过上式（6）对其进行奇异值分解，求解方向矩阵，但是鉴于我们需要通过地理系速度

进行拟合，如果GPS测量误差比较大则会导致估计效果不准确，因此，对式（6）做进一步的变换，两边进行积分后得到：

$\∫C_n^b\left(\mathrm{\τ}\right)\·{\stackrel{\·}{r}}_d^n\left(\mathrm{\τ}\right)\mathrm{dt}=C_i^b\∫{\stackrel{\·}{r}}_d^i\left(\mathrm{\τ}\right)\mathrm{d\τ}$ 式(7)

进一步展开得到

$C_n^b\left(\mathrm{\τ}\right)\·r_d^n\left(\mathrm{\τ}\right)-\∫{\stackrel{\·}{C}}_n^b\left(\mathrm{\τ}\right)\·r_d^n\left(\mathrm{\τ}\right)\mathrm{dt}=C_i^b\∫{\stackrel{\·}{r}}_d^i\left(\mathrm{\τ}\right)\mathrm{d\τ}$ 式(8)

从变换后的式子可以看出，需要求取变换矩阵

只与直接可测量的信号有关，即多普勒测量的沿仪器坐标系上的速度

陀螺仪输出的变换矩阵

角速度阵

（实际拟合过程中我们用GPS获得的真实航迹

代替）。通过上式可以在任意时间段内求解出多普勒计程仪与IMU之间的坐标变换矩阵。

我们看到，在式（8）中有

项，在进行误差角拟合过程中，我们需要知道该项的值。为了保证拟合的精度，相应的对其进行积分的时间间隔要保证尽量小，最好是在每个采样周期内对其进行一次积分运算。由于我们应用GPS位置信息

作为

的参考量，如果GPS的定位信息误差较大，则不可避免的会带来运算的误差。由于该项是由载体转弯引起的，因此在实际进行安装误差角拟合过程中，如果保证载体做直航运动，则可以忽略该项的影响，进而提高安装偏差角的拟合精度。

下面对本发明的具体实施过程进行详细说明。

请参考图1，本发明中的SINS/DVL（其中，SINS为捷联惯性导航系统）组合导航系统一体化误差标定方法，包括：

步骤1，实时获取载体相对DVL（多普勒计程仪）仪器坐标系的第一航行距离分量和GPS位置信息相对载体系的第二航行距离分量；优选地，为保证测量准确性，惯导系统首先进行初始对准，并通过GPS获得载体初始时刻的位置坐标

。

步骤2，将所述第一航行距离分量和第二航行距离分量进行基于SVD（基于奇异值分解）的最小二乘拟合，从而得到SINS/DVL一体化转换矩阵；

步骤3，判断步骤2估计到的所述转换矩阵是否满足精度要求，如果不满足则重复步骤1至2，直至拟合精度达到导航的要求。

本发明提出的基于奇异值分解（SVD）的最小二乘安装偏差角在线估计方法，通过采用GPS测量航迹与DVL推位航迹拟合的方法对DVL/IMU安装偏差角进行估计，不仅解决了常规最小二乘稳定性差的问题，同时对GPS的量测精度要求不高，方案实现简单，工程实用性强。

优选地，所述步骤1具体包括：使所述载体保持固定航向航行，同时采集DVL的速度输出信息、GPS的位置输出信息、SINS输出的航向信息和SINS输出的姿态信息；利用所述速度输出信息进行航位推算从而得到所述第一航行距离分量；根据所述位置输出信息实时解算出所述载体东北向航行距离分量，并利用捷联矩阵将其转化为相对载体系的所述第二航行距离分量。

优选地，所述航位推算根据下式公式（1）推算：

$r_d^i\left(t\right)=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\·}{r}}_d^i\left(t\right)\·\mathrm{\Δt}=\left[\begin{array}{c}{x}_d^i\left(t\right)\\ y_d^i\left(t\right)\end{array}\right]$ 公式（1）

其中，i表示惯性系，d表示DVL，N表示第N时刻，t表示时间；

是所述速度输出信息，

是所述第一航行距离分量，

分别为t时刻载体航行距离在DVL仪器坐标系x、y轴上的距离分量。

优选地，所述第二航行距离分量可通过下式公式（2）获得：

$r_g^b\left(t\right)=C_n^b\left(t\right)\·\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}{x}_g^b\left(t\right)\\ y_g^b\left(t\right)\end{array}\right]$ 公式(2)

其中，b表示载体系，n表示导航系，g表示GPS；

为所述第二航行距离信息；

为SINS随时间变化捷联矩阵；

x_g、y_g为载体相对初始时刻的东北向距离误差；

为GPS测得的载体航行距离在载体系x、y轴分量。

优选地，在所述公式（2）中：

$\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\δ\λ}\left(t\right)}{R_1}\·\sec L\left(t\right)\\ \frac{\mathrm{\δL}\left(t\right)}{R_2}\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ\λ}\left(t\right)\\ \mathrm{\δL}\left(t\right)\end{array}\right]=p_g^n\left(t\right)-p_g^n\left(0\right)$

$C_n^b\left(t\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]$

其中，t表示时间；

δλ、δL为载体相对初始时刻的经纬度误差；

R₁、R₂分别为地球子午面内的曲率半径和卯酉圈平面内的曲率半径；

ψ为载体航向角；

为GPS获得的载体初始时刻的位置坐标；

为GPS的位置输出信息。

优选地，所述步骤2中的最小二乘拟合具体包括以下步骤：

（1）根据t=1…N时刻求得的

计算二者相应的均值

${\stackrel{\‾}{r}}_d^i=\frac{1}{N}\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{r}_d^i\left(t\right)$

${\stackrel{\‾}{r}}_g^b=\frac{1}{N}\underset{t=1}{\overset{\mathrm{tN}}{\mathrm{\Σ}}}{r}_g^b\left(t\right)$

（2）计算

相对于各自均值的偏差

和

$q_d^i\left(t\right)=r_d^i\left(t\right)-{\stackrel{\‾}{r}}_d^i$

$q_g^b\left(t\right)=r_g^b\left(t\right)-{\stackrel{\‾}{r}}_g^b$

（3）计算3×3矩阵H

$H=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{q}_d^i\left(t\right)q_g^b{\left(t\right)}^T$

其中，T表示矩阵转置。

（4）对H矩阵进行奇异值分解

H=UΛV^T

（5）计算矩阵X

X=VU^T

（6）计算X的绝对值det(x)

如果det(x)=1，则

如果det(x)=-1，则需重新进行拟合估计

其中，X为所述一体化转换矩阵。

请参考图3，为了说明本发明的有益效果，在以下条件下对该方法进行了Matlab仿真实验：仿真条件：航速：5m/s；航向：20度；航向偏差：±0.5度；GPS：定位误差20米；DVL/IMU安装偏差角：35度；多普勒计程仪测速精度：0.4%；按固定时间间隔进行安装偏差角拟合估计，验证SVD最小二乘估计方法估计效果。

本发明具有以下优点：

（1）本发明所采用的基于SVD的最小二乘算法避免了传统最小二乘算法要求矩阵正定性的限制，解决了在实际工程应用过程中由于测量数据包含随机误差引起矩阵的病态特性问题，提高了算法的稳定性；

（2）本发明采用的基于DVL计算航迹与GPS真实航迹进行拟合的方案，克服了传统速度拟合方法对GPS测量精度要求较高的缺点，很好的解决了由于GPS量测噪声造成的速度拟合精度不高的问题，提高了拟合精度；

（3）基于优点（2），由于对GPS精度要求不是很高，实现简单、方便，工程应用性强。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种SINS/DVL组合导航系统一体化误差标定方法，其特征在于，包括：

步骤1，实时获取载体相对DVL仪器坐标系的第一航行距离分量和GPS位置信息相对载体系的第二航行距离分量；

步骤2，将所述第一航行距离分量和第二航行距离分量进行基于SVD的最小二乘拟合，从而得到SINS/DVL一体化转换矩阵；

步骤3，判断步骤2估计到的所述转换矩阵是否满足精度要求，如果不满足则重复步骤1至2，直至拟合精度达到导航的要求。

2.根据权利要求1所述的误差标定方法，其特征在于，所述步骤1具体包括：

使所述载体保持固定航向航行，同时采集DVL的速度输出信息、GPS的位置输出信息、SINS输出的航向信息和SINS输出的姿态信息；

利用所述速度输出信息进行航位推算从而得到所述第一航行距离分量；

根据所述位置输出信息实时解算出所述载体东北向航行距离分量，并利用捷联矩阵将其转化为相对载体系的所述第二航行距离分量。

3.根据权利要求2所述的误差标定方法，其特征在于，所述航位推算根据下

式公式（1）推算：

$r_d^i\left(t\right)=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\·}{r}}_d^i\left(t\right)\·\mathrm{\Δt}=\left[\begin{array}{c}{x}_d^i\left(t\right)\\ y_d^i\left(t\right)\end{array}\right]$ 公式(1)

其中，i表示惯性系，d表示DVL，N表示第N时刻，t表示时间；

是所述速度输出信息，

是所述第一航行距离分量，

分别为t时刻载体航行距离在DVL仪器坐标系x、y轴上的距离分量。

4.根据权利要求2所述的误差标定方法，其特征在于，所述第二航行距离分

量通过下式公式（2）获得：

$r_g^b\left(t\right)=C_n^b\left(t\right)\·\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}{x}_g^b\left(t\right)\\ y_g^b\left(t\right)\end{array}\right]$ 公式(2)

其中，b表示载体系，n表示导航系，g表示GPS；

为所述第二航行距离信息；

为SINS随时间变化捷联矩阵；

x_g、y_g为载体相对初始时刻的东北向距离误差；

为GPS测得的载体航行距离在载体系x、y轴分量。

5.根据权利要求4所述的误差标定方法，其特征在于，在所述公式（2）中：

$\left[\begin{array}{c}{x}_g\left(t\right)\\ y_g\left(t\right)\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\δ\λ}\left(t\right)}{R_1}\·\sec L\left(t\right)\\ \frac{\mathrm{\δL}\left(t\right)}{R_2}\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ\λ}\left(t\right)\\ \mathrm{\δL}\left(t\right)\end{array}\right]=p_g^n\left(t\right)-p_g^n\left(0\right)$

$C_n^b\left(t\right)=\left[\begin{array}{cc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]$

其中，t表示时间；

δλ、δL为载体相对初始时刻的经纬度误差；

R₁、R₂分别为地球子午面内的曲率半径和卯酉圈平面内的曲率半径；

ψ为载体航向角；

为GPS获得的载体初始时刻的位置坐标；

为GPS的位置输出信息。

6.根据权利要求3所述的误差标定方法，其特征在于，所述步骤2中的最小二乘拟合具体包括以下步骤：

（1）根据t=1…N时刻求得的

计算二者相应的均值

${\stackrel{\‾}{r}}_d^i=\frac{1}{N}\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{r}_d^i\left(t\right)$

${\stackrel{\‾}{r}}_g^b=\frac{1}{N}\underset{t=1}{\overset{\mathrm{tN}}{\mathrm{\Σ}}}{r}_g^b\left(t\right)$

（2）计算

相对于各自均值的偏差

和

$q_d^i\left(t\right)=r_d^i\left(t\right){\stackrel{\‾}{r}}_d^i$

$q_g^b\left(t\right)=r_g^b\left(t\right)-{\stackrel{\‾}{r}}_g^b$

（3）计算3×3矩阵H

$H=\underset{t=1}{\overset{N}{\mathrm{\Σ}}}{q}_d^i\left(t\right)q_g^b{\left(t\right)}^T$

其中，T表示矩阵转置。

（4）对H矩阵进行奇异值分解

H=UΛV^T

（5）计算矩阵X

X=VU^T

（6）计算X的绝对值det(x)

如果det(x)=1，则

如果det(x)=-1，则需重新进行拟合估计

其中，X为所述一体化转换矩阵。

Images