[發明專利]一種姿態自主冗余組合導航算法

有效

• 申請號: 201910271058.2
• 申請日: 2019-04-04
• 公開（公告）號: CN109916395B
• 公開（公告）日: 2023-06-23
• 發明（設計）人: 李德輝;王冠林;唐寧
• 申請（專利權）人: 山東智翼航空科技有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20
• 代理公司: 常州市英諾創信專利代理事務所(普通合伙) 32258
• 代理人: 于桂賢

本發明提供一種姿態自主冗余組合導航算法，包含一套自主式的AHRS算法和一套組合導航算法，兩套算法在同一導航周期內相繼完成解算。AHRS算法采用自適應矢量叉乘濾波，利用MEMS?IMU和磁力計數據進行解算出載體航向和姿態數據；組合導航算法基于載體動力學模型，利用IMU、磁力計、氣壓高度計、GNSS數據，采用高維度擴展卡爾曼濾波完成數據融合，解算出位置、速度和姿態信息。本算法中AHRS算法屬于純自主算法，與組合導航算法中的姿態數據互為冗余、熱備份，在GNSS失效后依然為載體提供可靠的航向、姿態信息，在GNSS重新定位后為組合導航算法提供初始姿態，使組合導航算法快速收斂。

技術領域

本發明涉及組合導航算法技術領域，特別是涉及一種姿態自主冗余組合導航算法。

背景技術

現有組合導航算法中，不存在姿態數據冗余，在衛星導航(GNSS)信號失效后，處于純慣導狀態，位置、速度和姿態誤差很快發散至無法使用，無法為載體提供可靠的航向、姿態信息；當GNSS重新定位后，無初始姿態信息，組合導航算法需要重新初始對準，收斂時間更長，甚至算法發散。

發明內容

本發明所要解決的技術問題是：為了克服現有技術中的不足，本發明提供一種基于微機電慣性測量組合/衛星導航/磁力計/氣壓高度計(MEMS?IMU/GNSS/Magnetometer/Baro)的姿態自主冗余組合導航算法，解決純組合導航算法中不存在姿態數據冗余，以及GNSS重新定位后收斂時間長的問題。向AHRS狀態姿態算法的切換問題，使算法切換由硬切換轉換到軟切換，確保算法在切換間快速收斂。

本發明解決其技術問題所要采用的技術方案是：

在基于微機電慣性測量組合/衛星導航/磁力計/氣壓高度計(MEMS?IMU/GNSS/Magnetometer/Baro)的組合導航算法中，其中，MEMS?IMU可簡寫為IMU，當GNSS失效時，廉價且低精度的MEMS?IMU無法維持純慣性導航定位精度，GNSS失效后，在短時間內組合導航算法就必須退化到AHRS狀態，確保為載體繼續提供姿態測量。組合導航算法切換到AHRS狀態，這涉及到狀態方程和量測方程的切換，本發明的算法中包含一套自主式的AHRS算法和一套組合導航算法，兩套算法在同一導航周期內相繼完成解算，以保證快速切換和收斂。

本發明所涉及的組合導航系統包括安裝于載體上的MEMS?IMU、GNSS、磁力計和氣壓高度計，其中，

IMU包含三軸正交陀螺儀和三軸正交加速度計，分別用于測量角速度和加速度(比力)；GNSS接收機用于輸出衛星導航的位置和速度；三軸正交磁力計，用于地磁測量；氣壓高度計用于測量大氣壓強，從而換算出氣壓高度。

本發明涉及到的坐標系包括載體坐標系、站心坐標系、東北天地理坐標系、導航坐標系、地磁東北天坐標系，其中，載體坐標系指以裝載導航系統的載體的右前上坐標系，安裝于載體上的具有坐標的傳感器(IMU、磁力計)其安裝與載體坐標系重合；站心坐標系為載體導航起始點為原點的東北天地理坐標系，組合導航的位置估計以站心坐標系表示；東北天地理坐標系的原點為載體質心，GNSS的速度量測以及狀態中的速度估計表示在本坐標系中，且認為等同于表示于站心坐標系中；在本算法中導航坐標系采用站心坐標系；地磁東北天坐標系以磁北為北向，東北天地理坐標系天向軸旋轉一個地磁偏角即與地磁東北天坐標系重合。

一種姿態自主冗余組合導航算法，包括一套自主式的AHRS算法和一套組合導航算法，兩套算法在同一導航周期內相繼完成解算，還包括以下步驟：

S1：由導航計算機讀取安裝于載體上的IMU、磁力計、氣壓高度計以及衛GNSS接收機的數據；

S2：在導航周期內，利用步驟S1中獲得的IMU數據和磁力計數據進行自適應矢量叉乘濾波數據融合，解算出載體航向和姿態信息，實現獨立且完全自主的AHRS算法；