[发明专利]一种初始方位信息辅助下潜航器快速传递对准方法

权利要求书

1.一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于包含以下步骤：

步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置，初始位置包括初始经度λ₀、初始纬度L₀，初始速度初始姿态信息初始姿态信息包括准确的方位角存在误差的俯仰角与横滚角装订完成后，潜航器与移动基准分离；

其中，t₀表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向-东向-地向(N-E-D)，地球坐标系e系的原点位于地心，e_z轴沿地球自转轴方向，e_x轴在赤道平面内指向格林威治子午线，e_y轴与另外两轴构成右手正交坐标系；

表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到，系相对惯性空间固定不动，系由对准初始时刻n系与惯性空间固联得到，系相对惯性空间固定不动，载体坐标系b系轴向沿载体横滚轴-俯仰轴-偏航轴(前-右-下)；

步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：

姿态更新方程：

其中，

n₀系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动；

为对准初始时刻导航系n₀系到当前时刻导航系n系的方向余弦矩阵；

为惯性坐标系系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

为载体坐标系b系到惯性坐标系系的方向余弦矩阵；

速度更新方程：

其中，

Δva=Cn0n(tk-1)Cin0n0(tk-1)Cib0in0(tk-1)∫tk-1tkCbib0fbdt]]>

Δvb=Δt((2ωien+ωenn)×ven(tk-1))]]>

Δvc=Δtgln]]>

t_k-1、t_k分别表示第k-1、k个潜航器速度更新时刻，为对应的速度，f^b为加表比力输出，为t_k-1时刻相应的方向余弦矩阵，Δt＝t_k-t_k-1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；

位置更新方程：

潜航器经纬度通过航迹推算进行更新，其中，

L_k-1、L_k分别表示k-1、k时刻潜航器的纬度，λ_k-1、λ_k分别表示k-1、k时刻潜航器的经度，v_n、v_e表示潜航器的北向、东向速度，R_N表示子午面曲率半径，h表示高度；

步骤三：构建卡尔曼滤波状态方程与观测方程如下：

选取横滚角误差δθ₀、俯仰角误差δγ₀、北向速度误差δv_n、东向速度误差δv_e为系统状态，即

x＝[δθ₀ δγ₀ δv_n δv_e ]^T (4)

构建系统状态方程如下，

x·=Fx+Gw---(5)]]>

其中，

上标T表示矩阵或向量的转置，x表示系统状态，表示状态微分，F表示状态转移矩阵，G表示系统噪声矩阵；为陀螺输出噪声，δf^b为加表输出噪声；

卡尔曼滤波状态方程按如下方法构建：

1)水平欧拉角误差微分方程构建如下

水平欧拉角误差为常值，其微分为零，即

δθ·0δγ·0T=02×1---(6)]]>

2)速度误差微分方程构建如下

δv·nδv·eT=v~·nv~·eT-v·nv·eT=[I2×202×1]M1Mδθ0δγ0T+[I2×202×1]M2I2×202×1TδvnδveT+[I2×202×1]M3δfb=M4δθ0δγ0T+M5δvnδveT+M6δfb---(7)]]>

其中，为估算的北向速度，为估算的东向速度，v_n为真实北向速度，v_e为真实东向速度，

3)由1)、2)步中水平欧拉角误差微分方程、速度误差微分方程得到卡尔曼滤波状态方程中状态转移矩阵及系统噪声矩阵相应为

F=02×202×2M4M5,G=02×302×302×3M6;]]>

取1s更新一次的多普勒速度为观测量，构建观测方程如下，

z＝Hx+v (8)

其中，

多普勒坐标系d系轴向沿多普勒测速仪的横滚轴-俯仰轴-偏航轴(前-右-下)；

z为观测向量，H为观测矩阵，v为测量噪声；

卡尔曼滤波观测方程按如下方法构建：

1)构建地理系n系下含有误差的多普勒测速输出与姿态矩阵失准角φ的关系

v~edn=Cn0nCin0n0[I-φ×]Cib0in0Cbib0Cdb(vedd+δvedd)---(9)]]>

其中，为多普勒测速仪测得的潜航器速度在n系下投影，为多普勒测速仪测得的潜航器速度在d系下的投影，多普勒测速仪测速误差在d系下的投影；

2)构建速度误差观测量z

潜航器惯导解算速度为将潜航器惯导解算速度的北向速度东向速度与多普勒测速仪输出中的北向速度东向速度作差得到速度误差观测量，

z=v~nv~eT-v~nDVLv~eDVLT=[I2×202×1](v~en-v~edn)=[I2×202×1](ven+δven-Cn0nCin0n0[I-φ×]Cib0in0Cbib0Cdb(vedd+δvedd))=[I2×202×1](δven+Cn0nCin0n0[Cbib0Cdbvedd×]φ-Cn0nCin0n0Cib0in0Cbib0Cdbδvedd)=δvnδveT+M7δθ0δγ0T+v---(10)]]>

其中，

3)确定观测方程z＝Hx+v中观测矩阵为

H＝[M₇ I_2×2]；

步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。

2.根据权利要求1所述的一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于：

在步骤二中所述的姿态更新方程按如下方法更新：

Cn0n≈I3×3+0-(λ-λ0+ωiet)sin LL0-L(λ-λ0+ωiet)sin L0-(λ-λ0+ωiet)cos LL-L0(λ-λ0+ωiet)cos L0---(11)]]>

其中，I_3×3表示三维单位矩阵，ω_ie为地球自转角速度，t为初始时刻到当前时刻的解算时间，λ₀、L₀为潜航器装订的初始经度、纬度，λ、L为当前时刻经度、纬度；

可由潜航器初始经纬度和t确定，将按如下分解，

Cin0n0=Cen0Cie0eCin0ie0---(12)]]>

其中，

C0n0=-sin L0cosλ0-sin L0sinλ0cosL0cos L0cosλ0cos L0sinλ0sinλ0-sinλ0cosλ00Cie0e=cos(ωiet)sin(ωiet)0-sin(ωiet)cos(ωiet)0001,Cin0ie0=(Cen0)T---(13)]]>

系由对准初始时刻e系与惯性空间固联得到，系相对惯性空间固定不动；

为地球坐标系e系到对准初始时刻导航系n₀系的方向余弦矩阵；

为惯性坐标系系到地球坐标系e系的方向余弦矩阵；

为惯性坐标系系到惯性坐标系系的方向余弦矩阵；

利用陀螺输出数据求解，传递对准完成时间内，忽略陀螺的零偏、随机游走误差，按如下微分方程和初值进行姿态更新求解，即

C·bib0=Cbib0[ωibb×],Cbib0(0)=I3×3---(14)]]>

其中，为载体角速度，为的初值。