[發明專利]一種捷聯慣性導航的誤差校正方法及裝置

在審

• 申請號: 201710131897.5
• 申請日: 2017-03-07
• 公開（公告）號: CN108571980A
• 公開（公告）日: 2018-09-25
• 發明（設計）人: 汪勇;胡祝銀;林威;胡樹立;章小兵
• 申請（專利權）人: 深圳市博安智控科技有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 重慶百潤洪知識產權代理有限公司 50219
• 代理人: 劉立春

1.一種捷聯慣性導航的誤差校正方法，其特征在于，包括：

對加速度計，磁力計進行姿態解算，以獲取終端的第一姿態數據；

根據上一次的第一姿態數據與當前的第一姿態數據，計算第一姿態變化率；

將所述第一姿態數據作為陀螺儀的初始姿態，采用四元數進行陀螺儀姿態解算，以獲取第二姿態數據；

根據上一次的第二姿態數據與當前的第二姿態數據，計算第二姿態變化率；

跟蹤偏差率的周期性變化，當所述偏差率達到極值時，根據所述第一姿態數據、所述第二姿態數據及所述姿態可信度，計算第三姿態數據，并通過所述第三姿態數據初始化陀螺儀姿態，以校正陀螺儀積分誤差，其中，所述姿態可信度由所述偏差率及修正參數計算獲得。

2.根據權利要求1所述的捷聯慣性導航的誤差校正方法，其特征在于，對加速度計，磁力計進行姿態解算，以獲取終端的第一姿態數據，包括：

計算第一磁力計矢量及加速度計矢量；

計算水平矢量，所述水平矢量等于所述第一磁力計矢量乘以所述加速度計矢量；

計算第二磁力計矢量，所述第二磁力計矢量等于所述加速度計矢量乘以所述水平矢量；

根據所述水平矢量、所述第二磁力計矢量及所述加速度計矢量，計算第一姿態矩陣；

根據所述第一姿態矩陣，計算終端的第一姿態數據。

3.根據權利要求1所述的捷聯慣性導航的誤差校正方法，其特征在于，所述根據上一次的第一姿態數據與當前的第一姿態數據，計算第一姿態變化率，包括：

獲取終端的角度軸數據，并進行歸一化；

根據上一次的第一姿態數據與當前的第一姿態數據，計算角度差值；

根據歸一化的所述角度軸數據、所述角度差值、上一次的第一姿態數據及當前的第一姿態數據，計算第一姿態變化率。

4.根據權利要求1所述的捷聯慣性導航的誤差校正方法，其特征在于，所述將所述第一姿態數據作為陀螺儀的初始姿態，采用四元數進行陀螺儀姿態解算，以獲取第二姿態數據，包括：

根據所述第一姿態數據，計算第二姿態矩陣；

計算陀螺儀的四元數，并根據所述四元數進行陀螺儀姿態解算，以獲取第三姿態矩陣；

根據所述第三姿態矩陣對所述第二姿態矩陣進行更新，以獲取第二姿態數據。

5.根據權利要求1所述的捷聯慣性導航的誤差校正方法，其特征在于，所述跟蹤偏差率的周期性變化，當所述偏差率達到極值時，根據所述第一姿態數據、所述第二姿態數據及所述姿態可信度，計算第三姿態數據，包括：

根據所述第一姿態變化率、所述第二姿態變化率和姿態偏差率，計算偏差率；

跟蹤偏差率的周期性變化，當所述偏差率達到極大值或極小值時，根據所述偏差率及修正參數，計算姿態可信度；

根據所述第一姿態數據、所述第二姿態數據及所述姿態可信度，進行融合解算，以獲取第三姿態數據。

6.一種捷聯慣性導航的誤差校正裝置，其特征在于，包括：

第一姿態計算模塊，用于對加速度計，磁力計進行姿態解算，以獲取終端的第一姿態數據；

第一姿態變化率計算模塊，用于根據上一次的第一姿態數據與當前的第一姿態數據，計算第一姿態變化率；

第二姿態計算模塊，用于將所述第一姿態數據作為陀螺儀的初始姿態，采用四元數進行陀螺儀姿態解算，以獲取第二姿態數據；

第二姿態變化率計算模塊，用于根據上一次的第二姿態數據與當前的第二姿態數據，計算第二姿態變化率；

校正模塊，用于跟蹤偏差率的周期性變化，當所述偏差率達到極值時，根據所述第一姿態數據、所述第二姿態數據及所述姿態可信度，計算第三姿態數據，并通過所述第三姿態數據初始化陀螺儀姿態，以校正陀螺儀積分誤差，其中，所述姿態可信度由所述偏差率及修正參數計算獲得。