技术摘要：
本发明涉及一种航姿参考系统的互补滤波姿态解算方法，其中，包括：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵利用第一组磁强计数据进行方位对准，计算初始航  全部
背景技术：
随着智能制造和尖端芯片的发展，无人机在军用和民用领域都得到广泛使用。为 了得到稳定的无人机飞行控制，导航系统成为不可或缺的机载设备之一。一般情况下，导航 系统由惯性导航和卫星导航配合磁强计等传感器组成。正常情况下，导航系统能不间断提 供位置、速度和姿态角(姿态角包括俯仰角、滚转角和航向角)。但由于卫星导航系统易受干 扰造成信号中断，为了保证飞机的安全，需要采用航姿参考系统作为飞机姿态传感器。另 外，对于一些消费级无人机或视距内的小型无人机，如果不需要获得位置信息，可以采用航 姿参考系统作为姿态传感器，减少卫星导航带来的系统成本。 航姿参考系统由陀螺仪、加速度计、磁强计、相关电路、结构和线缆等组成，利用互 补滤波算法，实现载体航向角、俯仰角和滚转角的解算。算法的原理是：没有位置和速度量 测信息时，飞机会保持匀速直线运动或盘旋运动，线加速度变化较小，此时加速度计测量的 信息为重力加速度和干扰加速度的叠加，利用陀螺仪输出的角速度积分可以得到系统的姿 态角，利用磁强计输出计算导航坐标系磁场分量。忽略重力加速度在不同位置的变化和干 扰加速度，利用导航系到本体系的姿态矩阵将导航系下重力加速度和磁场分量投影到本体 坐标系上，与加速度计和磁强计实际输出作叉乘，得到的结果作为陀螺仪漂移修正本体系 陀螺仪角速度输出，并进一步实现姿态更新。传统互补滤波姿态解算方法框图如图1所示。 其中，导航系的定义为：x轴和y轴在当地水平面，x轴指向正东，y轴指向正北，z轴 垂直于当地水平面向上。本体系的定义为：y轴沿着载体纵轴指向前，x轴沿着载体横轴指向 右，z轴符合右手定则。 目前，传统航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场分 布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到叉 乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特别 是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。第 二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪，磁 强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。 目前，现有的航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场 分布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到 叉乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特 别是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。 第二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪， 磁强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。 6 CN 111551175 A 说　明　书 2/9 页
技术实现要素：
本发明的目的在于提供一种航姿参考系统的互补滤波姿态解算方法，用于解决上 述现有技术的问题。 本发明一种航姿参考系统的互补滤波姿态解算方法，其中，包括：步骤一：采集陀 螺仪输出、加速度输出和磁强计输出，并进行归一化；步骤二：利用第一组加速度计数据进 行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵 第三步：利用第 一组磁强计数据进行方位对准，计算初始航向角 并初始化四元数Qk＝[Qk ,1  Qk ,2  Qk ,3  Q Tk ,4] 和本体系到导航系的姿态转换矩阵 第四步：采集陀螺仪输出、加速度输出和磁强 计输出数据；第五步：利用加速度和磁强计输出数据计算陀螺仪误差 计算当地导航系 磁场强度参考值 计算加速度输出在导航系的投影值fn，计算加速度叉乘结果fX，计算 磁矢量叉乘结果 计算误差 第六步：计算陀螺仪数据误差修正值 第七步：利用 陀螺仪的输出修正后进行姿态更新；第八步：根据更新后的姿态信息计算姿态角；第九步： 重复第四步到第八步，直到采样时刻结束。 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第一步 的计算过程具体包括：陀螺仪输出的角度增量为： 加速度计输出 的速度增量为： 磁强计输出为： 归一化导航 系重力加速度gn＝[0  0  1]T； 对采集的数据进行归一化为： 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，利用第 一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩 阵 包括： 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第三步 7 CN 111551175 A 说　明　书 3/9 页 具体包括： 定义水平系为：导航系绕z轴转过航向角后得到的坐标系，记磁强计在水平系的分 量为 计算航向角 初始化四元数Qk： 计算姿态转换矩阵 和 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第五步： 利用加速度和磁强计输出数据计算陀螺仪误差 计算当地导航系磁场强度参考值 计算加速度输出在导航系的投影值fn： 计算加速度叉乘结果fX： 计算磁矢量叉乘结果 8 CN 111551175 A 说　明　书 4/9 页 其中，符号×表示叉乘，即对于a＝[ax  ay  az]T，b＝[bx  by  b Tz] 取加速度叉乘fX前两项和磁叉乘 第三项，作为误差 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第六步 具体包括：设定比例系数kp＝10，积分系数ki＝0.01，初始采样时刻积分误差修正值 计算比例误差修正值 计算积分误差修正值 计算一个采样周期dT内陀螺仪误差修正值 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第七步 具体包括： 一个采样周期dT内角度增量： 等效转过的角度： 采用三阶毕卡逼近更新当前采用时刻的四元数：Q ＝[Q   Q   Q   Q ]Tk 1 k 1,1 k 1,2 k 1,3 k 1,4 ， 并进行归一化； 其中： 9 CN 111551175 A 说　明　书 5/9 页 根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第八步： 计算姿态角包括： 根据更新的四元数Qk 1更新转换矩阵 和 根据姿态矩阵计算姿态角： 其中： 为 的第i行第j列。 本发明针对目前航姿参考系统传统互补滤波姿态解算方法存在修正盲点和误差 耦合的技术缺陷，设计了一种改进的互补滤波姿态解算方法，可实现任意姿态角下加速度 计和磁强计对陀螺仪的角速度修正，同时在导航系下计算陀螺仪修正值，分离俯仰角、滚转 角和航向角通道，避免了磁强计误差对俯仰角和滚转角的耦合误差。 附图说明 图1为现有的互补滤波姿态解算框图； 图2本发明的互补滤波姿态解算框图。