[發明專利]一種基于平滑變結構濾波的GNSS/SINS/磁力計的組合方法

在審

• 申請號: 202011432157.3
• 申請日: 2020-12-07
• 公開（公告）號: CN112683270A
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 張秋昭;黃偉;孫澳;王方田;鄭南山;李增科
• 申請（專利權）人: 中國礦業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01C21/08;G01S19/45;G01S19/47
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 郝偉揚

1.一種基于平滑變結構濾波的GNSS/SINS/磁力計組合方法，其特征在于，包括以下步驟：

步驟一，在SINS系統中挑選狀態變量，包括慣導速度誤差[δV_E δV_N δV_U]、姿態角誤差[φ_E φ_N φ_U]、慣導位置誤差[δL δλ δh]；

步驟二，建立組合導航系統的狀態方程

步驟三，建立組合導航系統的量測方程Z＝HX+V；

步驟四，構建系統模型的可觀測性矩陣，對系統模型進行可觀測性分析；根據分析結果，評價基于GNSS/SINS/磁力計的組合系統是否具備完全可觀測性；若是，則進行平滑變結構濾波的應用；若否，則重復步驟一到四，重新挑選狀態變量和相應的觀測量，設計新的狀態方程和量測方程，直至系統為完全可觀系統；

步驟五，平滑變結構濾波的應用。

2.根據權利要求1所述的基于平滑變結構濾波的GNSS/SINS/磁力計組合方法，其特征在于：所述步驟二的組合導航系統的狀態方程中：

其中，

3.根據權利要求2所述的基于平滑變結構濾波的GNSS/SINS/磁力計組合方法，其特征在于：將加速度計偏置和陀螺漂移ε從基本狀態變量中刪去，用等效白噪聲取代，作為狀態方程中的新增系統噪聲，如下式：

狀態方程中可由系統噪聲表達式推得G和W，如下式：

系統噪聲中含有：陀螺零偏等效白噪聲w_g、加速度計偏置等效白噪聲w_a、陀螺儀測量白噪聲加速度計測量白噪聲

4.根據權利要求1所述的基于平滑變結構濾波的GNSS/SINS/磁力計組合方法其特征在于，所述步驟三的量測方程的具體構建步驟為：

加速度計和磁力計分別沿著載體坐標系三軸方向安裝，在構造觀測量之前，確定俯仰角θ、橫滾角γ及航向角ψ；在載體非加速運動狀態下并且載體坐標系和參考坐標系重合時，得到加速度計在北東地參考坐標系下的比力輸出為

fⁿ＝[0 0 g]

地磁場強度在北東地參考坐標系下的輸出為

當載體運動時，加速度計在載體坐標系下的輸出為

地磁場強度在載體坐標系下的輸出為

其中fⁿ為北東地坐標系下的加速度計比力輸出，g為當地重力加速度，Hⁿ為北東地坐標系下的磁力計三軸磁分量，上面四式存在以下關系：

由上面兩個方程可以得出以下方程組

對于上式，可得到q₀、q₁、q₂、q₃；

根據四元數和姿態角的轉換關系，得到載體姿態角；

陀螺儀輸出的載體系相對于慣性系的角速率為與導航系相對于慣性系的角速率相減后得到載體系相對于導航系的角速率一次積分后的得到載體在導航系下的載體姿態角

對于已經求出的兩組姿態角觀測量對其進行求差組成姿態角差觀測量：

其次SINS編排輸出的位置和速度觀測量與GPS輸出的位置和速度觀測量的差值作為另一組觀測量：

V＝[v_gps P_gps θ_gm]^T

上式中，v_gps為GPS速度測量白噪聲，P_gps為GPS位置測量白噪聲，θ_gm為加速度計和磁力計測量白噪聲。將上面兩個觀測量組合得到量測方程如下：

其中H矩陣為I_9×9。