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

摘要

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

主权项

一种匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法，其特征是包括如下步骤：（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}{\eta }_{{\mathrm{\delta V}}_E}& {\eta }_{{\mathrm{\delta V}}_N}& O_{1\times 2}& {\eta }_{{\phi }_E}& {\eta }_{{\phi }_N}& {\eta }_{{\phi }_U}& O_{1\times 9}\end{array}\right]}^T$状态转移矩阵A写为如下形式：$A=\left[\begin{array}{cc}{A}_{13\times 13}^{INS}& O_{13\times 3}\\ O_{3\times 13}& A_{3\times 3}^{DVL}\end{array}\right]$其中，对应惯导部分状态变量的状态转移矩阵部分，而为对应多普勒计程仪部分状态转移矩阵；$A_{13\times 13}^{INS}=\left[\begin{array}{c}\begin{array}{cccc}{A}_{4\times 4}^1& A_{4\times 3}^2& O_{4\times 3}& A_{4\times 3}^3\end{array}\\ \begin{array}{cccc}{A}_{3\times 4}^4& A_{3\times 3}^5& A_{3\times 3}^6& O_{3\times 3}\end{array}\\ O_{6\times 12}\end{array}\right],$$A_{4\times 3}^2=\left[\begin{array}{ccc}0& -g& 0\\ g& 0& 0\\ & O_{2\times 3}& \end{array}\right],A_{4\times 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\times 3}& \end{array}\right],$$A_{3\times 3}^5=-{\Omega }_{in}^n,A_{3\times 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\times 13}^{INS}& H_{2\times 3}^{DVL}\end{array}\right]$上式中的两部分为：$H_{2\times 13}^{INS}=\left[\begin{array}{cc}{I}_{2\times 2}& O_{2\times 11}\end{array}\right],H_{2\times 3}^{DVL}=\left[\begin{array}{cc}-I_{2\times 2}& O_{2\times 1}\end{array}\right]$其中，I_2×2为2×2的单位阵，0_i×j为i×j维零矩阵。