[发明专利]冗余配置激光陀螺航海惯导协同定位方法

摘要

本发明针对冗余配置激光陀螺航海惯导系统间缺少信息融合和有效协同定位方法的问题，公开了冗余配置激光陀螺航海惯导协同定位方法。本发明通过联合误差状态卡尔曼滤波、系统噪声影响分析、双轴旋转调制激光陀螺航海惯导相对性能在线评估、单轴旋转调制激光陀螺航海惯导确定性误差预测补偿等步骤，实现了无外界基准信息条件下，冗余配置的两套双轴旋转调制激光陀螺航海惯导间相对性能的在线评估，同时提高了冗余配置的单轴旋转调制激光陀螺航海惯导的定位精度。本发明可用于长航时高精度导航条件下冗余配置激光陀螺航海惯导间的信息融合，对于提高舰艇恶劣海况环境下的生存能力具有重大意义。

主权项

1.冗余配置激光陀螺航海惯导协同定位方法，其特征在于包括以下步骤：步骤一：分别确定单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2、单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导3间的联合误差状态，联合误差状态分别为其中，x₁₂(t)为单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2间的联合误差状态，x₁₃(t)为单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导3间的联合误差状态，φ_E12＝φ_E1‑φ_E2为单轴旋转调制激光陀螺航海惯导1东向姿态误差φ_E1与双轴旋转调制激光陀螺航海惯导2东向姿态误差φ_E2的差值，φ_N12＝φ_N1‑φ_N2为单轴旋转调制激光陀螺航海惯导1北向姿态误差φ_N1与双轴旋转调制激光陀螺航海惯导2北向姿态误差φ_N2的差值，φ_U12＝φ_U1‑φ_U2为单轴旋转调制激光陀螺航海惯导1天向姿态误差φ_U1与双轴旋转调制激光陀螺航海惯导2天向姿态误差φ_U2的差值，φ_E13＝φ_E1‑φ_E3为单轴旋转调制激光陀螺航海惯导1东向姿态误差φ_E1与双轴旋转调制激光陀螺航海惯导3东向姿态误差φ_E3的差值，φ_N13＝φ_N1‑φ_N3为单轴旋转调制激光陀螺航海惯导1北向姿态误差φ_N1与双轴旋转调制激光陀螺航海惯导3北向姿态误差φ_N3的差值，φ_U13＝φ_U1‑φ_U3为单轴旋转调制激光陀螺航海惯导1天向姿态误差φ_U1与双轴旋转调制激光陀螺航海惯导3天向姿态误差φ_U3的差值，δv_E12＝δv_E1‑δv_E2为单轴旋转调制激光陀螺航海惯导1东向速度误差δv_E1与双轴旋转调制激光陀螺航海惯导2东向速度误差δv_E2的差值，δv_N12＝δv_N1‑δv_N2为单轴旋转调制激光陀螺航海惯导1北向速度误差δv_N1与双轴旋转调制激光陀螺航海惯导2北向速度误差δv_N2的差值，δv_E13＝δv_E1‑δv_E3为单轴旋转调制激光陀螺航海惯导1东向速度误差δv_E1与双轴旋转调制激光陀螺航海惯导3东向速度误差δv_E3的差值，δv_N13＝δv_N1‑δv_N3为单轴旋转调制激光陀螺航海惯导1北向速度误差δv_N1与双轴旋转调制激光陀螺航海惯导3北向速度误差δv_N3的差值，δL₁₂＝δL₁‑δL₂为单轴旋转调制激光陀螺航海惯导1纬度误差δL₁与双轴旋转调制激光陀螺航海惯导2纬度误差δL₂的差值，δλ₁₂＝δλ₁‑δλ₂为单轴旋转调制激光陀螺航海惯导1经度误差δλ₁与双轴旋转调制激光陀螺航海惯导2经度误差δλ₂的差值，δL₁₃＝δL₁‑δL₃为单轴旋转调制激光陀螺航海惯导1纬度误差δL₁与双轴旋转调制激光陀螺航海惯导3纬度误差δL₃的差值，δλ₁₃＝δλ₁‑δλ₃为单轴旋转调制激光陀螺航海惯导1经度误差δλ₁与双轴旋转调制激光陀螺航海惯导3经度误差δλ₃的差值，ε_x1、ε_y1、ε_z1分别为单轴旋转调制激光陀螺航海惯导1体坐标系下相应坐标轴的陀螺常值漂移，ε_x2、ε_y2、ε_z2分别为双轴旋转调制激光陀螺航海惯导2体坐标系下相应坐标轴的陀螺常值漂移，ε_x3、ε_y3、ε_z3分别为双轴旋转调制激光陀螺航海惯导3体坐标系下相应坐标轴的陀螺常值漂移，分别为单轴旋转调制激光陀螺航海惯导1相应水平坐标轴的加速度计常值零偏，分别为双轴旋转调制激光陀螺航海惯导2相应水平坐标轴的加速度计常值零偏，分别为双轴旋转调制激光陀螺航海惯导3相应水平坐标轴的加速度计常值零偏，上标T表示转置；步骤二：分别建立单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2、单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导3间的联合误差状态方程，联合误差状态方程分别为其中，系统状态矩阵分别为：v_E、v_N分别为舰艇的东向速度、北向速度，ω_ie为地球自转角速度，L为舰艇的纬度位置，h为舰艇的高度，R_E、R_N分别为卯酉圈、子午圈曲率半径，f_E、f_N、f_U分别为东向、北向、天向比力值，分别为单轴旋转调制激光陀螺航海惯导1、双轴旋转调制激光陀螺航海惯导2和3各自的姿态矩阵，分别为单轴旋转调制激光陀螺航海惯导1、双轴旋转调制激光陀螺航海惯导2和3各自姿态矩阵的前两行前两列构成的子矩阵，0_i×j代表i行j列的零矩阵；系统噪声驱动矩阵分别为：系统噪声分别为：分别为单轴旋转调制激光陀螺航海惯导1体坐标系下相应坐标轴的陀螺噪声，分别为双轴旋转调制激光陀螺航海惯导2体坐标系下相应坐标轴的陀螺噪声，分别为双轴旋转调制激光陀螺航海惯导3体坐标系下相应坐标轴的陀螺噪声，分别为单轴旋转调制激光陀螺航海惯导1体坐标系下相应水平坐标轴的加速度计噪声，分别为双轴旋转调制激光陀螺航海惯导2体坐标系下相应水平坐标轴的加速度计噪声，分别为双轴旋转调制激光陀螺航海惯导3体坐标系下相应水平坐标轴的加速度计噪声；步骤三：分别建立单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2、单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导3间的观测方程，观测量分别为单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2及3间扣除杆臂效应后的速度之差、位置之差，观测方程分别为z₁₂(t)＝Hx₁₂(t)+v(t),z₁₃(t)＝Hx₁₃(t)+v(t)         (12)其中，z₁₂(t)＝[δv_E12 δv_N12 δL₁₂ δλ₁₂]^T,z₁₃(t)＝[δv_E13 δv_N13 δL₁₃ δλ₁₃]^T           (13)为观测矩阵，v(t)为观测噪声；z₁₂(t)、z₁₃(t)分别表示单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2、3间的观测量，表示单轴旋转调制激光陀螺航海惯导1的东向输出速度与双轴旋转调制激光陀螺航海惯导2的东向输出速度的差值，表示单轴旋转调制激光陀螺航海惯导1的北向输出速度与双轴旋转调制激光陀螺航海惯导2的北向输出速度的差值，表示单轴旋转调制激光陀螺航海惯导1的东向输出速度与双轴旋转调制激光陀螺航海惯导3的东向输出速度的差值，表示单轴旋转调制激光陀螺航海惯导1的北向输出速度与双轴旋转调制激光陀螺航海惯导3的北向输出速度的差值，表示单轴旋转调制激光陀螺航海惯导1的输出纬度与双轴旋转调制激光陀螺航海惯导2的输出纬度的差值，表示单轴旋转调制激光陀螺航海惯导1的输出经度与双轴旋转调制激光陀螺航海惯导2的输出经度的差值，表示单轴旋转调制激光陀螺航海惯导1的输出纬度与双轴旋转调制激光陀螺航海惯导3的输出纬度的差值，表示单轴旋转调制激光陀螺航海惯导1的输出经度与双轴旋转调制激光陀螺航海惯导3的输出经度的差值，I_2×2表示二阶单位矩阵；步骤四：确定系统噪声对单轴旋转调制激光陀螺航海惯导方位陀螺漂移估计值的影响，通过以下步骤实现：1).确定联合误差状态卡尔曼滤波器对应系统状态的稳态估计误差协方差矩阵扣除安装关系引起的杆臂误差的影响，单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2及3间的速度之差、位置之差理论值应为0，可近似为无观测噪声，无观测噪声条件下联合误差状态卡尔曼滤波器对应系统状态的稳态估计误差协方差矩阵P满足P＝(I‑KH)FPF^T(I‑KH)^T+(I‑KH)GQG^T(I‑KH)^T       (14)其中，K为卡尔曼滤波增益，Q为卡尔曼滤波器系统噪声协方差矩阵的设定值，F为系统状态矩阵，H为观测矩阵，G为系统噪声矩阵，I为单位矩阵；2).确定真实的系统噪声协方差矩阵与卡尔曼滤波器系统噪声协方差矩阵设定值间的关系为其中，α为未知标量，代表真实噪声协方差矩阵与设定值之间的差异；若α＞0表示真实的系统噪声协方差矩阵大于其设定值；若α＜0表示真实的系统噪声协方差矩阵小于其设定值；若α＝0表示真实的系统噪声协方差矩阵等于其设定值；3).确定真实的系统噪声协方差矩阵对应的系统状态的稳态估计误差协方差矩阵为4).确定真实的噪声协方差矩阵与卡尔曼滤波器系统噪声协方差矩阵设定值间的不一致导致的状态估计误差协方差所产生的变化ΔP为5).确定系统噪声对单轴旋转调制激光陀螺航海惯导方位陀螺漂移估计值的影响根据步骤4)中式(16)，如果真实的系统噪声协方差矩阵大于其设定值，即α＞0，则真实的系统状态估计误差的方差必大于标称的系统状态估计误差的方差；如果真实的系统噪声协方差矩阵小于其标称值，即α＜0，则真实的系统状态估计误差的方差必小于标称的系统状态估计误差的方差；对于稳态卡尔曼滤波器而言，系统状态估计误差的方差越小，相应的系统状态估计值的标准差越小，相应的单轴旋转调制激光陀螺航海惯导1方位陀螺漂移估计值的标准差越小；步骤五：分别根据单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2、3间的系统状态方程、观测方程进行卡尔曼滤波，对各自的系统状态进行估计，以两个卡尔曼滤波器稳定后单轴旋转调制激光陀螺航海惯导1的方位陀螺漂移估计值的标准差为评价指标完成双轴旋转调制激光陀螺航海惯导2、3间相对性能的在线评估，相同时间段内单轴旋转调制激光陀螺航海惯导1的方位陀螺漂移估计值的标准差更小的滤波器对应的双轴旋转调制激光陀螺航海惯导的随机误差更小，定位精度更高；单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导2构成的卡尔曼滤波器估计得到的单轴旋转调制激光陀螺航海惯导1的方位陀螺漂移估计值标准差σ₂(ε_z1)，单轴旋转调制激光陀螺航海惯导1与双轴旋转调制激光陀螺航海惯导3构成的卡尔曼滤波器估计得到的单轴旋转调制激光陀螺航海惯导1的方位陀螺漂移估计值标准差σ₃(ε_z1)分别为：每隔固定的时长计算一次单轴旋转调制激光陀螺航海惯导1方位陀螺漂移估计值的标准差，以此为指标比较两个滤波器对应双轴旋转调制激光陀螺航海惯导随机误差的大小；其中，N为该时间段内单轴旋转调制激光陀螺航海惯导1方位陀螺漂移估计值数目，为该时间段内单轴旋转调制激光陀螺航海惯导1方位陀螺漂移估计值的平均值；若σ₂(ε_z1)＜σ₃(ε_z1)，则双轴旋转调制激光陀螺航海惯导2的随机误差更小，定位精度更高，选择双轴旋转调制激光陀螺航海惯导2作为主惯导系统；若σ₂(ε_z1)＞σ₃(ε_z1)，则双轴旋转调制激光陀螺航海惯导3的随机误差更小，定位精度更高，选择双轴旋转调制激光陀螺航海惯导3作为主惯导系统；步骤六：以步骤五中估计得到的标准差更小的单轴旋转调制激光陀螺航海惯导1方位陀螺漂移为最后的估计值，对其造成的确定性的长期定位误差进行预测补偿，补偿方式为输出校正；单轴旋转调制激光陀螺航海惯导1方位陀螺漂移造成的确定性的长期定位误差的预测补偿步骤为：1).确定单轴旋转调制激光陀螺航海惯导1的误差状态x₁(t)为x₁(t)＝[φ_E1 φ_N1 φ_U1 δv_E1 δv_N1 δL₁ δλ₁]^T       (18)其中，φ_E1为单轴旋转调制激光陀螺航海惯导1的东向姿态误差，φ_N1为单轴旋转调制激光陀螺航海惯导1的北向姿态误差，φ_U1为单轴旋转调制激光陀螺航海惯导1的天向姿态误差，δv_E1为单轴旋转调制激光陀螺航海惯导1的东向速度误差，δv_N1为单轴旋转调制激光陀螺航海惯导1的北向速度误差，δL₁为单轴旋转调制激光陀螺航海惯导1的纬度误差，δλ₁为单轴旋转调制激光陀螺航海惯导1的经度误差；2).确定单轴旋转调制激光陀螺航海惯导1的误差状态方程中陀螺漂移、加速度计零偏构成的外界输入量u₁(t)为3).构建单轴旋转调制激光陀螺航海惯导1的误差状态方程其中，A₁(t)为系统状态矩阵，B₁(t)为外界输入矩阵，G₁(t)为系统噪声矩阵；为单轴旋转调制激光陀螺航海惯导1的陀螺噪声和加速度计噪声构成的系统噪声；4).离散化单轴旋转调制激光陀螺航海惯导1的误差状态方程(19)得到误差状态预测方程为其中，为系统状态x₁(t)的离散量，Φ₁(k)为系统状态矩阵A₁(t)的离散矩阵，Γ₁(k)为外界输入矩阵B₁(t)的离散矩阵，为外界输入量u₁(t)的离散量，k、k+1为离散化时刻，初始时刻5).根据误差状态预测方程对单轴旋转调制激光陀螺航海惯导1方位陀螺漂移造成的确定性的长期定位误差通过卡尔曼滤波进行预测补偿，补偿方式为输出校正。