[發明專利]一種快速收斂的慣性測量單元安裝姿態估計方法

有效

• 申請號: 202010062352.5
• 申請日: 2020-01-19
• 公開（公告）號: CN111076721B
• 公開（公告）日: 2023-03-28
• 發明（設計）人: 馮木櫸;翟昆朋;藺曉龍;高迪;何文濤
• 申請（專利權）人: 浙江融芯導航科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18;G01S19/49
• 代理公司: 上海旭誠知識產權代理有限公司 31220
• 代理人: 鄭立

1.一種快速收斂的慣性測量單元安裝姿態估計方法，其特征在于，包括以下步驟：

步驟一、設定粒子數N以及終止閾值N₀；

步驟二、初始化粒子群及權重序列/各粒子的狀態向量/均勻取自慣性測量單元在其可能的安裝空間內的安裝姿態角，權重w_j取為等權重：w_j＝1/N；

步驟三、置循環變量k←1，所述循環變量代表組合導航卡爾曼濾波器的迭代時刻；

步驟四、用每個粒子對應的先驗安裝姿態角作為所述組合導航卡爾曼濾波器的參數，產生N個不同的卡爾曼濾波器的實例，然后同時運行所述N個實例；

步驟五、讀取k時刻各實例所述狀態向量估計值，根據下面公式計算每個實例對應的GNSS天線速度的估計值

上式右側的{·}_j[k]表示，花括號內各估計量取第j個所述卡爾曼濾波器實例在k時刻的值，為k時刻陀螺儀的角速度測量值，/為由第j個所述卡爾曼濾波器參數/計算得到的先驗旋轉矩陣；

步驟六、讀取k時刻所述GNSS天線速度測量值及其精度σ_v[k]；

步驟七、根據下面公式更新每個粒子的權重：

并歸一化：

w_j←Aw_j，j＝1，2，…，N；

其中

步驟八、根據下面公式計算有效粒子數：

如果有效粒子數小于N₀則轉步驟九，否則置k←k+1，然后轉步驟五；

步驟九、置找出權重最大的粒子的序號；

步驟十、取第j_m個所述卡爾曼濾波器實例在k時刻的所述狀態向量的分估計值/根據歐拉角ψ_βα＝[φ，θ，ψ]^T到旋轉矩陣/的計算公式：

計算對應的旋轉矩陣：

然后由第j_m個粒子的所述狀態向量計算對應的旋轉矩陣：

最后計算v坐標系相對于b坐標系的旋轉矩陣估計值：

步驟十一、根據旋轉矩陣到歐拉角/的計算公式：

計算v系相對于b系的歐拉角：

這個姿態角即為所述慣性測量單元的所述安裝姿態角估計值。

2.根據權利要求1所述的快速收斂的慣性測量單元安裝姿態估計方法，其特征在于，所述步驟四中所述組合導航卡爾曼濾波器，其結構與迭代過程還包括：根據狀態轉移矩陣、觀測矩陣、狀態轉移噪聲矩陣、觀測噪聲矩陣，按標準卡爾曼濾波流程進行迭代：

預測：

誤差協方差矩陣的傳播：

P^-[k]＝Φ[k-1]P[k-1]Φ[k-1]^T+Q；

卡爾曼增益矩陣：

K[k]＝P^-[k]H[k]^T(H[k]P^-[k]H[k]^T+R[k])^-1；

所述狀態向量修正：

所述誤差協方差矩陣的修正：

P[k]＝(I-K[k]H[k])P^-[k]。

3.根據權利要求1所述的快速收斂的慣性測量單元安裝姿態估計方法，其特征在于，所述步驟四中所述組合導航卡爾曼濾波器，其結構與迭代過程還包括：所述卡爾曼濾波器以給定的所述先驗安裝姿態角作為參數，由所述先驗安裝姿態角按下面公式計算所述先驗旋轉矩陣/和/

4.根據權利要求1所述的快速收斂的慣性測量單元安裝姿態估計方法，其特征在于，所述步驟四中所述組合導航卡爾曼濾波器，其結構與迭代過程還包括：取b坐標系原點相對于地心地固坐標系的位置、速度、姿態誤差，陀螺儀和加速度計的零偏，失準角均投影到v系的a系到b系的桿臂以及v系到b系的桿臂組成24維所述狀態向量：