CN106574830A - 使用软约束和惩罚函数初始化惯性传感器 - Google Patents

Abstract

一种用于惯性传感器的方法，其在无需传感器以静止或者在公知的定位或取向下开始的情况下估计开始取向和速度。初始化使用编码为软约束集合的运动模式，这些软约束预期在初始化时段期间近似成立。惩罚度量被限定以便测量计算的运动轨迹与软约束的偏差。用于惯性传感器的运动微分方程以初始条件作为变量进行求解；最小化惩罚度量的初始条件用作对于传感器的实际初始条件的估计。用于具体应用的软约束和惩罚度量基于该应用所期望的运动模式类型进行选择。说明性案例包括初始化期间具有相对较小的运动的应用以及初始化期间具有近似周期性运动的应用。

Description

使用软约束和惩罚函数初始化惯性传感器

技术领域

一个或多个实施例涉及运动捕获以及与其关联的可能与一个用户或一台装备耦合的传感器的初始化领域。更具体地但非限制性地，一个或多个实施例允许使用软约束和惩罚函数初始化惯性传感器以便估计惯性传感器设备的初始条件。该初始化通常基于在初始化时间间隔期间从该设备捕获的数据并且基于对该间隔期间该设备的运动的软约束集合发生在某个时间点。

背景技术

惯性传感器通常用于运动跟踪和导航。它们提供了以下优点：它们可以用来在无需外部输入或参考的情况下估计物体的运动。这些系统典型地包括测量线性力的加速度计以及测量旋转运动的陀螺仪。

这样的系统多年来用作船舶和飞机的导航辅助设备。更近些时候，随着廉价的MEMS传感器变得广泛可用，惯性传感器日益嵌入到诸如智能电话之类的消费产品中。

纯惯性传感器不能检测均匀线性运动；这实质上是相对性原理的结果。它们也不能确定绝对位置或取向。实际上，惯性传感器可以用于跟踪位置和取向的变化。作为运动跟踪或导航设备，它们依靠航位推算技术从已知的起始条件并且从惯性传感器检测的变化估计新的位置和取向。

因此，使用惯性传感器的应用必须在借助于惯性数据测量位置和取向的变化之前使用某种方法确定传感器的初始条件。现有的系统通常将两种方法之一或者二者用于该初始化。

一种现有的惯性传感器初始化方法是将传感器置于已知的位置和取向，并且确保它在开始运动跟踪之前的一定时间段是静止的(没有线性或旋转运动)。然后，可以将传感器的初始位置和取向直接提供给运动跟踪算法，并且可以将初始线速度和角速度设置为零以便反映该静止初始状态。这种方法极为简单，但是在以下情形下可能不适用。有时，初始位置或初始取向不可能知晓。在一些应用中，在开始运动跟踪之前要求静止时间段可能不是可行的。

要求传感器在开始运动跟踪之前是静止的现有系统有时可以通过使用加速度计作为重力矢量传感器而获得传感器相对于竖直的初始倾斜。这种技术仅仅在传感器静止时且当存在可忽略的振动时才适用；否则，加速度计数据将反映真实加速度和重力的组合。

第二种现有的惯性传感器初始化方法是利用其他传感器数据馈送补充惯性传感器数据。例如，GPS系统经常结合惯性传感器使用以便提供具有绝对位置信息的数据馈送。磁体可以结合惯性传感器使用以便提供具有绝对取向信息的数据馈送。这些类型的组合了惯性和非惯性传感器的混合系统常常用于导航。例如，GPS系统可以用作主要导航工具，惯性传感器在GPS信号暂时不可用的情况下提供GPS更新之间的外推和备份。

混合系统的缺点是附加的传感器硬件和软件的成本和复杂性。特别是对于GPS(或者类似的定位系统，例如使用蜂窝电话信号的定位系统)而言，GPS接收器必须集成到传感器硬件中。这可能是不切实际的，或者它可能在某些诸如室内之类的环境下根本不起作用。

总之，已知的用于初始化惯性传感器的系统通常依赖于将传感器置于静止和已知的条件下或者利用附加的传感器馈送补充惯性传感器。任一方法对于特定的应用而言都具有局限和缺点。因此，需要一种可以在更一般的条件下使用并且无需附加的传感器硬件的惯性传感器初始化方法。

发明内容

本发明的实施例涉及一种基于在初始化时间间隔期间从惯性传感器设备捕获的数据并且基于对该间隔期间该设备的运动的软约束集合在某个时间点估计该设备的初始条件的方法。这些约束可以称为软约束，因为在一些实施例中它们预期仅仅对于初始化时间间隔期间所述设备的运动近似成立。对这些软约束的小偏离可以预期，但是大的偏离可以预期是不大可能的。不同的软约束可以由本发明的不同实施例使用，其潜在地反映了使用这些不同实施例的各种应用中预期的典型运动模式。

本发明的一个或多个实施例可以不要求惯性传感器在整个初始化时间间隔上静止或者处于特定的取向。此外，一些实施例可以不要求利用诸如GPS读数之类的其他非惯性数据补充从惯性传感器捕获的数据。因此，本发明的实施例可以用来在广泛的条件下且没有附加的传感器硬件的情况下通过在初始化期间使用预期运动模式作为软约束集合而估计惯性传感器的初始条件。例如，本发明的一个或多个实施例可以用来估计连续使用的装置的初始取向或初始速度，在连续使用中，要求该装置对于专门的初始化过程保持静止是不切实际的。这样的使用情况可能适用于例如具有集成的惯性传感器的体育装备，其中在比赛期间为了初始化装备的取向或速度而打断装备的用户是不切实际的。

本发明的实施例可以使用一个或多个惩罚度量量化惯性传感器设备的实际运动轨迹与软约束集合的偏差。和软约束本身一样，本发明的一个或多个实施例使用的惩罚度量可以基于使用这样的实施例的应用中的预期运动模式进行选择。

在一个或多个实施例中，用于惯性传感器的初始条件的估计可以通过最小化惩罚度量计算。实际上，该过程可以找到生成与为这样的实施例限定的软约束接近的运动轨迹(其可以包括位置和速度轨迹)的初始条件。对于现有技术中的典型惯性运动跟踪应用而言，惯性传感器的初始条件是已知的，并且传感器的运动轨迹通过对用于惯性传感器的公知运动微分方程积分(通过分析或者数值方法)进行计算。本发明的一个或多个实施例中使用的方法在某种意义上是典型运动跟踪方法的逆：初始条件可以被看作要通过最小化运动轨迹与软约束的偏差找到的变量。

确定最小化惩罚度量的初始条件可以通过用于函数最小化的已知技术集合中的任何一种进行。它们包括直接分析技术(其有时可以用于简单函数形式)以及用于函数优化的各种数值方法。本发明的实施例也可以应用近似最小化方法获得不确切但是有用的初始条件估计。这样的近似方法例如在具有相对较小计算能力的嵌入式处理器上执行初始化计算的情况下或者在获得近似估计在时间上非常关键的情况下可能是有用的。

在本发明的一个或多个实施例中，对惯性传感器的运动的软约束可以包括这样的约束：传感器的位置在初始化时段期间变化相对较小，并且传感器的初始速度相对较小。这些实施例可以使用由整个初始化时段上惯性传感器的净位置变化组成的简单惩罚度量。这种类型的简单惩罚度量例如在一个或多个实施例中可以用于高尔夫球挥杆分析应用，其中玩家在准备击球(address)期间移动他的高尔夫球杆(其包含集成或者附接的惯性传感器)，但是球杆头在该准备击球时段期间从不移动得离球太远，并且它在准备击球期间从不移动得太快。

在一个或多个实施例中，传感器的初始取向可以使用将运动微分方程分为取向方程以及位置和速度方程的技术直接计算。微分方程的这种分离得到净位置变化的表达式(惩罚度量)，其可以以分析方法最小化以便给出初始取向估计。该分析解决方案可以在不能为迭代数值最小化方法承担时间或计算资源的应用中提供益处。

在本发明的一个或多个实施例中，对惯性传感器的运动的软约束可以包括指示运动在初始化间隔期间为近似周期性的一个或多个约束。在一个或多个实施例中，惩罚度量可以包括整个单个选择的运动周期上的净位置变化和净速度变化。在一个或多个实施例中，这样的约束和惩罚度量可以例如用于棒球挥杆分析应用，其中击球手在等待投球的同时在准周期性来回运动中或者在重复圆周运动中移动他的球棒。

在本发明的一个或多个实施例中，可以在最小化惩罚度量之前在初步处理步骤中计算运动周期的估计。一个或多个实施例可以包括周期估计步骤，该步骤在初始化期间在惯性传感器数据上使用离散傅里叶变换(DFT)以便识别主导运动频率(以及因而主导周期)。初步周期估计步骤的使用提供了以下益处：所述周期可以从观察的运动动态地计算。因此，它可以是由一个或多个实施例使用的自适应方法，该方法使软约束(特别地，所述周期)适应为特定事件观察的特定运动模式。

在使用包括一定时段上的净位置变化或者净速度变化的惩罚度量的一个或多个实施例中，可以应用微分方程的分离以得到该初始化时段期间净速度和位置变化的表达式。在一个或多个实施例中，这些表达式可以以分析方法顺序地求解：首先，可以计算初始取向以便最小化净速度变化，并且然后可以计算初始速度以便最小化净位置变化。

在本发明的一个或多个实施例中，可以在确定惯性传感器的初始取向和初始速度的估计中利用这样的方法，该方法包括：在包括感兴趣活动的捕获时间间隔上收集来自惯性传感器的样本数据，其中样本数据包括加速度计数据和速率陀螺仪数据。加速度计数据和速率陀螺仪数据中的每一个都是三维矢量的时间序列；在一些实施例中，惯性传感器可以提供陀螺仪或加速度计的少于三个轴上的数据，但是该数据总是可以嵌入到三维矢量中。惯性传感器在任何时间点的取向都是三维空间的正交变换，其可以以诸如三乘三矩阵或者单位四元数之类的各种形式表示。惯性传感器在任何时间点的速度和位置中的每一个都是三维矢量。初始化时间间隔在捕获时间间隔内选择。软约束集合在初始化间隔期间的惯性传感器的位置和速度轨迹上限定。软约束集合的每一个限定可能的位置矢量和速度矢量轨迹的集合。在本发明的一个或多个实施例中，软约束表示初始化间隔期间感兴趣活动的典型位置和速度轨迹模式的近似。惩罚度量集合被限定来量化惯性传感器的计算的位置和速度轨迹与软约束的偏差。当初始取向和初始速度估计用作在整个初始化间隔上将样本陀螺仪和加速度计数据整合到取向、速度和位置中的初始条件时，计算初始化间隔开始处惯性传感器的初始取向和初始速度的估计以便最小化惩罚度量。在一个或多个实施例中，该整合使用惯性传感器的耦合运动微分方程，这些方程将取向、速度和位置的时间导数彼此关联且与测量的加速度计和陀螺仪数据关联。在一个或多个实施例中，使用一个微分方程，其中取向的时间导数等于取向乘以表示与陀螺仪矢量的叉积的斜对称矩阵。使用第二微分方程，其中速度矢量的时间导数等于取向乘以加速度计矢量，这个量添加至重力矢量。重力矢量项反映了以下事实：惯性传感器的加速度计测量相对于自由落体的表观运动而不是真实的加速度。使用第三微分方程，其中位置矢量的时间导数等于速度矢量。

在本发明的一个或多个实施例中，软约束包括以下要求：初始化间隔的开始与结束之间的位置变化为零，反映了惯性传感器在整个初始化间隔上预期有效地保持在相同的地方或者在有限的空间区域中。在一个或多个实施例中，惩罚度量包括这样的度量，其被限定为初始化间隔的结束与初始化间隔的开始之间的位置差异的绝对值。该度量测量位置轨迹与惯性传感器的零净运动的软约束的偏差。在一个或多个实施例中，将惯性传感器的初始速度的估计设置为零，并且使用惩罚度量的最小化以估计传感器的初始取向。

在本发明的一个或多个实施例中，软约束包括以下要求：惯性传感器的位置和速度在初始化间隔期间是周期性的。在一个或多个实施例中，通过在初始化间隔期间速率陀螺仪数据时间序列的相应轴上使用离散傅里叶变换对速率陀螺仪的每个轴计算频域信号，获得对于初始间隔期间的主导运动周期的估计。计算每个轴上的变换的频域信号的峰值幅度，并且获得所有轴上的最大峰值幅度。出现该最大峰值幅度的频率用作初始化间隔期间的运动主导频率。将主导运动周期计算为主导频率的倒数。在一个或多个实施例中，惩罚度量包括：速度惩罚度量，其是一个运动周期上速度变化的绝对值；以及位置惩罚度量，其是一个运动周期上位置变化的绝对值。选择初始化间隔内的指定时间段，其具有等于主导运动周期的长度。在一个或多个实施例中，首先通过在指定时段上最小化速度惩罚度量计算传感器的初始取向的估计，并且然后通过使用先前计算的初始取向的估计在指定时段上最小化位置惩罚度量计算传感器的初始速度的估计。

附图说明

通过本公开传达的思想的上述和其他方面、特征和优点根据以下结合附图呈现的、对于其更具体的描述将更加清楚明白，在附图中：

图1示出了本发明一个实施例的流程图，其图示出生成惯性传感器设备的初始取向的估计的方法中的主要步骤。

图2示出了图1步骤中的两个的详细视图，其图示出从惯性传感器捕获的数据以及初始化间隔的选择。

图3示出了图1步骤中的另两个的详细视图，其图示出对位置和速度的软约束的限定以及用于与这些软约束的偏差的惩罚度量的限定。

图4示出了图1流程图中的最后步骤的一个实施例的详细视图，其通过对运动微分方程积分并且将惩罚度量应用到得到的位置和速度而计算初始条件。

图5示出了所述方法的一个实施例，其中惩罚度量由整个初始化间隔上的位置变化组成，并且初始速度估计设置为零。

图6图示出所述方法的一个实施例，其使用特定技术通过从运动方程中分解出初始取向而计算最优初始取向。

图7进一步图示出图6的技术，示出获得用于初始取向估计的直接解的所述方法的一个实施例。

图8示出了所述方法的一个实施例的流程图，其将在初始化间隔期间要求周期性运动的软约束应用到位置和速度，并且使用离散傅里叶变换估计该周期运动的周期。

图9示出了图8步骤中的两个的详细视图，其图示出周期性约束以及用于与该周期性约束的偏差的惩罚度量的限定。

图10示出了图8步骤中的另两个的详细视图，其图示出使用离散傅里叶变换计算初始化间隔期间的主导周期。

图11示出了图8的最后两个步骤的一个实施例的详细视图，其图示出首先计算最优初始取向估计，随后计算最优初始速度估计。

具体实施方式

现在，将描述一种用于确定惯性传感器的初始取向和初始速度的估计的方法。在以下示例性描述中，阐述了大量特定细节以便提供对于整个本说明书中描述的思想的更透彻的理解。然而，普通技术人员将清楚，本文描述的思想的实施例可以在不结合本文描述的具体细节的所有方面的情况下实施。在其他实例中，没有详细描述本领域普通技术人员公知的特定方面以便不使本公开模糊不清。读者应当注意到，尽管贯穿本公开阐述了创新构思的实例，权利要求以及任何等效物的全部范围限定了本发明。

图1图示出确定惯性传感器的初始取向和初始速度的方法的一个实施例的流程图。在101处，从惯性传感器捕获样本数据。该样本数据包括整个捕获时间间隔上测量的一个或多个时间序列，其中该捕获时间间隔包括感兴趣活动。在102处，在该捕获时间间隔内，选择初始化间隔。该初始化间隔将用于初始取向和初始速度的估计计算。在103处，单独地限定对惯性传感器的位置和速度的软约束集合。在104处，限定测量计算的位置和速度与软约束的偏差的惩罚度量。在105处，使用来自初始化间隔的样本数据和惩罚度量通过在整个初始化间隔上最小化惩罚度量计算初始取向和初始速度的估计。

图2图示出在101处收集样本数据以及在102处选择初始化间隔的详细视图。在101处收集样本数据中，在具有开始时间203(t_i)和结束时间204(t_f)的捕获时间间隔[t_i,t_f]上收集惯性传感器数据。在一个实施例中，惯性传感器包含3轴加速度计和3轴速率陀螺仪。在其他实施例中，不同的数据可能可用，包括例如来自磁力计的数据或者来自GPS接收器的定位数据。在具有3轴加速度计和3轴速率陀螺仪的一个实施例中，从传感器捕获的样本数据包括表示为s(t)的加速度计数据时间序列201以及表示为ω(t)的速率陀螺仪数据时间序列202。s(t)和ω(t)的粗体符号表明它们是矢量(典型地在3维空间中)；然而，为了进行说明，这些值在曲线201和202中示为1维的。用于加速度计数据的符号s(t)反映以下事实：典型的加速度计测量比力而不是真实加速度。用于速率陀螺仪数据的符号ω(t)表明典型的速率陀螺仪测量角速度。

在图2中，在捕获间隔[t_i,t_f]内选择初始化间隔205。初始化间隔具有开始点206(t₀)和结束点207(t_n)。初始化间隔可以基于各种依赖于感兴趣事件的性质的标准在捕获间隔内选择。例如，在一个或多个实施例中，初始化间隔可以由捕获间隔开始处固定数量的秒组成。在其他实施例中，初始化间隔可以通过在捕获间隔中搜索指示相对较低的运动或活动的签名而动态地确定。一旦选择了初始化间隔205，整个初始化间隔上的加速度计数据208和陀螺仪数据209用于下面描述的估计初始取向和初始速度。

图3图示出限定软约束103和限定惩罚度量104的详细视图。表示为r(t)的惯性传感器的位置轨迹301和表示为v(t)的惯性传感器的速度轨迹305预期近似符合对于感兴趣活动而言典型的某些模式。通常，惯性传感器的位置和速度是3维空间中的矢量；然而，为了进行说明，它们在图3中示为1维值。位置和速度所预期的特定模式将取决于特定活动或者感兴趣活动。通常，对于位置和速度的软约束C_i可以表示为整个初始化间隔上可能的位置和速度轨迹的子集：本发明的一个实施例可以包括这些软约束的集合{C_i}。图3示出了说明性约束302和306。这些约束是仅仅用于说明的实例；其他实施例将包括其他类型和数量的软约束。表示为C_R的软约束302是对位置轨迹301的约束。该约束要求贯穿整个感兴趣时间间隔位置r(t)都保持在上限303和下限304内。表示为C_V的软约束306是对速度轨迹305的约束。约束306仅仅应用在轨迹的端点处；它要求在该端点处，速度处于C_V限定的极限内。这些说明性软约束302和306示出了可能的软约束类型的简单实例；其他实施例可以包括对位置、速度或者二者的更复杂的约束。

在图3中，在104处，限定惩罚度量以量化位置和速度轨迹与103处限定的软约束的偏差。表示为r(t)的测量或者计算的位置307不满足软约束302。表示为m_R的惩罚度量308测量r(t)与软约束的偏差。为了进行说明，将惩罚度量m_R限定为位置与软约束所施加的限制的最大绝对偏差。其他实施例可能限定不同的惩罚度量；例如，一个实施例可能使用位置与软约束的限制的平均绝对偏差，而不是如图3中所示的最大偏差。速度惩罚度量的说明类似于位置惩罚度量的说明。表示为v(t)的测量的或者计算的速度309不满足软约束306。表示为m_V的惩罚度量310测量v(t)与软约束的偏差。为了进行说明，将m_V限定为速度与软约束施加的限制的绝对偏差。由于说明性速度软约束306仅仅应用在时间间隔的端点处，因而速度惩罚度量310也测量仅仅在该端点处的与约束的偏差。和位置惩罚度量一样，该速度惩罚度量仅仅是一个实例。其他实施例可能限定不同的惩罚度量；例如，一个实施例可能使用速度与软约束的限制的绝对偏差的平方，而不是如图3中所示的偏差的绝对值。

图4图示出在105处计算最小化惩罚度量的初始取向和初始速度的详细视图。表示为s(t)的加速度计数据208以及表示为ω(t)的速率陀螺仪数据209是计算过程的输入。这些轨迹在整个初始化间隔[t₀,t_n]上可用。给定这些输入并且给定关于传感器取向(Q₀)和速度(v₀)的初始条件402，可以求解微分方程401以得到位置轨迹307(r(t))和速度轨迹309(v(t))。微分方程401是将取向Q(t)、速度v(t)和位置r(t)的时间倒数与观察的测量结果s(t)和ω(t)关联的联立方程组。这些微分方程是本领域技术人员公知的；它们是用于使用捷联惯性传感器的惯性导航的方程形式。401中的符号ω^×指示通过Sx＝ω×x限定的线性算子，其中×是矢量叉积。

在105的计算中，初始条件402被看作要估计的未知量。这些初始条件的每个估计得到用于位置和速度的特定轨迹配对307和309。然后，将惩罚度量308和310应用到这些计算的轨迹。这些惩罚度量生成计算的轨迹与软约束的偏差的聚合量度。本发明的不同实施例可以采用各种技术将惩罚度量集合结合到软约束的偏差的聚合量度中，例如对惩罚度量求和或者首先最小化一个度量并且然后按顺序最小化其他度量。在403处，调节初始条件以最小化观察的惩罚度量308和310。如图4所示，该调节过程可以是迭代的，以寻求最小化惩罚度量的初始条件。任何已知的用于最小化目标函数的技术都可以用来寻找最小化惩罚度量的初始条件。这样的技术是本领域技术人员公知的。在应用选择的最小化技术之后，105处的计算得到用于初始取向估计的表示为以及用于初始速度估计的表示为的初始条件的估计404。

图5图示出本发明的一个实施例，其具有用于位置轨迹的特定软约束，它反映了初始化间隔期间惯性传感器最小运动的期望。这种类型的软约束可以应用于例如这样的活动，其中在开始该活动之前，一个人或者一台装备是相对静止的。图5图示出图4中的过程如何被修改和专用于这种类型的实施例。像在图4中那样，加速度计数据208和陀螺仪数据209连同初始条件402一起是微分方程积分401的输入，得到位置轨迹307。在图5所示的实施例中，位置惩罚度量502被限定为整个初始化间隔上的净位置变化的幅度：|Δr|＝|r(t_n)-r(t₀)|。该惩罚度量测量初始化间隔期间位置轨迹多么密切地符合传感器零净运动的软约束。其他实施例可能使用不同的惩罚度量以测量该偏差，例如整个初始化间隔上与开始位置的最大位置变化。在图5所示的实施例中，通过将初始速度估计设置为v₀＝0而将另一简化501应用到初始条件。该简化再次反映了初始化间隔期间相对较小的运动的期望。其他实施例可能采用关于初始速度的不同假设。因此，最小化步骤503仅仅调节初始取向Q₀以最小化位置惩罚度量502(|Δr|)，得到表示为的初始取向的估计504。

图6图示出一种可以应用在本发明的一个或多个实施例中以便使用初始取向和初始速度的输入402以及传感器数据208和209对微分方程401积分的技术。在该技术中，首先单独地求解微分方程601。在603处，通过限定辅助变量P求解该方程其中Q＝Q₀P。这种替代得到方程初始条件为P(0)＝I(其中I为单位矩阵)。可以不依赖于初始取向Q₀对于P(t)直接求解该方程。(在大多数情况下，P(t)将通过数值积分计算，因为封闭形式的解很可能不存在。)其次，其余微分方程602可以使用603中计算的P(t)值作为输入604进行求解。在605中，对这两个方程积分，得到用于位置和速度的表达式：和在一个或多个实施例中，这些表达式用来计算最小化惩罚度量的初始条件402。

图7图示出用在一个或多个实施例中以便将605的表达式与特定位置惩罚度量502(|Δr|)一起用来直接计算最小化该惩罚度量的初始取向Q₀的技术。使用v₀＝0的简化501，并且在701处限定辅助变量h，其中得到表达式该表达式在702处图解表示出，值用于初始取向。由于Q₀为正交变换，可能值Q₀h的范围必须位于半径为|h|的球上。该球的二维视图被示为圆703。704中示出了到的旋转；所有情况下，Q₀h都在703上。因此，为了最小化|Δr|，几何上直观的是Q₀h必须指向-g方向，得到最优值706和705处的最小值|Δr|。该技术提供最多只有一个围绕g的任意旋转的最优反映了纯惯性传感器不能感测航向(heading)。在本发明的其他实施例中，诸如磁力计数据之类的其他传感器数据也可以用来估计初始航向。

图8图示出用于使用软约束的本发明一个实施例的流程图，该软约束要求惯性传感器的运动在初始化间隔期间近似周期性的。软约束的限定103包括在805处限定用于初始化间隔期间惯性传感器的速度和位置的周期性约束。与周期性软约束相应，惩罚度量的限定104包括806处的度量，其测量单个周期上的位置变化和速度变化作为惩罚度量。在102处选择初始化间隔，并且在801处计算该间隔期间运动周期的估计。该周期计算通过在802处借助于离散傅里叶变换将传感器陀螺仪数据的时间序列变换到频域并且在803处从变换的数据选择主导频率而执行。在804处，选择长度等于该周期的初始化间隔内的特定时间间隔。在105处，在两个阶段中最小化惩罚度量806：首先，在807处计算最小化速度惩罚度量的初始取向估计，并且然后在808处计算最小化位置惩罚度量的初始速度估计。本发明的其他实施例可以采用这些技术的子集；例如，可以使用相同的速度和位置惩罚度量，但是可以使用时域技术代替傅里叶变换估计运动周期。

图9提供了周期性软约束的限定805以及位置和速度惩罚度量的限定806的详细视图。在805处，软约束要求位置901(表示为r(t))和速度903(表示为v(t))是周期性的，具有某个周期903(表示为T)。这些软约束仅仅要求周期性运动；它们没有规定位置和速度曲线的周期或形状。在806处，计算的位置904和计算的速度906并非精确地为周期性的。位置惩罚度量905测量与确切周期性的偏差，作为一个周期上的位置变化的绝对值|Δr|＝|r(t+T)-r(t)|。类似地，速度惩罚度量907测量与确切周期性的偏差，作为一个周期上的速度变化的绝对值|Δv|＝|v(t+T)-v(t)|。

图10提供了运动周期的计算801的详细视图。使用离散傅里叶变换将包括用于陀螺仪x轴ω_x的时间序列1001以及用于ω_y和ω_z的相似时间序列的初始化间隔期间的时域速率陀螺仪数据ω(t)变换到频域。针对其峰值扫描表示为|Ω_x|的变换的ω_x的幅度1002。峰值1003出现在频率1004处。执行幅度|Ω_y|和|Ω_z|的类似扫描以便找到这些轴上的峰值以及出现这些峰值的频率。在1005(为了进行说明示于z轴上)处选择所述三个轴上的最大峰值幅度，并且出现该峰的表示为f_max的频率1006用作初始化期间运动的总体主导频率。然后，在1007处将主导周期计算为T＝1/f_max。

图11提供了在807处计算初始取向估计和在808处计算初始速度估计的详细视图。在804中找到的表示为[t₁,t₁+T]的长度为T的间隔1101用于这些计算。按顺序对惩罚度量905和907最小化。使用上面描述的且在图6中图示出的技术，获得表达式1102，得到用于速度惩罚度量的可以使用图7中所示的几何技术或者使用用于数值优化的任何其他公知技术对该表达式最小化，得到初始取向估计在808处的后续计算中，使用初始取向估计1103。再一次地，图6中所示的积分技术用来获得表达式1104，得到该表达式现在取决于仅仅单个未知数v₀，因为先前已经计算了现在，可以使用用于数值优化的任何公知技术最小化用于|Δr|的这个表达式以得到用于初始速度的估计

Claims (3)

1.一种用于确定惯性传感器的初始取向Q₀和初始速度v₀的估计的方法，包括步骤：

在包括感兴趣活动的捕获时间间隔[t_i,t_f]上收集来自惯性传感器的样本数据，所述样本数据包括加速度计数据s(t)和速率陀螺仪数据ω(t)；

在所述捕获时间间隔内选择初始化间隔

限定对于所述初始化间隔期间的所述惯性传感器的位置r(t)和速度v(t)的软约束集合{C_i}，其中每个其中所述软约束表示所述初始化间隔期间所述感兴趣活动的典型位置和速度轨迹模式的近似；

限定惩罚度量m_j(r(t)，v(t))以便量化计算的所述位置r(t)和所述速度v(t)的轨迹与所述软约束{C_i}的偏差；以及

当所述初始取向和初始速度用作在整个所述初始化间隔上借助于耦合的微分方程将所述样本数据整合到取向、位置和速度中的初始条件时，计算所述初始化间隔开始处所述惯性传感器的初始取向Q₀＝Q(t₀)和初始速度v₀＝v(t₀)的估计以便最小化所述惩罚度量。

2.权利要求1的方法，进一步包括：

在所述软约束之中包括以下要求：所述初始化间隔的开始与结束之间的位置变化为零，r(t_n)＝r(t₀)；

将所述初始速度的估计设置为零，v(t₀)＝0；以及

在所述惩罚度量之中包括所述初始化间隔的结束与所述初始化间隔的开始之间的位置差异的绝对值m(r(t)，v(t)＝|r(t_n)-r(t₀)|。

3.权利要求1的方法，进一步包括：

在软约束之中包括以下要求：所述惯性传感器的位置和速度在所述初始化间隔期间是周期性的；

通过将离散傅里叶变换应用到所述初始化间隔期间速率陀螺仪数据时间序列的相应轴对速率陀螺仪的每个轴计算频域信号，Ω_x(f)＝DFT(ω_x(t))，Ω_y(f)＝DFT(ω_y(t))，Ω_z(f)＝DFT(ω_z(t))；

选择主导频率f_max作为3个轴上具有最大变换幅度的频率，f_max＝argmax(max(|Ω_x(f)|，|Ω_y(f)|，|Ω_z(f)|))，并且根据所述主导频率将主导周期T计算为T＝1/f_max；

在所述惩罚度量之中包括：速度惩罚度量，其是一个运动周期上速度变化的绝对值m_v(r(t)，v(t))＝|v(t+T)-v(t)|；以及位置惩罚度量，其是一个运动周期上位置变化的绝对值m_r(r(t)，v(t))＝|r(t+T)-r(t)|；

选择所述初始化间隔内的指定时段以用于计算所述初始速度和初始取向的估计；

通过在所述指定时段上最小化所述速度惩罚度量计算所述初始取向的估计；以及

通过使用所述初始取向的估计在所述指定时段上最小化所述位置惩罚度量计算所述初始速度的估计。