[发明专利]一种姿态自主冗余组合导航算法

摘要

本发明提供一种姿态自主冗余组合导航算法，包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算。AHRS算法采用自适应矢量叉乘滤波，利用MEMS IMU和磁力计数据进行解算出载体航向和姿态数据；组合导航算法基于载体动力学模型，利用IMU、磁力计、气压高度计、GNSS数据，采用高维度扩展卡尔曼滤波完成数据融合，解算出位置、速度和姿态信息。本算法中AHRS算法属于纯自主算法，与组合导航算法中的姿态数据互为冗余、热备份，在GNSS失效后依然为载体提供可靠的航向、姿态信息，在GNSS重新定位后为组合导航算法提供初始姿态，使组合导航算法快速收敛。

主权项

1.一种姿态自主冗余组合导航算法，其特征在于：包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算，还包括以下步骤：S1：由导航计算机读取安装于载体上的IMU、磁力计、气压高度计以及GNSS的数据；S2：在导航周期内，利用步骤S1中获得的IMU数据和磁力计数据进行自适应矢量叉乘滤波数据融合，解算出载体航向和姿态信息，实现独立自主的AHRS算法；S3：完成载体姿态解算后，判断GNSS是否更新，如果GNSS数据没有更新，进入步骤S4；如果GNSS数据有更新，则进入步骤S5；S4：在GNSS未更新情况下，对捷联纯惯性导航时间t1进行累加计时，根据IMU的仪表精度可以确定捷联纯惯性导航误差累积到失效的门限时间T1，当捷联纯惯性导航时间t1超过该门限时间T1时，即t1＞T1，则认为捷联纯惯性导航误差偏大，捷联纯惯性导航结果失效，导航系统仅输出纯AHRS算法的姿态数据；当捷联纯惯性导航时间t1未超过该门限时间T1时，即t1≤T1，则认为捷联纯惯性导航误差在可接受的范围内，载体同时输出AHRS算法姿态以及捷联纯惯性导航算法的位置、速度和姿态；S5：判断是否为GNSS捕获0时刻，如果是GNSS定位计时t2清零，以纯AHRS算法的姿态数据、GNSS的位置和速度数据作为组合导航的初始数据，重置组合导航算法参数，然后再利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零；如果不是，则直接利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零，如果t2达到组合导航重新收敛的门限值T2后，全套导航参数有效，否则输出纯AHRS算法姿态数据。