CN112304337A

CN112304337A - 基于陀螺仪和加速度计的运动角度估计方法和***

Info

Publication number: CN112304337A
Application number: CN202011141659.0A
Authority: CN
Inventors: 卢成林
Original assignee: Shanghai Li Ke Semiconductor Technology Co ltd
Current assignee: Shanghai Li Ke Semiconductor Technology Co ltd
Priority date: 2020-10-22
Filing date: 2020-10-22
Publication date: 2021-02-02
Anticipated expiration: 2040-10-22
Also published as: CN112304337B

Abstract

本申请提供了一种基于陀螺仪和加速度计的运动角度估计方法、***和计算机可读介质。该方法包括：获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；获取陀螺仪偏差和加速度计偏差；根据加速度计偏差对当前时刻的加速度进行校准；使用预设陀螺仪校正系数和多组角速度均值对陀螺仪偏差进行IIR滤波；获取滤波后的上一时刻的加速度；使用预设加速度计校正系数和滤波后的上一时刻的加速度对校准后的当前时刻的加速度进行IIR滤波；以及根据滤波后的陀螺仪偏差和滤波后的当前时刻的加速度估计最终运动角度。该方法通过改进的IIR滤波器修正陀螺仪偏差和加速度，能够实现对运动角度的精准估计。

Description

基于陀螺仪和加速度计的运动角度估计方法和***

技术领域

本申请主要涉及数据处理领域，尤其涉及一种基于陀螺仪和加速度计的运动角度估计方法、***和计算机可读介质。

背景技术

随着智能手机和智能家居的普及以及智能化程度地不断完善，人们可以在经济承受范围内购选适合智能手机和扫地机器人等智能家居。以扫地机器人为例，在其工作过程中，定位方式至关重要，决定了扫地机器人能否把地面打扫干净。扫地机器人定位以及环境地图的构建是路径规划的核心部分，而扫地机器人的运动角度估计是定位的关键。

扫地机器人运动角度估计方法可以分为三类：基于光电编码器的方法、基于电子罗盘的方法、基于陀螺仪的方法。基于光电编码器的方法是利用光电转换原理将输入到轴的角度量转换成相应的电脉冲或数字量的角度检测装置，但需要依赖机械装置转换、难以克服滑差，且会受到轮子打滑等不确定因素造成的角度测量不准。基于电子罗盘的方法是利用地球磁场检测电子罗盘与地磁场方向的偏转角度，因此容易受电磁干扰的影响。基于陀螺仪的方法是通过测量扫地机器人的角速度来获取其运动角度，能够解决前两种方法所存在的问题。并且陀螺仪传感器具有造价低廉、不受环境因素的影响和实时性较好等特点，应用比较广泛。

但在实际情况中，大部分廉价陀螺仪传感器都存在着较大的偏差，使得最终估计出的角度值与实际值有较大误差。为了解决该误差问题，需要用户在使用扫地机器人之前或扫地机器人开机时进行陀螺仪的校准，这大大降低了用户的使用体验。而且在长时间使用后，陀螺仪仍会出现累计偏差，依然会对扫地机器人造成运动角度估计结果不理想等问题。现有的运动姿态估计算法大都采用基于卡尔曼滤波或者互补滤波的算法，但是卡尔曼滤波的计算量较大，对硬件要求比较高，实现起来也相对比较复杂。

一般来说，陀螺仪和加速度计的误差分为***误差和随机误差。***误差通常可以通过传感器的校准消除；而陀螺仪的随机误差(又称零偏)通常是在一定范围内随机飘移，因此需要对陀螺仪的零偏进行纠正。加速度计比较灵敏，但是其抖动和噪声会产生累积误差，因此需要对校准后的加速度计数据进行纠正。

因此，本领域技术人员亟需一种基于陀螺仪和加速度计的运动角度估计方法能够更好地修正陀螺仪偏差和加速度计数据，来实现更精准地运动角度估计。

发明内容

本申请要解决的技术问题是提供基于陀螺仪和加速度计的运动角度估计方法、***和计算机可读介质，能够更好地修正陀螺仪偏差和加速度计数据，实现更精准地运动角度估计。

为解决上述技术问题，本申请提供了一种基于陀螺仪和加速度计的运动角度估计方法，包括：获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；在所述陀螺仪和所述加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差；根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度；使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差；获取滤波后的上一时刻的加速度，若所述滤波后的上一时刻的加速度不存在，则将所述校准后的当前时刻的加速度作为所述滤波后的上一时刻的加速度；使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度；以及根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度。

在本申请的一实施例中，所述使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差包括：

IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias

IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias

IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias

其中gx_mean、gy_mean、gz_mean分别为x轴、y轴和z轴上的所述陀螺仪的多组角速度均值，α为所述预设陀螺仪校正系数，IIR_gx_bias、IIR_gy_bias、IIR_gz_bias分别为x轴、y轴和z轴上的所述陀螺仪偏差，IIR_gx_bias’、IIR_gy_bias’、IIR_gz_bias’分别为x轴、y轴和z轴上的所述滤波后的陀螺仪偏差。

在本申请的一实施例中，所述使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度包括：

IIR_ax’＝(1-β)*cor_ax+β*IIR_ax

IIR_ay’＝(1-β)*cor_ay+β*IIR_ay

IIR_az’＝(1-β)*cor_az+β*IIR_az

其中cor_ax、cor_ay、cor_az分别为x轴、y轴和z轴上的所述校准后的当前时刻的加速度，β为所述预设加速度计校正系数，IIR_ax、IIR_ay、IIR_az分别为x轴、y轴和z轴上的所述滤波后的上一时刻的加速度，IIR_ax’、IIR_ay’、IIR_az’分别为x轴、y轴和z轴上的滤波后的当前时刻的加速度。

在本申请的一实施例中，所述根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度使用以下任一种方法进行估计：四元数法、矩阵旋转法和欧拉旋转法。

在本申请的一实施例中，所述预设陀螺仪校正系数的取值范围为0.08-0.12。

在本申请的一实施例中，所述预设加速度计校正系数的取值范围为0.44-0.66。

在本申请的一实施例中，所述根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度包括：

cor_ax＝raw_ax*g*R-bias_ax

cor_ay＝raw_ay*g*R-bias_ay

cor_az＝raw_az*g*R-bias_az

其中cor_ax、cor_ay、cor_az分别为x轴、y轴和z轴上的所述校准后的当前时刻的加速度，raw_ax、raw_ay、raw_az分别为x轴、y轴和z轴上的所述当前时刻的加速度，g为重力加速度常量，R为预设单位转换因子，bias_ax、bias_ay、bias_az分别为x轴、y轴和z轴上的所述加速度计偏差。

为解决上述技术问题，本申请还提供了一种基于陀螺仪和加速度计的运动角度估计装置，包括：第一获取模块，用于获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；第二获取模块，用于在所述陀螺仪和所述加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差；加速度校准模块，用于根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度；偏差滤波模块，用于使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差；第三获取模块，用于获取滤波后的上一时刻的加速度，若所述滤波后的上一时刻的加速度不存在，则将所述校准后的当前时刻的加速度作为所述滤波后的上一时刻的加速度；加速度滤波模块，用于使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度；以及估计模块，用于根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度。

为解决上述技术问题，本申请还提供了一种基于陀螺仪和加速度计的运动角度估计***，包括：存储器，用于存储可由处理器执行的指令；以及处理器，用于执行所述指令以实现如上所述的方法。

为解决上述技术问题，本申请还提供了一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如上所述的方法。

与现有技术相比，本申请的基于陀螺仪和加速度计的运动角度估计方法、***和计算机可读介质通过改进的IIR滤波器修正陀螺仪偏差和加速度，能够对加速度计数据和陀螺仪数据进行更好的滤波，以较低的计算量实现对运动角度的精准估计。

附图说明

包括附图是为提供对本申请进一步的理解，它们被收录并构成本申请的一部分，附图示出了本申请的实施例，并与本说明书一起起到解释本申请原理的作用。附图中：

图1是根据本申请一实施例的基于陀螺仪和加速度计的运动角度估计方法的流程示意图。

图2是根据本申请一实施例的基于陀螺仪和加速度计的运动角度估计装置的框图。

图3是根据本申请一实施例示出的基于陀螺仪和加速度计的运动角度估计***的***框图。

具体实施方式

为了更清楚地说明本申请的实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构或操作。

如本申请和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

本申请中使用了流程图用来说明根据本申请的实施例的***所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各种步骤。同时，或将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。

本申请提供了一种基于陀螺仪和加速度计的运动角度估计方法，其中陀螺仪和加速度计是用于估计同一个设备的运动角度。图1是根据本申请一实施例的基于陀螺仪和加速度计的运动角度估计方法的流程示意图。如图1所示，本实施例的基于陀螺仪和加速度计的运动角度估计方法包括以下步骤：

步骤101，获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；

步骤102，在陀螺仪和加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差；

步骤103，根据加速度计偏差对当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度；

步骤104，使用预设陀螺仪校正系数和多组角速度均值对陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差；

步骤105，获取滤波后的上一时刻的加速度，若滤波后的上一时刻的加速度不存在，则将校准后的当前时刻的加速度作为滤波后的上一时刻的加速度；

步骤106，使用预设加速度计校正系数和滤波后的上一时刻的加速度对校准后的当前时刻的加速度进行IIR滤波(无限冲激响应滤波器)，并得到滤波后的当前时刻的加速度；以及

步骤107，根据滤波后的陀螺仪偏差和滤波后的当前时刻的加速度估计最终运动角度。

在步骤101中，基于陀螺仪和加速度计的运动角度估计***获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数。本实施例中所使用的陀螺仪为三轴陀螺仪，测量所得的数据为三轴数据，即陀螺仪测量得到的角速度为三轴角速度。本实施例中所使用的加速度计为三轴加速度计，测量所得的数据为三轴数据，即加速度计测量得到的加速度为三轴加速度。

陀螺仪的多组角速度均值的组数可以是由陀螺仪自身设置决定的，本申请对此不作限定。多组角速度均值可以是由陀螺仪直接提供，***直接从陀螺仪获取到多组角速度均值而无需进行计算。在一个示例中，陀螺仪的多组角速度均值可以存储在陀螺仪的先进先出(First Input First Output,FIFO)存储器中，***从陀螺仪的FIFO存储器处获取多组角速度均值。

预设陀螺仪校正系数和预设加速度计校正系数可以由用户根据应用场景和实际经验进行设置。在本申请的一实施例中，预设陀螺仪校正系数的取值范围可以为0.08-0.12。优选地，预设陀螺仪校正系数可以为0.1。在本申请的一实施例中，预设加速度计校正系数的取值范围可以为0.44-0.66。优选地，预设加速度计校正系数可以为0.55。

在步骤102中，在陀螺仪和加速度计保持水平放置且静止的情况下，***获取陀螺仪偏差和加速度计偏差。本实施例中的陀螺仪偏差和加速度计偏差是指在陀螺仪和加速度计保持水平放置且静止的情况下，陀螺仪和加速度计各轴的偏移量，其中陀螺仪偏差又称陀螺仪随机误差或零偏。

在步骤103中，***根据加速度计偏差对当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度。在本申请的一实施例中，***可以通过以下方式进行步骤103：

cor_ax＝raw_ax*g*R-bias_ax

cor_ay＝raw_ay*g*R-bias_ay

cor_az＝raw_az*g*R-bias_az

其中cor_ax、cor_ay、cor_az分别为x轴、y轴和z轴上的校准后的当前时刻的加速度。raw_ax、raw_ay、raw_az分别为x轴、y轴和z轴上的当前时刻的加速度。g为重力加速度常量，取值可以为9.80665。R为预设单位转换因子，用于转换不同的计量单位，用户可根据实际需要进行设定。在一个示例中，预设单位转换因子R可以设为0.001。bias_ax、bias_ay、bias_az分别为x轴、y轴和z轴上的加速度计偏差。

在步骤104中，***使用预设陀螺仪校正系数和多组角速度均值对陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差。在本申请的一实施例中，***所进行的IIR滤波为一阶IIR滤波，可以通过以下方式进行步骤104：

IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias

IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias

IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias

其中gx_mean、gy_mean、gz_mean分别为x轴、y轴和z轴上的陀螺仪的多组角速度均值；α为预设陀螺仪校正系数，α值越大表明陀螺仪多组角速度均值所占的比重越大，而偏差占比越小；IIR_gx_bias、IIR_gy_bias、IIR_gz_bias分别为x轴、y轴和z轴上的陀螺仪偏差；IIR_gx_bias’、IIR_gy_bias’、IIR_gz_bias’分别为x轴、y轴和z轴上的滤波后的陀螺仪偏差。

在步骤105中，***获取滤波后的上一时刻的加速度，若滤波后的上一时刻的加速度不存在，也就是说当前时刻为初始时刻，则将校准后的当前时刻的加速度作为滤波后的上一时刻的加速度来进行后续步骤的运算。

在步骤106中，***使用预设加速度计校正系数和滤波后的上一时刻的加速度对校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度。在本申请的一实施例中，***所进行的IIR滤波为一阶IIR滤波，可以通过以下方式进行步骤106：

IIR_ax’＝(1-β)*cor_ax+β*IIR_ax

IIR_ay’＝(1-β)*cor_ay+β*IIR_ay

IIR_az’＝(1-β)*cor_az+β*IIR_az

其中cor_ax、cor_ay、cor_az分别为x轴、y轴和z轴上的校准后的当前时刻的加速度；β为预设加速度计校正系数，β值越小表明校准后的当前时刻的加速度所占比重越大，而上一时刻的滤波后的加速度占比越小；IIR_ax、IIR_ay、IIR_az分别为x轴、y轴和z轴上的上一时刻的滤波后的加速度，IIR_ax’、IIR_ay’、IIR_az’分别为x轴、y轴和z轴上的滤波后的当前时刻的加速度。

在步骤107中，***根据滤波后的陀螺仪偏差和滤波后的当前时刻的加速度估计最终运动角度。在本申请的一实施例中，***可以通过以下任一种方式进行步骤107：四元数法、矩阵旋转法和欧拉旋转法。优选地，***使用四元数法来根据滤波后的陀螺仪偏差和滤波后的当前时刻的加速度估计最终运动角度。本实施例以四元数法为例对步骤107进行详细说明，包括以下步骤107a-107c：

步骤107a，***将滤波后的陀螺仪偏差用四元数表示，公式如下：

其中四元数的初始值quat_in₀＝1.0，quat_in₁＝0.0，quat_in₂＝0.0，quat_in₃＝0.0；gx_mean、gy_mean、gz_mean分别为x轴、y轴和z轴上的陀螺仪的多组角速度均值。

然后***再通过如下所示的一阶龙格库塔微分议程更新四元数：

quat_update_x＝Runge_Kutta_1st(gx_mean,deltaT)

quat_update_y＝Runge_Kutta_1st(gy_mean,deltaT)

quat_update_z＝Runge_Kutta_1st(gz_mean,deltaT)

其中deltaT为陀螺仪的采样周期。

步骤107b，***根据滤波后的当前时刻的加速度修正陀螺仪。由于陀螺仪存在的零漂，会导致测出的陀螺仪数据不准备，而加速度计静态性能好，没有陀螺仪的漂移严重，可以用来修正陀螺仪的姿态，将加速度计和陀螺仪的数据做向量积得到误差，依此修正陀螺仪，公式如下：

其中a_quat₀、a_quat₁、a_quat₂、a_quat₃为步骤106中获得的修正后的加速度计的四元数数据，g_quat₀、g_quat₁、g_quat₂、g_quat₃为修正前的陀螺仪四元数数据，g_quat₀'、g_quat₁'、g_quat₂'、g_quat₃'为修正后陀螺仪的四元数数据。

步骤107c，***根据四元数法估计最终运动角度。因为计算误差等因素导致四元数失去规范化特性，而使得四元数的模不再等于1。此外，归一化的四元数在旋转空间中不会拉伸，只有旋转角度，旋转矩阵满足正交性。因此，在计算运动角度之前需要对四元数进行归一化处理，归一化公式如下：

其中|quat|＝sqrt((gyro_quat₀')²+(gyro_quat₁')²+(gyro_quat₂')²+(gyro_quat₃')²)，为四元数的模值；

为归一化的陀螺仪四元数数据。

然后***根据四元数的方向余弦矩阵和欧拉角的对应关系求得设备的运动角度，公式如下：

其中g_quat₀'、g_quat₁'、g_quat₂'、g_quat₃'为步骤107b修正后的陀螺仪四元数数据，m、n、o为设备的最终精准估计的运动角度。

上述步骤107a、107b和107c所示的四元数法通过使用四元数旋转代替矩阵旋转和欧拉旋转，能够降低计算复杂度，同时可以防止出现欧拉角的万向节死锁现象。

综上步骤101-107所述，本申请的基于陀螺仪和加速度计的运动角度估计方法通过改进的IIR滤波器修正陀螺仪偏差和加速度，从而可以以较低的计算量实现对运动角度的精准估计。现有技术方案大都使用一阶低通IIR滤波器，但是加速度计数据需要滤除的是高频信号，与陀螺仪数据需要滤除低频信号刚好相反。因此现有技术方案只通过单一的低通IIR无法达到很好的滤波效果。与现有技术相比，本申请的基于陀螺仪和加速度计的运动角度估计方法能够避免上述问题，能够对加速度计数据和陀螺仪数据进行更好的滤波。

本申请还提供了一种基于陀螺仪和加速度计的运动角度估计装置。图2示出了根据本申请一实施例的基于陀螺仪和加速度计的运动角度估计装置的框图。如图2所示，该基于陀螺仪和加速度计的运动角度估计装置200包括第一获取模块201、第二获取模块202、加速度校准模块203、偏差滤波模块204、第三获取模块205、加速度滤波模块206以及估计模块207。

第一获取模块201，用于获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数。

第二获取模块202，用于在陀螺仪和加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差。

加速度校准模块203，用于根据加速度计偏差对当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度。

偏差滤波模块204，用于使用预设陀螺仪校正系数和多组角速度均值对陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差。

第三获取模块205，用于获取滤波后的上一时刻的加速度，若滤波后的上一时刻的加速度不存在，则将校准后的当前时刻的加速度作为滤波后的上一时刻的加速度。

加速度滤波模块206，用于使用预设加速度计校正系数和滤波后的上一时刻的加速度对校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度。

估计模块207，用于根据滤波后的陀螺仪偏差和滤波后的当前时刻的加速度估计最终运动角度。

上述模块201-207所执行的步骤可参考前述图1实施例中的相应步骤101-107，在此不再展开描述。

本申请还提供了一种基于陀螺仪和加速度计的运动角度估计***，包括：存储器，用于存储可由处理器执行的指令；以及处理器，用于执行指令以实现如上所述的基于陀螺仪和加速度计的运动角度估计方法。

图3示出了根据本申请一实施例示出的基于陀螺仪和加速度计的运动角度估计***的***框图。基于陀螺仪和加速度计的运动角度估计***300可包括内部通信总线301、处理器(Processor)302、只读存储器(ROM)303、随机存取存储器(RAM)304、以及通信端口305。当应用在个人计算机上时，基于陀螺仪和加速度计的运动角度估计***还可以包括硬盘307。内部通信总线301可以实现基于陀螺仪和加速度计的运动角度估计***300组件间的数据通信。处理器302可以进行判断和发出提示。在一些实施例中，处理器302可以由一个或多个处理器组成。通信端口305可以实现基于陀螺仪和加速度计的运动角度估计***300与外部的数据通信。在一些实施例中，基于陀螺仪和加速度计的运动角度估计***300可以通过通信端口305从网络发送和接受信息及数据。基于陀螺仪和加速度计的运动角度估计***300还可以包括不同形式的程序储存单元以及数据储存单元，例如硬盘307，只读存储器(ROM)303和随机存取存储器(RAM)304，能够存储计算机处理和/或通信使用的各种数据文件，以及处理器302所执行的可能的程序指令。处理器执行这些指令以实现方法的主要部分。处理器处理的结果通过通信端口传给用户设备，在用户界面上显示。

上述的基于陀螺仪和加速度计的运动角度估计方法可以实施为计算机程序，保存在硬盘307中，并可记载到处理器302中执行，以实施本申请中的任一种基于陀螺仪和加速度计的运动角度估计方法。

本申请还提供了一种存储有计算机程序代码的计算机可读介质，计算机程序代码在由处理器执行时实现如上所述的基于陀螺仪和加速度计的运动角度估计方法。

本申请还提供了一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如上所述的任一种基于陀螺仪和加速度计的运动角度估计方法。

基于陀螺仪和加速度计的运动角度估计方法实施为计算机程序时，也可以存储在计算机可读存储介质中作为制品。例如，计算机可读存储介质可以包括但不限于磁存储设备(例如，硬盘、软盘、磁条)、光盘(例如，压缩盘(CD)、数字多功能盘(DVD))、智能卡和闪存设备(例如，电可擦除可编程只读存储器(EPROM)、卡、棒、键驱动)。此外，本文描述的各种存储介质能代表用于存储信息的一个或多个设备和/或其它机器可读介质。术语“机器可读介质”可以包括但不限于能存储、包含和/或承载代码和/或指令和/或数据的无线信道和各种其它介质(和/或存储介质)。

应该理解，上文所描述的实施例仅是示意。本文描述的实施例可在硬件、软件、固件、中间件、微码或者其任意组合中实现。对于硬件实现，处理单元可以在一个或者多个特定用途集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理设备(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器和/或设计为执行本文所述功能的其它电子单元或者其结合内实现。

上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述申请披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示例性实施例的精神和范围。

同时，本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。

本申请的一些方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“***”。处理器可以是一个或多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DAPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器或者其组合。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。例如，计算机可读介质可包括，但不限于，磁性存储设备(例如，硬盘、软盘、磁带……)、光盘(例如，压缩盘CD、数字多功能盘DVD……)、智能卡以及闪存设备(例如，卡、棒、键驱动器……)。

同理，应当注意的是，为了简化本申请披露的表述，从而帮助对一个或多个申请实施例的理解，前文对本申请实施例的描述中，有时会将多种特征归并至一个实施例、附图或对其的描述中。但是，这种披露方法并不意味着本申请对象所需要的特征比权利要求中提及的特征多。实际上，实施例的特征要少于上述披露的单个实施例的全部特征。

虽然本申请已参照当前的具体实施例来描述，但是本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本申请，在没有脱离本申请精神的情况下还可作出各种等效的变化或替换，因此，只要在本申请的实质精神范围内对上述实施例的变化、变型都将落在本申请的权利要求书的范围内。

Claims

1.一种基于陀螺仪和加速度计的运动角度估计方法，包括：

获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；

在所述陀螺仪和所述加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差；

根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度；

使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差；

获取滤波后的上一时刻的加速度，若所述滤波后的上一时刻的加速度不存在，则将所述校准后的当前时刻的加速度作为所述滤波后的上一时刻的加速度；

使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度；以及

根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度。

2.如权利要求1所述的方法，其特征在于，所述使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差包括：

IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias

IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias

IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias

3.如权利要求1所述的方法，其特征在于，所述使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度包括：

IIR_ax’＝(1-β)*cor_ax+β*IIR_ax

IIR_ay’＝(1-β)*cor_ay+β*IIR_ay

IIR_az’＝(1-β)*cor_az+β*IIR_az

4.如权利要求1所述的方法，其特征在于，所述根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度使用以下任一种方法进行估计：四元数法、矩阵旋转法和欧拉旋转法。

5.如权利要求1所述的方法，其特征在于，所述预设陀螺仪校正系数的取值范围为0.08-0.12。

6.如权利要求1所述的方法，其特征在于，所述预设加速度计校正系数的取值范围为0.44-0.66。

7.如权利要求1所述的方法，其特征在于，所述根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度包括：

cor_ax＝raw_ax*g*R-bias_ax

cor_ay＝raw_ay*g*R-bias_ay

cor_az＝raw_az*g*R-bias_az

8.一种基于陀螺仪和加速度计的运动角度估计装置，包括：

第一获取模块，用于获取加速度计的当前时刻的加速度、陀螺仪的多组角速度均值、预设陀螺仪校正系数以及预设加速度计校正系数；

第二获取模块，用于在所述陀螺仪和所述加速度计保持水平放置且静止的情况下，获取陀螺仪偏差和加速度计偏差；

加速度校准模块，用于根据所述加速度计偏差对所述当前时刻的加速度进行校准，并得到校准后的当前时刻的加速度；

偏差滤波模块，用于使用所述预设陀螺仪校正系数和所述多组角速度均值对所述陀螺仪偏差进行IIR滤波，并得到滤波后的陀螺仪偏差；

第三获取模块，用于获取滤波后的上一时刻的加速度，若所述滤波后的上一时刻的加速度不存在，则将所述校准后的当前时刻的加速度作为所述滤波后的上一时刻的加速度；

加速度滤波模块，用于使用所述预设加速度计校正系数和所述滤波后的上一时刻的加速度对所述校准后的当前时刻的加速度进行IIR滤波，并得到滤波后的当前时刻的加速度；以及

估计模块，用于根据所述滤波后的陀螺仪偏差和所述滤波后的当前时刻的加速度估计最终运动角度。

9.一种基于陀螺仪和加速度计的运动角度估计***，包括：

存储器，用于存储可由处理器执行的指令；以及处理器，用于执行所述指令以实现如权利要求1-7任一项所述的方法。

10.一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如权利要求1-7任一项所述的方法。