[發明專利]一種用于提高慣性-地磁組合靜動態綜合性能的方法

有效

• 申請號: 201710270354.1
• 申請日: 2017-04-24
• 公開（公告）號: CN107084722B
• 公開（公告）日: 2020-02-14
• 發明（設計）人: 戎海龍;彭翠云;呂繼東;周炯如
• 申請（專利權）人: 常州大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種用于提高慣性-地磁組合靜動態綜合性能的方法，其特征在于：包括陀螺儀、加速度計和地磁傳感器，并包括以下步驟，

步驟1：由三軸加速度計輸出構成三維矢量a_i＝[a_ix a_iy a_iz]^T，其中a_ij(j＝x,y,z)為i時刻j軸向加速度計輸出值，由地磁傳感器輸出構成三維矢量m_i＝[m_ix m_iy m_iz]^T，其中，m_ij(j＝x,y,z)為i時刻j軸向地磁傳感器輸出值，由三軸陀螺儀輸出構成三維矢量ω_i＝[ω_ixω_iy ω_iz]^T，其中ω_ij(j＝x,y,z)為i時刻j軸向陀螺儀輸出值，由a_i和m_i獲得a_i×m_i，作為卡爾曼算法的輸入；

步驟2：利用卡爾曼算法實現對||a_i×m_i||的準確估計，其中卡爾曼算法的實現流程如下，

首先設x＝a_i×m_i，然后構建如下方程組，

其中

其中，CWT＝cos(ω_i0T)，SWT＝sin(ω_i0T)，TCWT＝1-CWT，T為采樣周期；

噪聲ξ(i)和η(i)的協方差陣計算方法為：

其中Q、R為常量，δ_ik為狄拉克函數；

待x估計出之后，再計算||a_i×m_i||，計算方法為，

步驟3：估計出的||a_i×m_i||隨后作為隱馬爾科夫模型的輸入；隱馬爾科夫模型的訓練方法為，使載體處于靜止狀態，然后通過步驟2獲得載體處于靜止狀態的觀測序列最后利用Baum-Welch算法對隱馬爾科夫模型進行訓練從而獲得模型參數λ₀＝(A₀,B₀,π₀)；載體處于運動狀態時，通過步驟2獲得載體處于運動狀態的觀測序列與所建立的隱馬爾科夫模型λ₀吻合的概率利用Forward算法計算出，并且有P(||a_i×m_i|||λ₀)是指觀測序列||a_i×m_i||與所建立的隱馬爾科夫模型λ₀匹配的概率，用于判斷載體是否運動并因而用于調整陀螺儀的權值；

步驟4：對于擴展卡爾曼算法，陀螺儀權值的調整方法是，

其中Q和R分別為調整前的卡爾曼算法的過程噪聲和觀測噪聲的協方差陣，而Q′和R′分別為調整后的卡爾曼算法的過程噪聲和觀測噪聲的協方差陣；

對于互補濾波算法，則P(||a_i×m_i|||λ₀)直接用于調整其唯一的權值K，

K＝P(||a_i×m_i|||λ₀) (8)

設P(||a_i×m_i|||λ₀)的值域范圍為[p_max,p_min]，一旦獲得隱馬爾科夫模型的參數λ₀＝(A₀,B₀,π₀)，該值域范圍很容易通過測試集測試手段得到；當該值域范圍不適合式(7)和式(8)，需要再調整，調整的方法為，

2.如權利要求1所述的用于提高慣性-地磁組合靜動態綜合性能的方法，其特征在于：步驟3具體采用遍歷式隱馬爾科夫模型，因為陀螺儀、加速度計和地磁傳感器這三種傳感器的輸出特性并沒有給出任何關于模型隱含狀態轉移概率的信息，對于每個隱含狀態，觀測概率通過下式計算，

其中F(X,μ_jm,Σ_jm)為高斯函數，定義為，

其中L為高斯函數的個數,μ_jm和Σ_jm分別為第m個高斯函數的均值和協方差。