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

在審

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

1.一種GPS拒止環境下高精度位姿估計算法，其特征在于實現以下步驟：

(1)構建基于PI調節器自適應互補濾波，補償后的四元數形式的姿態解算方程為:

式中δ∈R^3×1為PI反饋調節器產生的濾波結果，用于補償實測向量與預測向量的旋轉誤差；

(2)設計自適應模糊神經網絡，當GPS有效時，基于捷聯卡爾曼濾波結果對自適應神經網絡進行權值優化；當GPS無效時，基于訓練好的自適應神經網絡在線估計位置誤差，提高導航性能。

2.根據權利要求1所述的基于PI調節器自適應互補濾波，其特征在于：基于互補濾波的姿態算法，將加計的測量值通過PI反饋修正回路與陀螺儀的信息進行有效融合，同時引入加計抑制函數實現自適應反饋校正，在有效抑制姿態角度誤差的同時，提高了系統在高機動狀態下的抗干擾能力；

PI反饋調節器的濾波結果δ如下:

式中，e(ω)∈R^3×1為加速計的誤差向量，f^b∈R^3×1為加速計測量值歸一化的三軸加速度，g^b∈R^3×1為載體系下的重力加速度，k_p為比例系數，用以調節陀螺儀與加計之間的交叉頻率，k_i為積分系數，用以修正陀螺輸出角速度的漂移誤差，p(a)為加速計抑制函數，其表達式如下:

3.根據權利要求1所述的基于自適應模糊神經網絡的高精度位置估計算法，其特征在于：基于自適應模糊神經網絡預測無GPS下的系統位置信息，其權值參數的優化采取的是Fletcher-Reeves共軛梯度下降算法和最小二乘法組合算法，具有收斂速度快的優點，使得該算法具備更高的運算效率和更強的適應性，提升了對系統位置信息的預測精度；

GPS有效時，對自適應模糊神經網絡進行訓練，自適應模糊神經網絡是基于T-S型模糊邏輯系統的“If-Then”規則的多層自適應網絡，依次為模糊化層、模糊集運算層、歸一化層、逆模糊化層和總輸出層，神經網絡的權值參數分別包含在模糊化層和逆模糊化層中；

在前向通道中，模糊化層權值參數固定，采用最小二乘法辨識逆模糊化層權值參數，在后向通道中，逆模糊化層權值參數固定，采用Fletcher-Reeves共軛梯度法辨識模糊化層權值參數，直至滿足精度要求或達到最大的迭代次數；

Fletcher-Reeves共軛梯度法將最優化問題轉化為目標函數最小化問題，目標函數表達式如下：

沿負梯度方向搜索時，單步運算公式如下：

X_k+1＝X_k+a_kP_k

式中，k為當前時刻，k+1為下一時刻，X為待調整參數，a_k為根據線搜索準則確定的學習速率，P_k為當前時刻的共軛梯度向量，代表當前時刻的搜索方向，當滿足以下條件：

式中，g_k為當前時刻的迭代梯度向量；

則可以采用負梯度方向繼續進行搜索，P_k表達式如下：

式中，β_k為共軛參數，選用Fletcher-Reeves法計算，其表達式如下：

基于GPS收星數判斷GPS是否有效，當GPS無效時，使用訓練好的神經網絡的參數集，校正系統的位置誤差，對位置信息進行預測。