CN103512584A - 导航姿态信息输出方法、装置及捷联航姿参考*** - Google Patents

Abstract

本发明公开了一种导航姿态信息输出方法、装置及捷联航姿参考***，该方法应用于捷联航姿参考***中，包括：获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；对观测姿态和观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；对过程姿态和滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。采用本发明能够提高目前已有的捷联航姿参考***导航***的精度。

Description

导航姿态信息输出方法、装置及捷联航姿参考***

技术领域

本发明涉及通信领域，具体而言，涉及一种导航姿态信息输出方法、装置及捷联航姿参考***。

背景技术

捷联惯性导航技术是一门综合性技术，用于对运动体的姿态、速度和位置参数的确定，该项技术广泛应用于航天、航空、航海和大地测量等领域。

SINS（Strapdown Inertial Navigation System，捷联惯性导航***），其原理是根据牛顿提出的相对惯性空间的力学定律，提取陀螺、加速度计和磁向计的测量值实时地计算姿态矩阵，进而从姿态矩阵的元素获得运动载体的姿态和航向信息来进行导航计算。

惯性导航***制导的精度取决于惯性器件（例如，陀螺仪和加速度计）的精度和解算方法。由于MEMS器件的漂移误差的累积，惯性导航精度会随着时间的推移而降低，需要有GPS（Global Positioning Systems，全球定位***）或其他的外部辅助参考提高导航***的精度。

AHRS（Attitude and Haeding Reference System，捷联航姿参考***）包括多个惯性轴向传感器，能够为飞行器提供航向、横滚和俯仰信息，以及相应的角速率信息。目前这类***用来为飞行器提供准确可靠的姿态与航向信息，但是应用时该***一般不提供位置的导航信息，即使提供也只能在段时间内有效，长期的位置连续解算误差会不断积累。

针对相关技术中目前已有的捷联航姿参考***导航***的精度不够的问题，目前尚未提出有效的解决方案。

发明内容

针对目前已有的捷联航姿参考***导航***的精度不够的问题，本发明提供了一种导航姿态信息输出方法、装置及捷联航姿参考***，以至少解决上述问题。

根据本发明的一个方面，提供了一种导航姿态信息输出方法，应用于捷联航姿参考***中，包括：获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；对所述观测姿态和所述观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；对所述过程姿态和所述滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

优选地，所述递推出下一时刻的过程姿态，包括：利用微分方程数值解法递推出所述过程姿态。

优选地，所述微分方程数值解法包括四元数微分方程的RK4解法。

优选地，对所述观测姿态和所述观测航向进行滤波处理，包括：对所述观测姿态和所述观测航向进行低通数字滤波算法处理。

优选地，对所述观测姿态和所述观测航向进行滤波处理，包括：采用RC低通数字滤波器对所述观测姿态和所述观测航向进行滤波处理。

优选地，将所述过程姿态和所述滤波后的观测姿态进行有机融合，包括：采用与所述过程姿态紧耦合、与所述观测姿态松耦合的耦合策略，对所述过程姿态和所述滤波后的观测姿态进行有机融合。

优选地，所述过程姿态和所述滤波后的观测姿态的有机融合采用欧拉角的方式，其中，所述欧拉角为ZXY欧拉角，X俯仰角定义在（-90，+90），Y横滚角定义在（-180，+180），Z偏航角定义在（-180，+180）。

优选地，当所述俯仰角越过-90或+90度时，所述横滚角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；当所述俯仰角越过-90或+90度时，当所述横滚角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

优选地，当所述俯仰角越过-90或+90度时，所述偏航角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；当所述俯仰角越过-90或+90度时，当所述偏航角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

优选地，在所述获取校正后的陀螺仪数据之前，还包括：从所述捷联航姿参考***的传感器中获取确定导航姿态信息所需要的数据。

优选地，对于所述传感器的常值输出误差进行自动估计和补偿。

优选地，对于所述传感器的常值输出误差进行自动估计和补偿，包括：对所述陀螺仪数据和所述加速度计数据的误差校正采用传感器输出的各轴测量值的校正形式；对所述磁强计数据的误差校正采用地磁倾角的校正形式。

优选地，所述得到导航姿态信息并输出，包括：通过USART串口将得到的导航姿态信息输出。

优选地，输出所述导航姿态信息时支持多种输出协议模式。

优选地，所述输出协议模式包括基于Windows MFC+OpenGL+COMM组件的上位机动态显示。

根据本发明的另一方面，提供了一种导航姿态信息输出装置，包括：递推模块，用于获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；解算模块，用于获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；滤波模块，用于对所述观测姿态和所述观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；输出模块，用于对所述过程姿态和所述滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

优选地，所述装置设置于捷联航姿参考***中。

根据本发明的另一方面，提供了一种捷联航姿参考***，包括：传感测量单元，用于获取捷联航姿参考***各单元的数据；数据校正单元，用于对所述传感测量单元获取的数据进行校正；误差估计单元，用于对所述数据校正单元校正后的数据进行误差估计；姿态解算单元，用于对进行误差估计后的数据进行姿态解算，获取当前的导航姿态信息；输出显示单元，用于输出所述导航姿态信息。

在本发明实施例中，对陀螺仪数据、加速度计数据以及磁强计数据进行校正，并根据校正的数据得到过程姿态以及滤波后的观测姿态、观测航向，进而将过程姿态与观测姿态进行有机融合、互补滤波，从而得到较为精确的导航姿态信息，并输出。相对于现有技术的捷联航姿参考***，本发明提供的导航姿态信息输出方法中对于数据不断进行校正，从而保证了导航姿态信息的精确性，避免了因误差累积导致导航精度的下降。并且，不需要使用GPS等其他外部辅助参考，达到节省资源的目的。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例的导航姿态信息输出方法的处理流程图；

图2是根据本发明实施例的导航姿态信息输出装置的结构示意图；

图3是根据本发明实施例的捷联航姿参考***的结构示意图；

图4是根据本发明实施例的捷联航姿参考***的各单元的架构图；

图5是根据本发明实施例的实施例一的硬件配置方案示意图；

图6是根据本发明实施例的实施例一的IMU惯性单元硬件配置图；

图7是根据本发明实施例的实施例一的姿态解算算法方案示意图；

图8是根据本发明实施例的实施例三的由陀螺仪解算出过程姿态角的流程示意图；

图9是根据本发明实施例的实施例三的观测姿态的获取流程图；

图10是根据本发明实施例的实施例三的过程姿态和观测姿态的数据融合和跳变去除的处理流程图；

图11是根据本发明实施例的实施例三的基于OpenGL和MFC的三维动态显示的结构示意图。

具体实施方式

下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

相关技术中提及，目前已有的捷联航姿参考***导航***的精度不够。为解决该技术问题，本发明实施例提供了一种导航姿态信息输出方法，其处理流程图如图1所示，该方法应用于捷联航姿参考***中，包括如下步骤：

步骤S101、获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；

步骤S102、获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；

步骤S103、对步骤S102中获得的观测姿态和观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；

步骤S104、对步骤S101中递推的过程姿态和步骤S103中滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

在本发明实施例中，对陀螺仪数据、加速度计数据以及磁强计数据进行校正，并根据校正的数据得到过程姿态以及滤波后的观测姿态、观测航向，进而将过程姿态与观测姿态进行有机融合、互补滤波，从而得到较为精确的导航姿态信息，并输出。相对于现有技术的捷联航姿参考***，本发明提供的导航姿态信息输出方法中对于数据不断进行校正，从而保证了导航姿态信息的精确性，避免了因误差累积导致导航精度的下降。并且，不需要使用GPS等其他外部辅助参考，达到节省资源的目的。

如图1所示流程，步骤S101在实施时，需要根据校正后的陀螺仪数据递推出下一时刻的过程姿态，具体的，是利用微分方程数值解法递推出过程姿态。优选的，微分方程数值解法包括四元数微分方程的RK4解法。

如图1所示流程，步骤S103中提及对观测姿态和观测航向进行滤波处理，此处的滤波处理方式可以根据算法的不同有多种处理方式，优选的，对观测姿态和观测航向进行低通数字滤波算法处理。

同理，除滤波处理所使用的算法不同，该处理使用的设备也可能不同，即，对观测姿态和观测航向进行滤波处理的一种优选的实施方式为采用RC低通数字滤波器对观测姿态和观测航向进行滤波处理。当然，滤波器还存在其他多种类型，可以根据具体情况选择不同的滤波器。

在对当前的观测姿态和观测航向进行滤波处理后，本发明实施例会将步骤S101中递推得到的过程姿态和步骤S103中获得的滤波后的观测姿态进行有机融合，在融合过程中需要采用适当的耦合策略，例如，优选的，可以采用与过程姿态紧耦合、与观测姿态松耦合的耦合策略，对过程姿态和滤波后的观测姿态进行有机融合。实际应用中，也可以采用其他的耦合策略，例如，与过程姿态松耦合、与观测姿态紧耦合的耦合策略。上述仅仅是耦合策略的举例，实际应用中，具体的耦合策略根据具体情况而定。

在过程姿态和滤波后的观测姿态的有机融合过程，优选采用欧拉角的方式，其中，欧拉角为ZXY欧拉角，X俯仰角定义在（-90，+90），Y横滚角定义在（-180，+180），Z偏航角定义在（-180，+180）。

当采用欧拉角的方式对过程姿态和滤波后的观测姿态进行有机融合时，根据各角度的取值选择不同的操作：

当俯仰角越过-90或+90度时，横滚角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；

当俯仰角越过-90或+90度时，当横滚角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据；

当俯仰角越过-90或+90度时，偏航角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；

当俯仰角越过-90或+90度时，当偏航角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

如图1所示流程，步骤S101在实施之前，即在获取校正后的陀螺仪数据之前，本发明实施例选择从捷联航姿参考***的传感器中获取确定导航姿态信息所需要的数据。

实施时，从传感器中获得的数据会存在一定的误差，例如，常值输出误差。为减少常值输出误差，本发明实施例对于传感器的常值输出误差进行自动估计和补偿。优选的，考虑传感器的种类不同，以及不同的传输器中获取的数据不同，采用的校正方式也不同，例如，对陀螺仪数据和加速度计数据的误差校正采用传感器输出的各轴测量值的校正形式，再例如，对磁强计数据的误差校正可以采用地磁倾角的校正形式。

如图1所示流程，步骤S104在实施时提出得到导航姿态信息并输出，优选的，可以通过USART串口将得到的导航姿态信息输出。进一步，输出导航姿态信息时支持多种输出协议模式。例如，基于Windows MFC+OpenGL+COMM组件的上位机动态显示的输出协议模式。

为支持上述任意一个优选实施例，基于同一发明构思，本发明实施例还提供了一种导航姿态信息输出装置，其结构示意图如图2所示，包括：

递推模块201，用于获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；

解算模块202，与递推模块201耦合，用于获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；

滤波模块203，与解算模块202耦合，用于对解算模块202解算出的观测姿态和观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；

输出模块204，与递推模块201、滤波模块203耦合，用于对递推模块201递推出的过程姿态和滤波模块203滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

在一个优选的实施例中，图2所示的导航姿态信息输出装置设置于捷联航姿参考***中。

为支持上述任意一个优选实施例，基于同一发明构思，本发明实施例还提供了一种捷联航姿参考***，其结构示意图如图3所示，包括：

传感测量单元301，用于获取捷联航姿参考***各单元的数据；

数据校正单元302，用于对传感测量单元301获取的数据进行校正；

误差估计单元303，用于对数据校正单元校正302后的数据进行误差估计；

姿态解算单元304，用于对进行误差估计后的数据进行姿态解算，获取当前的导航姿态信息；

输出显示单元305，用于输出导航姿态信息。

为将本发明实施例提供的捷联航姿参考***的结构阐述地更清楚更明白更详尽，现分别对各单元进行具体说明，具体的架构图请参见图4。

为将本发明实施例提供的导航姿态信息输出方法阐述地更清楚更明白，现以具体实施例对其进行说明。

实施例一

本实施例提供了一种基于MEMS惯性测量元件（IMU，Inertial Measurement Unit）的捷联航姿参考***（AHRS,Attitude and Heading Reference System）及其姿态解算和数据融合方法，***包括：3轴陀螺仪，3轴加速度计和3轴磁罗盘共同组成的9轴惯性和地磁信号测量模块；基于陀螺仪的***姿态递推过程微分方程数值解算算法（RK4）；基于加速度计和磁罗盘的***姿态观测解算算法（RC低通滤波，去除有害加速度）；过程姿态和观测姿态的融合策略（欧拉角互补滤波，直观，去跳变）；姿态解算结果的输出显示方案设计（OpenGL+MSCOMM组件）；本发明基于低成本的MEMS惯性传感器和嵌入式的设计架构，采用简单有效的轻量级解算算法提供准确的导航姿态信息输出，环境适应性强，实现和维护简便。

基于MEMS传感器的方案可以采用陀螺和加速度计惯性器件组合，加计和地磁器件组合或只采用加速度计通过合理配置各传感器的数量和安装位姿，采用惯性原理解算出固连机体的姿态和航向信息。

采用陀螺仪、加速度计和磁强计的传感器配置方案，用加速度计和磁强计传感器所得的数据修正陀螺仪的误差，为本发明实施例采取的硬件配置方案。其硬件配置方案图请参见图5

解算算法

捷联惯性导航的姿态解算主要通过获得坐标系的转换矩阵，从中拾取关心的姿态和航向信息。即将运载体相对载体坐标系下测得的加速度和角速率转换成导航坐标系下，同时计算出运载体的姿态、速度和位置。目前描述动坐标相对参考坐标系方位关系的方法主要有欧拉角法，方向余弦法和四元数法。欧拉角法由于存在“奇点”现象，在工程实践上没得到广泛应用。方向余弦法可以全姿态工作，没有奇异点，但计算量比较大，实时性比较差，无法满足工程实践要求。而四元数法的微分方程式比方向余弦矩阵微分方程计算量有明显减少，故能满足工程实践的实时性要求。

对于解算算法，现有技术存在不可忽视的缺点：近年来，在姿态解算算法中，学者们主要采取毕卡逼近法和泰勒展开法求解四元数微分方程，将陀螺仪获得的过程姿态和加速度计、磁强计获得的观测姿态以四元数融合，并采用卡尔曼滤波方法去除噪声误差。四元数微分方程的毕卡逼近法和泰勒展开法求解，达不到姿态更新的精度和速度要求，将会影响到获得姿态的准确性。由于四元数不如欧拉角直观，在进行过程姿态和观测姿态的数据融合时将不利于算法的设计和调试。而对于卡尔曼滤波器算法，为其建立稳定可靠的更新方程是比较困难的，而且其计算量很大，对处理器的运算速度和精度要求很高。

基于四阶Tailor展开的RK4微分方程数值解法，其局部截断误差为O(h⁵)，可以达到较高的解算精度，同时再以互补滤波的方式实现过程姿态和观测姿态的数据融合，整个方案与kalman滤波相比计算量开销更小，而精度也可以保证。

本发明要保护技术方案采用IMU硬件方案。基于Cortex-M3的STM32F103CBT6做核心运算处理，采用集成有3轴加速度计和3轴陀螺仪的MPU6000MEMS运动处理芯片和集成有3轴磁阻计的HMC5883L芯片分别做原始惯性和磁性信息量测，姿态解算通过USART串口输出到上位PC机动态显示或直接为机器人平台提供姿态和航向参考信息。其中，本实施例采用的IMU惯性单元硬件配置图请参见图6。

关于姿态解算算法，本发明实施例利用陀螺仪数据进行四元数微分方程的动态递推数值解算（RK4—四阶龙格库塔法），得到一个实时性较高的***的动态过程姿态；利用加计和磁强计的数据进行长期的静态姿态量测，得到一个量测姿态；然后对以上两个姿态做数据融合和互补滤波处理，实现瞬时相信陀螺仪积分输出（陀螺仪积分姿态的高通滤波），获得良好的动态跟踪性能，长期相信加计和地磁的量测姿态（加计和地磁的观测姿态的低通滤波），实现输出的收敛和稳定。本发明实施例采用的姿态解算算法方案图请参见图7。

综上，基于本实施例提供的MEMS惯性测量***，姿态解算和数据融合，输出显示整体***结构图请参见图4。

实施例二

本实施例详细阐述了要保护技术方案。

本实施例提供了一种基于MEMS传感器的惯性航姿参考***，其特征在于包括传感测量，数据校正，姿态解算，误差估计和输出显示5个部分。

其中，姿态解算部分，包括以下步骤：

(1).获取校正后的陀螺仪数据，采用微分方程数值解法，递推出下一时刻的过程姿态；

(2).获取校正后的加速度计和磁强计数据，采用反三角函数解算出当前的观测姿态和观测航向；

(3).对步骤(2)中的解算结果，采用低通数字滤波算法处理，去除有害噪声的影响，并输出滤波后的观测姿态和观测航向；

(4)将步骤(1)中的过程姿态和步骤(3)中滤波后的观测姿态有机融合、互补滤波，然后输出。

过程姿态解算采用的是四元数微分方程的RK4解法，利用微分方程的4阶Tailor展开近似求解姿态四元数，相比卡尔曼滤波姿态预测方程中采用的一阶Tailor展开，可以达到较高的精度。

观测姿态解算采用了RC低通数字滤波器处理量测姿态，有效去除噪声加速度的影响。

姿态融合采用了一种与过程姿态紧耦合，与观测姿态松耦合的融合策略，互补滤波(Complementary Filter)。

关于姿态融合策略，姿态融合是以欧拉角方式进行的，并解决了欧拉角融合过程中的姿态跳动问题；欧拉角选用ZXY欧拉角，X俯仰角定义在（-90，+90），Y横滚角定义在(-180,+180)，Z偏航角定义在(-180,+180)，根据欧拉角与空间姿态的映射关系，俯仰角不会在-90和+90之间跨越，但是俯仰在越过-90或+90时，会引起横滚角增加或减小180，另外滚角和偏航角都会在-180和+180之间有跨越输出，从而导致姿态融合不连续，产生姿态跳动，设计了相应的去除姿态跳动的策略，包括如下内容：

(1).去除横滚跳动：当俯仰角越过-90或+90度时，横滚角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；当横滚角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

(2).去除偏航跳动：当俯仰角越过-90或+90度时，偏航角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；当偏航角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

关于测量参数校正，对于传感器的常值输出误差，***在运行过程中能在线进行自动的估计和补偿，实现精确的测量输出。

对于传感器常值输出误差动态校正，对陀螺仪和加计的校正形式直接采用传感器输出的各轴测量值的形式，对磁强计误差的校正形式采用地磁倾角的形式。

当解算出结果时，对结果进行输出和显示，是通过USART串口输出导航数据的。输出和显示过程中，可以支持多种自定义的输出协议模式，支持基于Windows MFC+OpenGL+COMM组件的上位机动态显示。

实施例三

在本例中，由陀螺仪解算出过程姿态角的流程示意图请参见图8。

设集体坐标系相对平台运动坐标系的转动四元数为

Q=q₀+q₁i+q₂j+q₃k

四元数Q的及时更新修正可通过解下面的四元数微分方程来实现

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{tbx}}^b& -{\mathrm{\ω}}_{\mathrm{tby}}^b& -{\mathrm{\ω}}_{\mathrm{tbz}}^b\\ {\mathrm{\ω}}_{\mathrm{tbx}}^b& 0& {\mathrm{\ω}}_{\mathrm{tbz}}^b& {-\mathrm{\ω}}_{\mathrm{tby}}^b\\ {\mathrm{\ω}}_{\mathrm{tby}}^b& {-\mathrm{\ω}}_{\mathrm{tbz}}^b& 0& {\mathrm{\ω}}_{\mathrm{tbx}}^b\\ {\mathrm{\ω}}_{\mathrm{tbz}}^b& {\mathrm{\ω}}_{\mathrm{tby}}^b& -{\mathrm{\ω}}_{\mathrm{tbx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

其中ω^b _tbx、ω^b _tby、ω^b _tbz分别为从陀螺仪上获取的校正后的角速率数据。

利用四阶龙格库塔法RK4求解上述微分方程

k₁=f(x_n,Q_n)

$k_2=f(x_{n+\frac{T}{2}},Q_n+\frac{T}{2}\×k_1)$

$k_3=f(x_{n+\frac{T}{2}},Q_n+\frac{T}{2}{\×k}_2)$

k₄=f(x_n+T,Q_n+T×k₃)

$Q_{n+1}=Q_n+\frac{T}{6}(k_1+2\×k_2+2\×k_3+k_4)$

按照上式可实现捷联矩阵的及时更新，其中T为计算周期。为保证四元数范数为1，还应对其进行规范化，

$Q_{\mathrm{norm}}=\frac{Q_n}{\sqrt{{q_0}^2+{q_1}^2+{q_2}^2+{q_3}^2}}$

根据姿态矩阵与四元数的关系

$T=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_0{q}_1+q_2{q}_3)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

再根据四元数与欧拉角的关系

$\mathrm{\γ}=\arctan (-\frac{T_{31}}{T_{33}}),$ θ=arcsin(T₃₂)， $\mathrm{\ψ}=\arctan \left(\frac{T_{12}}{T_{22}}\right)$

可递推出下一时刻的姿态角,即为过程姿态角。

进一步，由加速度计和磁强计，通过反三角函数得到观测姿态角

$\left\{\begin{array}{c}{\mathrm{\γ}}_m=\arcsin (-g_x/g)\\ {\mathrm{\θ}}_m=\arctan (g_y/g_z)\\ {\mathrm{\ψ}}_m=\arctan \frac{h_z\sin {\mathrm{\γ}}_m-h_y\cos {\mathrm{\γ}}_m}{h_x\cos {\mathrm{\γ}}_m+h_y\sin {\mathrm{\γ}}_m\sin {\mathrm{\θ}}_m+h_z\sin {\mathrm{\θ}}_m\sin {\mathrm{\γ}}_m}\end{array}\right.$

其中，g_x、g_y、g_z分别为轴上的加速度，g为当地重力加速度，

h_x，h_y，h_z分别为磁场在三轴上的分量。

对上式中获得的姿态角进行一阶低通RC滤波，如对于θ

其中x、y为上一时刻和下一时刻的权值，通过调整x和y可以调整滤波器的频率特性，拟制有害加速度对解算的干扰。

RC滤波所得的姿态角即为观测姿态，具体的流程请参见图9。

过程姿态和观测姿态的数据融合和跳变去除的处理流程请参见图10。

将过程姿态和观测姿态以欧拉角形式融合，所采用的融合方案为互补滤波（CF，Complementary Filer），对来自陀螺仪积分的姿态进行搞通滤波，获取陀螺良好的动态响应和跟踪特性；对加计和地磁的观测姿态进行低通滤波，获取两者良好的静态量测特性，通过以上互补，实现输出结果良好的动态和静态综合性能。

本实施例中欧拉角选用ZXY欧拉角，X俯仰角定义在（-90°，+90°），Y横滚角定义在（-180°，+180°），Z偏航角定义在（-180°，+180°），根据欧拉角与空间姿态的映射关系，俯仰角不会在-90和+90之间跨越，但是俯仰在越过-90或+90时，会引起横滚角增加或减小180，另外滚角和偏航角都会在-180和+180之间有跨越输出，从而导致姿态融合不连续，产生姿态跳动，设计了相应的去除姿态跳动的策略，包括如下内容：

(1).去除横滚跳动：当俯仰角越过-90或+90度时，横滚角在越过前后的输出要相差180，并保持在定义域内，禁止滤波并直接输出新数据；当横滚角在-180和+180之间跳变时，禁止滤波并直接输出新数据。

(2).去除偏航跳动：当俯仰角越过-90或+90度时，偏航角在越过前后的输出要相差180，并保持在定义域内，禁止滤波并直接输出新数据；当偏航角在-180和+180之间跳变时，禁止滤波并直接输出新数据。

在输出和显示过程中，本实施例采用基于OpenGL和MFC的三维动态显示。

基于Windows的MFC窗口程序框架，通过AvtiveX串口控件接收IMU模块输出的姿态数据，利用OpenGL绘制三维姿态，其结构示意图如图11所示。

本发明实施例技术方案带来的有益效果：

本发明实施例采用MEMS惯性和地磁传感器元件，以最小的配置方案，通过嵌入式的架构实现了微小型的姿态航向参考***（AHRS）的硬件设计和解算输出，采用RK4加互补滤波的解算方法，以较小的运算量和较高的精度实现了***，同时滤波方案是采用欧拉角的形式进行，有效去除了姿态的跳变问题，使***更加直观简便。

从以上的描述中，可以看出，本发明实现了如下技术效果：

在本发明实施例中，对陀螺仪数据、加速度计数据以及磁强计数据进行校正，并根据校正的数据得到过程姿态以及滤波后的观测姿态、观测航向，进而将过程姿态与观测姿态进行有机融合、互补滤波，从而得到较为精确的导航姿态信息，并输出。相对于现有技术的捷联航姿参考***，本发明提供的导航姿态信息输出方法中对于数据不断进行校正，从而保证了导航姿态信息的精确性，避免了因误差累积导致导航精度的下降。并且，不需要使用GPS等其他外部辅助参考，达到节省资源的目的。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (18)

1.一种导航姿态信息输出方法，其特征在于，应用于捷联航姿参考***中，包括：

获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；

获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；

对所述观测姿态和所述观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；

对所述过程姿态和所述滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

2.根据权利要求1所述的方法，其特征在于，所述递推出下一时刻的过程姿态，包括：利用微分方程数值解法递推出所述过程姿态。

3.根据权利要求2所述的方法，其特征在于，所述微分方程数值解法包括四元数微分方程的RK4解法。

4.根据权利要求1所述的方法，其特征在于，对所述观测姿态和所述观测航向进行滤波处理，包括：对所述观测姿态和所述观测航向进行低通数字滤波算法处理。

5.根据权利要求1或4所述的方法，其特征在于，对所述观测姿态和所述观测航向进行滤波处理，包括：采用RC低通数字滤波器对所述观测姿态和所述观测航向进行滤波处理。

6.根据权利要求1所述的方法，其特征在于，将所述过程姿态和所述滤波后的观测姿态进行有机融合，包括：采用与所述过程姿态紧耦合、与所述观测姿态松耦合的耦合策略，对所述过程姿态和所述滤波后的观测姿态进行有机融合。

7.根据权利要求6所述的方法，其特征在于，所述过程姿态和所述滤波后的观测姿态的有机融合采用欧拉角的方式，其中，所述欧拉角为ZXY欧拉角，X俯仰角定义在（-90，+90），Y横滚角定义在（-180，+180），Z偏航角定义在（-180，+180）。

8.根据权利要求7所述的方法，其特征在于，

当所述俯仰角越过-90或+90度时，所述横滚角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；

当所述俯仰角越过-90或+90度时，当所述横滚角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

9.根据权利要求7所述的方法，其特征在于，

当所述俯仰角越过-90或+90度时，所述偏航角在越过前后的输出要相差180，并保持在定义域内，此时禁止滤波并直接输出新数据；当所述俯仰角越过-90或+90度时，当所述偏航角在-180和+180之间跳变时，此时禁止滤波并直接输出新数据。

10.根据权利要求1所述的方法，其特征在于，在所述获取校正后的陀螺仪数据之前，还包括：从所述捷联航姿参考***的传感器中获取确定导航姿态信息所需要的数据。

11.根据权利要求10所述的方法，其特征在于，对于所述传感器的常值输出误差进行自动估计和补偿。

12.根据权利要求11所述的方法，其特征在于，对于所述传感器的常值输出误差进行自动估计和补偿，包括：

对所述陀螺仪数据和所述加速度计数据的误差校正采用传感器输出的各轴测量值的校正形式；

对所述磁强计数据的误差校正采用地磁倾角的校正形式。

13.根据权利要求1所述的方法，其特征在于，所述得到导航姿态信息并输出，包括：通过USART串口将得到的导航姿态信息输出。

14.根据权利要求13所述的方法，其特征在于，输出所述导航姿态信息时支持多种输出协议模式。

15.根据权利要求14所述的方法，其特征在于，所述输出协议模式包括基于Windows MFC+OpenGL+COMM组件的上位机动态显示。

16.一种导航姿态信息输出装置，其特征在于，包括：

递推模块，用于获取校正后的陀螺仪数据，递推出下一时刻的过程姿态；

解算模块，用于获取校正后的加速度计数据和磁强计数据，解算出当前的观测姿态和观测航向；

滤波模块，用于对所述观测姿态和所述观测航向进行滤波处理，并输出滤波后的观测姿态和观测航向；

输出模块，用于对所述过程姿态和所述滤波后的观测姿态进行有机融合、互补滤波，得到导航姿态信息并输出。

17.根据权利要求16所述的装置，其特征在于，所述装置设置于捷联航姿参考***中。

18.一种捷联航姿参考***，其特征在于，包括：

传感测量单元，用于获取捷联航姿参考***各单元的数据；

数据校正单元，用于对所述传感测量单元获取的数据进行校正；

误差估计单元，用于对所述数据校正单元校正后的数据进行误差估计；

姿态解算单元，用于对进行误差估计后的数据进行姿态解算，获取当前的导航姿态信息；输出显示单元，用于输出所述导航姿态信息。

Images