[发明专利]基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法

摘要

本发明提供了一种基于信息预评判及导航结果补偿修正的SINS/DVL/ES组合导航方法。本发明在SINS/DVL组合前对DVL的数据有效性进行判断，当DVL数据信息无效时，DVL导航信息不参与组合导航；同时，本发明将残差x2检测法引入，对组合导航结果是否平稳进行评估，并设计了组合导航结果振荡时的补偿修正方法对Kalman输出结果进行实时评判和修正补偿。本发明既能有效抑制SINS/DVL组合导航姿态、速度的发散速度，提高组合系统姿态、速度精度，在一定程度上提高位置精度；又能解决在DVL数据信息质量下降所带来姿态误差的振荡问题。 1

主权项

1.一种基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法，其特征在于，所述基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法为基于SINS/DVL/ES组合导航系统的导航方法，该系统系统包括多普勒速度计程仪、测深仪、SINS、SINS/DVL/ES信息预评判模块、Kalman滤波处理模块、组合导航补偿修正评判模块和补偿修正处理模块；所述的导航方法包括以下步骤：1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号D_ES，来自DVL的速度信号V_DVL和来自捷联SINS的姿态信号当SINS/DVL/ES信息预评判模块接收到DVL的速度信号V_DVL数据时，进行以下两个判断：a.D_ES是否小于阈值D_(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)；2)Kalman滤波模块接收来自SINS模块的速度V_ins，姿态信号，如果根据步骤1)中a、b条件同时满足时则Kalman滤波模块接收DVL的速度信号V_DVL，若a、b条件不同时满足则断开DVL与Kalman滤波模块；3)组合导航补偿修正模块判定是否对Kalman滤波处理模块的输出结果进行修正处理；并将其输出结果送至SINS；最后由SINS修正补偿输出经过修正补偿后的速度V_ins′，位置P_ins′，姿态导航信息，具体为：3.1)对Kalman滤波器的残差rk作如下二元假设：H0表示无故障，H1表示有故障；无故障时：E{r_k}＝0，其中Z_k∈R⁶为观测量，H_k为观测矩阵，为预报值，φ_k/k‑1为从k‑1时刻到k时刻的一步状态转移矩阵，A_k为方差；有故障时：E{rk}＝μ，E{(rk‑μ)(rk‑μ)T}＝Ak；故障检测函数为其中，γ_k是服从自由度为m的x²分布，即γ_k～χ²(m)，m为观测量Z_k的维数；3.2)在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理：若γk≤TD，判定不需要修正；其中TD由Pf＝P[γk>TD/H0]＝α代入预设的误检率Pf解算得到；若γk>TD，判定需要修正；此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行修正：(a)若Pout(k)‑Pout(k‑1)≥△PD时，其中△PD为相邻滤波修正时刻位置变化率，Pout(k)为k时刻的位置信息输出值，Pout(k‑1)为k‑1时刻的位置信息输出值，则Pout(k)＝Pout(k‑1)+ΔP·Tn，其取值依据为△P小于载体实际机动的位置变化率,Tn为滤波周期；同理当Vout(k)‑Vout(k‑1)≥ΔVD时，有Vout(k)＝Vout(k‑1)+ΔV·Tn，其中ΔVD为相邻滤波周期的速度变化率，Tn为滤波周期；Pout(k)为k时刻的位置信息输出值，Pout(k‑1)为k‑1时刻的位置信息输出值；(b)若Pout(k)‑Pout(k‑1)≤‑ΔPD，Vout(k)‑Vout(k‑1)≤‑ΔVD时，则Pout(k)＝Pout(k‑1)‑ΔP·Tn，Vout(k)＝Vout(k‑1)‑ΔV·Tn；(c)若|Pout(k)‑Pout(k‑1)|＜ΔPD，|Vout(k)‑Vout(k‑1)|＜ΔVD时，则直接使用Kalman滤波的误差估计值进行补偿修正，公式为其中为k时刻的位置修正值，X_p(k)为k时刻Kalman滤波位置估计值，为k时刻的速度修正值，X_V(k)为k时刻Kalman滤波速度估计值，当γk其中P_ins(k)、V_ins(k)分别为SINS解算位置、速度值；