一种微惯性航姿参考系统的姿态获取方法
【技术领域】
[0001] 本发明属于导航技术领域，更具体地，涉及一种微惯性航姿参考系统的姿态获取 方法。
【背景技术】
[0002] 在惯性导航系统中，姿态是反映载体运动的一个重要参数，通常姿态解算是根据 陀螺的数据和载体的速度、位置信息实时计算载体姿态矩阵，实现导航坐标系对地理坐标 系的实时跟随以及载体姿态的解算（俯仰角、横滚角和航向角）。而姿态精度对载体的速 度、位置计算、识别跟踪、运动轨迹、性能分析等有重要影响。但是由于陀螺和加速度计的精 度有限，并且随着时间的增加误差会不断的积累增大，尤其是低成本的MEMS陀螺漂移误差 更大，使系统的姿态解算精度越来越低，不能够满足工程技术要求。
【发明内容】
[0003] 针对现有技术的缺陷，本发明的目的在于提供一种微惯性航姿参考系统的姿态获 取方法，旨在解决现有技术中由于MEMS陀螺精度低、漂移误差大导致航姿参考系统姿态解 算精度较低的技术问题。
[0004] 本发明提供了一种微惯性航姿参考系统的姿态获取方法，包括下述步骤：
[0005] (1)根据传感器采集的载体加速度信息和三维磁场强度信息获得载体的姿态角初 值；并根据所述姿态角初值获得当前时刻的系统姿态矩阵；
[0006] (2)根据所述系统姿态矩阵获得陀螺漂移误差补偿量，并根据陀螺漂移误差调节 模型对陀螺输出的数据进行补偿；
[0007] (3)根据补偿后的陀螺输出的数据对下一个时刻的系统姿态矩阵进行更新，并根 据更新后的姿态矩阵获得载体的姿态角。
[0008] 更进一步地，步骤（1)具体包括：
[0009] (1. 1)获得载体坐标系下的载体加速度信1 和三维磁场强度 信息
[0010] (1. 2)根据所述载体加速度信息计算载体的俯仰角初值和横滚角初值；并根据所 述三维磁场强度信息计算载体的航向角初值；
[0011] (1.3)根据载体的俯仰角初值、横滚角初值和航向角初值并结合公式
[0013] 获得载体当前时刻的姿态矩阵^!;
[0014] 其中，hx hy hj别为X轴、Y轴、Z轴的磁场强度，a x ay \分别为载体在X轴、Y 轴、Ζ轴的加速度；载体的俯仰角
载体的横滚角
g为载体在地 理坐标系的重力加速度，载体的航向角
.,Hny= hx*cos γ+hy*sin Θ *sin γ -hz*cos Θ *sin γ ;Hnx= hy*cos Θ +hz*sin γ ;导航坐标系为东北天地理坐标系，中n表 示导航坐标系，b表示载体坐标系。
[0015] 更进一步地，，步骤（2)中，陀螺漂移误差调节模型为ω (t) = c〇gyrci(t) + c〇c(t);
[0016] 其中，载体坐标系下陀螺的角速度信息
ω；1 ω：： 分别为三个陀螺的输出信息，〇e(t)为陀螺漂移误差补偿量，c〇(t)为陀螺 补偿以后的输出
[0017] 由于陀螺误差会随着时间的增加不断累积，导致获取的姿态信息含有很大的累积 误差。加速度计测量信息是俯仰角、横滚角的观测量，磁强计信息是航向角的观测量，本方 法利用加速度计测得信息和磁强计测得的信息对陀螺误差进行补偿，消除陀螺的累积误 差，提高姿态获取精度。
[0018] 更进一步地，步骤（3)具体包括：
[0019] (3. 1)根据补偿后的陀螺输出数据ω⑴获得陀螺角增量反对称矩阵
[0021] (3. 2)根据公式
对下一个时刻的姿态矩阵G中四元数进 行更新；
[0022] (3. 3)根据更新后的四元数对下一个时刻的姿态矩阵q进行更新；
[0023] (3. 4)根据更新后的姿态矩阵计算下一个时刻载体的姿态角；
[0024] 其中，ωχ coy ωζ分别为补偿后三个陀螺的输出数据；At为系统采样周期；qA 下一个时刻的四元数，qk i为当前时刻的四元数，Μ为陀螺角增量的反对称矩阵；更新后的 系统姿态矩阵
[0026] q。，q2, q3为四元数；下一个时刻载体的姿态角包括俯仰角θ = arcsin(T32)、横 滚角，
[0027] 在本发明实施例中，载体的姿态信息可以用于系统控制和航姿参考。
[0028] 本发明提供的微惯性航姿参考系统的姿态获取方法，以陀螺、加速度计和磁强计 为系统基本传感器，并利用加速度计和磁强计的信息对陀螺的漂移进行有效补偿，降低陀 螺的漂移误差，根据补偿后的陀螺输出数据对姿态矩阵进行更新，从而提高航姿参考系统 的姿态解算精度，对惯性导航领域的姿态解算有较大贡献，本发明简单巧妙，符合实际需求 进步显著且实用性强，能够大规模推广使用。
【附图说明】
[0029] 图1是本发明实施例提供的一种微惯性航姿参考系统的姿态获取方法实现流程 图。
【具体实施方式】
[0030] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对 本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并 不用于限定本发明。
[0031] 本发明提供的微惯性航姿参考系统的姿态获取方法，解决了由MEMS陀螺精度低、 漂移误差大所导致的航姿参考系统姿态解算精度较低，不能满足工程化应用的问题。
[0032] 如图1所示，本发明实施例提供的微惯性航姿参考系统的姿态获取方法，包括以 下几个步骤：
[0033] 步骤1 :系统姿态矩阵初始化处理；
[0034] 步骤2 :读取加速度计数据和磁强计数据，并对陀螺输出数据进行误差补偿；
[0035] 步骤3 :根据补偿后的陀螺输出数据对系统姿态矩阵进行更新并根据更新后的姿 态矩阵求取系统姿态。
[0036] 作为本发明的一个实施例，步骤1中根据读取的加速度计和磁强计信息计算系统 初始姿态矩阵，具体通过以下步骤实现：
[0037] (la)选择东北天地理坐标系作为导航坐标系并从加速度计获得载体加速度信息
其中ax ay az分别为载体在X轴、Y轴、Z轴的加速度；以及从磁强计 获取三维磁场强度信息
：;其中hx hy hz分别为X轴、Y轴、Z轴的磁场强 度；其中，东北天地理坐标糸的三个方向分别对应载体的XYZ三个坐标轴，在导航领域是约 定成熟的定义，不再详细定义。