[發明專利]一種考慮撓曲變形和桿臂效應的大失準角傳遞對準方法

有效

• 申請號: 202110022114.6
• 申請日: 2021-01-08
• 公開（公告）號: CN112649022B
• 公開（公告）日: 2022-05-27
• 發明（設計）人: 張亞;高偉;邵劍波
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 鄧宇

1.一種考慮撓曲變形和桿臂效應的大失準角傳遞對準方法，其特征在于，包括以下步驟：

步驟一：在運載體上安裝高精度主慣導系統和精度較低的子慣導系統，完成啟動、預熱準備；

步驟二：計算主、子慣導的載體坐標系至慣性凝固坐標系之間的轉換矩陣，通過該矩陣將主、子慣導的比力及角速度信息投影至慣性凝固坐標系，然后將投影后的主慣導比力及角速度傳輸到子慣導計算機；

步驟三：將主、子慣導慣性凝固坐標系下的比力進行積分，選取主、子慣導之間的比力積分差值、角速度差值作為量測信息，構建基于羅德里格參數的撓曲變形和動態桿臂效應一體化非線性傳遞對準模型；

步驟四：利用邊緣平方根容積卡爾曼濾波器(Marginalized Square-root CubatureKalman Filter,MSRCKF)進行濾波，估計子慣導失準角，修正子慣導姿態，完成傳遞對準；

步驟三所述的構建基于羅德里格參數的撓曲變形和動態桿臂效應一體化非線性傳遞對準模型，具體方法為：

模型狀態向量選取為

各個狀態的狀態方程為

式中，

其中，為與失準角對應的羅德里格參數；ε_s為子慣導陀螺常值漂移；為加速度計常值零偏；為i_m系下的動態桿臂加速度積分；θ＝[θ_x θ_y θ_z]^T為撓曲變形角；μ＝[μ_x μ_y μ_z]^T為撓曲變形角速率；β＝2.146/τ，τ＝[τ_x τ_y τ_z]^T為相關時間；w_f為激勵白噪聲,其方差滿足Var(w_f)＝Q_f＝4β³σ²，其中σ＝[σ_x σ_y σ_z]^T為撓曲變形角方差；

根據上述狀態方程，構建系統狀態模型

式中，過程噪聲狀態轉移矩陣F表示為

其中，A_w1＝diag(-β²)；A_w2＝diag(-2β)；

過程噪聲驅動矩陣G表示為

量測方程為

式中，分別為及的積分；w_k表示由主慣導剛性桿臂補償殘余誤差、慣性器件誤差的積分引起的量測噪聲；

步驟四所述的利用邊緣平方根容積卡爾曼濾波器進行濾波，具體方法為：

1)時間更新過程

計算狀態一步預測以及狀態一步預測均方誤差陣平方根因子S_k/k-1

S_k/k-1＝Tria([F_k/k-1S_k-1 S_Q,k-1])

式中，Tria(·)表示經QR分解獲得的下三角矩陣，S_Q,k-1表示過程噪聲矩陣Q_k-1的平方根因子，即

2)量測更新過程

根據及S_k/k-1計算容積點

式中，ξ_i表示ξ的第i列，I_n表示n階單位矩陣；

容積點經過非線性量測方程傳播，計算量測預測容積點

計算量測預測值

計算新息協方差矩陣的平方根因子

S_zz,k/k-1＝Tria([η_k/k-1 S_R,k])

式中，S_R,k表示R_k的平方根因子，即

計算互協方差矩陣

式中

計算濾波增益

計算狀態估計

計算狀態估計均方誤差陣的平方根因子

S_k＝Tria([χ_k/k-1-K_kη_k/k-1 K_kS_R,k])。