一种补偿陀螺仪角速度数据的方法、装置、陀螺仪和系统与流程

本发明涉及传感器技术领域，特别涉及一种补偿陀螺仪角速度数据的方法、装置、陀螺仪和补偿陀螺仪角速度数据的系统。

背景技术：

陀螺仪在进行姿态角的测量时会存在累计误差，即本次计算的姿态角是基于前一次计算的姿态角的基础上获得的，这样就会导致陀螺仪上一次测量的误差累计到本次的结果中，进而随着时间的推移使得陀螺仪测量结果中的误差越来越大，影响陀螺仪的准确性。因此，减小或者消除累计误差，是陀螺仪测量准确与否的重要保证。但是，目前还无法实现对陀螺仪累计误差的消除。

技术实现要素：

鉴于上述问题，提出了本发明的一种补偿陀螺仪角速度数据的方法、装置、陀螺仪和补偿陀螺仪角速度数据的系统，以便消除陀螺仪的累积误差。

根据本发明的一个方面，提供了一种补偿陀螺仪角速度数据的方法，所述方法包括：

根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的第一时刻的磁场数据计算第一组四元数，以及根据所述第一时刻的加速度数据计算第一时刻的加速度值；

判断所述第一时刻的加速度值是否在预设加速度值范围内；

若加速度值在预设加速度值范围内，根据所述第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算所述陀螺仪的补偿系数和零偏值；

使用所述补偿系数和所述零偏值补偿所述陀螺仪采集的第二时刻的角速度数据，获得所述陀螺仪补偿后的第二时刻的角速度数据。

根据本发明的另一个方面，提供了一种补偿陀螺仪角速度数据的装置，所述装置包括：

第一计算单元，用于根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的第一时刻的磁场数据计算第一组四元数，以及根据所述加速度数据计算第一时刻的加速度值；

判断单元，用于判断所述第一时刻的加速度值是否在预设加速度值范围内；

第二计算单元，用于若加速度值在预设加速度值范围内，根据所述第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算所述陀螺仪的补偿系数和零偏值；

补偿单元，用于使用所述补偿系数和所述零偏值补偿所述陀螺仪采集的第二时刻的角速度数据，获得所述陀螺仪补偿后的第二时刻的角速度数据。

根据本发明的又一个方面，提供了一种补偿陀螺仪角速度数据的装置，所述装置包括存储器和处理器，所述存储器和所述处理器之间通过内部总线通讯连接，所述存储器存储有能够被所述处理器执行的计算机程序，所述计算机程序被所述处理器执行时能够实现前述的方法步骤。

根据本发明的再一个方面，提供了一种陀螺仪，所述陀螺仪包括如前所述的补偿陀螺仪角速度数据的装置。

根据本发明的再一个方面，提供了一种补偿陀螺仪角速度数据的系统，所述系统包括如前所述的陀螺仪、加速度传感器和磁力计；

所述加速度传感器，用于采集第一时刻的加速度数据，并将所述第一时刻的加速度数据发送给所述陀螺仪；

所述磁力计，用于采集第一时刻的磁场数据，并将所述第一时刻的磁场数据发送给所述陀螺仪；

所述陀螺仪，用于根据接收到的所述第一时刻的加速度数据和所述第一时刻的磁场数据计算第一组四元数，以及根据所述第一时刻的加速度数据计算第一时刻的加速度值；判断所述第一时刻的加速度值是否在预设加速度值范围内；若加速度值在预设加速度值范围内，根据所述第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算所述陀螺仪的补偿系数和零偏值；使用所述补偿系数和所述零偏值补偿所述陀螺仪采集的第二时刻的角速度数据，获得所述陀螺仪补偿后的第二时刻的角速度数据。

综上所述，本发明的有益效果是：首先判断加速度传感器采集的加速度值是否在预设加速度值范围内，若判断为是，说明相比较存在累计误差的陀螺仪计算的四元数来说，利用加速度传感器和磁力计计算的四元数是比较准确的，然后根据比较准确的四元数获得陀螺仪的补偿系数和零偏值，该补偿系数和零偏值标定了陀螺仪当前的累计误差，使用该补偿系数和零偏值补偿陀螺仪的角速度数据，可以补偿陀螺仪的累积误差。可见，本发明的技术方案利用加速度传感器和磁力计采集的数据对陀螺仪角速度数据补偿，可以消除陀螺仪的累计误差，保证陀螺仪准确性，增强用户体验。

附图说明

图1为本发明一个实施例提供的一种补偿陀螺仪角速度数据的方法流程示意图；

图2为本发明一个实施例提供的一种补偿陀螺仪角速度数据的装置的功能结构示意图；

图3为本发明另一个实施例提供的一种补偿陀螺仪角速度数据的装置的功能结构示意图；

图4为本发明一个实施例提供的一种陀螺仪的功能结构示意图；

图5为本发明一个实施例提供的一种补偿陀螺仪角速度数据的系统的功能结构示意图。

具体实施方式

本发明的设计思路是：因目前无法对陀螺仪的累积误差进行消除，即使采用融合算法也只能减少累积误差，又考虑到加速度传感器采集的加速度值接近重力加速度值的理想状态时，利用加速度传感器和磁力计计算的四元数是相对准确的，本发明利用该相对准确的四元数获得陀螺仪的补偿系数和零偏值，标定陀螺仪的累计误差，再用该补偿系数和零偏值补偿陀螺仪的角速度数据时，可以将陀螺仪的累积误差消除。为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

图1为本发明一个实施例提供的一种补偿陀螺仪角速度数据的方法流程示意图。如图1所示，该方法包括：

步骤s110，根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的第一时刻的磁场数据计算第一组四元数，以及根据第一时刻的加速度数据计算第一时刻的加速度值。

四元数通过姿态角计算。这里的姿态角包括俯仰角、翻滚角和航向角，因为在计算过程中角度的正弦、余弦、正切等计算不方便，所以引入了四元数的概念，四元数的目的就是为了简化计算。

本实施例中，步骤s110中的根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的磁场数据计算第一组四元数包括：根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的磁场数据计算姿态角；根据姿态角计算第一组四元数。

具体地，加速度传感器采集的加速度数据是x轴、y轴、z轴的加速度值，用ax、ay、az表示，则俯仰角和翻滚角

磁力计采集的磁场数据是x轴、y轴、z轴的磁场值，用mx、my、mz表示，则航向角为其中，arcsin是反正弦，arctan2是反正切。

也就是说，利用加速度传感器采集的加速度数据可以计算姿态角中的俯仰角和翻滚角，然后再结合磁力计采集的磁场数据可以计算姿态角中的航向角。

然后利用姿态角计算四元数。计算公式为：

q1＝cos(pitch/2)·cos(roll/2)·cos(yaw/2)+sin(pitch/2)·sin(roll/2)·sin(yaw/2)

q2＝cos(pitch/2)·sin(roll/2)·cos(yaw/2)-sin(pitch/2)·cos(roll/2)·sin(yaw/2)

q3＝sin(pitch/2)·cos(roll/2)·cos(yaw/2)+cos(pitch/2)·sin(roll/2)·sin(yaw/2)

q4＝cos(pitch/2)·cos(roll/2)·sin(yaw/2)-sin(pitch/2)·sin(roll/2)·cos(yaw/2)

其中，四元数表示为pitch为俯仰角，roll为翻滚角，yaw为航向角。

本实施例中，加速度传感器采集的x轴、y轴、z轴的加速度值分别用ax、ay、az表示，则步骤s110中的根据第一时刻的加速度数据计算第一时刻的加速度值为：

步骤s120，判断第一时刻的加速度值是否在预设加速度值范围内。

加速度传感器的缺点是在运动加速度过快时，测得的结果不准确，即计算的四元数不准确，最理想的情况是仅存在重力，加速度三个轴上的测量值是对重力加速度的分解，这是计算的四元数是相对准确的，在本实施例中，设定了对陀螺仪进行补偿的触发条件，即判断是否在预设加速度值范围内，满足该条件，说明加速度传感器的数据是相对准确的，计算的四元数也是相对准确的，此时以该四元数作为基准反推陀螺仪的正确角速度数据时，获得的角速度数据也是相对准确的。所以，只有满足该触发条件时，才可以对陀螺仪的角速度数据进行补偿，以消除陀螺仪的累计误差，也就是说，并非是任何时候采用本方案对陀螺仪进行补偿，都可以达到消除累积误差的目的。

这里的预设加速度值范围是[0.7g,1.3g]。

步骤s130，若加速度值在预设加速度值范围内，根据第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算陀螺仪的补偿系数和零偏值。

本实施例中，若加速度传感器采集的加速度值在预设加速度值范围内，则说明此时由加速度传感器和磁力计计算的姿态角是准确的，即四元数是准确的，可以对陀螺仪的累计误差进行标记；若加速度传感器采集的加速度值不在预设加速度值范围内，即使采用本方案对对陀螺仪的角速度数据进行标定补偿，则补偿结果仍然是不准确的，即无法达到消除陀螺仪的累计误差的效果。

在本实施例中，根据第一组四元数可以确定陀螺仪在没有累计误差的情况下的正确的角速度数据，根据该正确的角速度数据和当前陀螺仪采集的角速度数据，就可以计算出陀螺仪的补偿系数和零偏值，该补偿系数和零偏值可以标定累计误差。

步骤s140，使用补偿系数和零偏值补偿陀螺仪采集的第二时刻的角速度数据，获得陀螺仪补偿后的第二时刻的角速度数据。

综上所述，图1所示的方法，在计算第一四元数和加速度值后，首先判断加速度传感器采集的加速度值是否在预设加速度值范围内，若判断为是，说明相比较存在累计误差的陀螺仪计算的四元数来说，利用加速度传感器和磁力计计算的四元数是比较准确的，然后根据比较准确的四元数获得陀螺仪的补偿系数和零偏值，该补偿系数和零偏值标定了陀螺仪当前的累计误差，使用该补偿系数和零偏值补偿陀螺仪的角速度数据，可以补偿陀螺仪的累积误差。可见，本发明的技术方案利用加速度传感器和磁力计采集的数据对陀螺仪角速度数据补偿，可以消除陀螺仪的累计误差，保证陀螺仪准确性，增强用户体验。

获得陀螺仪补偿后的第二时刻的角速度数据后，为了获得第二时刻的姿态角，需要利用四元数进行计算，以简化计算。所以，姿态角的获取即是四元数的获取，四元数确定后，姿态角即可确定。在本发明的一个实施例中，图1所述的方法还包括：根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数；将第一组四元数和第二组四元数按照预设的比例进行融合计算，获得第一时刻的四元数；根据第一时刻的四元数和陀螺仪补偿后的第二时刻的角速度数据，计算第二时刻的四元数。

依据陀螺仪的角速度数据计算的四元数是依赖上一时刻的四元数的，即根据上一时刻的四元数和当前时刻的角速度数据计算当前时刻的四元数。在本实施例中，要计算第二时刻的四元数，需要将第一时刻的四元数计算出来。

在计算第一时刻的四元数时，首先根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数，然后将第一组四元数和第二组四元数按照预设的比例进行融合计算，获得第一时刻的四元数。这里采用融合算法，按照预设的比例从第一组四元数和第二组四元数中得到新的一组四元数，这组新的四元数就是第一时刻的四元数。例如，第一组四元数中的其中一个值是q11，第二组四元数中其中一个值是q21，预设的比例是3:1，则计算后第一时刻的四元数中的q1＝75％*q11+25％*q21，q2、q3、q4采用类似的方法进行计算。需要说明的是，这里预设的比例的确定可以采用现有融合算法中的比例的确定方式进行确定，这里不做具体限定。

获得第一时刻的四元数后，根据第一时刻的四元数和陀螺仪补偿后的第二时刻的角速度数据，计算第二时刻的四元数。具体是根据公式qk+1＝ak·qk计算的，在本实施例中，qk+1是第二时刻的四元数，ak是陀螺仪三个轴向的积分，与陀螺仪补偿后的第二时刻的角速度数据有关，qk是第一时刻的四元数，即

这里的ak可以表示为ωx,ωy,ωz为陀螺仪补偿后的三个轴的角速度数据，δt为陀螺仪的采样间隔，例如，若陀螺仪来自博士公司，采则样间隔δt为6ms。那么，利用公式qk+1＝ak·qk计算第二时刻的四元数为

需要说明的是，本实施例中的根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数也是采用上述的步骤进行的，即根据第一时刻的前一时刻的四元数和第一时刻的陀螺仪的角速度数据进行计算，即利用公式qk+1＝ak·qk计算，计算过程类似，在此不再赘述。

在本发明的一个实施例中，图1所述的方法还包括：若加速度值不在预设加速度值范围内，则根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数；将第一组四元数和第二组四元数按照预设的比例进行融合计算，获得第一时刻的四元数；根据第一时刻的四元数和陀螺仪采集的第二时刻的角速度数据，计算第二时刻的四元数。

在本实施例中，计算第二时刻的四元数的融合算法和上一实施例中的融合算法一样，按照预设的比例从第一时刻的四元数和第二组四元数中得到新的一组四元数，这组新的四元数就是第一时刻的四元数。根据第一时刻的四元数和陀螺仪采集的第二时刻的角速度数据，计算第二时刻的四元数，也是采用公式qk+1＝ak·qk计算，计算过程类似，在此不再赘述。

本方案是利用加速度传感器和磁力计获得的四元数反推陀螺仪的正确角速度数据，然后根据陀螺仪采集的角速度数据，计算补偿系数和零偏值，进而对累积误差进行标定。在本发明的一个实施例中，图1所示的步骤s130的根据第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算陀螺仪的补偿系数和零偏值包括：根据第一组四元数和第一时刻的前一时刻的四元数，计算陀螺仪第一时刻的角速度数据；根据陀螺仪计算出的第一时刻的角速度数据和采集的第一时刻的角速度数据，采用交叉逼近的方式计算陀螺仪的补偿系数和零偏值。

本实施例中的计算的陀螺仪第一时刻的角速度数据就是利用加速度传感器和磁力计获得的四元数反推陀螺仪的正确角速度数据。在计算陀螺仪的补偿系数和零偏值时，是利用陀螺仪的补偿公式进行计算的，其中，kx、ky、kz是陀螺仪的三轴的补偿系数，x_offset、y_offset、z_offset是陀螺仪三轴的零偏值，ωx'、ωy'、ωz'是计算出的陀螺仪的第一时刻的角速度数据，ωx、ωy、ωz是陀螺仪采集的第一时刻的角速度数据。

在本实施例中，因为只有三个补偿公式，却有六个未知量kx、ky、kz、x_offset、y_offset、z_offset，在公式的个数少于未知量的个数的情况下，正常的计算方法是不能确定全部的未知量的，所以在本实施例中，采用了交叉逼近的方式进行计算。

进一步地，上述的采用交叉逼近的方式计算陀螺仪的补偿系数和零偏值包括：获取陀螺仪的当前补偿系数和当前零偏值；查看记录的前一次计算补偿系数和零偏值时使用的已知量是当前补偿系数还是当前零偏值；若记录的前一次计算补偿系数和零偏值时使用的已知量是当前补偿系数，则本次采用获取的当前零偏值作为已知量，计算陀螺仪的补偿系数，将当前零偏值作为陀螺仪的零偏值，并使用计算出的陀螺仪的补偿系数更新当前补偿系数；若记录的前一次计算补偿系数和零偏值时使用的已知量是当前零偏值，则本次采用获取的当前补偿系数作为已知量，计算陀螺仪的零偏值，将当前补偿系数作为陀螺仪的补偿系数，并使用计算的陀螺仪的零偏值更新当前零偏值；记录本次计算使用的已知量是当前补偿系数还是当前零偏值。

在本实施例中，陀螺仪在出厂时会进行一次标定补偿，上述的六个未知量都会有一个初始值，在交叉逼近的方法中，每次计算均可以采用上一次计算的数据作为已知量，例如，上一次计算将x_offset、y_offset、z_offset作为已知量计算kx、ky、kz；本次计算，将上次计算得到的kx、ky、kz作为已知量，将x_ofset、y_offset、z_offset作为未知量，进行求解，然后以此类推。

在一个具体的例子中，补偿陀螺仪角速度数据的方法具体步骤为：

根据当前时刻陀螺仪采集的角速度数据确定陀螺仪当前时刻的三个轴向的积分，利用上一时刻的四元数和当前时刻的三个轴向的积分计算当前时刻的第一组四元数；利用加速度传感器和磁力计计算姿态角，继而利用计算出的姿态角计算第二组四元数；判断加速度传感器采集的加速度值是否在预设加速度值范围内；若判断为是，则根据第一组四元数和陀螺仪采集的当前时刻的角速度数据，计算陀螺仪的补偿系数和零偏值，并且采用融合算法，根据第一组四元数和第二组四元数计算当前时刻的四元数；使用计算的补偿系数和零偏值补偿陀螺仪采集的下一时刻的角速度数据，获得补偿后的下一时刻的角速度数据，进而确定下一时刻的三个轴向的积分；然后利用当前时刻的四元数和下一时刻的三个轴向的积分计算下一时刻的四元数。此时，下一时刻的四元数即是消除累积误差的四元数。若判断为否，采用融合算法，根据第一组四元数和第二组四元数计算当前时刻的四元数，利用下一时刻的角速度数据确定下一时刻的三个轴向的积分，利用当前时刻的四元数和下一时刻的三个轴向的积分计算下一时刻的四元数。

图2为本发明一个实施例提供的一种补偿陀螺仪角速度数据的装置的功能结构示意图。如图2所示，该补偿陀螺仪角速度数据的装置200包括：

第一计算单元210，用于根据加速度传感器采集的第一时刻的加速度数据和磁力计采集的第一时刻的磁场数据计算第一组四元数，以及根据加速度数据计算第一时刻的加速度值；

判断单元220，用于判断第一时刻的加速度值是否在预设加速度值范围内；

第二计算单元230，用于若加速度值在预设加速度值范围内，根据第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算陀螺仪的补偿系数和零偏值；

补偿单元240，用于使用补偿系数和零偏值补偿陀螺仪采集的第二时刻的角速度数据，获得陀螺仪补偿后的第二时刻的角速度数据。

在本发明的一个实施例中，第一计算单元210，还用于根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数。

图2所示的装置还包括：四元数计算单元，用于将第一组四元数和第二组四元数按照预设的比例进行融合计算，获得第一时刻的四元数；根据第一时刻的四元数和陀螺仪补偿后的第二时刻的角速度数据，计算第二时刻的四元数。

在本发明的一个实施例中，四元数计算单元，用于若加速度值不在预设加速度值范围内，则根据陀螺仪采集的第一时刻的角速度数据计算第二组四元数；将第一组四元数和第二组四元数按照预设的比例进行融合计算，获得第一时刻的四元数；根据第一时刻的四元数和陀螺仪采集的第二时刻的角速度数据，计算第二时刻的四元数。

在本发明的一个实施例中，第二计算单元230，具体用于根据第一组四元数和第一时刻的前一时刻的四元数，计算陀螺仪第一时刻的角速度数据；根据陀螺仪计算出的第一时刻的角速度数据和采集的第一时刻的角速度数据，采用交叉逼近的方式计算陀螺仪的补偿系数和零偏值。

在本发明的一个实施例中，第二计算单元230的采用交叉逼近的方式计算陀螺仪的补偿系数和零偏值具体是获取陀螺仪的当前补偿系数和当前零偏值；查看记录的前一次计算补偿系数和零偏值时使用的已知量是当前补偿系数还是当前零偏值；若记录的前一次计算补偿系数和零偏值时使用的已知量是当前补偿系数，则本次采用获取的当前零偏值作为已知量，计算陀螺仪的补偿系数，将当前零偏值作为陀螺仪的零偏值，并使用计算出的陀螺仪的补偿系数更新当前补偿系数；若记录的前一次计算补偿系数和零偏值时使用的已知量是当前零偏值，则本次采用获取的当前补偿系数作为已知量，计算陀螺仪的零偏值，将当前补偿系数作为陀螺仪的补偿系数，并使用计算的陀螺仪的零偏值更新当前零偏值；记录本次计算使用的已知量是当前补偿系数还是当前零偏值。

图3为本发明另一个实施例提供的一种补偿陀螺仪角速度数据的装置的结构示意图。如图3所示，补偿陀螺仪角速度数据的装置300包括存储器310和处理器320，存储器310和处理器320之间通过内部总线330通讯连接，存储器310存储有能够被处理器320执行的补偿陀螺仪角速度数据的计算机程序311，该补偿陀螺仪角速度数据的计算机程序311被处理器320执行时能够实现图1中所示的各实施例的方法步骤。

在不同的实施例中，存储器310可以是内存或者非易失性存储器。其中非易失性存储器可以是：存储驱动器(如硬盘驱动器)、固态硬盘、任何类型的存储盘(如光盘、dvd等)，或者类似的存储介质，或者它们的组合。内存可以是：ram(radomaccessmemory，随机存取存储器)、易失存储器、非易失性存储器、闪存。进一步，非易失性存储器和内存作为机器可读存储介质，其上可存储由处理器320执行的补偿陀螺仪角速度数据的计算机程序311。

图4为本发明一个实施例提供的一种陀螺仪的功能结构示意图。如图4所示，该陀螺仪400包括如图2或图3的补偿陀螺仪角速度数据的装置410。

图5为本发明一个实施例提供的一种补偿陀螺仪角速度数据的系统的功能结构示意图。如图5所示，该补偿陀螺仪角速度数据的系统500包括如图4所示的陀螺仪400、加速度传感器510和磁力计520；

加速度传感器510，用于采集第一时刻的加速度数据，并将第一时刻的加速度数据发送给陀螺仪400。

磁力计520，用于采集第一时刻的磁场数据，并将第一时刻的磁场数据发送给陀螺仪400。

陀螺仪400，用于根据接收到的第一时刻的加速度数据和第一时刻的磁场数据计算第一组四元数，以及根据第一时刻的加速度数据计算第一时刻的加速度值；判断第一时刻的加速度值是否在预设加速度值范围内；若加速度值在预设加速度值范围内，根据第一组四元数和陀螺仪采集的第一时刻的角速度数据，计算陀螺仪的补偿系数和零偏值；使用补偿系数和零偏值补偿陀螺仪采集的第二时刻的角速度数据，获得陀螺仪补偿后的第二时刻的角速度数据。

需要说明的是，图2或图3所示的装置，图4所示的陀螺仪和图5所示的系统的各实施例与图1所示的方法的各实施例对应相同，上文已有详细说明，在此不再赘述。

综上所述，本发明的有益效果是：首先判断加速度传感器采集的加速度值是否在预设加速度值范围内，若判断为是，说明相比较存在累计误差的陀螺仪计算的四元数来说，利用加速度传感器和磁力计计算的四元数是比较准确的，然后根据比较准确的四元数获得陀螺仪的补偿系数和零偏值，该补偿系数和零偏值标定了陀螺仪当前的累计误差，使用该补偿系数和零偏值补偿陀螺仪的角速度数据，可以补偿陀螺仪的累积误差。可见，本发明的技术方案利用加速度传感器和磁力计采集的数据对陀螺仪角速度数据补偿，可以消除陀螺仪的累计误差，保证陀螺仪准确性，增强用户体验。

以上所述，仅为本发明的具体实施方式，在本发明的上述教导下，本领域技术人员可以在上述实施例的基础上进行其他的改进或变形。本领域技术人员应该明白，上述的具体描述只是更好的解释本发明的目的，本发明的保护范围应以权利要求的保护范围为准。