《一种姿态陀螺仪倾斜补偿四元数解算装置.pdf》由会员分享，可在线阅读，更多相关《一种姿态陀螺仪倾斜补偿四元数解算装置.pdf（10页珍藏版）》请在专利查询网上搜索。
1、(10)申请公布号 CN 104316080 A (43)申请公布日 2015.01.28 CN 104316080 A (21)申请号 201410537979.6 (22)申请日 2014.10.13 G01C 25/00(2006.01) G01C 19/00(2013.01) (71)申请人 许志修 地址 430200 湖北省襄阳市樊城区铁沁花园 10 号楼 102 申请人 武汉创客数字科技有限公司 (72)发明人 许志修 (74)专利代理机构 北京恒都律师事务所 11395 代理人 李向东 (54) 发明名称 一种姿态陀螺仪倾斜补偿四元数解算装置 (57) 摘要 本发明提供了一种姿态。
2、陀螺仪倾斜补偿四元 数解算装置， 包括方位传感器， 用于感应地磁向 量从而计算航向夹角 ； 六轴运动传感器， 用于检 测装置运动的方向和速度， 计算三轴加速度和三 轴角速度 ； 压力传感器， 用于检测气压， 计算装置 高度值 ； 解算单元， 用于对所述三轴加速度、 三轴 角速度以及航向夹角进行实时倾斜补偿四元数解 算并且以卡尔曼滤波以及高度值进行数据优化处 理， 获得精确俯仰角、 横滚角、 航向角以及高度实 时参数。本发明能够消除基于陀螺仪实现的姿态 传感器发生倾斜时的数据误差， 从而使姿态传感 器能够提供精确的俯仰角、 横滚角和航向角， 具有 姿态值准确、 误差小、 无姿态死角、 成本低、 。
3、解算速 度快的有益效果。 (51)Int.Cl. 权利要求书 1 页 说明书 4 页 附图 4 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书1页 说明书4页 附图4页 (10)申请公布号 CN 104316080 A CN 104316080 A 1/1 页 2 1. 一种姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 包括 ： 方位传感器， 用于感 应地磁向量从而计算航向夹角 ； 六轴运动传感器， 用于检测装置运动的方向和速度， 计算三 轴加速度和三轴角速度 ； 压力传感器， 用于检测气压， 计算装置高度值 ； 解算单元， 用于对 所述三轴加速度、 三轴角速度。
4、以及航向夹角进行实时倾斜补偿四元数解算并且以卡尔曼滤 波以及高度值进行数据优化处理， 获得精确俯仰角、 横滚角、 航向角以及高度实时参数。 2. 根据权利要求 1 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述六 轴运动传感器通过 I2C 接口连接到解算单元， 并且所述六轴运动传感器通过中断引脚连接 解算单元。 3. 根据权利要求 2 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述方 位传感器与六轴运动传感器的主 I2C 接口连接， 并且方位传感器的数据准备引脚连接解算 单元。 4. 根据权利要求 3 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述压 。
5、力传感器通过 I2C 总线连接所述解算单元。 5. 根据权利要求 4 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述六 轴运动传感器包括 3 轴 MEMS 加速度计、 3 轴 MEMS 陀螺仪、 一路温度传感器、 ADC 转换器、 信 号调节模块、 传感器寄存器、 主 I2C 串行接口以及从 I2C 和 SPI 接口。 6. 根据权利要求 5 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述压 力传感器包括压力传感器单元、 ADC、 控制单元以及 EPROM。 7. 根据权利要求 6 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 六轴运 动传感器芯片的 I2。
6、C 主串行数据和时钟接口用于通过引线外接方位传 感器芯片的 I2C 数 据和时钟端口 ； 六轴运动传感器芯片的 I2C 串行数据和时钟引脚通过引线连接到解算单元 的单片机芯片的 I/O 端口 ； 六轴运动传感器的帧同步数字端口通过引线连接单片机芯片的 I/O端口 ； 并且六轴运动传感器的中断端口通过引线连接单片机的I/O端口 ； 方位传感器的 数据准备端口通过引线连接单片机芯片的 I/O 端口 ； 所述压力传感器的 I2C 串行数据和时 钟端口通过引线连接单片机芯片的 I/O 端口。 8. 根据权利要求 6 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 所述解 算单元获得所述由六轴运。
7、动传感器、 方位传感器、 压力传感器提供的航向夹角、 角速度、 加 速度、 气压数据， 将所获得的各类型数据分别以6次队列方式储存 ； 由解算单元将10次数据 通过卡尔曼滤波算法进行干扰矫正 ； 基于矫正后的数据获取自身姿态数据。 9. 根据权利要求 7 所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 在低速 运动时， 以航向夹角通过卡尔曼滤波算法进行干扰矫正后进行自身姿态数据解算 ； 在高速 运动时， 以角速度通过卡尔曼滤波算法进行干扰矫正后进行自身姿态数据解算。 权 利 要 求 书 CN 104316080 A 2 1/4 页 3 一种姿态陀螺仪倾斜补偿四元数解算装置 技术领域 0。
8、001 本发明涉及陀螺仪领域， 更具体地， 涉及一种姿态陀螺仪倾斜补偿四元数解算装 置。 背景技术 0002 陀螺仪在高速旋转时， 能够抗拒任何外力和干扰的影响， 保持其自转轴相对于惯 性空间方向上稳定不变。 因此， 通过测量相对于陀螺仪的转轴的夹角变化， 能够实现对承载 陀螺仪的设备 ( 如飞行器 ) 姿态偏离的检测和校正。此种利用陀螺仪实现的控制系统也被 称之为姿态稳定系统。 0003 目前市场上所使用的陀螺仪大多为磁罗盘磁阻传感器， 磁罗盘在倾斜时会出现磁 偏移的情况， 在倾斜到 90 度时会产生到 180 度左右的巨大误差。因而目前陀螺仪大多只能 在自身保持水平的情况下正常工作。现有技。
9、术中可采用机械结构悬挂陀螺仪， 使其一直保 持为水平状态， 以达到正确测量的结果， 或者使用价格高昂的激光陀螺仪。 以上解决方案都 会使陀螺仪的结构复杂化， 提高产品成本。 发明内容 0004 为了克服现有技术中的上述缺陷， 本发明提供了一种姿态陀螺仪倾斜补偿四元数 解算装置。本发明的目的是消除基于陀螺仪实现的姿态传感器发生倾斜时的数据误差， 从 而使姿态传感器能够提供精确的俯仰角、 横滚角和航向角。 0005 本发明所述的姿态陀螺仪倾斜补偿四元数解算装置， 其特征在于， 包括 ： 方位传感 器， 用于感应地磁向量从而计算航向夹角 ； 六轴运动传感器， 用于检测装置运动的方向和速 度， 计算三。
10、轴加速度和三轴角速度 ； 压力传感器， 用于检测气压， 计算装置高度值 ； 解算单 元， 用于对所述三轴加速度、 三轴角速度以及航向夹角进行实时倾斜补偿四元数解算并且 以卡尔曼滤波以及高度值进行数据优化处理， 获得精确俯仰角、 横滚角、 航向角以及高度实 时参数。 0006 优选的是， 所述六轴运动传感器通过 I2C 接口连接到解算单元， 并且所述六轴运 动传感器通过中断引脚连接解算单元。 0007 优选的是， 所述方位传感器与六轴运动传感器的主 I2C 接口连接， 并且方位传感 器的数据准备引脚连接解算单元。 0008 优选的是， 所述压力传感器通过 I2C 总线连接所述解算单元。 0009。
11、 优选的是， 所述六轴运动传感器包括3轴MEMS加速度计、 3轴MEMS陀螺仪、 一路温 度传感器、 ADC 转换器、 信号调节模块、 传感器寄存器、 主 I2C 串行接口以及从 I2C 和 SPI 接 口。 0010 优选的是， 所述压力传感器包括压力传感器单元、 ADC、 控制单元以及 EPROM。 0011 优选的是， 六轴运动传感器芯片的 I2C 主串行数据和时钟接口用于通过引线外接 方位传感器芯片的 I2C 数据和时钟端口 ； 六轴运动传感器芯片的 I2C 串行数据和时钟引脚 说 明 书 CN 104316080 A 3 2/4 页 4 通过引线连接到解算单元的单片机芯片的 I/O 。
12、端口 ； 六轴运动传感器的帧同步数字端口通 过引线连接单片机芯片的 I/O 端口 ； 并且六轴运动传感器的中断端口通过引线连接单片机 的 I/O 端口 ； 方位传感器的数据准备端口通过引线连接单片机芯片的 I/O 端口 ； 所述压力 传感器的 I2C 串行数据和时钟端口通过引线连接单片机芯片的 I/O 端口。 0012 优选的是， 所述解算单元以卡尔曼滤波和压力传感器计算出的高度值进行数据优 化处理获得精确的俯仰角、 横滚角和航向角。 进一步优选的是， 所述解算单元获得所述由六 轴运动传感器、 方位传感器、 压力传感器提供的航向夹角、 角速度、 加速度、 气压数据， 将所 获得的各类型数据分别。
13、以6次队列方式储存 ； 由解算单元将10次数据通过卡尔曼滤波算法 进行干扰矫正 ； 基于矫正后的数据获取自身姿态数据。更进一步优选的是， 在低速运动时， 以航向夹角通过卡尔曼滤波算法进行干扰矫正后进行自身姿态数据解算 ； 在高速运动时， 以角速度通过卡尔曼滤波算法进行干扰矫正后进行自身姿态数据解算。 0013 本发明检测自身与地球磁场的相对关系， 因而不受地域、 封闭环境等影响， 并带有 压力传感器用于检测气压， 以获取自身高度， 因而也不受空间海拔的影响， 可广泛用于航 空、 医疗、 车辆、 物联网、 机器人、 信息模拟及数字娱乐等各个领域， 具有姿态值准确、 误差 小、 无姿态死角、 成本。
14、低、 解算速度快的有益效果。 附图说明 0014 下面结合附图和具体实施方式对本发明作进一步详细的说明 ： 0015 图 1 是本发明所述的姿态陀螺仪倾斜补偿四元数解算装置结构示意图 ； 0016 图 2 是本发明所述六轴运动传感器与解算单元电路连接关系示意图 ； 0017 图 3 是本发明所述方位传感器的电路连接关系示意图 ； 0018 图 4 是本发明所述压力传感器与解算单元电路连接关系示意图 ； 0019 图 5 是本发明所述六轴运动传感器的芯片电路示意图 ； 0020 图 6 是本发明所述方位传感器的芯片电路示意图 ； 0021 图 7 是本发明所述压力传感器的芯片电路示意图 ； 00。
15、22 图 8 是本发明所述解算单元的单片机芯片电路示意图。 具体实施方式 0023 为了使本技术领域的人员更好地理解本发明的技术方案， 并使本发明的上述目 的、 特征和优点能够更加明显易懂， 下面结合实施例及实施例附图对本发明作进一步详细 的说明。 0024 图 1 是本发明所述的姿态陀螺仪倾斜补偿四元数解算装置结构示意图， 参见该附 图， 所述装置包括 ： 方位传感器， 用于感应地磁向量从而计算航向夹角 ； 六轴运动传感器， 用于检测装置运动的方向和速度， 计算三轴加速度和三轴角速度 ； 压力传感器， 用于检测 气压， 计算装置高度值 ； 解算单元， 用于对所述三轴加速度、 三轴角速度以及航。
16、向夹角进行 实时倾斜补偿四元数解算并且以卡尔曼滤波以及高度值进行数据优化处理， 获得精确俯仰 角、 横滚角、 航向角以及高度实时参数。 0025 本装置的方位传感器、 六轴运动传感器以及压力传感器以 3.3V 电源 VDD 供电， 与 实现解算单元的单片机之间以 I2C 传输方式通信。并且数据可以通过装置的 UART 接口以 说 明 书 CN 104316080 A 4 3/4 页 5 RS232 串口的形式与 PC 或其他设备等上位机通信， 可根据上位机发送的命令， 以改变其发 送频率、 初始值、 偏移量等参数。单片机烧写调试使用 SWD 接口。使用两个 LED 灯指示装置 的供电与通信状态。
17、。 0026 图 2 是所述六轴运动传感器与解算单元电路连接关系示意图。本发明所述的六轴 运动传感器通过 I2C 接口连接到作为解算单元和主控制器的单片机。在初始化阶段， 单片 机将要设置该六轴运动传感器的测量感度和采样频率， 同时使能数据更新中断。之后单片 机配置自己的引脚为中断输入INT。 这样在一个新的采样完成后六轴运动传感器产生中断， 呼叫单片机读取新的数据。 0027 图 3 是所述方位传感器的电路连接关系示意图。方位传感器并没有直接与单片机 的I2C接口连接， 而是与六轴运动传感器的AUXI2C接口连接， 这样， 使单片机有两种方式访 问方位传感器的数据。 先将六轴运动传感器的AU。
18、X接口设置成直连式， 单片机可以通过I2C 直接访问方位传感器 ； 然后开启六轴运动传感器的 AUX 接口为主控制器， 通过 AUXI2C 接口 六轴运动传感器负责采集方向传感器的采样结果,并将结果存放在自己的FIFO寄存器中， 单片机通过访问六轴运动传感器读取方位传感器的转换结果。 以上两个方式都不需要额外 的增加或者是修改电路， 只需要设置六轴运动传感器相应的控制寄存器。方位传感器的数 据准备引脚 DRDY 连接到了单片机， 通过查询该引脚的电平， 判断是否有新的转换数据。 0028 图 4 是压力传感器与解算单元电路连接关系示意图。压力传感器挂接在单片机的 I2C 总线上， 从而单片机可。
19、以直接访问读取压力传感器的转换数据。 0029 下面介绍本发明所述姿态陀螺仪倾斜补偿四元数解算装置的六轴运动传感器、 方 位传感器、 压力传感器的内部架构。 0030 六轴运动传感器内部集成了 3 轴 MEMS 加速度计、 3 轴 MEMS 陀螺仪以及一个可扩 展的数字运动处理器 DMP ； 并且可以利用 I2C 接口连接一个第三方的数字传感器， 即本发明 中作为方位传感器的三轴磁力计。在扩展之后， 六轴运动传感器就可以通过其 I2C 或 SPI 接口输出一个 9 轴的信号。六轴运动传感器的 3 轴 MEMS 加速度计包括 X、 Y、 Z 三轴上的加 速度测量模块， 3 轴 MEMS 陀螺仪包。
20、括 X、 Y、 Z 三轴上的角速度测量模块， 以及一路温度传感 器。 为了精确跟踪快速和慢速的运动， 传感器的测量范围都是用户可控的， 陀螺仪可测范围 为 250， 500， 1000， 2000 / 秒 (dps)， 加速度计可测范围为 2， 4， 8， 16g。 采用 16 位的 ADC， 将其测量的模拟量转化为可输出的数字量。信号调节模块以数字输出包 括 6 轴或 9 轴的旋转矩阵、 四元数 (quaternion)、 欧拉角格式 (Euler Angle forma) 的融 合演算数据至传感器寄存器。在对外信号接口方面， 包括 ： 主 I2C 串行接口， 通过串行接口 Bypass 以。
21、 AUX_CL 和 AUX_DA 端口实现主串行数据以及主串行时钟连接 ； 以及从 I2C 和 SPI 接口， 通过SDO端口提供SPI串行数据输出， 并且通过SDA和SCL提供I2C串行数据和时钟。 FSYNC 端口提供帧同步数字输入。具有中断状态寄存器， INT 端口提供中断数字输出。 0031 方位传感器采用磁阻式三轴传感器并应用特殊辅助电路来测量磁场。 通过施加供 电电源， 该传感器可以将量测轴方向上的任何入射磁场转变成一种差分电压输出。磁阻传 感器是由一个镍铁 ( 坡莫合金 ) 薄膜放置在硅片上， 并构成一个带式电阻元件。在磁场存 在的情况下， 桥式电阻元件的变化将引起跨电桥输出电压。
22、的相应变动。这些磁阻元件两两 对齐， 形成一个共同的敏感轴， 随着磁场在敏感方向上不断增强， 电压也就正向增长。因为 输出只与沿着轴方向上的磁阻元件成比例， 其他磁阻电桥也放置在正交方向上， 就能精密 说 明 书 CN 104316080 A 5 4/4 页 6 测量其他方向的磁场强度。 0032 压力传感器的内部架构包括压力传感器单元、 ADC、 控制单元以及 EPROM。 0033 图5至图8分别示出了本发明所述姿态陀螺仪倾斜补偿四元数解算装置的六轴运 动传感器、 方位传感器、 压力传感器以及解算单元的芯片电路图。如图 5 所示的六轴运动传 感器的芯片电路， 其中 AUX_DA 和 AUX。
23、_CL 端口作为 I2C 主串行数据和时钟接口， 用于通过引 线 AUX_SDA 和 AUX_SCL 外接方位传感器的芯片 I2C 数据和时钟端口 SDA 和 SCL， 实现对方 位传感器提供的三轴方位角数据的接收 ； 六轴运动传感器芯片的端口 SDA 和 SCL 作为 I2C 串行数据和时钟引脚， 通过引线 SDA 和 SCL 连接到作为解算单元和主控单元的单片机的 I/ O端口PB6和PB7， 通过该端口实现三轴加速度、 三轴角速度、 温度以及上述方位角等采样数 据向单片机的上传 ； 六轴运动传感器的 FSYNC 端口通过引线 MP_FS 连接单片机的 I/O 端口 PA7， 实现帧同步数。
24、字输入 ； 并且六轴运动传感器的INT端口通过引线MP_INT连接单片机的 I/O 端口 PA6， 实现中断控制。方位传感器的数据准备端口 DRDY 通过引线 HM_INT 连接单片 机的 I/O 端口 PA5， 通过查询该引脚的电平， 判断是否有新的转换数据。压力传感器的 I2C 串行数据和时钟端口 SDA 和 SCL 作为 I2C 串行数据和时钟引脚， 通过引线 SDA 和 SCL 连接 单片机的 I/O 端口 PB6 和 PB7， 实现气压传感数据的上传。 0034 作为装置的主控单元和解算单元， 单片机芯片对于姿态解算这种需要大量数学运 行的程序， 需要更快的处理速度以便进行更多的解算。
25、与优化。 本装置的各个传感器通过I2C 接口与该单片机连接， 同时各传感器的数据中断引脚与单片机的 I/O 相连。使得各传感器 在完成 ADC 轮换后， 单片机可在第一时间读取最新的数据， 快速响应姿态的变化。这样的连 接使得控制器拥有最大的主动权， 最快地获取各传感器的状态和转换结果。 0035 解算单元所执行的姿态陀螺仪倾斜补偿四元数解算是以卡尔曼滤波和压力传感 器计算出的高度值进行数据优化处理， 以达到获取精确姿态的结果， 从而使姿态传感器能 够提供精确的俯仰角、 横滚角和航向角。 姿态陀螺仪倾斜补偿四元数解算的具体过程如下 ： 解算单元以 500 次 / 秒的频率， 通过 I2C 接口。
26、读取由六轴运动传感器、 方位传感器、 压力传 感器提供的航向夹角、 角速度、 加速度、 气压数据， 将所获得的各类型数据分别以 6 次队列 方式储存 ； 由解算单元将 10 次数据通过卡尔曼滤波算法进行干扰矫正， 在设备低速运动时 进行航向夹角数值矫正， 在高速运动时以六轴运动传感器提供角速度进行解算， 以达到实 时同步无偏差 ； 将矫正的最后 4 次数据进行解算， 获取自身姿态数据 ； 并且解算单元将解算 结果通过 R232 通信接口以预定的数据速率和波特率进行输出。 0036 本发明检测自身与地球磁场的相对关系， 因而不受地域、 封闭环境等影响， 并带有 压力传感器用于检测气压， 以获取自。
27、身高度， 因而也不受空间海拔的影响， 可广泛用于航 空、 医疗、 车辆、 物联网、 机器人、 信息模拟及数字娱乐等各个领域， 具有姿态值准确、 误差 小、 无姿态死角、 成本低、 解算速度快的有益效果。 0037 以上所述， 仅为本发明的具体实施方式， 本发明还可以应用在其它设备中 ； 以上 描述中的尺寸和数量均仅为参考性的， 本领域技术人员可根据实际需要选择适当的应用尺 寸， 而不脱离本发明的范围。 本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术 人员在本发明揭露的技术范围内， 可轻易想到的变化或替换， 都应涵盖在本发明的保护范 围之内。因此， 本发明的保护范围应该以权利要求所界定的保护范围为准。 说 明 书 CN 104316080 A 6 1/4 页 7 图 1 图 2 说 明 书 附 图 CN 104316080 A 7 2/4 页 8 图 3 图 4 说 明 书 附 图 CN 104316080 A 8 3/4 页 9 图 5 图 6 图 7 说 明 书 附 图 CN 104316080 A 9 4/4 页 10 图 8 说 明 书 附 图 CN 104316080 A 10 。