[發明專利]一種面向無人船組合導航信息融合的多尺度數據分塊算法

在審

• 申請號: 201911287615.6
• 申請日: 2019-12-14
• 公開（公告）號: CN110926466A
• 公開（公告）日: 2020-03-27
• 發明（設計）人: 王寧;楊毅;陳帥
• 申請（專利權）人: 大連海事大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S19/42
• 代理公司: 大連東方專利代理有限責任公司 21212
• 代理人: 李洪福

1.一種面向無人船組合導航信息融合的多尺度數據分塊算法，其特征在于：包括以下步驟：

A、建立組合導航系統數學模型

針對無人船組合導航多傳感器信息融合問題，建立無人船組合導航系統的數學模型；所述無人船組合導航系統包括捷聯慣導系統傳感器即SINS傳感器、全球定位系統傳感器即GPS傳感器、三維電子羅盤傳感器即Compass傳感器和嵌入式處理器；所述嵌入式處理器內采用多尺度數據分塊算法進行傳感器信息融合，輸出無人船的位置、速度和姿態信息；采用SINS傳感器的誤差方程作為組合導航系統的狀態方程，導航坐標系選用東北天坐標系即NEU，通過對SINS傳感器的性能及誤差源分析，組合導航系統的狀態方程和量測方程分別描述為：

其中，為組合導航系統的狀態估計值；F(t)為組合導航系統的狀態轉移矩陣；X(t)為組合導航系統的狀態變量；W(t)為組合導航系統的白噪聲矩陣；i＝1、2、3，Z₁(t)為SINS/GPS子濾波器速度的量測值，Z₂(t)為SINS/GPS子濾波器位置的量測值，Z₃(t)為SINS/Compass子濾波器姿態的量測值，H_i(t)為組合導航系統的量測矩陣，V_i(t)為組合導航系統的量測噪聲，t為時間；

組合導航系統的狀態變量X(t)為：

式中，分別為無人船東、北、天方向的姿態誤差角；δV_E、δV_N、δV_U分別為無人船東、北、天方向的速度誤差；δλ、δL、δh分別為無人船的緯度、經度、高度的誤差；ε_bx、ε_by、ε_bz分別為無人船導航坐標系下陀螺儀東、北、天方向的常值漂移；ε_rx、ε_ry、ε_rz分別為無人船導航坐標系下陀螺儀東、北、天方向的慢變漂移；分別為無人船導航坐標系下加速度計東、北、天方向的常值隨機誤差；

無人船組合導航系統共有3組觀測值，分別為GPS傳感器測得的無人船速度與SINS傳感器測得的無人船速度之間的差值、GPS傳感器測得的無人船位置與SINS傳感器測得的無人船位置之間的差值、Compass傳感器測得的無人船姿態與SINS傳感器測得的無人船姿態之間的差值；

取GPS傳感器測得的無人船速度與SINS測得的無人船速度之間的差值作為SINS/GPS子濾波器速度的量測值，定義速度量測方程為：

式中：

V₁(t)＝[M_E,M_N,M_U]^T

其中，H₁(t)為SINS/GPS子濾波器速度的量測矩陣；V₁(t)為SINS/GPS子濾波器速度的量測噪聲，當做白噪聲處理；V_IE、V_IN、V_IU分別為SINS傳感器測得的無人船的東、北、天方向的速度；V_GE、V_GN、V_GU分別為GPS傳感器測得的無人船的東、北、天方向的速度；M_E、M_N、M_U分別為GPS傳感器測得的無人船的東、北、天方向的速度誤差；

取GPS傳感器測得的無人船位置與SINS傳感器測得的無人船位置之間的差值作為SINS/GPS子濾波器位置的量測值，定義位置量測方程為：

式中：

V₂(t)＝[N_N,N_E,N_U]^T

R_M＝R_e(1-2f+3f sin² L)

R_N＝R_e(1+f sin² L)

其中，H₂(t)為SINS/GPS子濾波器位置的量測矩陣；V₂(t)為SINS/GPS子濾波器位置的量測噪聲，當做白噪聲處理；N_N、N_E、N_U分別為GPS傳感器測得的無人船的緯度、經度、高度的誤差；λ_I、L_I、h_I分別為SINS傳感器測得的無人船的東、北、天方向的位置；λ_G、L_G、h_G分別為GPS傳感器測得的無人船的東、北、天方向的位置；R_e為地球赤道半徑，取6378137m；f為地球扁率，取1/298.257；O_3×6為3行6列的零矩陣；O_3×9為3行9列的零矩陣；L為緯度；

將Compass傳感器測得的無人船姿態與SINS傳感器測得的無人船姿態之間的差值作為SINS/Compass子濾波器的量測值，則觀測方程為：

式中：

H₃(t)＝[I_3×3,0_3×15]

V₃(t)＝[δα_c,δβ_c,δγ_c]^T

其中，H₃(t)為SINS/Compass子濾波器姿態的量測矩陣；V₃(t)為SINS/Compass子濾波器姿態的量測噪聲，是均值為零的觀測白噪聲；分別為SINS測得的無人船的東、北、天方向的姿態，分別為Compass測得的無人船的東、北、天的姿態；δα_E、δα_N、δα_U分別為Compass測得的無人船的東、北、天方向的姿態誤差；I_3×3為3行3列的單位矩陣，O_3×15為3行15列單位矩陣；

B、建立組合導航多尺度系統模型

B1、描述組合導航多尺度系統

對組合導航系統的狀態方程和量測方程進行離散化，根據多尺度理論，各傳感器以不同采樣率對數據進行觀測，在最高采樣率下即最細尺度上建立狀態方程，將組合導航多尺度系統模型描述為：

X_N(m+1)＝F_N(m)X_N(m)+W_N(m)m≥0

Z_d(m)＝H_d(m)X(m)+V_d(m)

其中，N＝3，傳感器d以不同的采樣率S_d進行觀測，且傳感器之間的的采樣是異步的，其中d＝1、2、3，分別表示SINS傳感器、GPS傳感器、Compass傳感器，SINS傳感器為最高采樣率的傳感器，即最細尺度傳感器；Compass傳感器為最低采樣率的傳感器，即最粗尺度的傳感器；GPS傳感器為中采樣率的傳感器，即中尺度的傳感器；其中，m為組合導航系統數學模型離散化后的時間；

各傳感器采樣率間的關系滿足：

S₃＝2S₂

S₂＝3S₁

X_N(m)表示m時刻在最高采樣率下的組合導航多尺度系統的狀態變量；F_N(m)表示m時刻在最高采樣率下的組合導航多尺度系統的一步轉移矩陣；W_N(m)表示m時刻在最高采樣率下的組合導航多尺度系統的噪聲，其為零均值高斯白噪聲，并滿足特點如下：

E{W_N(m)}＝0

其中，g是采樣時刻，δ_mg為克羅尼克函數、表示同一傳感器不同采樣時刻的的相關性；

Z_d(m)表示組合導航多尺度系統在m時刻的量測值；Z₁(m)、Z₂(m)、Z₃(m)分別為離散化后m時刻的SINS/GPS子濾波器速度的量測值、SINS/GPS子濾波器位置的量測值、SINS/Compass子濾波器姿態的量測值、H_d(m)表示組合導航多尺度系統m時刻的量測矩陣；V_d(m)表示組合導航多尺度系統m時刻的觀測噪聲，其中V_d(m)為高斯白噪聲序列，其協方差為R_d(m)，并滿足特點如下：

E{V_d(m)}＝0

E{V_d(m)W_N(m)}＝0

狀態變量的初始值X_N(0)為隨機向量，且有：

E{X_N(0)}＝X₀

E{[X_N(0)-X₀][X_N(0)-X₀]^T}＝P₀

B2、對組合導航系統進行數據分塊

針對組合導航系統各傳感器多速率異步采樣，建立組合導航系統多尺度系統模型，采用數據分塊的算法進行數據分塊，將一定長度的采樣時間內的狀態數據列為一個數據塊，對一個數據塊的狀態數據進行最優融合，定義數據塊的長度為：

即狀態向量和測量向量被分割成長度為M_d的數據塊；

進行數據分塊后的狀態方程表示為如下形式：

X_N(m+1)＝X_N(m)F_N(m)+W_N(m)

組合導航多尺度系統噪聲的協方差陣Q_i(m)表示為：

E{W_N(m)}＝0

B3、建立多尺度轉換關系

傳感器d的狀態信息在傳感器N的尺度上的有如下的轉換關系，所述的狀態信息包括位置、速度和姿態：

其中：

其中，為維矩陣，由個n維矩陣單位陣組成，其中n為3；

由上述推導得，傳感器d與傳感器N之間在尺度上有如下關系：

組合導航多尺度系統的量測方程為：

Z_d(m)＝H_d(m)X_d(m)+V_d(m)

其中，組合導航多尺度系統的量測值為：

組合導航多尺度系統的量測噪聲為：

組合導航多尺度系統的量測矩陣為：

E{V_d(m)}＝0

R_d(m)＝diag{R_d(mM+1),R_d(mM+2),...,R_d(mM)}

其中，j＝1、2、3；

C、設計最優融合算法

在多傳感器異步信息融合過程中，如果傳感器d在m時刻有測量值，那么m與n_d之間呈整數倍關系；通過用mod(m,n_d)等于零來判斷傳感器d在m時刻有無測量值，有如下兩種情況：

其中，n₁＝1,n₂＝3,n₃＝6；

C1、濾波更新1：若傳感器d在m時刻無測量值，則最細尺度上的融合估計值和相應的誤差協方差為：

由卡爾曼濾波得到m時刻的估計值和相應的誤差協方差P_N(m|m)；

狀態一步預測矩陣為：

狀態估計矩陣為：

濾波增益矩陣為：

一步預測均方誤差陣為：

P_N(m|m)＝[I-K_N(m)H_N(m)]P_N(m+1|m)

估計均方誤差陣為：

P_N(m|m-1)＝F_N(m-1)P_N(m-1|m-1)F_N(m-1)+Q_N(m-1)

C2、濾波更新2：若傳感器d在m時刻有測量值，則m時刻基于所有傳感器的最優估計為：

其中，

β₀(d,m)和β_p(d_p,m)為信息分配因子，分別表示如下：

p＝1,2

由卡爾曼濾波得到q_p時刻的估計值和相應的誤差協方差如下：

狀態一步預測矩陣為：

狀態估計矩陣為：

濾波增益矩陣為：

一步預測均方誤差陣為：

估計均方誤差陣為：

結束。