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

权利要求书

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

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

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

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

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

Vdvln=CbnVdvlb;]]>

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

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

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

2.根据权利要求1所述的匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法，其特征是所述卡尔曼滤波的状态方程和观测方程建立形式如下：

选取状态向量为：

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

滤波模型的状态方程为：

X·(t)=A(t)X(t)+W(t)]]>

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

W(t)=ηδVEηδVNO1×2ηφEηφNηφUO1×9T]]>

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

A=A13×13INSO13×3O3×13A3×3DVL]]>

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

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

A13×13INS=A4×41A4×32O4×3A4×33A3×44A3×35A3×36O3×3O6×12,]]>

A4×32=0-g0g0002×3,]]>A4×33=Cb(11)nCb(12)nCb(13)nCb(21)nCb(22)nCb(23)nO2×3,]]>

A3×35=-Ωinn,]]>A3×36=Cbn]]>

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

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

Z=VE-VdvlEnVN-VdvlNnT]]>

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

观测方程为：

Z(t)=VE-VdvlEnVN-VdvlEn=H(t)X(t)+un]]>

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

un=-uEn-uNnT]]>

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

H=H2×13INSH2×3DVL]]>

上式中的两部分为：

H2×13INS=I2×2O2×11,]]>H2×3DVL=-I2×2O2×1]]>

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