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

权利要求书

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滤波器的残差r_k作如下二元假设：H₀表示无故障，H₁表示有故障；

无故障时：

E{r_k}＝0，

其中Z_k∈R⁶为观测量，H_k为观测矩阵，为预报值，φ_k/k-1为从k-1时刻到k时刻的一步状态转移矩阵，A_k为方差；

有故障时：

E{r_k}＝μ，E{(r_k-μ)(r_k-μ)^T}＝A_k；

故障检测函数为其中，γ_k是服从自由度为m的x²分布，即γ_k～χ²(m)，m为观测量Z_k的维数；

3.2)在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理：若γ_k≤T_D，判定不需要修正；其中T_D由P_f＝P[γ_k>T_D/H₀]＝α代入预设的误检率P_f解算得到；

若γ_k>T_D，判定需要修正；此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行修正：

(a)若P_out(k)-P_out(k-1)≥△P_D时，其中△P_D为相邻滤波修正时刻位置变化率，P_out(k)为k时刻的位置信息输出值，P_out(k-1)为k-1时刻的位置信息输出值，则P_out(k)＝P_out(k-1)+ΔP·T_n，其取值依据为△P小于载体实际机动的位置变化率,T_n为滤波周期；

同理当V_out(k)-V_out(k-1)≥ΔV_D时，有V_out(k)＝V_out(k-1)+ΔV·T_n，其中ΔV_D为相邻滤波周期的速度变化率，T_n为滤波周期；P_out(k)为k时刻的位置信息输出值，P_out(k-1)为k-1时刻的位置信息输出值；

(b)若P_out(k)-P_out(k-1)≤-ΔP_D，V_out(k)-V_out(k-1)≤-ΔV_D时，

则P_out(k)＝P_out(k-1)-ΔP·T_n，V_out(k)＝V_out(k-1)-ΔV·T_n；

(c)若|P_out(k)-P_out(k-1)|＜ΔP_D，|V_out(k)-V_out(k-1)|＜ΔV_D时，则直接使用Kalman滤波的误差估计值进行补偿修正，公式为

其中为k时刻的位置修正值，X_p(k)为k时刻Kalman滤波位置估计值，为k时刻的速度修正值，X_V(k)为k时刻Kalman滤波速度估计值，

当γ_k<T_D时，发生在DVL信号有效的情况时，

令P_out(k)、V_out(k)初始值为0；

此阶段不需要对Kalman滤波处理模块的输出结果进行修正，

其中P_ins(k)、V_ins(k)分别为SINS解算位置、速度值；

2.根据权利要求1所述的SINS/DVL/ES组合导航方法，其特征在于，所述步骤1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号，来自DVL的速度信号V_DVL和来自捷联SINS的姿态信号；当SINS/DVL/ES信息预评判模块接收到DVL的速度信号V_DVL数据时，进行以下两个判断：a.D_ES是否小于阈值D_(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)，具体包括以下步骤：

1.1)SINS/DVL/ES信息预评判模块接收来自测深仪发送的深度信号D_ES进行判断，如果D_ES小于阈值D_(th)，其中D_(th)的值为所述SINS/DVL/ES组合导航系统的DVL产品的D_(th)值，说明当前DVL所提供的速度数据V_DVL能真实反应载体相对于海底的运动速度，DVL将V_DVL送至Kalman滤波处理模块，运行1.2)，否则说明当前DVL的速度数据V_DVL无效，V_DVL测得的数据不能真实反应载体相对于海底的运动速度，而是相对于水流的速度，此时立即停止DVL的速度数据V_DVL参与信息融合，程序重新运行1.1)进行下一次阈值比较，进入下一轮评判；

1.2)SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，当纵摇角增量Δθ大于纵摇角增量阀值Δθ(th)时，所述Δθ(th)由具体的DVL和系统设计要求决定，AUV的瞬间纵摇上界为8度；说明AUV的瞬间纵摇变化将影响DVL的测速精度，如果AUV的纵摇角增量小于上界值时DVL将V_DVL送至Kalman滤波处理模块，否则V_DVL数据不参与Kalman滤波。