[發明專利]一種多加速度計慣導系統的組合導航方法

有效

• 申請號: 202110320397.2
• 申請日: 2021-03-25
• 公開（公告）號: CN113267183B
• 公開（公告）日: 2023-01-06
• 發明（設計）人: 周一覽;趙帥;舒曉武
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 鄭海峰

1.一種多加速度計慣導系統的組合導航方法，其特征在于，所述的多加速度計慣導系統包括一體化三軸陀螺(7)、三軸中心加速度計、三軸輔助加速度計和數據采集處理板(8)；所述的一體化三軸陀螺位于載體中心位置，所述的三軸中心加速度計分別安裝在一體化三軸陀螺的三個坐標軸正方向上，所述的三軸輔助加速度計分別安裝在一體化三軸陀螺的三個坐標軸負方向上，所述的一體化三軸陀螺、三軸中心加速度計、三軸輔助加速度計的信號輸出端連接數據采集處理板；所述的三軸中心加速度計和三軸輔助加速度計的安裝位置與載體中心的距離相等；

所述的組合導航方法包括：

1)通過數據采集處理板(8)同步采集多加速度計慣導系統中的三軸中心加速度計、三軸輔助加速度計輸出的比力值，以及一體化三軸陀螺(7)輸出的載體三軸角速度值；

2)根據一體化三軸陀螺(7)輸出的載體三軸角速度值判斷多加速度計慣導系統的運動狀態，若三軸角速度值大于閾值，則判斷為處于高運動狀態，進入步驟3)，利用多加速度計對三軸角速度值進行動態補償；否則，判斷為處于低運動狀態，將一體化三軸陀螺(7)輸出的載體三軸角速度值作為最終角速度值，進入步驟4)；

3)將利用加速度計桿臂效應解算出的載體三軸角速度值與一體化三軸陀螺輸出的載體三軸角速度值相融合，得到姿態角修正值作為最終動態補償后的角速度值，進入步驟4)；

4)利用三軸中心加速度計、三軸輔助加速度計輸出的比力值，計算載體中心的三軸比力，與低運動狀態下一體化三軸陀螺輸出的載體三軸角速度值或高運動狀態下步驟3)得到的姿態角修正值進行組合導航更新。

2.根據權利要求1所述的一種多加速度計慣導系統的組合導航方法，其特征在于，所述的步驟3)具體為：

3.1)利用加速度計桿臂效應解算出載體三軸角速度值，計算公式為：

其中，f₁、f₂、f₃分別為三軸中心加速度計輸出的對應于X軸、Y軸、Z軸的比力值，f₄、f₅、f₆分別為三軸輔助加速度計輸出的對應于X軸、Y軸、Z軸的比力值，分別為根據加速度計解算出的X軸角加速度、Y軸角加速度、Z軸角加速度，ω_xa、ω_ya、ω_za分別為根據加速度計解算出的X軸角速度、Y軸角速度、Z軸角速度，ΔT為數據采集處理板的采樣時間，l表示三軸中心加速度計或三軸輔助加速度計與坐標系中心的距離；

3.2)將一體化三軸陀螺輸出的載體三軸角速度值與根據加速度計解算出的載體三軸角速度值的差值作為魯棒容積卡爾曼濾波器的輸入，根據濾波增益得到姿態角修正值，計算公式為：

Δω_x＝ω_x-ω_xa

Δω_y＝ω_y-ω_ya

Δω_z＝ω_z-ω_za

式中，ω_x、ω_y、ω_z為一體化三軸陀螺輸出的載體三軸角速度值，Δω_x、Δω_y、Δω_z為三軸角速度差值，K_k為濾波增益，ω_xx、ω_yy、ω_zz為姿態角修正值。

3.根據權利要求2所述的一種多加速度計慣導系統的組合導航方法，其特征在于，利用魯棒容積卡爾曼濾波器隨采樣時間更新后的濾波增益，實現隨時間變化的動態補償，具體為：

濾波器狀態更新：

x_k+1＝f(x_k,u_k)+w_k

z_k+1＝h(x_k+1)+v_k+1

式中，f(·)為狀態方程，h(·)為量測方程，x、u、z分別對應狀態變量、控制變量及量測變量，w為系統噪聲、v為量測噪聲，下角標k表示當前時刻，下角標k+1表示下一時刻；

濾波器時間更新：

式中，Q為系統噪聲的協方差矩陣，N為預設的容積點數量，為容積點為i時對應的k-1時刻的系統噪聲，為k-1時刻到k時刻的狀態變量一步估計值，為容積點為i時對應的k-1時刻的狀態變量值，Q_k-1為k-1時刻系統噪聲的協方差矩陣，上角標T代表轉置，P_k∣k-1為k-1時刻到k時刻的一步預測誤差協方差矩陣；

濾波器量測更新：

式中，R_k為k時刻量測噪聲的協方差矩陣，為k-1時刻到k時刻的量測變量一步估計值，為容積點為i時對應的k-1時刻到k時刻的系統噪聲，為容積點為i時對應的k-1時刻到k時刻的狀態變量值，為k-1時刻到k時刻的自相關協方差矩陣，為k-1時刻到k時刻的互相關協方差矩陣；

濾波增益更新：

式中，K_k表示k時刻的濾波增益，為k時刻的自相關協方差矩陣，為k時刻的互相關協方差矩陣。