CN108627153B - 一种基于惯性传感器的刚体运动追踪系统及其工作方法 - Google Patents

Abstract

本发明公开了一种基于惯性传感器的刚体运动追踪系统及其工作方法，该系统包括可附着在刚体上的惯性传感器节点和数据采集处理终端。惯性传感器节点包含惯性传感器、计算模块和无线通信模块。数据采集处理终端包含无线通信模块和计算机系统。用户将惯性传感器节点布置在刚体上，节点实时采集传感器数据并无线传输至数据采集处理终端。当运动结束，计算机系统执行运动追踪和运动分析算法，对数据做预处理，获知惯性传感器节点的加速度，并计算其速度和运动轨迹，再基于惯性传感器节点的姿态以及刚体上其他点和传感器节点的相对位置关系对这些点的运动轨迹进行还原，最终可以呈现刚体选定点的运动轨迹并对运动过程中发生的平移和旋转进行分析。

Description

一种基于惯性传感器的刚体运动追踪系统及其工作方法

技术领域

本发明涉及惯性传感器感知识别，运动追踪以及运动分析，具体是一种基于惯性传感器的刚体运动追踪系统及其工作方法。

背景技术

运动追踪是指对目标对象的运动状态进行跟踪，主要包括测量、跟踪、记录目标对象在三维空间中的运动轨迹。当前运动追踪解决方案主要基于计算机图形学和图像处理技术，超声波、雷达、激光等定位技术以及惯性传感器。其中最主流的解决方案一般是基于计算机图形学和图像处理技术的。通过部署在三维空间中的多个摄像头，将运动物体(通常会布置标记节点)的运动状况以图像的方式记录下来，再由计算机系统对这些图像数据进行处理，最终得到运动物体在不同时刻的空间坐标(X,Y,Z)。基于计算机视觉的解决方案可以获得很高的精度，但其需要较大规模的部署，而且容易受视觉因素干扰，当运动物体受遮挡时精度会有很大偏差。

随着惯性传感器，可穿戴计算技术的发展，基于可穿戴惯性传感器以及智能终端的运动追踪技术也备受关注。借助可穿戴惯性传感器节点，用户可以实时获取惯性传感器数据，再由终端设备上的运动追踪和运动分析算法处理，可以实现对目标刚体的运动过程进行追踪和分析。目前一些基于惯性传感器的运动追踪工作主要关注于还原目标对象的运动姿态或者运动轨迹，缺少对运动过程中不同状态之间转换过程的分析，如何能够兼顾运动追踪与运动分析是一个值得探究的课题。

发明内容

为了解决上述技术问题，本发明提出一种基于惯性传感器的刚体运动追踪系统及工作方法，该系统和方法仅需要一个惯性传感器节点和数据采集处理终端，部署方便，操作简单，能够相对精准地捕捉附着惯性传感器节点的刚体的运动过程，获取各个时刻刚体的运动加速度和速度信息，还原刚体上多个选定点的运动轨迹，并为刚体运动过程建立数学模型，对运动过程中发生的平移和旋转进行可靠的分析。

为实现上述发明目的，本发明的技术方案如下：

本发明的一种基于惯性传感器的刚体运动追踪系统，该系统包括：

惯性传感器节点：内置多种惯性传感器，具备一定的计算、存储和通信能力，可以方便地附着在刚体表面。

数据采集处理终端：具备计算、存储和通信能力的计算机系统，接收从惯性传感器节点传送过来的各种传感器数据，存储并处理，最终生成刚体运动的轨迹以及刚体运动过程中发生的平移和旋转的分析结果，并向用户展示。

刚体是指在运动过程中以及受力作用后，形状和大小基本不变，且内部各点的相对位置不变或者变化程度小到可以忽略不计的物体。所述的刚体运动，是指刚体在三维空间中作旋转、平移的运动，可以理解为三维空间中一组三维坐标到另一组三维坐标的映射。此外，所述的运动追踪是指对目标对象的运动状态进行跟踪，主要包括测量、跟踪、记录目标对象在三维空间中的运动轨迹。

所述惯性传感器节点固定安装于刚体表面，其内部包括：

惯性传感器模块：包含加速度计，陀螺仪和磁力计，用于实时测量相应的惯性传感器数据。

计算模块：用于处理惯性传感器原始数据，通过内置的信号滤波算法实时获取惯性传感器节点的姿态信息，从而估计出重力加速度在惯性传感器自身参考坐标系三个轴上的分量，和原始加速度数据相减即可获取线性加速度。

无线通信模块：用于与终端设备进行信息交互，主要用于接收数据采集处理终端的指令，并周期性地向终端设备发送传感器数据。

进一步的，所述的数据采集处理终端内部主要包括：

无线通信模块：用于与惯性传感器节点进行信息交互。

计算机系统：是指具备计算、存储和通信能力的计算机系统，通过无线通信模块接收惯性传感器数据作为输入，执行运动追踪和运动分析算法，获知刚体每一时刻的加速度和速度信息，计算刚体运动的轨迹，并分析刚体在运动过程中平移和旋转的情况。

基于上述追踪系统，本发明还介绍工作方法，包括以下步骤：

1)用户将单个惯性传感器节点布置在刚体上，准备触发刚体运动。

2)用户为惯性传感器节点与数据采集处理终端建立无线连接。

3)用户在数据采集处理终端开启数据采集，惯性传感器节点开始实时采集各类传感器数据并周期性发送至数据采集处理终端，刚体运动开始。

4)刚体运动结束，用户在数据采集处理终端终止数据采集。

5)数据采集处理终端通过计算机系统执行运动追踪和运动分析算法，获知刚体每一时刻的加速度和速度信息，生成刚体运动的轨迹以及刚体运动过程中发生的平移和旋转的分析结果，并向用户展示。

进一步的，其中，所述步骤5)中的运动追踪和运动分析算法，包括以下步骤：

步骤5.1数据预处理，先对原始传感器数据进行平滑操作，然后计算每一时刻的三轴线性加速度数据和陀螺仪数据的幅值，利用阈值判断的方法确定运动起始和终止的时刻。具体来讲，运动起始时刻，两类传感器数据的幅值会分别超过一个阈值，运动终止的时刻，两类传感器数据的幅值都会回到相应阈值之下。通过大量实验数据选定合理的阈值，即可准确并抽取出运动过程对应的数据。接着对抽取出的线性加速度数据进行参考坐标系的转换，这里的参考坐标系的转换是指利用重力加速度向量

和磁力计数据向量

构建一个地理坐标系，具体来讲，首先由于

指向北方，所以水平面上指向地理东方的向量

可以由下式求解：

又由于

不严格水平，所以按如下算式得到水平面上指向地地理北方的向量

于是

这三个正交的向量便可以构成地理坐标系，这三个向量各自规范化之后可以构建方向余弦矩阵DCM，这样便可以实现从惯性传感器节点自身坐标系到地理坐标系的转换：

(X_g,Y_g,Z_g)’＝DCM×(X,Y,Z)’

其中(X_g,Y_g,Z_g)为地理坐标系下的传感器数据，(X,Y,Z)为基于惯性传感器节点自身坐标系的原始数据。

根据具体应用场景的需要，可以利用同样的方法对地理坐标系再做一次变换,得到应用场景对应的参考坐标系,最后我们就可以得到运动过程中相对于固定参考坐标系的线性加速度数据。

步骤5.2执行运动追踪，基于惯性传感器节点上一时刻的运动状态和当前时刻的惯性传感器数据更新当前时刻惯性传感器节点的运动状态,从而获取惯性传感器节点每一时刻在三维空间中的相对位置和姿态信息。

步骤5.3执行轨迹还原，根据惯性传感器节点当前时刻在空间中的相对位置以及其姿态，结合刚体上选定点和惯性传感器节点的相对位置关系，对刚体上这些选定点的位置进行估计，从而还原出刚体上多个点的运动轨迹。

步骤5.4执行运动分析，选取刚体中的某个平面的运动轨迹，对选定时间片长度的运动过程进行分析，建立数学模型，将连续的运动等效分解为平移和旋转的组合，运动的旋转情况可以利用四元数的方法求解，最终输出运动过程中发生的平移和旋转的情况，平移由三维空间的一个向量来表示，旋转可以由一个四元数(包含旋转轴和旋转角)来表示。

进一步的，所述步骤5.2中惯性传感器节点的运动状态主要包括惯性传感器节点在空间中的速度、相对位置及其姿态，具体的运动状态更新步骤包括：

步骤5.2.1相对位置信息更新：运动追踪算法以惯性传感器节点开始运动的位置作为应用场景对应的参考坐标系的原点，在时域上先对线性加速度进行一次积分，得到各个时刻的速度信息，再利用零速校正等误差修正手段，对速度进行补偿。接着对补偿后的速度在时域上继续做一次积分，求解相邻时刻发生的位移，实现在每一采样时刻对惯性传感器节点在空间中的相对位置(即三维坐标)进行更新。

步骤5.2.2姿态信息更新：运动初始阶段由磁力计和重力加速度的瞬时数据构建包含惯性传感器节点姿态信息的矩阵作为惯性传感器节点的初始姿态，并记录下初始阶段节点局部坐标系至地理坐标系的旋转矩阵，后续阶段利用陀螺仪的数据在时域上的一次积分对初始姿态进行更新，并在一些合适的时间点利用磁力计和重力加速度测得的姿态进行校准，从而在每一采样时刻对惯性传感器节点的姿态信息进行更新。

本发明提供了一种基于惯性传感器的刚体运动追踪系统及其工作方法，相对于基于计算机视觉的运动追踪系统，更加轻量级，易部署，成本低廉且不受视觉因素限制。相较于其他同类型的基于惯性传感器的运动追踪系统，本系统能够还原刚体上多个点的运动轨迹，并且给出可靠的运动分析结果。

本发明的有益效果具体表现为：

1、提出了一种新的对刚体在三维空间中运动过程进行分析的方法：在分析运动轨迹之外引入对运动过程中平移和旋转情况的分析。通过建立几何模型来将刚体在三维空间中的运动分解成平移和旋转的组合，使得运动分析的结果更为直观。

2、可以实现对刚体上多个选定点的运动轨迹进行还原：通过构建一个与应用场景对应的固定的参考坐标系，充分利用惯性传感器实时姿态信息以及刚体上选定点与惯性传感器节点的相对位置关系，实现对刚体上多个选定点的运动轨迹进行还原。

3、容易部署：不需要在目标周围提前部署大量摄像头以及线缆，只需要用户将惯性传感器节点布置于待追踪的刚体之上即可。

4、成本相对低廉：不需要购置高成本的摄像头、交换机、线缆等设备，只需一台常见的数据采集处理终端和成本相对低廉的惯性传感器节点即可实现对刚体运动过程的追踪和分析。

附图说明

图1为基于惯性传感器的刚体运动追踪系统架构图。

图2为基于惯性传感器的刚体运动追踪系统的运动追踪和运动分析算法流程图。

图3为基于惯性传感器的刚体运动追踪系统系统工作方法涉及的参考坐标系示意图。

图4为刚体上选定点与惯性传感器节点的相对位置关系图。

图5为刚体运动示意图。

图6为刚体运动过程建模分析示意图。

具体实施方式

为了便于本领域技术人员的理解，下面结合附图对本发明作进一步的说明。

图1是基于惯性传感器的刚体运动追踪系统架构图，主要由附着在刚体上的惯性传感器节点和数据采集处理终端两部分组成，如图中所示，两者主要通过蓝牙无线通信进行数据交互。

所述惯性传感器节点固定安装于刚体表面，可以选择粘贴于刚体表面，确保惯性传感器模块与刚体的相对位置关系固定。惯性传感器节点中包含惯性传感器、计算模块和通信模块。惯性传感器包含加速度计，陀螺仪和磁力计，用于实时测量相应的惯性传感器数据；计算模块是用于处理原始传感器数据的，通过内置的信号滤波算法实时获取惯性传感器节点的姿态信息，从而估计出重力加速度在惯性传感器自身参考坐标系三个轴上的分量，和原始加速度数据相减即可获取线性加速度；无线通信模块用于与终端设备进行信息交互，主要是接收数据采集处理终端的指令，并周期性地向终端设备发送传感器数据。

数据采集处理终端主要由无线通信模块和计算机系统组成。无线通信模块用于与惯性传感器节点进行信息交互；计算机系统是指具备计算、存储和通信能力的计算机系统，它通过无线通信模块接收惯性传感器数据作为输入，执行运动追踪和运动分析算法，计算刚体运动的轨迹，并分析刚体在运动过程中平移和旋转的情况。

图2是运动追踪和运动分析算法流程图,运动追踪和运动分析算法主要包括数据预处理，运动追踪，轨迹还原和运动分析。

数据预处理部分主要是按时间顺序从前往后扫描线性加速度和陀螺仪传感器数据，准确寻找出运动开始和运动结束的时刻。首先对传感器数据进行平滑，可以用简单的均值平滑滤除一些高频噪声。然后分别计算三轴线性加速度和陀螺仪数据的幅值AccM和GyroM，即分别求解两个三维向量的2-范数。这里，通过前期大量实验数据的分析，我们分别对两种三轴传感器数据的幅值有一个阈值thresA和thresG，我们通过设置一个定长的滑动窗口，统计窗口内传感器数据大于和小于对应阈值的比率。当两种传感器数据的幅值小于设定阈值的比率均大于ρ(本系统设置为80％)时，则说明当前窗口运动状态不活跃，而当有一个传感器数据的幅值大于设定阈值的比率大于ρ时，则说明当前窗口运动状态活跃。我们找出第一个不活跃状态迁移到活跃状态的窗口，然后设定运动起始时间t_s，再往后搜索由活跃状态向不活跃状态迁移的窗口，设定运动终止时间t_e,至此，我们只需截取t_s和t_e之间的传感器数据，再对截取后的线性加速度数据进行参考坐标系的转换，作为算法往后部分的输入。这里的参考坐标系的转换是指利用重力加速度向量

和磁力计数据向量

构建一个地理坐标系，具体来讲，首先由于

指向北方，所以水平面上指向地理东方的向量

可以由下式求解：

又由于

不严格水平，所以按如下算式得到水平面上指向地地理北方的向量

于是

这三个正交的向量便可以构成地理坐标系，这三个向量各自规范化之后可以构建方向余弦矩阵DCM，这样便可以实现从惯性传感器节点自身坐标系到地理坐标系的转换：

(X_g,Y_g,Z_g)’＝DCM×(X,Y,Z)’

其中(X_g,Y_g,Z_g)为地理坐标系下的传感器数据，(X,Y,Z)为基于惯性传感器节点自身坐标系的原始数据。

根据具体应用场景的需要，可以利用同样的方法对地理坐标系再做一次变换,得到应用场景对应的参考坐标系,最后我们就可以得到运动过程中相对于固定参考坐标系的线性加速度数据。

运动追踪部分主要是利用预处理阶段生成的基于应用场景对应的参考坐标系下的线性加速度数据和陀螺仪数据，对惯性传感器节点的运动轨迹进行更新，同时生成每一采样时刻的惯性传感器节点姿态信息。具体来说，轨迹更新部分，算法以惯性传感器节点开始运动的位置作为应用场景对应的参考坐标系的原点，在时域上先对线性加速度进行一次积分，得到各个时刻的速度信息，由于线性加速度数据存在噪声，积分会导致误差累积，我们利用零速校正等误差修正手段，对速度进行修正。接着对补偿后的速度在时域上继续做一次积分，求解相邻时刻发生的位移，实现对惯性传感器节点在空间中的相对位置(即三维坐标)进行更新。所述的零速校正主要是基于运动终止时刻，惯性传感器节点的速度为零的这样一个假设，这样在对线性加速度做第一次积分得到各时刻的速度的时候可以对速度做一个补偿，从而可以减少累积误差。姿态更新部分，运动初始阶段由磁力计和重力加速度的瞬时数据构建包含惯性传感器节点姿态信息的矩阵作为惯性传感器节点的初始姿态，并记录下初始阶段节点局部坐标系至地理坐标系的旋转矩阵，后续阶段利用陀螺仪的数据在时域上的一次积分对初始姿态进行更新，并在一些合适的时间点利用磁力计和重力加速度测得的姿态进行校准，以消除陀螺仪带来的累积误差，从而对惯性传感器节点的姿态信息进行更新。这里所说的合适的时间点主要由这些因素判定：

1)实时的角速度信息，当角速度的值在一定阈值以下，我们认为此刻由磁力计和重力加速度测得的姿态信息是比较可靠的，可以用于校准。

2)陀螺仪计算得到的姿态变化与利用磁力计和重力加速度得到的姿态变化的相似性，当由两种手段测得的姿态变化比较相似时，我们认为此刻由磁力计和重力加速度测得的姿态信息也是比较可靠的，可以用于校准。

轨迹还原部分利用运动追踪部分生生成的惯性传感器节点轨迹信息和姿态信息，结合预先获知的节点自身参考坐标系下刚体上选定点与惯性传感器节点的相对位置关系，得到应用场景对应的固定参考坐标系下刚体上选定点与惯性传感器节点的相对位置关系，然后对刚体上选定点的运动轨迹进行估算还原。所述刚体上选定点与惯性传感器节点的相对位置关系具体可以参见下文对图4的说明。

运动分析部分以轨迹还原部分输出的刚体上选定点的轨迹作为输入，利用我们建立的刚体运动过程的数学模型进行分析。具体来讲，我们选定刚体上的一个平面上不在同一直线上的三个点，他们的运动轨迹是已知的，相当于这个平面的运动过程是已知的，这样利用我们建立的刚体运动过程数学模型可以将刚体在一个时刻至另一时刻的运动过程等效简化为一次平移和一次旋转的组合。最后我们给出运动的分析结果便是运动过程中所发生平移的向量

旋转过程的旋转轴

以及绕旋转轴逆时针转过的角度θ_equal。关于刚体运动过程的数学模型可以参见下文对图5,图6的说明。

图3是基于惯性传感器的刚体运动追踪系统工作方法中涉及的几种参考坐标系的示意图。图中左半部分以点N为坐标系原点、X_NY_NZ_N为坐标轴的坐标系是惯性传感器节点自身的坐标系，惯性传感器节点产生的原始传感器数据都是基于这个坐标系的，而以点O为坐标系原点，XYZ为坐标轴的坐标系则是前面所述的应用场景对应的参考坐标系，这里只是以其中一种可能的情况作为示例，具体应用场景对应的参考坐标系可以有不同的定义。图中右半部所示的是地理坐标系，我们可以看到X轴指向地理东方，Y轴指向地理北方，Z轴指向重力的反方向。在实际的算法流程中，参考坐标系转换的过程便是将线性加速度数据由惯性传感器节点自身的坐标系转换至地理坐标系，再由地理坐标系转换至应用场景对应的参考坐标系。

图4是刚体上一些选定点与惯性传感器节点的相对位置关系的示意。图中点N代表惯性传感器节点，在刚体上选取不在同一平面的O、A、B、C四个点作为示例，图中给出的坐标系是惯性传感器节点自身的坐标系。简单起见，这里将点N作为坐标系原点。在这个坐标系下，由于刚体本身的形状大小以及惯性传感器的部署位置是已知的，所以点O、A、B、C、 N的坐标都是可以测算得到的，亦即图中标示出的四个向量

都是可以测算得到的。由此我们便可以通过惯性传感器节点N在三维空间中的相对位置推算刚体上选定点 O、A、B、C在三维空间中的相对位置。具体来说，就是每一时刻都将选定点与惯性传感器节点的相对位置关系(即四个向量

)由节点自身坐标系转换到地理坐标系，再转换至图3所示的应用场景对应的参考坐标系。而我们在运动追踪部分求解的惯性传感器节点 N的运动轨迹(即每一时刻惯性传感器节点在三维空间中的相对位置)是基于应用场景对应的参考坐标系的，这样就可以利用应用场景对应的参考坐标系下的四个向量

求解点O、A、B、C在应用场景对应的参考坐标系下的坐标了，最终可以实现对刚体上选定点的轨迹还原。

图5是刚体运动过程的一个示例，不失一般性，这里用刚体上不在同一平面的四个点来代表一个刚体。我们可以看到图中刚体四个顶点的起始位置为O、A、B、C，经过运动之后，对应顶点的位置变换到了O′、A′、B′、C′，这就是刚体运动前后状态的一般化表示，如前文所述，相当于发生了一组三维坐标(O、A、B、C)到另一组三维坐标(O′、A′、B′、C′)的映射。

图6是对刚体运动过程分析建模的一个示意图，刚体的运动过程是比较复杂的，但我们在分析过程中可以在保证运动前后状态不变的情况下对运动过程进行等效的变换以达到简化运动过程的目的。我们将相邻时间片之间刚体的运动过程分解为平移和旋转，这需要建立几何模型来描述。图6(a)是对运动过程进行分解的第一步，我们选取刚体上的一个点(O)，其指向运动之后对应点(O′)的向量

即是刚体作平移运动的向量。如图6(b)，平移过后，刚体位置变换为O₁、A₁、B₁、C₁，其中O₁与O′重合，在第二步中，我们需要将刚体的边O₁A₁绕过点O′的轴旋转至与O′A′重合，旋转轴

的计算如下所示：

而旋转角度θ₀则可以由向量

与向量

点乘获取。如图6(c)，第一次旋转过后，刚体位置变换为O₂、A₂、B₂、C₂，边O₂A₂与边O′A′重合。此时，我们选择与O₂、A₂不在同一直线上的第三个点B₂，在这一步中，刚体需要绕轴

(转换为单位向量

)旋转至平面O₂A₂B₂与平面O′A′B′共面，旋转角θ₁可以直接计算两个平面的夹角获取。具体地，这里是通过先求解图6(c) 中所示的两个平面的法向量

和

再求解两个法向量之间的夹角θ来获取两个平面之间的夹角θ₁。最后，图(d)展示了经过第二次旋转之后的刚体位置O₃、A₃、B₃、C₃，亦即位置O′、A′、 B′、C′。图4展示的一次平移两次旋转的刚体运动分解过程是比较直观的，实际计算过程中，平移的变换过程利用平移过程向量的加减便能实现，旋转的变换过程则可以利用四元数的方法来求解。另外，我们可以利用四元数的方法将两次旋转进行复合，这样，刚体的运动过程就简化为一次平移和一次绕固定轴的旋转。我们以四元数

和

代表上述过程中的两次旋转，

为旋转前点坐标的四元数表示，

为第一次旋转后点坐标的四元数表示，

第二次旋转后点坐标的四元数表示，具体的旋转复合过程如下所示：

就是复合之后的旋转四元数，由它可以获取旋转轴

和旋转角θ_equal。最终的运动分析便是对选定的任意两个时刻之间的运动过程进行分析，给出简化运动过程中发生的平移的向量

旋转的旋转轴

以及逆时针旋转过的角度θ_equal。

本发明有诸多应用途径，上述实施例仅是本发明的一种优选实施方式，因此本发明并不局限于上述实施方式。在不脱离本发明原理的情况下，本技术领域的技术人员可能在本发明的启示下设计出其他实施方式，这些也应视为本发明的保护范围。

Claims (4)

1.一种基于惯性传感器的刚体运动追踪系统，其特征在于该系统包括：

惯性传感器节点：内置多种惯性传感器，所述传感器具备计算、存储和通信能力，周期性地发送传感器数据至数据采集处理终端，所述惯性传感器节点附着在刚体表面；

数据采集处理终端：具备计算、存储和通信能力的计算机系统，接收从所述惯性传感器节点传送过来的各种传感器数据，存储并处理，最终生成刚体运动的轨迹以及刚体运动过程中发生的平移和旋转的分析结果，并向用户展示；

所述的刚体运动追踪是指对目标刚体的运动状态进行跟踪，包括测量、跟踪、记录目标刚体在三维空间中的运动轨迹；

所述惯性传感器节点固定安装于刚体表面，其内部包括：

惯性传感器模块：所述惯性传感器模块可以选择粘贴于刚体表面，确保惯性传感器模块与刚体的相对位置关系固定，中包括三轴加速度计、陀螺仪和磁力计，用于实时测量相应的惯性传感器数据；

计算模块：用于处理惯性传感器原始数据，通过内置的信号滤波算法实时获取惯性传感器节点的姿态信息，从而估计出重力加速度在惯性传感器自身参考坐标系三个轴上的分量，和原始加速度数据相减即可获取线性加速度；

所述计算模块对抽取出的线性加速度数据进行参考坐标系的转换，参考坐标系的转换是指利用重力加速度向量

和磁力计数据向量

构建一个地理坐标系；

首先由于

指向北方，所以水平面上指向地理东方的向量

可以由下式求解：

按如下算式得到水平面上指向地地理北方的向量

于是

这三个正交的向量便可以构成地理坐标系，这三个向量各自规范化之后构建方向余弦矩阵DCM，实现从惯性传感器节点自身坐标系到地理坐标系的转换：

(X_g,Y_g,Z_g)’＝DCM×(X,Y,Z)’

其中(X_g,Y_g,Z_g)为地理坐标系下的传感器数据，(X,Y,Z)为基于惯性传感器节点自身坐标系的原始数据；

传感器无线通信模块：用于与终端设备进行信息交互。

2.根据权利要求1所述的基于惯性传感器的刚体运动追踪系统，其特征在于，所述数据采集处理终端包括：

处理终端无线通信模块：用于与惯性传感器节点进行信息交互；

计算机系统：指具备计算、存储和通信能力的计算机，通过处理终端无线通信模块接收惯性传感器数据作为输入，执行运动追踪和运动分析算法，获知刚体每一时刻的加速度和速度信息，计算刚体运动的轨迹，并分析刚体在运动过程中平移和旋转的情况。

3.一种基于惯性传感器的刚体运动追踪系统工作方法，其特征在于，包括以下步骤：

1)用户将单个惯性传感器节点布置在刚体上，准备触发刚体运动；

2)用户为惯性传感器节点与数据采集处理终端建立无线连接；

3)用户在数据采集处理终端开启数据采集，惯性传感器节点开始实时采集各类传感器数据并周期性发送至数据采集处理终端，刚体运动开始；

4)刚体运动结束，用户在数据采集处理终端终止数据采集；

5)数据采集处理终端通过计算机系统执行运动追踪和运动分析算法，获知刚体每一时刻的加速度和速度信息，生成刚体运动的轨迹，并建模分析刚体运动过程中发生的平移和旋转的情况，最后向用户展示；

所述步骤5)中的运动追踪和运动分析算法，包括以下步骤：

步骤5.1数据预处理，先对原始传感器数据进行平滑操作，然后计算每一时刻的三轴线性加速度数据和陀螺仪数据的幅值，利用阈值判断的方法确定运动起始和终止的时刻，并抽取出运动起始和终止时刻之间的数据，再对这部分数据进行参考坐标系的转换，从而得到运动过程中相对于一个固定参考坐标系下的线性加速度数据；

对抽取出的线性加速度数据进行参考坐标系的转换，参考坐标系的转换是指利用重力加速度向量

和磁力计数据向量

构建一个地理坐标系；

首先由于

指向北方，所以水平面上指向地理东方的向量

可以由下式求解：

按如下算式得到水平面上指向地地理北方的向量

于是

这三个正交的向量便可以构成地理坐标系，这三个向量各自规范化之后构建方向余弦矩阵DCM，实现从惯性传感器节点自身坐标系到地理坐标系的转换：

(X_g,Y_g,Z_g)’＝DCM×(X,Y,Z)’

其中(X_g,Y_g,Z_g)为地理坐标系下的传感器数据，(X,Y,Z)为基于惯性传感器节点自身坐标系的原始数据；

步骤5.2执行运动追踪，基于惯性传感器节点上一时刻的运动状态和当前时刻的惯性传感器数据更新当前时刻惯性传感器节点的运动状态,从而获取惯性传感器节点每一时刻在三维空间中的相对位置和姿态信息；

步骤5.3执行轨迹还原，根据惯性传感器节点当前时刻在空间中的相对位置以及其姿态，结合刚体上的选定点和惯性传感器节点的相对位置关系，对刚体上这些选定点的位置进行估计，从而还原出刚体上多个点的运动轨迹；

步骤5.4执行运动分析，选取刚体中的某个平面的运动轨迹，对选定时间片长度的运动过程进行分析，建立数学模型，将连续的运动等效分解为平移和旋转的组合，最终输出运动过程中发生的平移和旋转的情况；

所述步骤5.1中参考坐标系的转换是指基于加速度计和磁力计测量的传感器数据，构建地理参考坐标系，再根据实际应用场景对地理坐标系进行一次转换，得到应用场景对应的参考坐标系，最终将线性加速度数据转换至应用场景对应的参考坐标系下。

4.根据权利要求3所述的基于惯性传感器的刚体运动追踪系统工作方法，其特征在于，

所述步骤5.2中惯性传感器节点的运动状态主要包括惯性传感器节点在空间中的速度、相对位置及其姿态，运动状态更新步骤包括：

步骤5.2.1相对位置信息更新：运动追踪算法以惯性传感器节点开始运动的位置作为应用场景对应的参考坐标系的原点，在时域上先对线性加速度进行一次积分，得到各个时刻的速度信息，再利用零速校正误差修正手段，对速度进行补偿；接着对补偿后的速度在时域上继续做一次积分，求解相邻时刻发生的位移，实现对惯性传感器节点在空间中的相对位置进行更新；

步骤5.2.2姿态信息更新：运动初始阶段由磁力计和重力加速度的瞬时数据构建包含惯性传感器节点姿态信息的矩阵作为惯性传感器节点的初始姿态，并记录下初始阶段节点局部坐标系至地理坐标系的旋转矩阵，后续阶段利用陀螺仪的数据在时域上的一次积分对初始姿态进行更新，并利用磁力计和重力加速度测得的姿态进行校准，尽可能减少陀螺仪累积误差的影响，从而对惯性传感器节点的姿态信息进行更新。

Images