[發明專利]基于因子圖的動態分配與校正多源信息融合方法

有效

• 申請號: 201910177064.1
• 申請日: 2019-03-08
• 公開（公告）號: CN109883426B
• 公開（公告）日: 2022-01-14
• 發明（設計）人: 王偉;賈通;李欣;黃平
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01S19/45;G01S19/47
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.基于因子圖的動態分配與校正多源信息融合方法，其特征在于，該方法包括以下步驟：

步驟1：建立導航系統的數學模型；

所述的建立導航系統的數學模型包括系統方程和量測方程，系統方程根據間接法選取系統15維狀態量：

其中，為姿態角誤差；δV＝[δV_e δV_n δV_u]^T為速度誤差；δp＝[δL δλ δh]^T為位置誤差；b_g＝[b_gx b_gy b_gz]^T為陀螺常值漂移；b_a＝[b_ax b_ay b_az]^T為加速度計常值偏置；

系統誤差模型使用小失準角條件下的線性誤差模型：

設系統的狀態方程為：

式中，F(t)為狀態轉移矩陣；w(t)為系統狀態轉移的噪聲；根據Gauss假設，將其定義為白噪聲；則系統的噪聲協方差矩陣Q為：

式中，S_rg、S_ra為陀螺與加速度計的噪聲功率譜密度構成的對角矩陣；

量測方程包括：GPS量測方程、EC量測方程以及OD量測方程，其中GPS量測方程表示為下式：

式中，v₁(t)為GPS速度、位置誤差噪聲；H_GPS為GPS量測矩陣：

EC量測方程表示為下式：

式中，Z_EC為姿態誤差，包括航向、俯仰、橫滾誤差；v₂(t)為高斯白噪聲；H_EC為EC量測矩陣，H_EC(t)＝[I_3×3 0_3×3 0_3×3 0_3×3 0_3×3]；M為誤差角轉換矩陣，表示由姿態誤差角轉換為平臺失準角：

OD量測方程表示為下式：

式中，v₃(t)為OD速度、位置誤差噪聲；H_OD為OD量測矩陣：

步驟2：使用多傳感器信息對遲滯量測量進行補償；

設INS、GPS、OD、EC的采樣時間分別為T_INS、T_GPS、T_OD、T_EC；設GPS滯后的時間為ΔT，以INS進行插值時由于GPS滯后時間內INS更新次數為k＝ΔT/T_INS，在滯后時間內系統狀態轉移的協方差矩陣為Q_ΔT＝kQ；INS的補償量與OD的補償量分配系數分別為α_INS(α_v1,α_v2,α_v3,α_p1,α_p2,α_p3)、β_OD(β_v1,β_v2β_v3,β_p1,β_p2,β_p3)，且α+β＝1；

分別計算出相鄰時間t_i～t_j內載體的近似線加速度與

根據各自線加速度分別計算出速度與位置補償量：

最終的補償量為兩者加權融合得到：

步驟3：使用即插即用因子圖模型建立系統狀態轉移與量測的概率模型；

系統的狀態空間描述為：

組合導航系統的估計問題被轉化為一階馬爾可夫鏈，由馬爾可夫鏈的性質得：

式中，f(X_i|X_i-1)服從高斯分布，f(X_i|X_i-1)的均值與方差分別為A_i-1X_i-1，f(Z_i|X_i)服從高斯分布，f(Z_i|X_i)的均值與方差分別為C_iX_i與

由系統的概率模型得：

p(X_ik|Z_k)＝p(X_ik|X_i-1)p(Z_k|X_ik)

式中p(X_ik|X_i-1)＝1/n(p(X_i|X_i-1))，認為每個量測與狀態量融合的結果的精度是等概率的，于是其對應協方差為：

σ²(X_ik|X_i-1)＝n(σ²(X_i|X_i-1))

由高斯分布得：

式中，均值方差

步驟4：對所有可用量測值進行數據融合得到最優估計；

根據上式可得各狀態量與量測融合的結果再根據每一個子系統所包含的變量的均值與方差進行融合：