基于相互补偿的多陀螺仪系统

技术领域

本发明涉及一种传感系统，特别是陀螺仪传感系统。

背景技术

在精准农业领域，陀螺仪常常应用在包括自动收割机和自动农药喷洒机等在内的机具定位和导航系统中；在智能家居领域，陀螺仪的应用主要包括智能扫地机器和服务机器人等；在智能工业与智能物流领域，陀螺仪主要应用在航姿参考系统，及无人机惯性导航中，包括无人机航姿参考系统、航拍稳定系统和无人机快递系统；在车联网领域，陀螺仪常用在车辆及货运智能管理系统中，例如可对车辆位置、行车轨迹进行监测，也可以对车辆姿态进行管理和求救；在智能医疗领域，陀螺仪的常见应用包括聋哑人手势识别系统和仿生义肢等。

微机械陀螺即MEMS陀螺，也称为硅微陀螺，它的制作是通过采用半导体生产中成熟的沉积、蚀刻和掺杂等工艺，将机械装置和电子线路集成在微小的硅芯片上完成的，最终形成的是一种集成电路芯片大小的微型陀螺仪。MEMS陀螺的最大的短板或者致命的缺点就是存在较大的误差，这就导致最终输出的姿态角度误差(主要是YAW角)比较大，稳定性差，不能满足某些高要求的场景的需求。现有技术计算姿态角度，主要有两种方式，一是利用陀螺积分计算姿态角度，这种方法简单，计算量小，但是对误差的补偿有限，适用于对精度、稳定性要求不高的场景。另外一种是利用数据融合计算姿态角度，这种方法需要加入额外的传感器，比如加速度计或磁力计，增加成本，计算量比较复杂，但对陀螺仪的误差有一定的补偿作用，能保证一定的精度，适用于对精度中等要求的场景。但上述两种方法，都不能满足需要高精度和高稳定度的应用场景的需求。

发明内容

本发明提出了一种基于多陀螺的相互补偿方法，极大补偿MEMS陀螺仪的不确定性误差，提高姿态角度YAW角的输出稳定度和准确度。由于陀螺的误差的随机性是客观存在，而且是不可能完全消除的。本发明提出了一种可以有效消除误差随机性的方法，并能提高姿态角的输出稳定度和准确度。

在概率论和统计学中，数学期望(mean)(或均值，亦简称期望)是试验中每次可能结果的概率乘以其结果的总和，是最基本的数学特征之一。它反映随机变量平均取值的大小。本发明提出使用多个陀螺的数学期望来作为最终进入算法进行数据融合的陀螺仪数据。另外，在姿态角算法系统中，通过对虚拟陀螺仪、加速度计和磁力计的各路采集数据分别进行预处理后送入姿态角解算和数据融合处理模块，也可以提高姿态输出姿态角(YAW)角的精度。因此，通过冗余测量和最优估计得到的输出角速率可以较任何单一元件的性能更稳定。

本发明提供一种虚拟陀螺仪模块。该模块包括陀螺矩阵(1)、数据获取模块(2)和算法补偿模块(3)。其中陀螺矩阵(1)由N个陀螺仪构成(N为大于等于2的整数)，N个陀螺仪可以是不同类型，不同厂家的陀螺仪，也可以是同一类型，同一厂家的陀螺仪。数据获取模块(2)，用于高速采集N个陀螺仪的数据输出，并传输给后面的算法补偿模块(3)。算法补偿模块用于接收数据获取模块高速采集的N路陀螺仪数据，经过随机误差补偿和最优估计输出虚拟陀螺仪数据。输出的虚拟陀螺仪数据提供给后级的姿态角算法系统。其中，算法补偿模块包括随机误差模型模块(4)和最优估计模块(5)。算法补偿模块中的随机误差模型模块，用于接收数据获取模块高速采集的N路陀螺仪数据，对接收的N路陀螺仪数据进行平滑滤波和数据处理，数据经过随机误差模型模块的平滑滤波处理后，送入最优估计模块，最优估计模块对平滑滤波后的数据采用数学期望的方法，对数据加权平均获得最终的最优估计输出，作为虚拟陀螺仪输出。

本发明提供一种姿态角算法系统，该系统包括传感器输入模块、数据获取模块和姿态角计算模块。其中传感器输入模块包括虚拟陀螺仪模块、加速度计和磁力计。姿态角计算模块(3)包括数据预处理模块(4)、姿态解算和数据融合处理模块(5)。数据获取模块用于采集虚拟陀螺仪模块、加速度计和磁力计的输出数据。数据获取模块将采集到的数据送入数据预处理模块，进行相应的校准滤波，减除常值偏移等处理。

在数据预处理模块中，针对从加速度计采集的数据，先将采集到数据Rawa经过校准系数运算后，生成校准后Calibrated数据，再经过一维Kalman滤波处理获得预处理后的加速度数据Acc。

在数据预处理模块中，针对从虚拟陀螺仪模块采集到的数据，先将原始数据Rawg经过校准系数运算后，生成校准后Calibrated数据，再经过实时校准模块的滤波处理获得预处理后的虚拟陀螺仪数据Gyro。

在数据预处理模块中，针对从磁力计采集到的数据，先将采集到的数据Rawm经过校准系数运算后，生成校准后Calibrated数据，再经过一维Kalman滤波处理获得预处理后的磁力计输出数据Mag。

Acc/Gyro/Mag分别表示经过预处理模块分别处理后得到的加速度计、虚拟陀螺仪模块和磁力计数据。

经过数据预处理模块处理后获得的加速度计、虚拟陀螺仪和磁力计数据送入姿态解算和数据融合模块，通过姿态解算和数据融合处理，最终输出高精度的姿态角数值。姿态角输出，主要包括翻滚角(Roll)，俯仰角(Pitch)和航向角(Yaw)输出，也可以输出用于3D图像展示的四元数等信息。

附图说明

图1虚拟陀螺仪模块结构图

图2姿态角算法系统

具体实施方式

图1为虚拟陀螺仪模块结构。该模块包括陀螺矩阵(1)、数据获取模块(2)和算法补偿模块(3)。其中陀螺矩阵(1)由N个陀螺仪构成(N为大于等于2的整数)，N个陀螺可以是不同类型，不同厂家的陀螺仪，也可以是同一类型，同一厂家的陀螺仪。数据获取模块(2)，用于高速采集陀螺仪的数据输出，并传输给后面的算法补偿模块(3)。算法补偿模块用于接收数据获取模块高速采集的N路陀螺仪数据，最终经过随机误差补偿和最优估计输出虚拟陀螺数据给姿态角算法系统。其中，算法补偿模块包括随机误差模型模块(4)和最优估计模块(5)。算法补偿模块中的随机误差模型模块，用于接收数据获取模块高速采集的N路陀螺仪数据，对接收的N路陀螺仪数据进行平滑滤波和数据处理，获取陀螺仪的最优估计值。在随机误差模型模块内部，我们设计了多个滑动窗口(窗口大小m)对每一路陀螺数据进行平滑滤波处理。可以采用如下公式：

G1＝[G1(t0)+G1(t1)+…+G1(tm)]/m；

G2＝[G2(t0)+G2(t1)+…+G2(tm)]/m；

……

Gn＝[Gn(t0)+Gn(t1)+…+Gn(tm)]/m；

……

GN＝[GN(t0)+GN(t1)+…+GN(tm)]/m；

以上公式中，

-t0,t1,…tm分别是不同采样时刻；

-Gn(tm)是第n路的陀螺仪在tm时刻的采样值；

-Gn为平滑滤波处理后的第n路陀螺仪数据的输出。

数据获取模块(2)采集获得的N路陀螺仪数据经过滤波随机误差模型模块的平滑滤波处理后，送入最优估计模块，最优估计模块对平滑滤波后的数据进行概率分析，通过概率分析的方法对输入的数据评估计算，获得最优的陀螺仪的输出，作为虚拟陀螺输出送入后级姿态角算法系统。

在最优估计模块中利用概率分析原理，采用数学期望的方法，将经过平滑滤波处理后的N路陀螺仪数据加权平均获得最终的最优估计输出，采用如下公式：

G＝E[G1,G2,…Gn…GN]

以上公式中：

Gn表示第n路陀螺仪经过平滑滤波处理后的输出。

E表示对所有N路的陀螺仪输出求取数学期望。

G为最终最优估计输出，即虚拟陀螺仪输出。

图2为姿态角算法系统的原理图。该系统包括传感器输入模块、数据获取模块和姿态角计算模块。其中传感器输入模块包括虚拟陀螺仪模块、加速度计和磁力计，虚拟陀螺仪模块即为图1所示的虚拟陀螺仪模块。姿态角计算模块(3)包括数据预处理模块(4)、姿态解算和数据融合处理模块(5)。数据获取模块用于采集虚拟陀螺仪模块、加速度计和磁力计的输出数据。即图1所示的虚拟陀螺仪模块系统输出的N路陀螺仪的最优估计值、加速度计的输出值和磁力计的输出值传入数据获取模块。数据获取模块，用于高速采集虚拟陀螺仪模块、加速度计和磁力计的数据输出，并传输给后续的数据预处理模块(4)。采集到的数据进入数据预处理模块，进行相应的校准滤波，减除常值偏移处理。

在数据预处理模块中，针对从加速度计采集的数据，先将采集到数据Rawa经过校准系数运算后，生成校准后Calibrated数据，再经过一维Kalman滤波处理获得预处理后的加速度数据Acc。计算公式如下：Acc＝FK(CoeffKa*Rawa+CoeffBa)。

在数据预处理模块中，针对从虚拟陀螺仪模块采集到的数据，先将原始数据Rawg经过校准系数运算后，生成校准后Calibrated数据，再经过实时校准模块的滤波处理获得预处理后的输出虚拟陀螺仪数据Gyro。计算公式如下：Gyro＝CoeffKg*Rawg+CoeffBg–GyroBias

在数据预处理模块中，针对从磁力计采集到的数据，先将采集到的数据Rawm经过校准系数运算后，生成校准后Calibrated数据，再经过一维Kalman滤波处理获得预处理后的磁力计输出数据Mag。计算公式如下：Mag＝FK(CoeffKm*Rawm+CoeffBm)

针对以上公式中的各项参数予以说明：

Acc/Gyro/Mag分别表示经过预处理模块分别处理后得到的加速度计、虚拟陀螺仪和磁力计数据。

FK()表示kalman处理函数。

CoeffK，CoeffB表示校准系数的两个部分。

CoeffKa，CoeffBa表示加速度计的校准系数。

CoeffKg，CoeffBg表示陀螺仪的校准系数。

CoeffKm，CoeffBm表示磁力计的校准系数。

GyroBias表示陀螺仪实时校准模块的输出。

经过数据预处理模块处理后获得的加速度计、虚拟陀螺仪和磁力计数据送入姿态解算和数据融合模块(5)，通过姿态解算和数据融合处理，最终输出高精度的姿态角数值。姿态角输出，主要包括翻滚角(Roll)，俯仰角(Pitch)和航向角(Yaw)输出，也可以输出用于3D图像展示的四元数等信息。此模块的姿态解算和数据融合处理主要采用常规的捷联惯导算法，在此就不详细描述了。

本发明技术方案能比较准确的估计出GYRO不确定性的误差，进而通过补偿，大幅提高姿态输出姿态角(YAW)角的精度。

本发明实施例中处理器可以是通用处理器，例如但不限于，中央处理器(CentralProcessing Unit，CPU)，也可以是专用处理器，例如但不限于，数字信号处理器(DigitalSignal Processor，DSP)、应用专用集成电路(Application Specific IntegratedCircuit，ASIC)和现场可编程门阵列(Field Programmable Gate Array，FPGA)等。此外，处理器还可以是多个处理器的组合。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的模块及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。