[發明專利]一種GPS拒止環境下高精度位姿估計算法

在審

• 申請號: 202111060355.6
• 申請日: 2021-09-10
• 公開（公告）號: CN113777645A
• 公開（公告）日: 2021-12-10
• 發明（設計）人: 雷旭升;王銳;付發
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01S19/49
• 分類號: G01S19/49;G01C21/18;G01C21/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

一種GPS拒止環境下高精度位姿估計算法，涉及基于PI調節器的自適應互補濾波姿態算法和基于自適應模糊神經網絡的高精度位置估計算法。首先在陀螺姿態解算方程的基礎上，設計加計抑制函數，構建PI反饋修正回路來提升系統在高動態下的姿態測量精度；其次構建自適應模糊神經網絡，在GPS有效時基于Fletcher?Reeves共軛梯度下降算法和最小二乘法組合算法優化模糊神經網絡權值參數，以實現在GPS拒止環境下的高精度位置估計。本發明具有實時性好、抗擾動能力強、學習速度快、預測精度高等優點，可用于中小型無人機在GPS拒止環境下的高精度導航。

技術領域

本發明涉及一種GPS拒止環境下高精度位姿估計算法，適用于動載體在GPS拒止區域的高精度導航領域。

背景技術

近年來，中小型無人機由于尺寸小、重量輕、隱蔽性好、性價比高等優點在軍事和民用領域被廣泛應用，由于成本和體積的限制，其導航系統一般以GPS/SINS組合導航為基礎進行研究。

隨著無人機應用場景的廣泛，在實際飛行中可能通過密林、高樓、隧道等復雜環境，會出現GPS拒止區域，GPS無法提供持續有效的導航信息輔助INS系統。此時慣性器件產生的誤差漂移、標度因子不穩定等問題因為得不到修正而不斷積累，導致組合導航系統的整體性能下降。因此，研究GPS拒止環境下的高精度位姿估計導航算法具有十分重要的意義。

捷聯慣導算法、Kalman濾波算法、互補濾波算法等常被用于為系統提供導航信息解算。捷聯慣導算法數據更新速度快，在運行時間較短時穩定性好，但長時解算導航信息時，其誤差隨時間增大，位姿數據發生漂移、精度發散，難以滿足長時間、高精度的導航需求。Kalman濾波算法依靠GPS作為量測值，當GPS信號出現異常，Kalman濾波過程中的量測更新將無法進行，此時位姿解算結果將由于誤差累積隨時間發散，不再可靠。互補濾波算法被用于迭代更新姿態信息，根據傳感器的頻域特性對數據進行融合處理，能夠有效的抑制姿態角的漂移，但只適用于靜止或低速狀態，在高機動狀態下將引入較大的加速度誤差，系統的適應性較差。

發明內容

本發明解決的技術問題是：GPS/SINS組合導航系統在GPS信號受干擾或被遮擋時姿態和位置數據發散的問題，提出了一種GPS拒止環境下高精度位姿估計算法，基于加計的測量值作為觀測量，引入PI反饋修正回路實現對陀螺輸出的自適應補償從而校正系統的姿態信息，同時構建自適應模糊神經網絡，采取組合算法對無GPS下的系統位置信息進行預測估計，實現GPS拒止環境下的高精度導航。

本發明的技術解決方案為：首先在陀螺姿態解算方程的基礎上，構建PI反饋修正回路，引入加計對陀螺的自適應補償來解決姿態信息發散的問題；其次構建自適應模糊神經網絡，基于Fletcher-Reeves共軛梯度下降算法和最小二乘法組合算法優化參數，在GPS有效狀態下進行訓練以實現GPS無效時為系統提供實時的高精度位置信息。其實現步驟如下：

(1)基于陀螺姿態解算的四元數方程，

其中，Q＝[q₀ q₁ q₂ q₃]^T為由標量q₀和矢量q₁i+q₂j+q₃k組成用于描述載體轉動的四元數，t為當前時刻，t-1為前一時刻，Δt為算法解算周期，為從地理系到載體系的陀螺角速度投影矢量；

構建PI反饋修正回路，基于加速計的測量值作為觀測量，引入加速計抑制函數，對系統加速狀態進行判斷，為陀螺的角速度提供自適應補償，減小高機動狀態下加速計的誤差對陀螺修正量的干擾，實現對陀螺角度漂移的抑制；

補償后的四元數形式的姿態解算方程為: