CN204188173U - 一种姿态陀螺仪倾斜补偿四元数解算装置 - Google Patents

Abstract

本实用新型提供了一种姿态陀螺仪倾斜补偿四元数解算装置，包括方位传感器，六轴运动传感器，压力传感器，解算单元，获得精确俯仰角、横滚角、航向角以及高度实时参数。本实用新型能够消除基于陀螺仪实现的姿态传感器发生倾斜时的数据误差，从而使姿态传感器能够提供精确的俯仰角、横滚角和航向角，具有姿态值准确、误差小、无姿态死角、成本低、解算速度快的有益效果。

Description

一种姿态陀螺仪倾斜补偿四元数解算装置

技术领域

本实用新型涉及陀螺仪领域，更具体地，涉及一种姿态陀螺仪倾斜补偿四元数解算装置。 

背景技术

陀螺仪在高速旋转时，能够抗拒任何外力和干扰的影响，保持其自转轴相对于惯性空间方向上稳定不变。因此，通过测量相对于陀螺仪的转轴的夹角变化，能够实现对承载陀螺仪的设备(如飞行器)姿态偏离的检测和校正。此种利用陀螺仪实现的控制系统也被称之为姿态稳定系统。 

目前市场上所使用的陀螺仪大多为磁罗盘磁阻传感器，磁罗盘在倾斜时会出现磁偏移的情况，在倾斜到90度时会产生到180度左右的巨大误差。因而目前陀螺仪大多只能在自身保持水平的情况下正常工作。现有技术中可采用机械结构悬挂陀螺仪，使其一直保持为水平状态，以达到正确测量的结果，或者使用价格高昂的激光陀螺仪。以上解决方案都会使陀螺仪的结构复杂化，提高产品成本。 

实用新型内容

为了克服现有技术中的上述缺陷，本实用新型提供了一种姿态陀螺仪倾斜补偿四元数解算装置。本实用新型的目的是消除基于陀螺仪实现的姿态传感器发生倾斜时的数据误差，从而使姿态传感器能够提供精确的俯仰角、横滚角和航向角。 

本实用新型所提供的姿态陀螺仪倾斜补偿四元数解算装置，其特征在于，包括：方位传感器，六轴运动传感器，压力传感器，解算单元；所述六轴运动传感器通过I²C接口连接到解算单元，并且所述六轴运动传感器通过中断引脚连 接解算单元；所述方位传感器与六轴运动传感器的主I²C接口连接，并且方位传感器的数据准备引脚连接解算单元；所述压力传感器通过I²C总线连接所述解算单元；其中六轴运动传感器芯片的I²C主串行数据和时钟接口用于通过引线外接方位传感器芯片的I²C数据和时钟端口；六轴运动传感器芯片的I²C串行数据和时钟引脚通过引线连接到解算单元的单片机芯片的I/O端口；六轴运动传感器的帧同步数字端口通过引线连接单片机芯片的I/O端口；并且六轴运动传感器的中断端口通过引线连接单片机的I/O端口；方位传感器的数据准备端口通过引线连接单片机芯片的I/O端口；所述压力传感器的I²C串行数据和时钟端口通过引线连接单片机芯片的I/O端口。 

优选的是，所述六轴运动传感器包括3轴MEMS加速度计、3轴MEMS陀螺仪、一路温度传感器、ADC转换器、信号调节模块、传感器寄存器、主I²C串行接口以及从I²C和SPI接口。 

优选的是，所述压力传感器包括压力传感器单元、ADC、控制单元以及EPROM。 

本实用新型的改进点在于：通过方位传感器检测自身与地球磁场的相对关系，因而不受地域、封闭环境等影响；带有压力传感器用于检测气压，以获取自身高度，因而也不受空间海拔的影响；各部分的电路连接关系采用I²C执行数据传输，并且采用中断机制，解算单元可在第一时间读取最新的数据，快速响应姿态的变化，这样的连接使得控制器拥有最大的主动权，最快地获取各传感器的状态和转换结果，保证了数据的及时处理。 

本实用新型可广泛用于航空、医疗、车辆、物联网、机器人、信息模拟及数字娱乐等各个领域，具有姿态值准确、误差小、无姿态死角、成本低、解算速度快的有益效果。 

附图说明

下面结合附图和具体实施方式对本实用新型作进一步详细的说明： 

图1是本实用新型所述的姿态陀螺仪倾斜补偿四元数解算装置结构示意图； 

图2是本实用新型所述六轴运动传感器与解算单元电路连接关系示意图； 

图3是本实用新型所述方位传感器的电路连接关系示意图； 

图4是本实用新型所述压力传感器与解算单元电路连接关系示意图； 

图5是本实用新型所述六轴运动传感器的芯片电路示意图； 

图6是本实用新型所述方位传感器的芯片电路示意图； 

图7是本实用新型所述压力传感器的芯片电路示意图； 

图8是本实用新型所述解算单元的单片机芯片电路示意图。 

具体实施方式

为了使本技术领域的人员更好地理解本实用新型的技术方案，并使本实用新型的上述目的、特征和优点能够更加明显易懂，下面结合实施例及实施例附图对本实用新型作进一步详细的说明。 

图1是本实用新型所述的姿态陀螺仪倾斜补偿四元数解算装置结构示意图，参见该附图，所述装置包括：方位传感器，用于感应地磁向量从而计算航向夹角；六轴运动传感器，用于检测装置运动的方向和速度，计算三轴加速度和三轴角速度；压力传感器，用于检测气压，计算装置高度值；解算单元，用于基于所述三轴加速度、三轴角速度以及航向夹角获得精确俯仰角、横滚角、航向角以及高度实时参数。 

本装置的方位传感器、六轴运动传感器以及压力传感器以3.3V电源VDD供电，与实现解算单元的单片机之间以I²C传输方式通信。并且数据可以通过装置的UART接口以RS232串口的形式与PC或其他设备等上位机通信，可根据上位机发送的命令，以改变其发送频率、初始值、偏移量等参数。单片机烧写调试使用SWD接口。使用两个LED灯指示装置的供电与通信状态。 

图2是所述六轴运动传感器与解算单元电路连接关系示意图。本实用新型所述的六轴运动传感器通过I²C接口连接到作为解算单元和主控制器的单片机。在初始化阶段，单片机将要设置该六轴运动传感器的测量感度和采样频率，同时使能数据更新中断。之后单片机配置自己的引脚为中断输入INT。这样在一个新的采样完成后六轴运动传感器产生中断，呼叫单片机读取新的数据。 

图3是所述方位传感器的电路连接关系示意图。方位传感器并没有直接与单片机的I²C接口连接，而是与六轴运动传感器的AUXI²C接口连接，这样，使单片机有两种方式访问方位传感器的数据。先将六轴运动传感器的AUX接口设置成直连式，单片机可以通过I²C直接访问方位传感器；然后开启六轴运动传感器的AUX接口为主控制器，通过AUXI²C接口六轴运动传感器负责采集方向传感器的采样结果,并将结果存放在自己的FIFO寄存器中，单片机通过访问六轴运动传感器读取方位传感器的转换结果。以上两个方式都不需要额外的增加或者是修改电路，只需要设置六轴运动传感器相应的控制寄存器。方位传感器的数据准备引脚DRDY连接到了单片机，通过查询该引脚的电平，判断是否有新的转换数据。 

图4是压力传感器与解算单元电路连接关系示意图。压力传感器挂接在单片机的I²C总线上，从而单片机可以直接访问读取压力传感器的转换数据。 

下面介绍本实用新型所述姿态陀螺仪倾斜补偿四元数解算装置的六轴运动传感器、方位传感器、压力传感器的内部架构。 

六轴运动传感器内部集成了3轴MEMS加速度计、3轴MEMS陀螺仪以及一个可扩展的数字运动处理器DMP；并且可以利用I²C接口连接一个第三方的数字传感器，即本实用新型中作为方位传感器的三轴磁力计。在扩展之后，六轴运动传感器就可以通过其I²C或SPI接口输出一个9轴的信号。六轴运动传感器的3 轴MEMS加速度计包括X、Y、Z三轴上的加速度测量模块，3轴MEMS陀螺仪包括X、Y、Z三轴上的角速度测量模块，以及一路温度传感器。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒(dps)，加速度计可测范围为±2，±4，±8，±16g。采用16位的ADC，将其测量的模拟量转化为可输出的数字量。信号调节模块以数字输出包括6轴或9轴的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据至传感器寄存器。在对外信号接口方面，包括：主I²C串行接口，通过串行接口Bypass以AUX_CL和AUX_DA端口实现主串行数据以及主串行时钟连接；以及从I²C和SPI接口，通过SDO端口提供SPI串行数据输出，并且通过SDA和SCL提供I²C串行数据和时钟。FSYNC端口提供帧同步数字输入。具有中断状态寄存器，INT端口提供中断数字输出。 

方位传感器采用磁阻式三轴传感器并应用特殊辅助电路来测量磁场。通过施加供电电源，该传感器可以将量测轴方向上的任何入射磁场转变成一种差分电压输出。磁阻传感器是由一个镍铁(坡莫合金)薄膜放置在硅片上，并构成一个带式电阻元件。在磁场存在的情况下，桥式电阻元件的变化将引起跨电桥输出电压的相应变动。这些磁阻元件两两对齐，形成一个共同的敏感轴，随着磁场在敏感方向上不断增强，电压也就正向增长。因为输出只与沿着轴方向上的磁阻元件成比例，其他磁阻电桥也放置在正交方向上，就能精密测量其他方向的磁场强度。 

压力传感器的内部架构包括压力传感器单元、ADC、控制单元以及EPROM。 

图5至图8分别示出了本实用新型所述姿态陀螺仪倾斜补偿四元数解算装置的六轴运动传感器、方位传感器、压力传感器以及解算单元的芯片电路图。如图5所示的六轴运动传感器的芯片电路，其中AUX_DA和AUX_CL端口作为I²C 主串行数据和时钟接口，用于通过引线AUX_SDA和AUX_SCL外接方位传感器的芯片I²C数据和时钟端口SDA和SCL，实现对方位传感器提供的三轴方位角数据的接收；六轴运动传感器芯片的端口SDA和SCL作为I²C串行数据和时钟引脚，通过引线SDA和SCL连接到作为解算单元和主控单元的单片机的I/O端口PB6和PB7，通过该端口实现三轴加速度、三轴角速度、温度以及上述方位角等采样数据向单片机的上传；六轴运动传感器的FSYNC端口通过引线MP_FS连接单片机的I/O端口PA7，实现帧同步数字输入；并且六轴运动传感器的INT端口通过引线MP_INT连接单片机的I/O端口PA6，实现中断控制。方位传感器的数据准备端口DRDY通过引线HM_INT连接单片机的I/O端口PA5，通过查询该引脚的电平，判断是否有新的转换数据。压力传感器的I²C串行数据和时钟端口SDA和SCL作为I²C串行数据和时钟引脚，通过引线SDA和SCL连接单片机的I/O端口PB6和PB7，实现气压传感数据的上传。 

作为装置的主控单元和解算单元，单片机芯片对于姿态解算这种需要大量数学运行的程序，需要更快的处理速度以便进行更多的解算与优化。本装置的各个传感器通过I²C接口与该单片机连接，同时各传感器的数据中断引脚与单片机的I/O相连。使得各传感器在完成ADC轮换后，单片机可在第一时间读取最新的数据，快速响应姿态的变化。这样的连接使得控制器拥有最大的主动权，最快地获取各传感器的状态和转换结果。解算单元获取精确姿态的结果，从而使姿态传感器能够提供精确的俯仰角、横滚角和航向角。 

本实用新型检测自身与地球磁场的相对关系，因而不受地域、封闭环境等影响，并带有压力传感器用于检测气压，以获取自身高度，因而也不受空间海拔的影响，可广泛用于航空、医疗、车辆、物联网、机器人、信息模拟及数字娱乐等各个领域，具有姿态值准确、误差小、无姿态死角、成本低、解算速度 快的有益效果。 

以上所述，仅为本实用新型的具体实施方式，本实用新型还可以应用在其它设备中；以上描述中的尺寸和数量均仅为参考性的，本领域技术人员可根据实际需要选择适当的应用尺寸，而不脱离本实用新型的范围。本实用新型的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本实用新型揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本实用新型的保护范围之内。因此，本实用新型的保护范围应该以权利要求所界定的保护范围为准。 

Claims (3)

1.一种姿态陀螺仪倾斜补偿四元数解算装置，其特征在于，包括：方位传感器，六轴运动传感器，压力传感器，解算单元；所述六轴运动传感器通过I²C接口连接到解算单元，并且所述六轴运动传感器通过中断引脚连接解算单元；所述方位传感器与六轴运动传感器的主I²C接口连接，并且方位传感器的数据准备引脚连接解算单元；所述压力传感器通过I²C总线连接所述解算单元；其中六轴运动传感器芯片的I²C主串行数据和时钟接口用于通过引线外接方位传感器芯片的I²C数据和时钟端口；六轴运动传感器芯片的I²C串行数据和时钟引脚通过引线连接到解算单元的单片机芯片的I/O端口；六轴运动传感器的帧同步数字端口通过引线连接单片机芯片的I/O端口；并且六轴运动传感器的中断端口通过引线连接单片机的I/O端口；方位传感器的数据准备端口通过引线连接单片机芯片的I/O端口；所述压力传感器的I²C串行数据和时钟端口通过引线连接单片机芯片的I/O端口。 

2.根据权利要求1所述的姿态陀螺仪倾斜补偿四元数解算装置，其特征在于，所述六轴运动传感器包括3轴MEMS加速度计、3轴MEMS陀螺仪、一路温度传感器、ADC转换器、信号调节模块、传感器寄存器、主I²C串行接口以及从I²C和SPI接口。 

3.根据权利要求2所述的姿态陀螺仪倾斜补偿四元数解算装置，其特征在于，所述压力传感器包括压力传感器单元、ADC、控制单元以及EPROM。