CN112729297A - 一种基于多mems传感器的微型航姿定位装置 - Google Patents

Abstract

本发明公开了一种基于多MEMS传感器的微型航姿定位装置，其特征在于：包括航向姿态参考模组、中心控制模组、多功能模组、车平台模组、飞行模组、动力模组，所述航向姿态参考模组包括8片MEMS惯性测量元件、航姿功能模组，所述航向姿态参考模组、中心控制模组、多功能模组、飞行模组、动力模组均安装在车平台模组上，所述航向姿态参考模组、多功能模组、车平台模组、飞行模组、动力模组均与中心控制模组电连接。本发明导航准确，功能丰富，成本低廉。

Description

一种基于多MEMS传感器的微型航姿定位装置

技术领域

本发明涉及涉及导航装置领域，特别是一种基于多MEMS传感器的微型航姿定位装置。

背景技术

随着现代科技的发展，智能机器人、无人机、自动驾驶车辆以及物联网技术等智能技术已经成为当前科学研究和人们社会生活关注的热点之一。对物体运动的精确导航定位是这些智能技术发展的重要支撑。虽然目前导航定位手段有很多，比如卫星定位、无线电、雷达、超声波、视觉、射频等等，但这些定位方式都应用场景有限，且易受外界干扰，可靠性差，也无法提供快速、完整的导航信息。因此，目前还没有能够完全能替代惯性导航系统的导航定位手段。

惯性导航系统作为一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。它具有完全自主、对外界环境无要求、不受干扰、输出信息量大、输出信息实时性强等优点，在高精度武器装备、航空航天航海及民用相关领域(如地质勘探、重力测量、石油钻井、大地测量等)已经得到了广泛应用。长期以来，由于价格昂贵，体积和质量大，使用要求高等因素，惯性导航系统主要应用于精确制导武器、飞机、潜艇、舰船等高价值平台上。但随着微机电系统技术的发展，新型MEMS惯性器件的产生并逐渐趋于成熟，这使得使用低成本MEMS惯性器件取代中、低精度的平台式惯导成为可能。

航向姿态参考系统(Attitude and Heading Reference System，AHRS)，简称航姿参考系统，是惯性导航技术的一个重要分支，是由微陀螺仪和微加速度计等MEMS惯性测量元件构成的微小型惯性导航系统。它利用微陀螺仪和微加速度计测出载体的角速度和加速度信号，经过导航计算机的解算，输出载体的实时姿态和航向等信息，再传给载体的姿态控制系统和推进系统提供指令，从而实现确定载体空间位置和姿态的功能。航姿参考系统可以应用于诸多领域，比如：无人飞行器导航定位和自动稳定控制，摄影设备的自动稳定，机器人的控制和稳定，包括汽车和摩托车等车辆的动态性能分析，人体运动测量，战士、消防员、警察和急救队员的训练，虚拟环境中应用于头部跟踪器或者头盔显示器的跟踪等，以及钻井以及管道检测、文物维护等任何需要6自由度运动跟踪等其他民用工业领域的应用，因此设计一种基于多MEMS传感器的微型航姿定位装置，提供低成本、高精度、多功能的导航作业机械解决方案。

发明内容

本发明的目的就是解决现有技术中的问题，提出一种基于多MEMS传感器的微型航姿定位装置，能够使通过采用MEMS器件和嵌入式微处理器，采用低成本器件并通过软件算法校正，大幅减小体积重量和使用成本。通过采用四元素法进行导航姿态解算，采用8片MEMS惯性测量元件阵列进行姿态数据采集，计算量较小且可靠性高，可以适用于全姿态测量，提高航向姿态测量系统的整体精度。

为实现上述目的，本发明提出了一种基于多MEMS传感器的微型航姿定位装置，包括航向姿态参考模组、中心控制模组、多功能模组、车平台模组、飞行模组、动力模组，所述航向姿态参考模组包括8片MEMS惯性测量元件、航姿功能模组，所述航向姿态参考模组、中心控制模组、多功能模组、飞行模组、动力模组均安装在车平台模组上，所述航向姿态参考模组、多功能模组、车平台模组、飞行模组、动力模组均与中心控制模组电连接。

作为优选，所述MEMS惯性测量元件用于采集角速度和加速度数据，所述航姿功能模组用于接收并解算来自MEMS惯性测量元件的数据并传输给中心控制模组，所述MEMS惯性测量元件包括三角轴微陀螺仪、三轴微加速度计、数字运动处理器、和温度传感器。

作为优选，所述航向姿态参考模组的核心算法包括四元素全姿态导航解算和非线性卡尔曼滤波数据融合，所述中心控制模组的核心算法是系统级的误差分析与补偿。

作为优选，所述中心控制模组用于接收和处理来自航向姿态参考模组的数据，所述中心控制模组根据航向姿态参考模组的数据控制车平台模组和飞行模组，所述中心控制模组还用于控制多功能模组，使其采集数据和执行命令。

作为优选，所述多功能模组包括多功能主控板、机械手、采集器组、执行器组，所述采集器组包含若干麦克风、光敏元件、温湿度元件，所述执行器组包括若干喇叭、若干电机、若干舵机、若干LED灯、若干结构件，所述机械手、采集器组、执行器组与多功能主控板电连接。

作为优选，所述车平台模组位于整个装置的最底部，用于支撑整个装置并提供陆地行走能力，所述车平台模组包括车平台主体、车轮组、隐藏模组，所述车平台主体上设置有车平台主控板，所述车平台主控板与中心控制模组电连接。

作为优选，所述隐藏模组包括塑料结构件若干和舵机若干，其作用是将多功能模组封闭或打开，所述舵机和车平台主控板电连接。

作为优选，所述飞行模组包括飞行平台和飞行连接件，所述飞行平台上设置有飞行主控板，所述飞行主控板和中心控制模组电连接，所述飞行连接件用于连接飞行平台和车平台主体。

本发明的有益效果：本发明通过采用MEMS器件和嵌入式微处理器，大幅减小传统惯性导航系统的重量和体积。相比国外同类产品，采用低成本器件，通过软件算法校正，在达到大致相当精度的条件下，大幅减低使用成本。通过采用四元素法进行导航姿态解算，计算量较小且可靠性高，可以实现任意姿态下的导航解算，避免出现全姿态解算过程中的奇异问题，同时还适用于低动态或者静止时的情况。通过采用8片MEMS惯性测量元件阵列进行姿态数据采集，采用四元数微分方程作为系统状态方程，将非线性Kalman滤波算法对陀螺仪和加速度计信息进行数据融合，实现对MEMS陀螺的漂移的抑制，提高航向姿态测量系统的整体精度。

本发明的特征及优点将通过实施例结合附图进行详细说明。

附图说明

图1是本发明一种基于多MEMS传感器的微型航姿定位装置的技术路线图；

图2是本发明一种基于多MEMS传感器的微型航姿定位装置的系统架构图。

具体实施方式

参阅图1-2，本发明一种基于多MEMS传感器的微型航姿定位装置，包括航向姿态参考模组1、中心控制模组2、多功能模组3、车平台模组4、飞行模组5、动力模组6，所述航向姿态参考模组1包括8片MEMS惯性测量元件11、航姿功能模组12，所述航向姿态参考模组1、中心控制模组2、多功能模组3、飞行模组5、动力模组6均安装在车平台模组4上，所述航向姿态参考模组1、多功能模组3、车平台模组4、飞行模组5、动力模组6均与中心控制模组2电连接。

作为优选，所述MEMS惯性测量元件11用于采集角速度和加速度数据，所述航姿功能模组12用于接收并解算来自MEMS惯性测量元件11的数据并传输给中心控制模组2，所述MEMS惯性测量元件11包括三角轴微陀螺仪111、三轴微加速度计112、数字运动处理器113、和温度传感器114。

作为优选，所述航向姿态参考模组1的核心算法包括四元素全姿态导航解算和非线性卡尔曼滤波数据融合，所述中心控制模组2的核心算法是系统级的误差分析与补偿。

作为优选，所述中心控制模组2用于接收和处理来自航向姿态参考模组1的数据，所述中心控制模组2根据航向姿态参考模组1的数据控制车平台模组4和飞行模组5，所述中心控制模组2还用于控制多功能模组3，使其采集数据和执行命令。

作为优选，所述多功能模组3包括多功能主控板121、机械手31、采集器组32、执行器组33，所述采集器组32包含若干麦克风、光敏元件、温湿度元件，所述执行器组33包括若干喇叭、若干电机、若干舵机、若干LED灯、若干结构件，所述机械手31、采集器组32、执行器组33与多功能主控板121电连接。

作为优选，所述车平台模组4位于整个装置的最底部，用于支撑整个装置并提供陆地行走能力，所述车平台模组4包括车平台主体41、车轮组42、隐藏模组44，所述车平台主体41上设置有车平台主控板411，所述车平台主控板411与中心控制模组2电连接。

作为优选，所述隐藏模组44包括塑料结构件若干和舵机若干，其作用是将多功能模组3封闭或打开，所述舵机和车平台主控板411电连接。

作为优选，所述飞行模组5包括飞行平台51和飞行连接件52，所述飞行平台51上设置有飞行主控板52，所述飞行主控板511和中心控制模组2电连接，所述飞行连接件52用于连接飞行平台51和车平台主体41。

本发明工作过程：

本发明一种基于多MEMS传感器的微型航姿定位装置在工作过程中，首先，车平台模组4为装置提供陆地行走功能或者飞行模组5为装置提供空中飞行功能，在行走/飞行过程中，航向姿态参考模组1采集装置的角速度和加速度，解算后发送给中心控制模组2，中心控制模组对数据进行误差校正，得到准确的航向姿态数据。中心控制模组2控制多功能模组3进行一些特定的作业，例如抓取、清扫、声光信号发送等。

本发明一种基于多MEMS传感器的微型航姿定位装置，设备通过硬件设计和算法设计两部分，实现导航准确度提高、设备成本降低、覆盖多种作业领域，具体的提出一些实施例：

实施例1：微型航向姿态参考系统硬件结构以STM32嵌入式微控制器和MEMS惯性传感器为核心，采用IIC进行通信。同时辅以温度传感器，以实现对MEMS传感器随温度变化特性的补偿。

实施例2：以STM32F407ZGT6为主控芯片，8片MEMS惯性测量元件MPU6050传感器，采用IIC进行通信，可对载体在运动过程中的角速度和加速度进行实时采集并储存，及时将数据通过RS485通信接口传给上位机，并在相关软件的辅助下进行数据融合解算，解算出载体在运动过程中的姿态和航向。

实施例3：MPU6050是一款6轴运动器处理器，它集成了三角轴MEMS陀螺仪，三轴MEMS加速计和以及一个可扩展的数字运动处理器(DMP)相结合，可用IIC连接一个第三方的数字传感器，比如磁力计，扩展之后就可以通过其IIC或SPI接口输出一个9轴的信号，同时MPU6050还自带一个温度传感器。采用8片MPU6050列阵进行运动过程中姿态数据的采集，每片MPU6050的SCL和SDA角连接到STM32芯片上，并且其SDA和SCL脚上应加上4.7K的上拉电阻。CLKIN、FSYNC、AD0接地，其VLOCIG脚用于IIC输出提供逻辑电平，REGOUT脚用于校准滤波电容连线。

实施例4：实时全姿态导航解算。航姿参考系统的核心是实时对载体进行姿态估计，即是计算出姿态转换矩阵。姿态转换矩阵实质表征了载体在载体坐标系相对于导航坐标系的旋转关系。采用适用于数值计算的四元数法运算量较小，能够有效的避开奇异点，从而实现全姿态的实时导航解算^[3，4]。

角位置用四元数q表示：

q＝[q₀，q₁，q₂，q₃]^T (1)

四元数由一个实数和一个向量组成；v＝[q₁，q₂，q₃]^T q₀表示实数，表示向量。为了正确地表示一个旋转，四元数必须有单位规范。

计算四元数范数：

采用标准公式进行角度旋转的转换表示从四元数到欧拉角形式。选择欧拉角XYZ序列，而横摇、俯仰、偏航角由下式表示：

给出了时间连续形式的状态方程：

是表示物体旋转的四元数框架，结合到IMU传感器，相对于惯性n-坐标系，

是旋转矩阵：

连续时间系统可以写成：

写为离散方程为：

实施例5：基卡尔曼滤波的姿态数据融合。限于MEMS传感器的精度及稳定性，仅靠单一传感器无法提供稳定的姿态信息^[4，5]。系统采用由微机械陀螺仪作为主传感器输出三轴方向的角速度计算出载体实时姿态四元素值，利用加速度计输出三轴方向的加速度计算载体俯仰角，滚转角，航向角作为观测量。通过卡尔曼滤波进行融合估计，得到实时的航向姿态估计值。

采用8片集成的MEMS惯性6轴运动传感器MPU6050，构成传感器阵列，通过卡尔曼滤波算法，将输出数据进行融合，构成虚拟惯性测量系统，仿真结果表明，虚拟惯性传感器的精度得到了明显提高。

离散型卡尔曼滤波基本方程；

连续线性系统状态方程和量测方程可用以下方程式表达^[6-9]：

其中，X(t)为系统状态矢量，F(t)为状态矩阵，W(t)为系统白噪声过程矢量，W(t)为均值为0的白噪声。G(t)为系统动态噪声矩阵；H(t)为量测矩阵；V(t)为量测白噪声过程矢量，V(t)为均值为0的白噪声。

将连续方程离散化，得以下离散方程：(T为迭代周期)

对于卡尔曼滤波，{W_{k，k＝1，2，K}}和{Vk_{，k＝1，2，K}}为期望为0的白噪声

则有：

Q_k为系统的噪声方差阵，为非负定阵，R_k为量测噪声方差阵，R_k是一个正定矩阵。状态矢量Xk的线性最小方差滤波估计

依照以下方程一步步递推。

状态一步预测：

状态估计：

滤波增益：

一步增益均方误差：

估计均方误差：

实施例6：系统级的误差分析与补偿。MEMS传感器稳定性较差，受温度影响严重。考虑一方面通过对信号误差建模与分析从理论上对传感器误差进行补偿，主要包括：温度补偿，零偏校正，测量误差模型等。另一方面，通过转台实验确定微型航姿参考系统的误差模型系数，并根据实验数据通过统计分析方法分析出随机误差的统计特性进行系统级的融合估计。

建立测量误差模型通过转台试验测得传感器输出静态动态原始数据。使用改进的Allan总方差法进行MEMS陀螺仪随机误差分析^[7，10-14]，可以快速辨识各噪声项。得出误差模型系数。

改进的Allan总方差算法：

记原始数据序列为ω＝{ω₁，ω₂，…，ω_N}，共N个数据，数据的采样间隔记为τ₀。对原始数据进行重新采样分组，假设将原始数据分为了M组，每组含有m个数据，每组数据的平均值作为陀螺的输出序列，此时的数据相关时间变为τ_m＝mτ₀。记重新采样过程的滑动步幅为d。当1＜d＜m，且m是d的整数倍时，称为非完全重叠采样，此时各组数据之间是关联的。

每组数据平均值的通用表达式为

式中

M^*表示所分组数。

改进的总方差法通用表达式为：

在惯性器件中，一般将随机误差分解为量化噪声、角度随机游走、零偏不稳定性、速率随机游走和速率斜坡。其功率谱密度分别为：

将这五个随机误差项的功率谱密度代入到(17)式，得到它们的Allan方差表达式分别为：

式中Q为量化噪声系数，N为角度随机游走系数，B为零偏不稳定性系数，K为速率游走系数，R为速率斜坡系数，f₀表示频率。在σ(τ)～τ的双对数曲线中，不同误差出现在τ的不同区域。由式(19)可知，在双对数曲线中，量化噪声、角度随机游走、零偏不稳定性、速率随机游走和速率斜坡的斜率分别为-1，-0.5，0，+0.5和+1，这使得随机误差的各项系数容易分离和辨识。

误差模型建立：

建立陀螺仪误差模型

式中，b_i为速率随机游走噪声，

为角度随机游走噪声。ω表示真实角速率，对阵列中的每个陀螺都是相同的，

表示第i个陀螺的实际输出：

表示第i个陀螺的角度随机游走噪声，建模为白噪声，方差为Q_ai。i表示陀螺阵列中的第i个陀螺；b_i表示第i个陀螺的速率游走噪声，它建模为白噪声n_bi的积分，n_bi的方差为Q_bi。

上述实施例是对本发明的说明，不是对本发明的限定，任何对本发明简单变换后的方案均属于本发明的保护范围。

Claims (8)

1.一种基于多MEMS传感器的微型航姿定位装置，其特征在于：包括航向姿态参考模组(1)、中心控制模组(2)、多功能模组(3)、车平台模组(4)、飞行模组(5)、动力模组(6)，所述航向姿态参考模组(1)包括8片MEMS惯性测量元件(11)、航姿功能模组(12)，所述航向姿态参考模组(1)、中心控制模组(2)、多功能模组(3)、飞行模组(5)、动力模组(6)均安装在车平台模组(4)上，所述航向姿态参考模组(1)、多功能模组(3)、车平台模组(4)、飞行模组(5)、动力模组(6)均与中心控制模组(2)电连接。

2.根据权利要求1中所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述MEMS惯性测量元件(11)用于采集角速度和加速度数据，所述航姿功能模组(12)用于接收并解算来自MEMS惯性测量元件(11)的数据并传输给中心控制模组(2)，所述MEMS惯性测量元件(11)包括三角轴微陀螺仪(111)、三轴微加速度计(112)、数字运动处理器(113)、和温度传感器(114)。

3.根据权利要求1所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述航向姿态参考模组(1)的核心算法包括四元素全姿态导航解算和非线性卡尔曼滤波数据融合，所述中心控制模组(2)的核心算法是系统级的误差分析与补偿。

4.根据权利要求1所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述中心控制模组(2)用于接收和处理来自航向姿态参考模组(1)的数据，所述中心控制模组(2)根据航向姿态参考模组(1)的数据控制车平台模组(4)和飞行模组(5)，所述中心控制模组(2)还用于控制多功能模组(3)，使其采集数据和执行命令。

5.根据权利要求1所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述多功能模组(3)包括多功能主控板(121)、机械手(31)、采集器组(32)、执行器组(33)，所述采集器组(32)包含若干麦克风、光敏元件、温湿度元件，所述执行器组(33)包括若干喇叭、若干电机、若干舵机、若干LED灯、若干结构件，所述机械手(31)、采集器组(32)、执行器组(33)与多功能主控板(121)电连接。

6.根据权利要求1所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述车平台模组(4)位于整个装置的最底部，用于支撑整个装置并提供陆地行走能力，所述车平台模组(4)包括车平台主体(41)、车轮组(42)、隐藏模组(44)，所述车平台主体(41)上设置有车平台主控板(411)，所述车平台主控板(411)与中心控制模组(2)电连接。

7.根据权利要求5所述的隐藏模组(44)，其特征在于：所述隐藏模组(44)包括塑料结构件若干和舵机若干，其作用是将多功能模组(3)封闭或打开，所述舵机和车平台主控板(411)电连接。

8.根据权利要求1所述的一种基于多MEMS传感器的微型航姿定位装置，其特征在于：所述飞行模组(5)包括飞行平台(51)和飞行连接件(52)，所述飞行平台(51)上设置有飞行主控板(52)，所述飞行主控板(511)和中心控制模组(2)电连接，所述飞行连接件(52)用于连接飞行平台(51)和车平台主体(41)。

Images