[發明專利]基于信息預評判及補償修正的SINS/DVL/ES組合導航方法

有效

• 申請號: 201610010208.0
• 申請日: 2016-01-07
• 公開（公告）號: CN105783940B
• 公開（公告）日: 2018-06-19
• 發明（設計）人: 徐曉蘇;童金武;張濤;李瑤;王捍兵;孫進;劉義亭;姚逸卿;楊冬瑞;陳立平;石宏飛;金博楠
• 申請（專利權）人: 東南大學
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/16;G01S7/40;G01S13/58;G01S13/86;G01S13/88
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 李昊

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濾波。