CN105091907A - Sins/dvl组合中dvl方位安装误差估计方法 - Google Patents

Abstract

本发明公开了一种SINS/DVL组合中DVL方位安装误差估计方法，通过选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中IMU的方位安装误差作为状态向量，构建***状态方程；选择SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建***量测方程；由Kalman滤波器作为SINS/DVL信息融合滤波器；选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。该方法不需要外部GPS参考辅助导航信息，不需要将DVL速度转换至导航坐标系，并且能够根据可观测性分析结果，选择改善方位安装误差估计性能的可行方案。

Description

SINS/DVL组合中DVL方位安装误差估计方法

技术领域

本发明涉及导航领域，具体涉及一种SINS/DVL组合中DVL方位安装误差估计方法。

背景技术

对于基于积分工作方式的惯性导航***(StrapdownInertialNavigation,SINS)而言，其定位误差随着时间累积，难以满足长时间的高精度定位要求，多普勒测速仪(DopplerVelocityLog，DVL)由于能够提供高精度连续速度信息，是常用的辅助导航设备。将DVL与SINS组合，从而抑制定位误差的发散。DVL的安装偏差角是影响SINS/DVL组合定位精度的主要因素，为了提高SINS/DVL组合导航***的定位精度，需要准确标定捷联惯性器件与多普勒测速仪之间的安装偏差。目前，对方位安装误差标定通常利用外部GPS基准信息，通过比较GPS的轨迹和推算轨迹得到方位安装误差的标定值。然而，这种方法破坏掉了水下航行器的隐蔽性。

发明内容

发明目的：为了克服上述问题，本发明在航行器处于水下时，利用DVL的速度辅助信息，完成DVL方位安装误差的估计并校正。在保证SINS/DVL组合定位精度的同时提高了隐蔽性。

技术方案：一种SINS/DVL组合中DVL方位安装误差估计方法，包括以下步骤：

(1)选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中IMU的方位安装误差作为状态向量，构建***状态方程；选择SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建***量测方程；

(2)由Kalman滤波器作为SINS/DVL信息融合滤波器；

(3)选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。

进一步地，所述步骤(1)具体包括如下步骤：

1)选取SINS的误差状态为：速度误差δV＝[δV_EδV_N]^T，姿态误差φ＝[φ_Eφ_Nφ_U]^T，位置误差δP＝[δLδλ]^T，加速度计零偏 $v={\left[\begin{array}{cc}{\▿}_x& {\▿}_y\end{array}\right]}^T,$ 陀螺仪零偏ε＝[ε_xε_yε_z]^T，共计12维；并将DVL刻度系数误差δΚ＝[δΚ_xδΚ_y]^T和DVL与IMU的z轴方向的方位安装误差扩充为状态向量；则SINS/DVL组合导航***模型的状态向量为15维：

其中，δV_E、δV_N分别为SINS的东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别为纬度、经度误差；分别为x、y轴方向的加速度计零偏；ε_x、ε_y、ε_z分别为x、y、z轴方向的陀螺仪零偏；δΚ_x、δΚ_y分别为x、y轴方向的DVL刻度系数误差；

将DVL刻度系数误差、DVL与IMU的方位安装误差认为是常值，即：

其中，是X_DVL的导数，是δK_x的导数，是δK_y的导数，是的导数；

根据SINS/DVL误差模型，可得SINS/DVL的线性状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)=\left[\begin{array}{cc}{F}_{SINS}& 0_{12\×3}\\ 0_{3\×12}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{W}_{SINS}\\ 0_{3\×1}\end{array}\right]---\left(3\right)$

上式写成矩阵形式为：

式中，F(t)为***的状态转移矩阵；W_SINS为状态向量的过程噪声矩阵；w_V为SINS过程噪声矩阵中速度误差矩阵；w_φ为SINS过程噪声矩阵中失准角矩阵；w_P为SINS过程噪声矩阵中位置误差矩阵；为δV的导数；为φ的导数；为δP的导数；为的导数；为ε的导数；为δK的导数；为的导数；

$\begin{array}{cc}{F}_{11}=\left[\begin{array}{cc}\frac{V_N}{R}\tan L& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\\ -2\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0\end{array}\right],& F_{12}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{13}=\left[\begin{array}{cc}\left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_N& 0\\ \left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_E& 0\end{array}\right],& F_{14}=\left[\begin{array}{cc}{C}_b^n\left(1,1\right)& C_b^n\left(1,2\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{21}=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]& F_{24}=-\left[\begin{array}{ccc}{C}_b^n\left(1,1\right)& C_b^n\left(1,3\right)& C_b^n\left(1,3\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)& C_b^n\left(2,3\right)\\ C_b^n\left(3,1\right)& C_b^n\left(3,2\right)& C_b^n\left(3,3\right)\end{array}\right]\end{array}$

$F_{22}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& -\left({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}\right)\\ -\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0& -\frac{V_N}{R}\\ {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0\end{array}\right]$

$\begin{array}{cc}{F}_{31}=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{1}{R}\sec L& 0\end{array}\right]& F_{33}=\left[\begin{array}{cc}0& 0\\ 0& \frac{V_E}{R}\tan L\sec L\end{array}\right]\end{array}$

其中，ω_ie、R与L分别为地球自转角速度、地球半径以及载体所在地理纬度；f_E、f_N与f_U为加速度测量值在导航系n中沿东向、北向与天向轴的投影；为载体系b向导航系n的转换矩阵；V_E与V_N分别为SINS的东向与北向速度；为矩阵中第M行第N列的元素；

2)以SINS载体系速度与DVL安装坐标系速度差值作为量测向量，并构建***量测方程；

考虑DVL刻度系数误差和随机测量误差，DVL测量值为：

$\begin{array}{cc}{\tilde{V}}_{DVL}^{mi}=(1+{\mathrm{\δK}}_i)V_{DVL}^{mi}+{\mathrm{\δV}}_{di},& i=x,y---\left(5\right)\end{array}$

式中，上标m表示DVL安装坐标系；表示DVL在m系中的真实值，δV_di为测量噪声；i＝x,y为DVL的水平安装方向；为DVL的测量值；

在载体系b下，SINS的速度计算值为：

${\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T{C}_{n^{\′}}^n(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n)---\left(6\right)$

式中，为SINS速度真实值；为SINS解算的速度误差；为SINS在导航系n中解算的速度在b系的投影值；为n'系到n系的转换矩阵，且有：

$C_{n^{\′}}^n=I+\mathrm{\φ}\×---\left(7\right)$

式中，n'为计算导航坐标系；

将式(7)代入式(6)得到：

$\begin{array}{c}{\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)+{\left(C_b^n\right)}^T\left(\mathrm{\φ}\×\right)\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)\\ \≈V_{SINS}^b+{\left(C_b^n\right)}^T{\mathrm{\δV}}_{SINS}^n-{\left(C_b^n\right)}^T\left({\mathrm{\δV}}_{SINS}^n\×\right)\mathrm{\φ}\end{array}---\left(8\right)$

式中，为SINS在导航系n中的速度真实值在载体系b中的投影；

由于DVL与SINS间存在安装误差角SINS载体系速度与DVL安装坐标系速度之间存在如下关系：

式中，为DVL安装坐标系m向载体系b的转换矩阵。为包含方位安装误差在内的安装误差角向量，忽略水平安装误差，有：

从而有：

根据式(8～10)，有：

式中，δV_d为DVL测量噪声，δV_d即δV_di,i＝x,y；即 $V_{DVL}^m\×\mathrm{\δ}K={\left[\begin{array}{cc}{\mathrm{\δK}}_x{V}_{DVL}^{mx}& {\mathrm{\δK}}_y{V}_{DVL}^{my}\end{array}\right]}^T;$

式(11)即为SINS/DVL的量测方程，写成矩阵形式：

Z＝HX+V(12)

式中 $Z=\left[\begin{array}{cccc}{\tilde{V}}_{SINS}^{bx}& -{\tilde{V}}_{DVL}^{mx}& {\tilde{V}}_{SINS}^{by}& -{\tilde{V}}_{DVL}^{my}\end{array}\right],$ V为量测噪声(一般用零均值白噪声序列描述)；H为量测矩阵：

$H=\left[\begin{array}{cccccccc}{C}_n^b(1,1)& C_n^b(1,2)& C_n^b(1,3)V_N& -C_n^b(1,3)V_E& -C_n^b(1,1)V_N+C_n^b(1,2)V_E& 0_{1\×8}& V_{SINS}^{by}& -V_{DVL}^{mx}\\ C_n^b(2,1)& C_n^b(2,2)& C_n^b(2,3)V_N& -C_n^b(2,3)V_E& -C_n^b(2,1)V_N+C_n^b(2,2)V_E& 0_{1\×8}& -V_{SINS}^{bx}& -V_{DVL}^{my}\end{array}\right]---\left(13\right)$

式中，与为SINS速度在x与y轴的投影。

进一步地，所述的步骤2)具体包括：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k-1}\right)\\ K_k=P_{k,k-1}{H}_k^T{\left(H_k{P}_{k,k-1}{H}_k^T+R_k\right)}^{-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ P_k=\left(I-K_k{H}_k\right)P_{k,k-1}\end{array}\right.---\left(14\right)$

式中，K_k为增益矩阵；P_k为状态向量的协方差矩阵；R_k为量测噪声序列V_k的协方差矩阵；Q_k为过程噪声序列W_k的协方差矩阵；为状态向量估计值；为状态向量一步预测值；P_k,k-1为状态向量的协方差矩阵一步预测值；Φ_k,k-1为状态转移矩阵，即式(3)中F(t)阵的离散形式；Γ_k-1为过程噪声转移矩阵。

进一步地，所述的步骤3)具体包括：

求取线性时变***的总可观测矩阵的表达式如下：

${\tilde{Q}}_s^T=\left[\begin{array}{ccccc}{\tilde{Q}}_1& {\tilde{Q}}_2& ...& {\tilde{Q}}_j& ...\end{array}\right]---\left(15\right)$

式中，为连续***在第j时间段的可观测矩阵，其形式为：

${\tilde{Q}}_j^T={\left[\begin{array}{ccccc}{\left(H_j\right)}^T& {\left(H_j{F}_j\right)}^T& {\left(H_j{F}_j^2\right)}^T& ...& \left(H_j{F}_j^{p-1}\right)\end{array}\right]}^T---\left(16\right)$

其中，p＝15；p-1为矩阵F_j的乘方次数；H_j为连续***在第j时间段的量测矩阵；F_j为第j时间段的状态转移矩阵；

利用列主元高斯消元法将式(16)转换为上三角矩阵，并再进一步化为对角矩阵得到状态的可观测性情况，并根据对角矩阵对角元素的绝对值得到SINS/DVL组合导航***在转弯运动下的可观测度。

有益效果：1)补偿过程中，不需要外部GPS参考辅助导航信息，保证了隐蔽性；2)在SINS与DVL组合过程中，不需要将DVL速度转换至导航坐标系；3)根据可观测性分析结果，可以选择改善DVL方位安装误差估计的可行方案。

附图说明

图1为本发明使用的DVL方位安装误差补偿方案示意图；

图2为本发明DVL方位安装误差估计图；

具体实施方式

本发明针对DVL辅助SINS构成的水下组合导航***，选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中IMU的方位安装误差作为状态向量，构建***状态方程；选择SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建***量测方程；由Kalman滤波器作为SINS/DVL信息融合滤波器；选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。

下面结合附图对本发明实施方法做更详细地描述：

一种SINS/DVL组合中DVL方位安装误差估计方法，选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中IMU的方位安装误差作为状态向量，构建***状态方程；选择SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建***量测方程。具体包括如下步骤：

1)选取SINS的误差状态为：速度误差δV＝[δV_EδV_N]^T，姿态误差φ＝[φ_Eφ_Nφ_U]^T，位置误差δP＝[δLδλ]^T，加速度计零偏 $\▿={\left[\begin{array}{cc}{\▿}_x& {\▿}_y\end{array}\right]}^T,$ 陀螺仪零偏ε＝[ε_xε_yε_z]^T，共计12维；并将DVL刻度系数误差δΚ＝[δΚ_xδΚ_y]^T和DVL与IMU的z轴方向的方位安装误差扩充为状态向量；则SINS/DVL组合导航***模型的状态向量为15维：

其中，δV_E、δV_N分别为SINS的东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别为纬度、经度误差；分别为x、y轴方向的加速度计零偏；ε_x、ε_y、ε_z分别为x、y、z轴方向的陀螺仪零偏；δΚ_x、δΚ_y分别为x、y轴方向的DVL刻度系数误差；

将DVL刻度系数误差、DVL与IMU的方位安装误差认为是常值，即：

其中，是X_DVL的导数，是δK_x的导数，是δK_y的导数，是的导数；

根据SINS/DVL误差模型，可得SINS/DVL的线性状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)=\left[\begin{array}{cc}{F}_{SINS}& 0_{12\×3}\\ 0_{3\×12}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{W}_{SINS}\\ 0_{3\×1}\end{array}\right]---\left(3\right)$

上式写成矩阵形式为：

式中，F(t)为***的状态转移矩阵；W_SINS为状态向量的过程噪声矩阵；w_V为SINS过程噪声矩阵中速度误差矩阵；w_φ为SINS过程噪声矩阵中失准角矩阵；w_P为SINS过程噪声矩阵中位置误差矩阵；

$\begin{array}{cc}{F}_{11}=\left[\begin{array}{cc}\frac{V_N}{R}\tan L& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\\ -2\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0\end{array}\right],& F_{12}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{13}=\left[\begin{array}{cc}\left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_N& 0\\ \left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_E& 0\end{array}\right],& F_{14}=\left[\begin{array}{cc}{C}_b^n\left(1,1\right)& C_b^n\left(1,2\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{21}=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]& F_{24}=-\left[\begin{array}{ccc}{C}_b^n\left(1,1\right)& C_b^n\left(1,3\right)& C_b^n\left(1,3\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)& C_b^n\left(2,3\right)\\ C_b^n\left(3,1\right)& C_b^n\left(3,2\right)& C_b^n\left(3,3\right)\end{array}\right]\end{array}$

$F_{22}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& -\left({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}\right)\\ -\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0& -\frac{V_N}{R}\\ {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0\end{array}\right]$

$\begin{array}{cc}{F}_{31}=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{1}{R}\sec L& 0\end{array}\right]& F_{33}=\left[\begin{array}{cc}0& 0\\ 0& \frac{V_E}{R}\tan L\sec L\end{array}\right]\end{array}$

其中，ω_ie、R与L分别为地球自转角速度、地球半径以及载体所在地理纬度；f_E、f_N与f_U为加速度测量值在导航系n中沿东向、北向与天向轴的投影；为载体系b向导航系n的转换矩阵；V_E与V_N分别为SINS的东向与北向速度；

2)以SINS载体系速度与DVL安装坐标系速度差值作为量测向量，并构建***量测方程；

考虑DVL刻度系数误差和随机测量误差，DVL测量值为：

$\begin{array}{cc}{\tilde{V}}_{DVL}^{mi}=(1+{\mathrm{\δK}}_i)V_{DVL}^{mi}+{\mathrm{\δV}}_{di},& i=x,y---\left(5\right)\end{array}$

式中，上标m表示DVL安装坐标系；表示DVL在m系中的真实值，δV_di为测量噪声；i＝x,y为DVL的水平安装方向；为DVL的测量值；

在载体系b下，SINS的速度计算值为：

${\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T{C}_{n^{\′}}^n(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n)---\left(6\right)$

式中，为SINS速度真实值；为SINS解算的速度误差；为SINS在导航系n中解算的速度在b系的投影值；为n'系到n系的转换矩阵，且有：

$C_{n^{\′}}^n=I+\mathrm{\φ}\×---\left(7\right)$

式中，n'为计算导航坐标系；

将式(7)代入式(6)得到：

$\begin{array}{c}{\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)+{\left(C_b^n\right)}^T\left(\mathrm{\φ}\×\right)\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)\\ \≈V_{SINS}^b+{\left(C_b^n\right)}^T{\mathrm{\δV}}_{SINS}^n-{\left(C_b^n\right)}^T\left({\mathrm{\δV}}_{SINS}^n\×\right)\mathrm{\φ}\end{array}---\left(8\right)$

式中，为SINS在导航系n中的速度真实值在载体系b中的投影；

由于DVL与SINS间存在安装误差角SINS载体系速度与DVL安装坐标系速度之间存在如下关系：

式中，为DVL安装坐标系m向载体系b的转换矩阵。为包含方位安装误差在内的安装误差角向量，忽略水平安装误差，有：

从而有：

根据式(8～10)，有：

式中，δV_d为DVL测量噪声，δV_d即δV_di,i＝x,y；即 $V_{DVL}^m\×\mathrm{\δ}K={\left[\begin{array}{cc}{\mathrm{\δK}}_x{V}_{DVL}^{mx}& {\mathrm{\δK}}_y{V}_{DVL}^{my}\end{array}\right]}^T;$

式(11)即为SINS/DVL的量测方程，写成矩阵形式：

Z＝HX+V(12)

式中 $Z=\left[\begin{array}{cccc}{\tilde{V}}_{SINS}^{bx}& -{\tilde{V}}_{DVL}^{mx}& {\tilde{V}}_{SINS}^{by}& -{\tilde{V}}_{DVL}^{my}\end{array}\right],$ V为量测噪声(一般用零均值白噪声序列描述)；H为量测矩阵：

$H=\left[\begin{array}{cccccccc}{C}_n^b(1,1)& C_n^b(1,2)& C_n^b(1,3)V_N& -C_n^b(1,3)V_E& -C_n^b(1,1)V_N+C_n^b(1,2)V_E& 0_{1\×8}& V_{SINS}^{by}& -V_{DVL}^{mx}\\ C_n^b(2,1)& C_n^b(2,2)& C_n^b(2,3)V_N& -C_n^b(2,3)V_E& -C_n^b(2,1)V_N+C_n^b(2,2)V_E& 0_{1\×8}& -V_{SINS}^{bx}& -V_{DVL}^{my}\end{array}\right]---\left(13\right)$

式中，与为SINS速度在载体系b中在x与y轴的投影。

一种SINS/DVL组合中DVL方位安装误差估计方法，由Kalman滤波器作为SINS/DVL信息融合滤波器；具体包括：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k-1}\right)\\ K_k=P_{k,k-1}{H}_k^T{\left(H_k{P}_{k,k-1}{H}_k^T+R_k\right)}^{-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ P_k=\left(I-K_k{H}_k\right)P_{k,k-1}\end{array}\right.---\left(14\right)$

式中，K_k为增益矩阵；P_k为状态向量的协方差矩阵；R_k为量测噪声序列V_k的协方差矩阵；Q_k为过程噪声序列W_k的协方差矩阵；为状态向量估计值；为状态向量一步预测值；P_k,k-1为状态向量的协方差矩阵一步预测值；Φ_k,k-1为状态转移矩阵，即式(3)中F(t)阵的离散形式；Γ_k-1为过程噪声转移矩阵。

一种SINS/DVL组合中DVL方位安装误差估计方法，选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。具体包括：

求取线性时变***的总可观测矩阵的表达式如下：

${\tilde{Q}}_s^T=\left[\begin{array}{ccccc}{\tilde{Q}}_1& {\tilde{Q}}_2& ...& {\tilde{Q}}_j& ...\end{array}\right]---\left(15\right)$

式中，为连续***在第j时间段的可观测矩阵，其形式为：

${\tilde{Q}}_j^T={\left[\begin{array}{ccccc}{\left(H_j\right)}^T& {\left(H_j{F}_j\right)}^T& {\left(H_j{F}_j^2\right)}^T& ...& \left(H_j{F}_j^{p-1}\right)\end{array}\right]}^T---\left(16\right)$

其中，p＝15；p-1为矩阵F_j的乘方次数；H_j为连续***在第j时间段的量测矩阵；F_j为第j时间段的状态转移矩阵；

利用列主元高斯消元法将式(16)转换为上三角矩阵，并再进一步化为对角矩阵得到状态的可观测性情况，并根据对角矩阵对角元素的绝对值得到SINS/DVL组合导航***在转弯运动下的可观测度。

利用Matlab模拟惯性仪表、DVL仪表数据。

分析载体三种机动对DVL方位安装误差可观测性的影响，三种机动模型为：

转弯运动：航向角为45°，水平姿态角均为0°，初始速度为5m/s，0s开始以角速度0.9°/s进行转弯运动，持续40s后继续匀速。选择如下两种机动方式作为转弯运动的比较：1)匀速直线运动：航向角为45°，水平姿态角均为0°，初始速度为5m/s；2)匀加速直线运动：航向角为45°，水平姿态角均为0°，初始速度为5m/s，0s后以加速度0.5m/s²进行加速运动，持续40s后匀速，

通过上述仿真数据模拟得到惯导仪表理论数据，并在其上叠加相应的仪表误差作为仪表实际采集数据，惯导对所述仪表实际采集数据进行采样，用于导航解算，采样周期为5ms。类似地，利用仿真数据模拟DVL仪表实际数据。

仿真的相关参数：

初始位置：东经118°、北纬32°；

舰船初始航向：45°；

陀螺仪零偏：0.04°/h；

陀螺白噪声误差：0.04°/h；

加速度计零偏：50ug；

加速度计白噪声误差：50ug；

DVL方位安装误差：1°；

DVL刻度系数误差：0.2％；

DVL随机量测噪声：0.002m/s

DVL方位安装误差补偿方法的验证

在普通PC机进行算法验证。仿真进行2000s，仿真过程过程中，(1)产生仪表数据；(2)根据仪表数据构建SINS/DVL滤波方程；(3)由转弯运动获取DVL方位安装误差的最佳可观测度；(4)利用Kalman滤波器进行DVL方位安装误差估计。

图2中各曲线表明，本发明设计的方法有效完成了方位安装误差的估计。

表1中统计数据表明，转弯机动为常见机动中的最佳选择。

表1

尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。

Claims (4)

1.一种SINS/DVL组合中DVL方位安装误差估计方法，其特征在于，包括以下步骤：

(1)选择SINS速度误差、姿态误差、位置误差、加速度计零偏、陀螺仪零偏、DVL刻度系数误差、DVL与SINS中IMU的方位安装误差作为状态向量，构建***状态方程；选择SINS载体系速度与DVL安装坐标系速度差值作为量测向量，构建***量测方程；

(2)由Kalman滤波器作为SINS/DVL信息融合滤波器；

(3)选择转弯运动以获取SINS/DVL方位安装误差的最佳可观测度，即最佳估计效果。

2.根据权利要求1所述的SINS/DVL组合中DVL方位安装误差估计方法，其特征在于，所述步骤(1)具体包括如下步骤：

1)选取SINS的误差状态为：速度误差δV＝[δV_EδV_N]^T，姿态误差φ＝[φ_Eφ_Nφ_U]^T，位置误差δP＝[δLδλ]^T，加速度计零偏 $\▿={\left[\begin{array}{cc}{\▿}_x& {\▿}_y\end{array}\right]}^T,$ 陀螺仪零偏ε＝[ε_xε_yε_z]^T，共计12维；并将DVL刻度系数误差δΚ＝[δΚ_xδΚ_y]^T和DVL与IMU的方位安装误差扩充为状态向量；则SINS/DVL组合导航***模型的状态向量为15维：

其中，δV_E、δV_N分别为SINS的东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别为纬度、经度误差；分别为x、y轴方向的加速度计零偏；ε_x、ε_y、ε_z分别为x、y、z轴方向的陀螺仪零偏；δΚ_x、δΚ_y分别为x、y轴方向的DVL刻度系数误差；

将DVL刻度系数误差、DVL与IMU的方位安装误差认为是常值，即：

其中，是X_DVL的导数，是δK_x的导数，是δK_y的导数，是的导数；

根据SINS/DVL误差模型，可得SINS/DVL的线性状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)=\left[\begin{array}{cc}{F}_{SINS}& 0_{12\×3}\\ 0_{3\×12}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{W}_{SINS}\\ 0_{3\×1}\end{array}\right]---\left(3\right)$

上式写成矩阵形式为：

式中，F(t)为***的状态转移矩阵；W_SINS为状态向量的过程噪声矩阵；w_V为SINS过程噪声矩阵中速度误差矩阵；w_φ为SINS过程噪声矩阵中失准角矩阵；w_P为SINS过程噪声矩阵中位置误差矩阵；

$\begin{array}{cc}{F}_{11}=\left[\begin{array}{cc}\frac{V_N}{R}\tan L& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\\ -2\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0\end{array}\right],& F_{12}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{13}=\left[\begin{array}{cc}\left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_N& 0\\ \left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L\right)V_E& 0\end{array}\right],& F_{14}=\left[\begin{array}{cc}{C}_b^n\left(1,1\right)& C_b^n\left(1,2\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{21}=\left[\begin{array}{cc}0& -\frac{1}{R}\\ \frac{1}{R}& 0\\ \frac{\tan L}{R}& 0\end{array}\right]& F_{24}=-\left[\begin{array}{ccc}{C}_b^n\left(1,1\right)& C_b^n\left(1,3\right)& C_b^n\left(1,3\right)\\ C_b^n\left(2,1\right)& C_b^n\left(2,2\right)& C_b^n\left(2,3\right)\\ C_b^n\left(3,1\right)& C_b^n\left(3,2\right)& C_b^n\left(3,3\right)\end{array}\right]\end{array}$

$F_{22}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& -\left({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}\right)\\ -\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& 0& -\frac{V_N}{R}\\ {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0\end{array}\right]$

$\begin{array}{cc}{F}_{31}=\left[\begin{array}{cc}0& \frac{1}{R}\\ \frac{1}{R}\sec L& 0\end{array}\right]& F_{33}=\left[\begin{array}{cc}0& 0\\ 0& \frac{V_E}{R}\tan L\sec L\end{array}\right]\end{array}$

其中，ω_ie、R与L分别为地球自转角速度、地球半径以及载体所在地理纬度；f_E、f_N与f_U为加速度测量值在导航系n中沿东向、北向与天向轴的投影；为载体系b向导航系n的转换矩阵；V_E与V_N分别为SINS的东向与北向速度。

2)以SINS载体系速度与DVL安装坐标系速度差值作为量测向量，并构建***量测方程；

考虑DVL刻度系数误差和随机测量误差，DVL测量值为：

$\begin{array}{cc}{\tilde{V}}_{DVL}^{mi}=(1+{\mathrm{\δK}}_i)V_{DVL}^{mi}+{\mathrm{\δV}}_{di},& i=x,y---\left(5\right)\end{array}$

式中，上标m表示DVL安装坐标系；表示DVL在m系中的真实值，δV_di为测量噪声；i＝x,y为DVL的水平安装方向；为DVL的测量值；

在载体系b下，SINS的速度计算值为：

${\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T{C}_{n^{\′}}^n(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n)---\left(6\right)$

式中，为SINS速度真实值；为SINS解算的速度误差；为SINS在n中解算的速度在b系的投影值；为n'系到n系的转换矩阵，且有：

$C_{n^{\′}}^n=I+\mathrm{\φ}\×---\left(7\right)$

式中，n'为计算导航坐标系；

将式(7)代入式(6)得到：

$\begin{array}{c}{\tilde{V}}_{SINS}^b={\left(C_b^n\right)}^T\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)+{\left(C_b^n\right)}^T\left(\mathrm{\φ}\×\right)\left(V_{SINS}^n+{\mathrm{\δV}}_{SINS}^n\right)\\ \≈V_{SINS}^b+{\left(C_b^n\right)}^T{\mathrm{\δV}}_{SINS}^n-{\left(C_b^n\right)}^T\left({\mathrm{\δV}}_{SINS}^n\×\right)\mathrm{\φ}\end{array}---\left(8\right)$

式中，为SINS在导航系n中的速度真实值在载体系b中的投影；

由于DVL与SINS间存在安装误差角SINS载体系速度与DVL安装坐标系速度之间存在如下关系：

式中，为DVL安装坐标系m向载体系b的转换矩阵。为包含方位安装误差在内的安装误差角向量，忽略水平安装误差，有：

从而有：

根据式(8～10)，有：

式中，δV_d为DVL测量噪声，δV_d即δV_di,i＝x,y；即

$V_{DVL}^m\×\mathrm{\δ}K={\left[\begin{array}{cc}{\mathrm{\δK}}_x{V}_{DVL}^{mx}& {\mathrm{\δK}}_y{V}_{DVL}^{my}\end{array}\right]}^T;$

式(11)即为SINS/DVL的量测方程，写成矩阵形式：

Z＝HX+V(12)

式中V为量测噪声；H为量测矩阵：

$H=\left[\begin{array}{cccccccc}{C}_n^b(1,1)& C_n^b(1,2)& C_n^b(1,3)V_N& -C_n^b(1,3)V_E& -C_n^b(1,1)V_N+C_n^b(1,2)V_E& 0_{1\×8}& V_{SINS}^{by}& -V_{DVL}^{mx}\\ C_n^b(2,1)& C_n^b(2,2)& C_n^b(2,3)V_N& -C_n^b(2,3)V_E& -C_n^b(2,1)V_N+C_n^b(2,2)V_E& 0_{1\×8}& -V_{SINS}^{bx}& -V_{DVL}^{my}\end{array}\right]---\left(13\right)$

式中，与为SINS速度在载体系b中在x与y轴的投影。

3.根据权利要求2所述的一种SINS/DVL组合中DVL方位安装误差估计方法，其特征在于：所述的步骤2)具体包括：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k-1}\right)\\ K_k=P_{k,k-1}{H}_k^T{\left(H_k{P}_{k,k-1}{H}_k^T+R_k\right)}^{-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ P_k=\left(I-K_k{H}_k\right)P_{k,k-1}\end{array}\right.---\left(14\right)$

式中，K_k为增益矩阵；P_k为状态向量的协方差矩阵；R_k为量测噪声序列V_k的协方差矩阵；Q_k为过程噪声序列W_k的协方差矩阵；为状态向量估计值；为状态向量一步预测值；P_k,k-1为状态向量的协方差矩阵一步预测值；Φ_k,k-1为状态转移矩阵，即式(3)中F(t)阵的离散形式；Γ_k-1为过程噪声转移矩阵。

4.根据权利要求3所述的一种SINS/DVL组合中DVL方位安装误差估计方法，其特征在于：所述的步骤3)具体包括：

求取线性时变***的总可观测矩阵的表达式如下：

${\tilde{Q}}_s^T=\left[\begin{array}{ccccc}{\tilde{Q}}_1& {\tilde{Q}}_2& ...& {\tilde{Q}}_j& ...\end{array}\right]---\left(15\right)$

式中，为连续***在第j时间段的可观测矩阵，其形式为：

${\tilde{Q}}_j^T={\left[\begin{array}{ccccc}{\left(H_j\right)}^T& {\left(H_j{F}_j\right)}^T& {\left(H_j{F}_j^2\right)}^T& ...& \left(H_j{F}_j^{p-1}\right)\end{array}\right]}^T---\left(16\right)$

其中，p＝15；p-1为矩阵F_j的乘方次数；H_j为连续***在第j时间段的量测矩阵；F_j为第j时间段的状态转移矩阵；

利用列主元高斯消元法将式(16)转换为上三角矩阵，并再进一步化为对角矩阵得到状态的可观测性情况，并根据对角矩阵对角元素的绝对值得到SINS/DVL组合导航***在转弯运动下的可观测度。