CN106813679B - 运动物体的姿态估计的方法及装置 - Google Patents

Abstract

本发明提供了运动物体的姿态估计的方法及装置。本发明被用于估计运动物体的姿态。本发明基于陀螺仪数据来估计姿态，然后基于加速计/磁力计数据来计算校正增益和校正矢量。校正增益和校正矢量被用于校准所述陀螺仪数据，最后经校准的陀螺仪数据被用于更新所述物体姿态。与现有技术相比，当根据加速计的读数，物体正以高加速度运动时，本发明能够减弱基于加速计数据的重力估计结果的校准。

Description

运动物体的姿态估计的方法及装置

技术领域

本发明涉及物体姿态估计的方法及装置，更具体地，涉及用于估计运动物体的姿态的方法及装置。

背景技术

现在为了估计运动物体的姿态和方向，传统上通过四元数或欧拉角(俯仰(pitch)角、滚转(roll)角和偏航(yaw)角)来表示物体姿态。两种类型的表示可以互相转换。此外，已知四元数表示从一个坐标系统到另一个坐标系统的变换。这种变换也可以表示为转换矩阵。转换矩阵的典型示例用于将矢量从地心坐标系(level frame)(例如，东、北、上)变换到体坐标系。

针对在无失真的地磁场中的静止物体，可以通过加速计数据来计算静止物体的姿态，或者基于加速计数据和磁力计数据来充分且精确地计算静止物体的姿态。然而，针对运动物体，将重力加速度和运动加速度合并为来自加速计的测量。在知道物体姿态之前，无法明确地将它们分开。此外，在物体周围的地磁场常因铁或磁性材料而失真。这种影响给基于加速计数据或基于加速计数据和磁力计数据的姿态估计结果带来偏差。

另一方面，如果知道上次的物体姿态，则能够通过陀螺仪数据来估计物体姿态。来自陀螺仪的测量基本上不受上述环境噪声(物体运动、磁场失真等)的影响。然而，估计处理基于积分运算。常常遇到零漂移问题，这会对姿态估计结果引起累积偏差。

在相关文献中，公开了不同姿态估计方法。针对该问题的典型解决方案是将来自两种类型的传感器数据的估计结果融合在一起。两种广泛使用的数据融合方法是基于卡尔曼滤波器法和基于增益调度法。其中，增益调度是利用线性控制器族来控制非线性系统的最流行的方法之一。增益调度基于调度变量的当前测量来调整控制器增益。用户能够设计控制器增益如何随测量的变量而改变。在姿态估计系统中，利用预定义调度基于传感器测量来计算增益。该增益用于合并所述两个估计结果或校准陀螺仪数据以改善物体姿态。

美国专利第US8688403 B2号公报公开了基于惯性传感器数据来估计物体姿态的方法。该方法使用来自加速计和磁力计的数据计算第一姿态矩阵梯度，然后使用来自陀螺仪的数据计算第二姿态矩阵梯度。通过混合系数来使第一姿态矩阵梯度与第二姿态矩阵梯度融合。最后，融合的梯度用于更新精细的姿态矩阵。在此，通过将来自加速计的数据与重力进行比较，并且还将来自磁力计的数据与一般已知当地磁场进行比较，来确定所述混合系数。

作为基于增益调度法的非常重要的步骤，增益(混合系数)计算步骤并未被清楚地记载在该专利中，而是在发明说明书中仅被粗略地说明并凭经验被定义。当物体突然或不规则地运动时，运动物体的姿态改变很大，如何设计基于测量的、适应的校正增益是针对该方法的关键问题。

此外，理想地讲，在地心坐标系中重力矢量是常数(0，0，g)，但是来自不同加速计的重力测量可能略微不同，因此如何精确地从加速计数据中去除重力分量是物体姿态估计的挑战所在。

另外，不存在定量地描述物体的运动加速度与基于姿态估计的加速计/磁力计数据的偏差之间的关系的理论数学函数。在该专利中凭经验来估计混合系数。不存在验证计算出的混合系数的有效性和正确性的步骤。

发明内容

因此，在本公开中提出了新的姿态估计方法和装置，以解决上述问题中的至少一个，来提高在真实世界中的运动物体的姿态估计的精确度。本发明基于陀螺仪数据来估计姿态，然后基于传感器数据来计算校正增益和校正矢量。校正增益和校正矢量用于校准陀螺仪数据，最后校准的陀螺仪数据用于更新物体姿态。

根据本发明的一个方面，提供了一种姿态估计方法，所述姿态估计方法包括：校正增益确定步骤，其用于基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；校正矢量计算步骤，其用于通过分别基于所述陀螺仪数据和加速计，将所述参考矢量从地心坐标系转换到体坐标系，来计算校正矢量，然后进行两个转换的参考矢量之间的叉积运算，以生成校正矢量；传感器数据校准步骤，其用于使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；姿态估计步骤，用于利用经校准的陀螺仪数据来估计所述姿态；此外，所述校正增益确定步骤还包括以下步骤：基于所述加速计数据来估计在体坐标系中的重力分量；通过从加速计数据中去除重力分量来计算所述物体的运动加速度；基于所述物体的所述运动加速度的预定义函数来计算所述校正增益。此外，所述姿态估计方法还包括调整步骤，所述调整步骤基于利用当前陀螺仪数据和经校准的陀螺仪数据计算的姿态估计偏差，来调整所述校正增益。

本发明旨在使用校正矢量和适应的校正增益来校准陀螺仪数据，以精确地估计物体姿态。因此，本发明由三个模块构成：校正矢量生成、校正增益确定和物体姿态校准，由此在物体处于静止的情况下，本发明能够增强基于加速计数据的物体姿态估计，而在物体以高加速度运动的情况下，减弱基于加速计数据的物体姿态估计。此外，基于姿态估计偏差来调整在上次估计处理中计算出的校正增益。

在本文中呈现的本发明的优势是，能够使得物体姿态估计更精确，具体而言，当物体突然运动时，由于本发明通过使用校正矢量和校正增益以直接校准陀螺仪数据，因此可以将不同的传感器数据融合以精确地估计物体姿态。

附图说明

图1是示出能够执行本发明的实施例的计算机系统的硬件构造的框图。

图2是示出物体姿态估计装置的构造的框图。

图3是示出根据本发明的一个实施例的装置进行物体姿态估计的流程图。

图4是根据本发明的一个实施例的校正增益的确定方法的流程图。

图5A是示出根据本发明的一个实施例的校正增益确定的线性函数的示意图。

图5B是示出根据本发明的一个实施例的校正增益确定的高斯函数的示意图。

图6是根据本发明的一个实施例的校正矢量的生成方法的流程图。

具体实施方式

参照上述所列附图，本部分描述了特定实施例及其详细结构和操作。请注意，下文将要描述的实施例仅以说明而不是限制的方式进行阐述，因此它们不限制本发明的范围且能够在本发明的范围内被改变为各种形式。在此，鉴于该教导，本领域的技术人员能够认识到存在与在此描述的示例实施例等同的范围。

图1是示出能够执行本发明的实施例的计算机系统的硬件构造的框图。

如图1所示，系统至少包括计算机100，该计算机100包括CPU 101、RAM 102、ROM103、系统总线104、输入设备105、输出设备106和驱动器107。例如，计算机100是物体姿态估计系统或姿态航向参考系统(AHRS)。注意，计算机100可以包括一个或多个计算机，并且多个计算机可以个别地实现计算机100的各功能。

CPU 101根据RAM 102或ROM 103中存储的程序执行全部过程。当CPU 101进行诸如本发明的实施例等的各种处理时，RAM 102被用作临时存储区域。

输入设备105可以是测量设备(例如，传感器、加速计、陀螺仪或磁力计)或用户输入接口或网络接口，其使得用户能够向计算机100发出各种指令。

输出设备106包括输出外围接口、显示设备(即，监视器、CRT、液晶显示器或图形控制器)，其使得用户能够输出本发明的校准的姿态结果。

驱动器107被构造为驱动诸如硬盘、存储卡或光盘(例如，CD-ROM或DVD ROM)等存储介质。例如，测量数据或用于进行物体姿态估计处理的程序被存储在存储介质中，并被驱动器107驱动。

系统总线104连接CPU 101、RAM 102、ROM 103、输入设备105、输出设备106和驱动器107。数据在系统总线104上进行通信。如本文所用，术语“连接”是指逻辑上或物理上直接地连接或者通过一个或更多媒介间接地连接。

图1中示出的系统仅是说明性的，并不旨在限制本发明，包括其应用或使用。例如，当启动用于进行物体姿态估计处理的程序时，通过执行本发明中公开的所有步骤(例如，图3、图4和图6中例示的步骤)，CPU 101从输入设备105获得输入测量数据，计算用于校准输入数据的校正矢量和校正增益，并获得重新估计的或更新的姿态。之后，CPU 101通过系统总线104将结果发送到输出设备106。也能够将结果存储在RAM 102中。经由网络接口，也能够将结果发送到远程计算机以用于其他应用。

此外，能够执行本发明的装置(例如，图2例示的装置)的各个单元、设备、部件和/或装配，它们被构造为通过软件、硬件、固件及其任意组合来进行姿态估计。

图2是示出物体姿态估计装置的构造的框图。图3是示出由图2示出的装置进行的物体姿态估计的流程图。CPU 101使用RAM 102或ROM 103中存储的程序和测量数据来执行本发明的方法。

如图2所示，在此，物体姿态估计装置200可以包括：传感器数据输入单元201、校正增益确定单元202、校正矢量生成单元203、传感器数据校准单元204以及姿态更新和输出单元205。

装置200将首先从陀螺仪数据的积分来估计大致的当前姿态。因此，传感器数据输入单元201被构造为针对姿态估计装置200，通过测量设备206来获取运动物体的当前姿态。在一个实施例中，输入可以是来自加速计和陀螺仪的三维数据，有时，磁力计数据也可以是输入。

校正增益确定单元202被构造为通过考虑加速度值的幅值以使其变为适应的值，来确定校正增益。CPU 101控制校正增益确定单元202来执行图3所示的步骤S301，以基于当前物体运动程度计算校正增益。在一个实施例中，校正增益确定单元202能够基于物体运动加速度的范数来计算校正增益。此外，能够基于历史姿态估计结果、历史校正增益以及基于陀螺仪或加速计/磁力计数据的当前姿态估计，来调整校正增益。

校正增益确定单元202基于来自传感器的即时测量和校正增益的历史数据，来计算校正增益。此外，基于在最近一次估计处理中计算的姿态估计偏差，来验证并调整计算出的校正增益。基于以下概念进行设计：

1.物体的运动加速度越接近零(静止或以匀速速度运动)，则基于加速计数据计算的物体的倾斜角(俯仰和滚转)越精确。

2.磁力计数据越接近当前位置的理论地磁场，则基于磁力计数据计算的物体的方向(偏航角)越精确。

3.此外，陀螺仪数据基本上不受物体加速度和磁场失真的影响。基于陀螺仪数据的姿态估计偏差来自陀螺仪数据的零漂移和附加的机械噪声。如果已知最近一次的物体的真实姿态，并且从最近估计到当前估计的时间差固定，则推论出使用陀螺仪数据的姿态估计偏差是稳定的。利用校准的陀螺仪数据，能够精确地估计被认为是物体的真实姿态的物体姿态。利用所述真实姿态，能够计算并分析基于当前陀螺仪数据的姿态估计偏差。如果观察到某些反常的姿态估计的偏差，则能够调整校正增益以解决该问题。这是校正增益验证和调整的步骤。

现在转到图4，图4是根据本发明的一个实施例的校正增益的确定方法的流程图。

在步骤S401中，CPU 101基于给定的传感器数据检测物体的静止状态。在一个实施例中，陀螺仪数据用于判断静止状态。首先，定义在静止状态下的最大角速率幅值的阈值。由用户根据应用的环境，将各轴上的陀螺仪数据与第一预定义阈值进行比较。例如，当用户手握住物体时，可以给出相对更大的阈值，以过滤无意识的手抖动；而可以给出相对更小的阈值以增大检测的灵敏度。假设角速率的单位是弧度/秒，本发明中的阈值的默认值是0.05。如果每个轴上的陀螺仪数据的幅值都小于阈值，则物体被判断为处于静止，反之亦然。

在一个实施例中，陀螺仪数据的范数被计算，并与第二预定义阈值进行比较。如果范数小于阈值，则物体被判断为处于静止。

在另一实施例中，加速计数据的变化率被计算，并与第三预定义阈值进行比较，以判断物体是否静止。能够通过不同时间在3个轴上的加速计数据的差来计算变化率。然后，通过两个采样时间的间隔来分割在3个轴上的加速计数据的差。最后，计算加速计数据的3个归一化的差的范数，作为变化率。在本发明中，阈值的默认值是0.5，然而，如前所述，可以根据应用环境来进行调整。

在又一实施例中，如上述所述的、基于加速计数据的静止状态检测方法，来计算磁力计数据的变化率。

在步骤S402中，CPU 101计算处于静止状态的物体的加速度的范数的平均值。如果物体被判断为处于静止状态，则计算并存储加速计数据的范数。在本发明中，处于静止的物体的累积加速计数据用于计算范数的平均值以替换重力的理论值。理论上来讲，静止物体的加速度等于重力，然而，无法完全去除与加速计数据相关的偏差及机械噪声。在一个实施例中，静止物体的最近的加速计数据样本被存储在固定长度缓冲器中。一旦只有新数据被推入到缓冲器中，就重新计算平均值。缓冲器的长度与传感器的采样率相关。在本发明中，缓冲器的长度是50，而采样率是50Hz。

在步骤S403中，与步骤S401和步骤S402并行地，CPU 101利用陀螺仪数据计算物体姿态。假设由四元数(q0，q1，q2，q3)来表示最近的物体姿态。以ω_x，ω_y和ω_z来表示陀螺仪数据。通过对间隔时间Δt内的角速率进行积分，能够得到在各轴上的旋转角度。

通过将旋转角度与初始角度相加，能够计算更新的四元数(q′₀，q′₁，q′₂，q′₃)。

然后，将四元数归一化，以使其变为单位矢量：

在步骤S404中，CPU 101估计在体坐标系的3个轴上的重力分量。在一个实施例中，计算在体坐标系的3个轴上的重力分量。

从更新的四元数(q′₀，q′₁，q′₂，q′₃)，能够计算从地心坐标系到体坐标系的转换矩阵(CRb)。利用CRb，通过以CRb左乘原始矢量，能够将地心坐标系中的矢量转换到体坐标系。

计算以G表示的、重力的范数的平均值。众所周知，重力的方向是垂直向下。因此已知地心坐标系(东，北，上)中的重力矢量是

通过左乘上述转换矩阵CRb，能够如下计算在体坐标系中的重力矢量(重力分量)

在步骤S405中，CPU 101通过从加速计数据中去除重力分量来计算物体的运动加速度。当加速计数据

被直接输入时，从加速计数据中减去体坐标系中的重力分量来获得运动加速度

因此，以

来计算并表示物体运动加速度的范数。

在步骤S406中，CPU 101基于运动加速度的范数的函数，来计算校正增益。针对该计算设计了运动加速度的范数的分段函数。在一个实施例中，定义分段函数为如下的线性函数：

y＝1.0-x/M if 0.0≤x≤M

y＝0.0 else (6)

其中，x是作为物体运动加速度的范数的函数的变量；y是作为校正增益的函数的因变量；M是能够由用户调整的预定义常量。

在本发明中，M的默认值等于在步骤S402中已经计算出的静止物体的加速度的累积范数的平均值。

在又一实施例中，本发明提供了用于由用户调谐校正增益的范围的方法。基于上述实施例，如下列出了改进的函数定义：

y＝K*(1.0-x/M)+ε if 0.0≤x≤M

y＝ε else (7)

其中，x、y、M与在上述式(6)中说明的相同。K是常量且是针对用户的可调整参数，以控制校正增益的范围，并且其默认值是2.0。ε是另一常量且也是针对用户的可调整参数，以控制最小校正增益值，并且其默认值是0.0。

图5A是示出用于校正增益确定的线性函数的示意图。校正的增益在范围[0，1]内。如果物体运动加速度的范数等于0，是指物体处于静止状态，则校正增益等于1。然而，如果物体运动加速度的范数超过预定义常量M，则校正增益等于0。

在本发明的另一实施例中，以如下另一种方式来定义分段函数：

y＝exp(-x²/(2M²)) if 0.0≤x≤M

y＝0.0 else (8)

其中，x是作为物体运动加速度的范数的函数的变量；y是作为校正增益的函数的因变量；M是能够由用户调整的预定义常量且其默认值等于静止物体的加速度的累积范数的平均值。

此外，本发明提供了用于由用户调谐校正增益的范围的方法。基于上述实施例，改进的函数如下：

y＝K*exp(-x²/(2M²))+ε if 0.0≤x≤M

y＝ε else (9)

其中，x、y、M与在上述式(8)中说明的相同。K是常量且是针对用户可调参数，以控制校正增益的范围，并且其默认值是2.0。ε是另一常量且也是针对用户可调的参数，以控制最小校正增益值，并且其默认值是0.0。

图5B是示出用于校正增益确定的高斯函数的示意图。校正的增益在范围[0，1]内。如果物体运动加速度的范数等于0，是指物体处于静止状态，则校正增益等于1。然而，如果物体运动加速度的范数超过预定义常量M，则校正增益等于0。

在步骤S406之后，存在基于最近一次的姿态估计偏差来调整校正增益的可选步骤(S407)。通常，如下所示，陀螺仪传感器产生与物体角速率成比例的输出，加上偏移且加上机械噪声。

Ω＝K*Ω_o+OF+N (10)

其中，Ω是来自陀螺仪的输出，Ω₀是物体的真实加速度，K是比例系数，OF是偏移，N是噪声。

传统上，陀螺仪数据不受物体运动加速度和周边磁场的失真的影响。在大多数应用中，定期收集陀螺仪数据。这是指两个邻近样本之间的时间间隔几乎固定。由于基于积分运算来计算物体的角度变化，因此在两个邻近样本之间，基于陀螺仪数据的姿态估计偏差是稳定的。

在本发明中，基于当前陀螺仪数据来估计物体姿态，并且通过稍后描述的校准陀螺仪数据来估计更新的物体姿态。因此能够计算两个估计结果之间的偏差。如上所述，偏差相对稳定。如果识别到来自前次周期的反常偏差，则是指校正增益需要被调整。在一个实施例中，收集每个周期的偏差以计算其平均值(μ)。如果平均值与给定偏差(dn)之间的差超过预定阈值(T)，则校正增益(CG)将被强制设置为预定义值(N)。在本发明中，T的默认值等于平均值的1/3，并且N是0.0。

CG＝N if|μ-dn|≥T (11)

在一个实施例中，收集每个周期的偏差以计算其平均值(μ)和标准方差(δ)。如果平均值与给定偏差(dn)之间的距离大于δ的预定阈值倍数(D)，则校正增益被设定为预定义值(N)。在本发明中，D的默认值是3，而N是0.0。

CG＝N |if|μ-dn|≥D*δ (12)

校正矢量生成单元203被构造为基于传感器数据的不同类型来生成校正矢量。CPU101控制校正矢量生成单元203来执行图3所示的步骤S302，以计算校正矢量。基于下列概念来设计校正矢量。

1.根据基于陀螺仪数据计算出的转换矩阵，将地心坐标系中的参考矢量转换到体坐标系。

2.根据基于加速计数据、或基于加速计数据和磁力计数据计算出的转换矩阵，将地心坐标系中的参考矢量转换到体坐标系。

3.执行在两个转换的参考矢量之间的叉积运算，以生成校正矢量。

现在转向图6，图6是根据本发明的一个实施例的校正矢量的生成方法的流程图。

在步骤S601中，CPU 101基于陀螺仪数据更新最近的四元数。换言之，陀螺仪数据用于更新物体姿态。这与在步骤S403中描述的过程相同。

在步骤S602中，CPU 101基于更新的四元数来计算第一转换矩阵。将更新的四元数转换为第一转换矩阵，该第一转换矩阵能够将矢量从地心坐标系转换到体坐标系。这与在步骤S404中说明的过程相同。

在步骤S603中，CPU 101基于加速计数据、或基于加速计数据和磁力计数据来更新最近的四元数。除了陀螺仪数据之外，加速计数据和磁力计数据也能够用于粗略地估计当前物体姿态。假定当前物体处于静止状态，并且周围磁场未失真。因此能够通过下式估计物体的欧拉角(俯仰角、滚转角和偏航角)。

roll＝-1*atan2(a_y，a_z) (13)

pitch＝atan2(α_x，a_z) (14)

yaw＝atan2(h_y，h_x) (15)

其中，

h_x＝m_x*cos(pitch)+m_y*sin(roll)*sin(pitch)-m_z*cos(roll)*sin(pitch)

h_y＝m_y*cos(roll)+m_z*sin(roll)

加速计数据：(a_x，a_y，a_z)；磁力计数据：(m_x，m_y，m_z)

通过下式将欧拉角转换为四元数。

q0＝CosHRoll*CosHPitch*CosHYaw+SinHRoll*SinHPitch*SinHYaw (16)

q1＝CosHRoll*SinHPitch*CosHYaw+SinHRoll*CosHPitch*SinHYaw (17)

q2＝CosHRoll*CosHPitch*SinHYaw-SinHRoll*SinHPitch*CosHYaw (18)

q3＝SinHRoll*CosHPitch*CosHYaw-CosHRoll*SinHPitch*SinHYaw (19)

其中：

最后，将四元数归一化，以确定其为单位矢量：

其中，

在步骤S604中，CPU 101基于更新的四元数来计算第二转换矩阵。将更新的四元数转换为第二转换矩阵，该第二转换矩阵能将矢量从地心坐标系转换到体坐标系。这与在步骤S404中描述的过程相同。

在步骤S605中，CPU 101分别基于第一转换矩阵和第二转换矩阵，将参考矢量从地心坐标系转换到体坐标系。在本发明的一个实施例中，在全局坐标系统(东，北，上)中的理论重力矢量被用作参考矢量，并且假定在当前位置处的重力幅值是9.8m/s²时，理论重力矢量的值是(0，0，9.8)。在东-北-上坐标系统中的理论地磁场矢量被用作参考矢量，并且假定标准地磁是50uG且在当前位置处的斜角是60度时，理论地磁场矢量的值是(0，50*sin(PI/3.0)，50*cos(PI/3.0)。在一个实施例中，在将已知矢量从地心坐标系转换到体坐标系之前，将该已知矢量标准化为单位矢量。

在步骤S606中，CPU 101基于两个转换的矢量来计算校正矢量。基于两个转换的矢量来计算校正矢量。其涉及执行在两个转换的矢量之间的叉积运算。

其中，

是校正矢量。

是基于依据陀螺仪数据估计的第一转换矩阵，从地心坐标系转换到体坐标系的参考矢量。

是基于由加速计和/或磁力计数据估计的第二转换矩阵，从地心坐标系转换到体坐标系的参考矢量。

是

与

之间的交叉角度的正弦。

是与矢量

和

正交的单位矢量。

传感器数据校准单元204被构造为使用校正矢量和校正增益，精确地校准用于重新估计物体姿态的陀螺仪数据。CPU 101控制传感器数据校准单元204，以执行图3所示的步骤S303。在一个实施例中，传感器数据校准单元204基于针对传感器数据补偿的校正增益，将校正矢量融合到陀螺仪数据中。补偿的陀螺仪数据被用于重新估计四元数。在可选步骤中，将更新的四元数转换为欧拉角和/或转换矩阵。

例如，传感器数据校准单元204通过下式，基于校正增益，将陀螺仪数据与校正矢量融合：

其中，

是当前陀螺仪数据。

是补偿的陀螺仪数据。

CG是在步骤S301中确定的校正增益。

是在步骤S302中生成的校正矢量。

在得到校准的陀螺仪数据之后，CPU 101计算陀螺仪数据校准前后的姿态估计偏差。在一个实施例中，校准(例如，步骤S403)前的当前陀螺仪数据减去校准的陀螺仪数据(例如，步骤S303)，得到差。然后，计算差的范数作为姿态估计偏差。在另一实施例中，当前陀螺仪数据(例如，步骤S403)和校准的陀螺仪数据(例如，步骤S303)被施加到叉积运算。来自叉积运算的输出矢量的范数被用作姿态估计偏差。此外，在相应的欧拉角之间的差的求和也能够被用作姿态估计偏差。

姿态更新和输出单元205被构造为以不同方式更新并输出物体姿态。补偿的陀螺仪数据被用于精确地估计物体姿态。CPU 101控制姿态更新和输出单元205以执行图3所示的步骤S304。在一个实施例中，姿态更新和输出单元205基于校准的陀螺仪数据来更新四元数，其中，更新物体姿态的方法与在步骤S403中描述的相同。

发明应用

一些应用需要欧拉角而非来自AHRS的四元数。在本发明中，CPU 101如下式将更新的四元数转换为欧拉角(俯仰、滚转和偏航)。

pitch＝-1*asin(2.0*(q0*q2-q1*q3)) (23)

roll＝-1*atan2(2*(q2*q3+q0*q1)，1.0-2.0*(q1*q1+q2*q2)) (24)

yaw＝-1*atan2(2.0*(q1*q2+q0*q3)，1-2.0*(q2*q2+q3*q3)) (25)

本发明提供了仅基于加速计数据和陀螺仪数据的俯仰、滚转和偏航估计方法。因此，本发明是AHRS的一部分。

在另一应用中，CPU 101基于更新的四元数来计算转换矩阵，以满足来自外部应用的潜在需求。用于计算转换矩阵的方法与在步骤S404中描述的相同。

已经针对一定优选实施例及其特定特征详细示出并描述了本发明。然而，应当注意，上述实施例旨在描述发明的原理，并非限制其范围。因此，如本领域常用技术那样显而易见的，在不脱离由所附的权利要求提出的本发明精神和范围的情况下，在形式和详情上，可以做出各种改变和变型。针对描述的实施例的其他实施例和变型对于本领域中的技术人员是显而易见的，并且可以在不脱离由以下权利要求限定的本发明的精神和范围的情况下做出。此外，发明者预期追踪多种类型的铰链式刚体的姿态，这些刚体包括但不限于蜂窝电话、假肢器官、机械臂以及运动物体。同样地，发明者预期其他的校正增益实施例，这些校正增益实施例包括但不限于校正增益的调整。此外，除非明确规定，否则在权利要求中对以单数形式的元素的参考并不旨在是指“一个且仅一个”，而是“一个或更多”。此外，能够实现在此示例性公开的实施例，而无需任何未在此特别公开的元素。

上述针对图2描述的单元和装置是用于实现各种步骤的示例性和/或优选的模块。模块可以是硬件单元(例如，处理器、专用集成电路等)和/或软件模块(例如，计算机程序)。以上并非穷尽性地描述了用于实现各种步骤的模块。然而，由于存在进行一定处理的步骤，因此存在用于实现同样处理的对应模块或单元(由硬件和/或软件实现)。由以上和以下描述的步骤以及与这些步骤相对应的单元的所有组合的技术方案被包括在本申请的公开中，只要它们组成的技术方案是完整的并且可应用的即可。

Claims (20)

1.一种依据陀螺仪数据和加速计数据的、针对运动物体的姿态估计方法，所述姿态估计方法包括：

校正增益确定步骤，其用于基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；

校正矢量计算步骤，其用于通过基于所述陀螺仪数据和加速计数据，将参考矢量从地心坐标系转换到体坐标系，来计算校正矢量；

传感器数据校准步骤，其用于使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；

姿态估计步骤，其用于利用经校准的陀螺仪数据来估计所述姿态，

其中，所述校正增益确定步骤还包括以下步骤：基于所述加速计数据来估计在体坐标系中的重力分量；通过从所述加速计数据中去除重力分量来计算物体的运动加速度；基于所述物体的所述运动加速度的预定义函数来计算所述校正增益。

2.根据权利要求1所述的姿态估计方法，所述姿态估计方法还包括调整步骤，所述调整步骤基于利用当前陀螺仪数据和经校准的陀螺仪数据而计算的姿态估计偏差，来调整所述校正增益。

3.根据权利要求1或2所述的姿态估计方法，其中，所述估计在体坐标系中的重力分量还包括：

基于所述加速计数据、陀螺仪数据和磁力计数据中的至少一个，来确定所述物体处于静止的状态；

在所述物体处于静止状态，计算所述运动加速度的范数；

在所述物体处于静止状态，计算所述运动加速度的范数的平均值；

利用计算出的范数的平均值，构建在所述地心坐标系中的重力矢量；

基于使用陀螺仪数据而计算出的转换矩阵，将所述重力矢量从地心坐标系转换到体坐标系。

4.根据权利要求1或2所述的姿态估计方法，其中，所述运动加速度的预定义函数是分段函数，并且如果所述运动加速度的范数超过预定义阈值，则将所述校正增益设置为预定义常量；否则，使用所述运动加速度的范数的连续函数来计算所述校正增益。

5.根据权利要求1或2所述的姿态估计方法，其中，所述校正矢量计算步骤还包括以下步骤：

基于使用陀螺仪数据计算出的转换矩阵，将所述地心坐标系中的参考矢量转换到体坐标系；

基于使用加速计数据计算出的转换矩阵，或基于加速计数据和磁力计数据，将地心坐标系中的参考矢量转换到体坐标系；

执行两个转换的参考矢量之间的叉积运算，以生成校正矢量。

6.根据权利要求1所述的姿态估计方法，其中，所述传感器数据校准步骤还包括以下步骤：

通过将校正矢量乘以校正增益，来计算矢量；

将所述矢量与在体坐标系的不同轴上的当前陀螺仪数据相加。

7.根据权利要求2所述的姿态估计方法，其中，所述姿态估计偏差的计算还包括以下步骤：

依据陀螺仪数据分别得到当前姿态估计和经校准的姿态估计；

计算所述当前姿态估计与所述经校准的姿态估计之差，以获得所述姿态估计偏差。

8.根据权利要求2所述的姿态估计方法，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的差大于预定义阈值，则将所述校正增益强制调整为预定义值。

9.根据权利要求2所述的姿态估计方法，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的距离大于所述姿态估计偏差的标准方差的预定义阈值倍数，则将所述校正增益强制调整为预定义值。

10.根据权利要求4所述的姿态估计方法，其中，所述运动加速度的范数的所述连续函数是线性函数或高斯函数。

11.根据权利要求7所述的姿态估计方法，其中，所述差是以下项目中的一个：

1)所述当前陀螺仪数据的范数减去所述经校准的陀螺仪数据；

2)依据所述当前陀螺仪数据与所述经校准的陀螺仪数据之间的叉积运算的输出矢量的范数；

3)相应的欧拉角之间的差的求和。

12.一种针对运动物体的姿态估计装置，所述姿态估计装置包括：

输入单元，其被构造为从陀螺仪数据和加速计数据输入传感器数据；

校正增益确定单元，其被构造为基于利用所述陀螺仪数据和加速计数据而测量的物体运动程度，来确定校正增益；

校正矢量计算单元，其被构造为通过基于所述陀螺仪数据和加速计数据，将参考矢量从地心坐标系转换到体坐标系，来计算校正矢量；

传感器数据校准单元，其被构造为使用所述校正增益和所述校正矢量，来校准所述陀螺仪数据；

姿态估计和输出单元，其被构造为利用经校准的陀螺仪数据来估计并输出所述姿态，

其中，所述校正增益确定单元还基于所述加速计数据来估计在体坐标系中的重力分量；通过从所述加速计数据中去除重力分量来计算所述物体的运动加速度；基于物体的所述运动加速度的预定义函数来计算所述校正增益。

13.根据权利要求12所述的姿态估计装置，所述校正增益确定单元还基于利用当前陀螺仪数据和经校准的陀螺仪数据而计算的姿态估计偏差，来调整所述校正增益。

14.根据权利要求12或13所述的姿态估计装置，其中，所述估计在体坐标系中的重力分量还包括：

基于所述加速计数据、陀螺仪数据和磁力计数据中的至少一个，确定所述物体处于静止的状态；

在所述物体处于静止状态，计算所述运动加速度的范数；

在所述物体处于静止状态，计算所述运动加速度的范数的平均值；

利用计算出的范数的平均值，构建在所述地心坐标系中的重力矢量；

基于使用陀螺仪数据而计算出的转换矩阵，将所述重力矢量从地心坐标系转换到体坐标系。

15.根据权利要求12或13所述的姿态估计装置，其中，所述运动加速度的预定义函数是分段函数，并且如果所述运动加速度的范数超过预定义阈值，则将所述校正增益设置为预定义常量；否则，使用所述运动加速度的范数的连续函数来计算所述校正增益。

16.根据权利要求12或13所述的姿态估计装置，其中，所述校正矢量计算单元还基于使用陀螺仪数据计算出的转换矩阵，将所述地心坐标系中的参考矢量转换到体坐标系；基于使用加速计数据计算出的转换矩阵，或基于加速计数据和磁力计数据，将地心坐标系中的参考矢量转换到体坐标系；执行两个转换的参考矢量之间的叉积运算，以生成校正矢量。

17.根据权利要求13所述的姿态估计装置，其中，所述姿态估计偏差的计算还包括以下步骤：

从陀螺仪数据分别得到所述当前姿态估计和经校准的姿态估计；

计算所述当前姿态估计与所述经校准的姿态估计之差，以获得所述姿态估计偏差。

18.根据权利要求13所述的姿态估计装置，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的差大于预定义阈值，则将所述校正增益强制调整为预定义值。

19.根据权利要求13所述的姿态估计装置，其中，如果新姿态估计偏差与所述姿态估计偏差的平均值之间的距离大于所述姿态估计偏差的标准方差的预定义阈值倍数，则将所述校正增益强制调整为预定义值。

20.根据权利要求17所述的姿态估计装置，其中，所述差是以下项目中的一个：

1)所述当前陀螺仪数据的范数减去经校准的陀螺仪数据；

2)来自所述当前陀螺仪数据与经校准的陀螺仪数据之间的叉积运算的输出矢量的范数；

3)相应的欧拉角之间的差的求和。

Images