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

摘要

本发明针对现有传递对准方法在保证潜航器惯导对准精度的条件下，不能实现移动基准与潜航器快速分离和一次性多潜航器布放的问题，公开了一种初始方位信息辅助下潜航器快速传递对准方法。本发明包括以下四步1、初始信息装订，信息装订完毕后潜航器与移动基准即分离；2、潜航器姿态更新方程、速度更新方程、位置更新方程确定；3、卡尔曼滤波状态方程与观测方程确定；4、卡尔曼滤波并对潜航器姿态、速度误差修正，完成传递对准。本发明实现了移动基准与潜航器的快速分离，同时能够保证潜航器惯导对准精度，且能实现一次性多潜航器布放。

主权项

一种初始方位信息辅助下潜航器快速传递对准方法，其特征在于包含以下步骤：步骤一：移动基准惯导(MINS)对每一个潜航器惯导(SINS)装订初始位置，初始位置包括初始经度λ0、初始纬度L0，初始速度初始姿态信息初始姿态信息包括准确的方位角存在误差的俯仰角与横滚角装订完成后，潜航器与移动基准分离；其中，t0表示对准初始时刻，表示潜航器相对于地球的速度在地理坐标系n系下的投影，地理坐标系n系定义为北向‑东向‑地向(N‑E‑D)，地球坐标系e系的原点位于地心，ez轴沿地球自转轴方向，ex轴在赤道平面内指向格林威治子午线，ey轴与另外两轴构成右手正交坐标系；表示惯性坐标系系到惯性坐标系系的方向余弦矩阵，系由对准初始时刻载体坐标系b系与惯性空间固联得到，系相对惯性空间固定不动，系由对准初始时刻n系与惯性空间固联得到，系相对惯性空间固定不动，载体坐标系b系轴向沿载体横滚轴‑俯仰轴‑偏航轴(前‑右‑下)；步骤二：潜航器惯导(SINS)建立姿态更新方程、速度更新方程、位置更新方程分别如下：姿态更新方程：其中，n0系定义为对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动；为对准初始时刻导航系n0系到当前时刻导航系n系的方向余弦矩阵；为惯性坐标系系到对准初始时刻导航系n0系的方向余弦矩阵；为惯性坐标系系到惯性坐标系系的方向余弦矩阵；为载体坐标系b系到惯性坐标系系的方向余弦矩阵；速度更新方程：其中，Δva=Cn0n(tk-1)Cin0n0(tk-1)Cib0in0(tk-1)∫tk-1tkCbib0fbdt]]>Δvb=Δt((2ωien+ωenn)×ven(tk-1))]]>Δvc=Δtgln]]>tk‑1、tk分别表示第k‑1、k个潜航器速度更新时刻，为对应的速度，fb为加表比力输出，为tk‑1时刻相应的方向余弦矩阵，Δt＝tk‑tk‑1为惯导更新时间间隔，为地理系下的地球自转角速度，为地理系下的转移角速度，为地理系下的重力；位置更新方程：潜航器经纬度通过航迹推算进行更新，其中，Lk‑1、Lk分别表示k‑1、k时刻潜航器的纬度，λk‑1、λk分别表示k‑1、k时刻潜航器的经度，vn、ve表示潜航器的北向、东向速度，RN表示子午面曲率半径，h表示高度；步骤三：构建卡尔曼滤波状态方程与观测方程如下：选取横滚角误差δθ0、俯仰角误差δγ0、北向速度误差δvn、东向速度误差δve为系统状态，即x＝[δθ0 δγ0 δvn δve ]T                                                 (4)构建系统状态方程如下，x·=Fx+Gw---(5)]]>其中，上标T表示矩阵或向量的转置，x表示系统状态，表示状态微分，F表示状态转移矩阵，G表示系统噪声矩阵；为陀螺输出噪声，δfb为加表输出噪声；卡尔曼滤波状态方程按如下方法构建：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)]]>其中，为估算的北向速度，为估算的东向速度，vn为真实北向速度，ve为真实东向速度，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＝[M7 I2×2]；步骤四：根据卡尔曼滤波状态方程和观测方程进行滤波，对横滚角误差、俯仰角误差、北向速度误差、东向速度误差进行估计，对潜航器的姿态、速度进行修正，完成传递对准。