CN103148868B - 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法 - Google Patents

Abstract

本发明提供的是一种匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法。将多普勒计程仪测速误差在地理系上的投影扩展为状态变量，在此基础上建立多普勒计程仪/捷联惯性导航组合对准系统。相对于单纯的将多普勒计程仪测速误差扩展为状态变量，本发明的方法可以提高多普勒计程仪测速误差的可观测测度，有效的将测速误差估计出，解决多普勒计程仪测速误差对对准精度的影响。

Description

匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法

技术领域

本发明涉及的是一种组合导航初始对准的方法。

背景技术

初始对准技术是惯性导航系统的关键技术之一，对准精度直接影响导航系统的性能。高精度初始对准是导航领域，特别是船舶导航领域的难题。提高对准精度的最简单方法是提高器件的精度，然而通过增大硬件投入来提高精度，难以在短期内取得显著成效。因此，如何通过系统优化的方式提高对准精度成为重点的研究方向。现今以误差估计并补偿的方式提高对准精度成为研究的热门。

航行中DVL（多普勒计程仪）辅助的船用捷联惯导初始对准，可以采用卡尔曼滤波的组合对准方式，对系统的误差进行估计并补偿。卡尔曼滤波器的估计精度与系统的可观测度有关，合理的选择状态变量，可以提高系统的可观测度，进而提高估计的精度，使对准的精度提高。文章“新的两位置组合对准方法研究”解决的是静基座组合对准的问题，不能用来解决航行中的对准问题。本发明采用DVL侧量误差在地理系上的投影作为状态变量的方式进行组合，可以有效解决匀速直航情况下的对准问题，而且相对于单纯的将DVL测速误差扩展为状态变量而言，可以提高DVL测速误差的可观测度，并可以有效地将DVL测速误差估计出，解决DVL测速误差对对准精度的影响。

发明内容

本发明的目的在于提供一种对准精度高的匀速直航下基于DVL地理系测速误差估计的组合对准方法。

本发明的目的是这样实现的：

（1）船舶处于匀速直航状态；

（2）启动多普勒计程仪；

（3）启动惯导系统，使其完成粗对准，获得初始时刻的捷联矩阵和相应的四元数Q；

（4）利用捷联矩阵将多普勒计程仪测得的速度转化到地理系上：

$V_{\mathrm{dvl}}^n=C_b^n{V}_{\mathrm{dvl}}^b;$

（5）以惯导解算速度V_E和多普勒计程仪测量速度在地理系上的投影的差值作为量测信息，利用卡尔曼滤波的方法对组合对准系统的各误差量进行估计；

（6）惯导系统利用四元数更新计算本时刻的捷联矩阵

（7）重复步骤（4）-（6）进入下一个时间循环，系统计算周期为T，经过20min后，组合对准系统的各误差量被准确估计出，将其补偿到对准系统中，对准结束。

所述卡尔曼滤波的状态方程和观测方程建立形式如下：

选取状态向量为：

其中：δV_E、δV_N分别为东向速度误差和北向速度误差，δφ、δλ分别为纬度和经度误差，φ_E、φ_N、φ_U分别为惯导系统的失准角在地理系东北天三个轴上的分量，ε_x、ε_y、ε_z和ΔA_x、ΔA_y、ΔA_z分别表示捷联惯导三轴陀螺漂移和加速度计零偏，表示多普勒计程仪测速常值误差在地理系东北天三轴向的投影；

滤波模型的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)$

其中，W(t)为误差状态矢量，

$W\left(t\right)={\left[\begin{array}{ccccccc}{\mathrm{\η}}_{{\mathrm{\δV}}_E}& {\mathrm{\η}}_{{\mathrm{\δV}}_N}& O_{1\×2}& {\mathrm{\η}}_{{\mathrm{\φ}}_E}& {\mathrm{\η}}_{{\mathrm{\φ}}_N}& {\mathrm{\η}}_{{\mathrm{\φ}}_U}& O_{1\×9}\end{array}\right]}^T$

状态转移矩阵A写为如下形式：

$A=\left[\begin{array}{cc}{A}_{13\×13}^{\mathrm{INS}}& O_{13\×3}\\ O_{3\×13}& A_{3\×3}^{\mathrm{DVL}}\end{array}\right]$

其中，对应惯导部分状态变量的状态转移矩阵部分，而为对应多普勒计程仪部分状态转移矩阵；

对应惯导部分状态变量的状态转移矩阵部分：

$A_{13\×13}^{\mathrm{INS}}=\left[\begin{array}{ccccc}{A}_{4\×4}^1& A_{4\×3}^2& & O_{4\×3}& A_{4\×3}^3\\ A_{3\×4}^4& A_{3\×3}^5& & A_{3\×3}^6& O_{3\×3}\\ & & O_{6\×12}& & \end{array}\right],$

$A_{4\×3}^2=\left[\begin{array}{ccc}0& -g& 0\\ g& 0& 0\\ & 0_{2\×3}& \end{array}\right],$ $A_{4\×3}^3=\left[\begin{array}{ccc}{C}_{b\left(11\right)}^n& C_{b\left(12\right)}^n& C_{b\left(13\right)}^n\\ C_{b\left(21\right)}^n& C_{b\left(22\right)}^n& C_{b\left(23\right)}^n\\ & O_{2\×3}& \end{array}\right],$

$A_{3\×3}^5=-{\mathrm{\Ω}}_{\mathrm{in}}^n,$ $A_{3\×3}^6=C_b^n$

其中，R为地球半径，g为重力加速度，V_E，V_N分别惯导东向和北向解算速度，为惯导解算的纬度，为捷联矩阵对应位置上的元素，表示的反对称阵，为地理系相对于惯性空间的转动速度在地理系上的投影；而为对应DVL部分状态转移矩阵， 为的反对称阵，为载体相对于地理系的转动角速度在地理上的投影；

系统选取惯导和计程仪的速度差为观测量：

$Z={\left[\begin{array}{cc}{V}_E-V_{\mathrm{dvlE}}^n& V_N-V_{\mathrm{dvlN}}^n\end{array}\right]}^T$

其中，V_E、V_N分别为惯导东向和北向的解算速度，分别为多普勒计程仪地理系上的测量速度；

观测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{V}_E-V_{\mathrm{dvlE}}^n\\ V_N-V_{\mathrm{dvlE}}^n\end{array}\right]=H\left(t\right)X\left(t\right)+u^n$

其中，uⁿ为量测误差矢量；

$u^n={\left[\begin{array}{cc}{-u}_E^n& -u_N^n\end{array}\right]}^T$

H(t)也被写为两部分的形式

$H=\left[\begin{array}{cc}{H}_{2\×13}^{\mathrm{INS}}& H_{2\×3}^{\mathrm{DVL}}\end{array}\right]$

上式中的两部分为：

$H_{2\×13}^{\mathrm{INS}}=\left[\begin{array}{cc}{I}_{2\×2}& O_{2\×11}\end{array}\right],$ $H_{2\×3}^{\mathrm{DVL}}=\left[\begin{array}{cc}{-I}_{2\×2}& O_{2\×1}\end{array}\right]$

其中，I_2×2为2×2的单位阵，O_i×j为i×j维零矩阵。

本发明的有益效果通过如下方法得以验证：

Matlab仿真试验

在以下条件下进行仿真：

初始位置选取纬度经度λ＝126.6705°。设船体推进速度恒定，保持以15m/s的速度匀速直航，航向为135°。航行中伴随着三轴摇摆，俯仰、横滚和偏航三轴摇摆的幅度为3°、5°、4°，周期为8s、6s、10s；纵荡、横档和升沉幅度为0.2m/s²，周期为5s。

捷联惯导三轴陀螺常值漂移为0.01°/h；三轴加速度计零偏为10^-4m/s²；初始失准角分别为：φ_x＝0.1°，φ_y＝0.1°，φ_z＝0.5°；DVL测速误差系数ΔC为0.05；不考虑DVL与捷联惯导的坐标系安装偏差。

分别使用两种对准方案进行组合对准仿真实验。比较基于载体系测速误差估计的组合对准（方案一）和基于地理系测速误差估计的对准方案（方案二）的仿真结果。

仿真试验结果：图2为上述条件下方案一和方案二的惯导速度误差值估计误差曲线，图3为方案一和方案二的失准角估计误差曲线，图4为方案一和方案二的位置误差值估计误差曲线，图5为方案一和方案二的DVL测量误差值估计误差曲线。从图2~5中可以看出，采用方案一和方案二的水平失准角估计精度几乎没有差别，除此之外方案二对其余各项的估计精度都有较大的提高。

本发明的方法将多普勒计程仪测速误差在地理系上的投影扩展为状态变量，在此基础上建立多普勒计程仪/捷联惯性导航组合对准系统。相对于单纯的将多普勒计程仪测速误差扩展为状态变量，本发明的方法可以提高多普勒计程仪测速误差的可观测测度，有效的将测速误差估计出，解决多普勒计程仪测速误差对对准精度的影响。

附图说明

图1本发明的流程图。

图2a-图2d方案一和方案二的惯导速度误差值估计误差曲线，其中：图2a为方案一δV_E估计误差；，图2b为方案二δV_E估计误差；图2c为方案一δV_N估计误差；图2d为方案二δV_N估计误差。

图3a-图3f方案一和方案二的失准角估计误差曲线，其中：图3a为方案一φ_x估计误差；图3b为方案二φ_x估计误差；图3c为方案一φ_y估计误差；图3d为方案二φ_y估计误差；图3e为方案一φ_z估计误差；图3f为方案二φ_z估计误差。

图4a-图4d方案一和方案二的位置误差值估计误差曲线；其中：图4a为方案一δφ估计误差；图4b为方案二δφ估计误差；图4c为方案一δλ估计误差；图4d为方案二δλ估计误差。

图5a-图5d方案一和方案二的DVL测量误差值估计误差曲线，其中：图5a为方案一估计误差投影值；图5b为方案二估计误差投影值；图5c为方案一估计误差投影值；图5d为方案二估计误差投影值。

具体实施方式

下面结合图1举例对本发明做更详细的描述：

（1）将设备安装在船上，船处于匀速直航状态；

（2）启动多普勒计程仪；

（3）启动惯导系统，使其完成粗对准，获得初始时刻的捷联矩阵和相应的四元数Q；

（4）利用捷联矩阵将DVL测得的速度转化到地理系上：

$V_{\mathrm{dvl}}^n=C_b^n{V}_{\mathrm{dvl}}^b$

（5）以惯导解算速度V_E和DVL测量速度在地理系上的投影的差值作为量测信息，利用卡尔曼滤波的方法对组合对准系统的各误差量进行估计；

步骤（5）中，卡尔曼滤波的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)$

其中，X(t)为状态向量：

其中：δV_E、δV_N分别为东向速度误差和北向速度误差，δφ、δλ分别为纬度和经度误差，φ_E、φ_N、φ_U分别为惯导系统的失准角在地理系东北天三个轴上的分量，ε_x、ε_y、ε_z和ΔA_x、ΔA_y、ΔA_z分别表示捷联惯导三轴陀螺漂移和加速度计零偏， 表示DVL测速常值误差在地理系东北天三轴向的投影；

W(t)为误差状态矢量：

$W\left(t\right)={\left[\begin{array}{ccccccc}{\mathrm{\η}}_{{\mathrm{\δV}}_E}& {\mathrm{\η}}_{{\mathrm{\δV}}_N}& O_{1\×2}& {\mathrm{\η}}_{{\mathrm{\φ}}_E}& {\mathrm{\η}}_{{\mathrm{\φ}}_N}& {\mathrm{\η}}_{{\mathrm{\φ}}_U}& O_{1\×9}\end{array}\right]}^T$

A(t)为状态转移矩阵：

$A=\left[\begin{array}{cc}{A}_{13\×13}^{\mathrm{INS}}& O_{13\×3}\\ O_{3\×13}& A_{3\×3}^{\mathrm{DVL}}\end{array}\right]$

对应惯导部分状态变量的状态转移矩阵部分，而为对应DVL部分状态转移矩阵。对应惯导部分状态变量的状态转移矩阵部分

$A_{13\×13}^{\mathrm{INS}}=\left[\begin{array}{ccccc}{A}_{4\×4}^1& A_{4\×3}^2& & O_{4\×3}& A_{4\×3}^3\\ A_{3\×4}^4& A_{3\×3}^5& & A_{3\×3}^6& O_{3\×3}\\ & & O_{6\×12}& & \end{array}\right],$

$A_{4\×3}^2=\left[\begin{array}{ccc}0& -g& 0\\ g& 0& 0\\ & 0_{2\×3}& \end{array}\right],$ $A_{4\×3}^3=\left[\begin{array}{ccc}{C}_{b\left(11\right)}^n& C_{b\left(12\right)}^n& C_{b\left(13\right)}^n\\ C_{b\left(21\right)}^n& C_{b\left(22\right)}^n& C_{b\left(23\right)}^n\\ & O_{2\×3}& \end{array}\right],$

$A_{3\×3}^5=-{\mathrm{\Ω}}_{\mathrm{in}}^n,$ $A_{3\×3}^6=C_b^n$

其中，R为地球半径，g为重力加速度，V_E，V_N分别惯导东向和北向解算速度，为惯导解算的纬度，为捷联矩阵对应位置上的元素，表示的反对称阵，为地理系相对于惯性空间的转动速度在地理系上的投影。而为对应DVL部分状态转移矩阵， 为的反对称阵，为载体相对于地理系的转动角速度在地理上的投影。

卡尔曼滤波的观测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{V}_E-V_{\mathrm{dvlE}}^n\\ V_N-V_{\mathrm{dvlE}}^n\end{array}\right]=H\left(t\right)X\left(t\right)+u^n$

其中，uⁿ为量测误差矢量。

$u^n={\left[\begin{array}{cc}{-u}_E^n& -u_N^n\end{array}\right]}^T$

H(t)也被写为两部分的形式

$H=\left[\begin{array}{cc}{H}_{2\×13}^{\mathrm{INS}}& H_{2\×3}^{\mathrm{DVL}}\end{array}\right]$

上式中的两部分为：

$H_{2\×13}^{\mathrm{INS}}=\left[\begin{array}{cc}{I}_{2\×2}& O_{2\×11}\end{array}\right],$ $H_{2\×3}^{\mathrm{DVL}}=\left[\begin{array}{cc}{-I}_{2\×2}& O_{2\×1}\end{array}\right]$

其中，I_2×2为2×2的单位阵，o_i×j为i×j维零矩阵。

（6）惯导系统利用四元数更新计算本时刻的捷联矩阵

（7）重复步骤（4）-（6）进入下一个时间循环，系统计算周期为T（周期值视具体系统而定），经过20min后，组合对准系统的各误差量被准确估计出，将其补偿到对准系统中，对准结束。

Claims (1)

1.一种匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法，其特征是包括如下步骤：

（1）船舶处于匀速直航状态；

（2）启动多普勒计程仪；

（3）启动惯导系统，使其完成粗对准，获得初始时刻的捷联矩阵和相应的四元数Q；

（4）利用捷联矩阵将多普勒计程仪测得的速度转化到地理系上：

$V_{dvl}^n=C_b^n{V}_{dvl}^b;$

（5）以惯导解算速度V_E和多普勒计程仪测量速度在地理系上的投影的差值作为量测信息，利用卡尔曼滤波的方法对组合对准系统的各误差量进行估计；

（6）惯导系统利用四元数更新计算本时刻的捷联矩阵

（7）重复步骤（4）-（6）进入下一个时间循环，系统计算周期为T，经过20min后，组合对准系统的各误差量被准确估计出，将其补偿到对准系统中，对准结束；

所述卡尔曼滤波的状态方程和观测方程建立形式如下：

选取状态向量为：

其中：δV_E、δV_N分别为东向速度误差和北向速度误差，δφ、δλ分别为纬度和经度误差，φ_E、φ_N、φ_U分别为惯导系统的失准角在地理系东北天三个轴上的分量，ε_x、ε_y、ε_z和ΔA_x、ΔA_y、ΔA_z分别表示捷联惯导三轴陀螺漂移和加速度计零偏，表示多普勒计程仪测速常值误差在地理系东北天三轴向的投影；

滤波模型的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)$

其中，W(t)为误差状态矢量，

$W\left(t\right)={\left[\begin{array}{ccccccc}{\mathrm{\η}}_{{\mathrm{\δV}}_E}& {\mathrm{\η}}_{{\mathrm{\δV}}_N}& O_{1\×2}& {\mathrm{\η}}_{{\mathrm{\φ}}_E}& {\mathrm{\η}}_{{\mathrm{\φ}}_N}& {\mathrm{\η}}_{{\mathrm{\φ}}_U}& O_{1\×9}\end{array}\right]}^T$

状态转移矩阵A写为如下形式：

$A=\left[\begin{array}{cc}{A}_{13\×13}^{INS}& O_{13\×3}\\ O_{3\×13}& A_{3\×3}^{DVL}\end{array}\right]$

其中，对应惯导部分状态变量的状态转移矩阵部分，而为对应多普勒计程仪部分状态转移矩阵；

$A_{13\×13}^{INS}=\left[\begin{array}{c}\begin{array}{cccc}{A}_{4\×4}^1& A_{4\×3}^2& O_{4\×3}& A_{4\×3}^3\end{array}\\ \begin{array}{cccc}{A}_{3\×4}^4& A_{3\×3}^5& A_{3\×3}^6& O_{3\×3}\end{array}\\ O_{6\×12}\end{array}\right],$

$A_{4\×3}^2=\left[\begin{array}{ccc}0& -g& 0\\ g& 0& 0\\ & O_{2\×3}& \end{array}\right],A_{4\×3}^3=\left[\begin{array}{ccc}{C}_{b\left(11\right)}^n& C_{b\left(12\right)}^n& C_{b\left(13\right)}^n\\ C_{b\left(21\right)}^n& C_{b\left(22\right)}^n& C_{b\left(23\right)}^n\\ & O_{2\×3}& \end{array}\right],$

$A_{3\×3}^5=-{\mathrm{\Ω}}_{in}^n,A_{3\×3}^6=C_b^n$

其中，R为地球半径，g为重力加速度，V_E，V_N分别惯导东向和北向解算速度，为惯导解算的纬度，为捷联矩阵对应位置上的元素，表示的反对称阵，为地理系相对于惯性空间的转动速度在地理系上的投影； 为的反对称阵，为载体相对于地理系的转动角速度在地理上的投影；

系统选取惯导和计程仪的速度差为观测量：

$Z={\left[\begin{array}{cc}{V}_E-V_{dvlE}^n& V_N-V_{dvlN}^n\end{array}\right]}^T$

其中，V_E、V_N分别为惯导东向和北向的解算速度，分别为多普勒计程仪地理系上的测量速度；

观测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{V}_E-V_{dvlE}^n\\ V_N-V_{dvlE}^n\end{array}\right]=H\left(t\right)X\left(t\right)+u^n$

其中，uⁿ为量测误差矢量；

$u^n={\left[\begin{array}{cc}-u_E^n& -u_N^n\end{array}\right]}^T$

H(t)也被写为两部分的形式

$H=\left[\begin{array}{cc}{H}_{2\×13}^{INS}& H_{2\×3}^{DVL}\end{array}\right]$

上式中的两部分为：

$H_{2\×13}^{INS}=\left[\begin{array}{cc}{I}_{2\×2}& O_{2\×11}\end{array}\right],H_{2\×3}^{DVL}=\left[\begin{array}{cc}-I_{2\×2}& O_{2\×1}\end{array}\right]$

其中，I_2×2为2×2的单位阵，0_i×j为i×j维零矩阵。