[發明專利]面向仿生偏振導航輔助定姿的分級卡爾曼融合方法及裝置

有效

• 申請號: 201911030649.7
• 申請日: 2019-10-28
• 公開（公告）號: CN110779514B
• 公開（公告）日: 2021-04-06
• 發明（設計）人: 孫婷;田澤宇;趙開春;韓文科
• 申請（專利權）人: 北京信息科技大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/02;G01C21/16;G01C21/20
• 代理公司: 北京清亦華知識產權代理事務所(普通合伙) 11201
• 代理人: 王艷斌

1.一種面向仿生偏振導航傳感器輔助定姿的分級卡爾曼融合方法，其特征在于，包括以下步驟：

將IMU、成像式仿生偏振光傳感器固定安裝在載體上，建立遵循右手定則的載體坐標系；

獲取IMU數據、方位角信息和所述載體坐標系下的太陽矢量信息；

計算以當地時間為標準的導航坐標系下的太陽矢量；

以IMU中陀螺儀的測量數據、四元數為系統狀態量，并基于四元數微分方程建立系統的狀態模型；

以所述IMU中加速計的測量數據、重力矢量作為輸入建立分級擴展卡爾曼融合系統的一級子系統測量模型；

以所述仿生偏振導航傳感器的輸出、所述導航坐標系下的太陽矢量為輸入建立分級擴展卡爾曼融合系統的二級子系統測量模型；

將一級子系統測量模型和二級子系統測量模型進行組合，得到完整的基于仿生偏振導航傳感器輔助定姿的分級擴展卡爾曼融合系統，包括：更新時間，并根據載體的初始時刻姿態確定四元數初值，設定初始化狀態變量和狀態變量的協方差矩陣P1₀；預測是依據第k-1次測量數據預測第k次測量的狀態量，為第k-1次測量估計的狀態量，A為分級擴展卡爾曼融合系統的狀態轉移矩陣；計算一級子系統的預測協方差矩陣P1_k|k-1、量測預測的誤差M1_k|k-1和量測轉移矩陣H1_k；根據所述預測協方差矩陣P1_k|k-1、所述量測預測的誤差M1_k|k-1和所述量測轉移矩陣H1_k更新所述一級子系統中的卡爾曼增益，估計所述一級子系統狀態值，并更新所述一級子系統協方差矩陣；根據更新的X＝[q₀ q₁ q₂ q₃]^T解算出k時刻的載體坐標系相對于導航坐標系的橫滾角γ和俯仰角θ，q₀、q₁、q₂和q₃為四元數；計算二級子系統的預測協方差矩陣P2_k|k-1、量測預測的誤差M2_k|k-1和量測轉移矩陣H2_k；根據所述預測協方差矩陣P2_k|k-1、所述量測預測的誤差M2_k|k-1和所述量測轉移矩陣H2_k更新二級系統中的卡爾曼增益，估計所述二級子系統狀態值更新所述二級子系統協方差矩陣；根據更新的X＝[q₀ q₁ q₂ q₃]^T解算出k時刻載體系相對于導航系的偏航角；

解算出三軸的姿態角度，得到三軸姿態測量結果。

2.根據權利要求1所述的方法，其特征在于，所述導航坐標系下的太陽矢量的表示公式為：

其中，s_x＝cos(h_s)sin(a_s)，s_y＝cos(h_s)cos(a_s)，s_z＝sin(h_s)，導航坐標系中的X軸指向西、Y軸指向南、Z軸指向天頂，其中，

h_s＝arcsin(sinδsinβ+cosδcosβcost)，

其中，δ表示太陽赤緯，β表示觀測點的緯度，t表示太陽時角。

3.根據權利要求1所述的方法，其特征在于，所述系統狀態模型的建立公式為：

其中，X_k為分級擴展卡爾曼融合系統的待估計的參數，A為系統的狀態轉移矩陣，W_k-1是系統噪聲。

4.根據權利要求3所述的方法，其特征在于，所述一級子系統測量模型和所述二級子系統測量模型為：

其中，V_1k是量測噪聲，噪聲為高斯白噪聲，V_1k的協方差矩陣為R_1k，V_2k是二級子系統量測噪聲，噪聲為高斯白噪聲，V_2k的協方差陣為R_2k。