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

有效

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

本發明公開了一種快速收斂的慣性測量單元安裝姿態估計方法，涉及慣性導航/衛星導航組合導航技術領域。本發明將車載的低成本消費級慣性測量單元的安裝姿態角分解為一個大角度的先驗安裝姿態角和一個小角度的失準角，采用粒子濾波器估計大角度的先驗安裝姿態角，同時采用卡爾曼濾波器估計小角度的失準角。通過本發明的實施，能夠完成對慣性測量單元安裝姿態角的高精度估計，且具有較快的收斂時間；解決了當前已有方法中限制安裝姿態角為小角度或收斂時間較長等問題；將本發明應用于所述組合導航系統可以顯著提升其導航精度，具有較大的實用價值和可觀的經濟效益。

技術領域

本發明屬于慣性導航/衛星導航組合導航技術領域，具體涉及一種快速收斂的慣性測量單元安裝姿態估計方法。

背景技術

對于采用低成本消費級慣性測量單元的車載慣性導航/衛星導航組合導航系統，在衛星導航信號較弱或中斷時，導航解算的誤差會迅速增大。采用車輛非完整約束(車輛在正常行駛時側向和垂直方向速度分量為0)對系統進行輔助，可以在衛星導航信號較弱或中斷時實時校正導航解算的誤差，從而有效解決該場景下導航解算誤差迅速增大的問題。慣性測量單元相對于車輛的安裝姿態信息是正確應用車輛非完整約束的必要條件，但在實際應用中，通常無法精確測定該安裝姿態角，而必須采用一些估計的方法來獲取該安裝姿態角的值。現有文獻中提到的方法大多都存在局限性，例如，VINANDE等人的論文中提出了一種采用加速度計數據對慣性測量單元的安裝姿態進行估計的方法，但該方法應用于低成本消費級慣性測量單元時，具有較大誤差，且收斂時間較慢(幾分鐘)；又如，WU等人的論文中也提出過一種基于卡爾曼濾波器對慣性測量單元的安裝姿態進行估計的方法，但該方法限制安裝姿態角為小角度，在實際應用中，這樣的要求可能無法滿足。

因此，本領域的技術人員致力于開發一種快速收斂的慣性測量單元安裝姿態估計方法，以達到提高慣性測量單元安裝姿態角估計精度的目的。

發明內容

有鑒于現有技術的上述缺陷，本發明所要解決的技術問題是如何解決車載低成本消費級慣性測量單元無法精確估計安裝姿態角且收斂時間較長的問題。

為實現上述目的，本發明公開了一種快速收斂的慣性測量單元安裝姿態估計方法，涉及慣性導航衛星導航組合導航技術領域，包括以下步驟：

符號約定：ψ_βα表示β坐標系到α坐標系的歐拉角(姿態角)，表示β坐標系到α坐標系的旋轉矩陣；i表示慣性坐標系，e表示地心地固坐標系，b表示慣性測量單元坐標系(原點位于慣性測量單元中心，坐標軸與慣性測量單元的三個敏感軸重合)，b⁰表示先驗慣性測量單元坐標系(與慣性測量單元坐標系相差一個小角度歐拉角的坐標系)，v表示車體坐標系(原點位于車輛后輪中心，x軸指向車輛正前方，y軸指向車輛正右方，z軸指向車輛正下方)，a表示GNSS天線坐標系(原點位于所述GNSS天線相位中心)；/表示量x的估計值，/表示量x的測量值。

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

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

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

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

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

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