[發明專利]一種慣性導航的預積分輔助組件及數據處理方法

有效

• 申請號: 202011101350.9
• 申請日: 2020-10-15
• 公開（公告）號: CN112097767B
• 公開（公告）日: 2022-07-12
• 發明（設計）人: 許起超;金華興
• 申請（專利權）人: 杭州知路科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S19/33;G01S19/47
• 代理公司: 北京沁優知識產權代理有限公司 11684
• 代理人: 張亞娟

1.一種慣性導航的預積分輔助組件，其特征在于：包括殼體，所述殼體內設置有主控模塊，所述主控模塊的輸入端連接有雙模GPS模塊和IMU模塊，所述IMU模塊即慣性傳感器模塊，所述主控模塊的輸出端連接有網絡模塊和串口模塊，所述網絡模塊為單芯片網絡接口模塊，所述串口模塊為由接口轉換芯片組成的模塊，所述主控模塊的引腳上還連接有用于在不同環境下對數據校準執行的狀態控制及顯示模塊，所述狀態控制及顯示模塊包括若干個用于顯示當前慣性導航數據采集的狀態指示燈、一個零偏校準按鍵和一個指南針校準按鍵，所述雙模GPS模塊的接收端連接有用于接收GPS和北斗雙衛星數據的雙模天線；

所述慣性導航的預積分輔助組件的數據處理方法，包括以下步驟：

第一步，主控模塊接收雙模GPS模塊和IMU模塊采集的數據；

第二步，IMU模塊進行數據校準；

第三步，將雙模GPS模塊和IMU模塊進行數據同步；

第四步，對IMU模塊的數據進行預積分計算；

第五步，對校準后的磁力數據進行三角函數轉換，得到具體的偏航角，實現指南針的效果，偏航角為對磁力計所輸出的磁力數據進行三角函數計算得到一個相對于磁北的航向角，其計算公式如下：

式中，ψ_m表示最終輸出的航向角，γ和θ分別代表姿態的橫滾角和翻滾角，為磁力計的x，y，z軸的磁力輸出；

第六步，將經過處理后得到的GPS數據、IMU預積分數據和偏航角數據通過串口模塊和網絡模塊進行實時高頻輸出顯示；

所述的第四步中預積分計算步驟為：先將預積分增量進行初始化處理，該預積分增量以第k幀的IMU坐標系為參考坐標系，將xyz三軸上的位移增量ΔP初始化為0、xyz三軸上的速度增量ΔV初始化為0和將前一幀到當前幀的旋轉姿態增量ΔQ初始化為單位四元數，在數據同步處理和初始化后，對獲取到的兩幀GPS數據內的所有IMU數據進行積分，根據IMU數據的時間間隔，通過中值法將GPS數據內的第k幀和第k+1幀之間所有的IMU數據進行積分計算，并更新第k+1幀的位移增量、速度增量和旋轉姿態增量；

使用中值法對IMU數據進行積分計算，其中，計算第i個時刻與第i+1個時刻的位移變化量ΔP_i+1的公式為：

式中，δt為第i個時刻至第i+1個時刻的時間間隔，a為IMU模塊所測得的加速度數據，b_a為加速度的偏移量；

其中，計算第i個時刻與第i+1個時刻的速度變化量ΔV_i+1的公式為：

式中，w表示IMU模塊測得的角速度數據，b_w表示角速度的偏移量。

2.根據權利要求1所述的一種慣性導航的預積分輔助組件，其特征在于：所述的IMU模塊為高性能九軸MEMS模塊，其包括三軸加速度計、三軸陀螺儀和三軸磁力計，所述的IMU模塊的采樣率為1000Hz。

3.根據權利要求1所述的一種慣性導航的預積分輔助組件，其特征在于：所述的雙模GPS模塊為高性能GPS/北斗雙模定位模塊，包括IPX接口和可充電后備電池，所述的雙模GPS模塊具有20Hz的更新速率，2.5米的定位精度，追蹤靈敏度為-165dBm。

4.根據權利要求1所述的一種慣性導航的預積分輔助組件，其特征在于：所述的主控模塊的芯片型號為STM32F4系列單片機，其頻率為168MHz。

5.根據權利要求1所述的一種慣性導航的預積分輔助組件，其特征在于：所述的殼體包括底座、上蓋和用于減小振動的橡膠阻尼墊，所述底座內部安裝所述橡膠阻尼墊，所述底座上部與所述上蓋抵接，所述底座的側表面設置有所述狀態指示燈、零偏校準按鍵、指南針校準按鍵和雙模天線的接口，所述底座的底面還設有用于固定電路板的沉頭孔和通孔，所述上蓋的側表面設有所述網絡模塊和串口模塊的輸出端的開孔，所述上蓋的頂部設有用于固定上蓋的沉頭孔和通孔。

6.根據權利要求1所述的一種慣性導航的預積分輔助組件，其特征在于：所述的第一步中雙模GPS模塊的數據采集速率為20Hz，IMU模塊的數據采集速率為500Hz。