一种运动物体的多维数据测量装置及方法与流程

本发明属于运动数据测量技术领域，更具体地，涉及一种运动物体的多维数据测量装置及方法。

背景技术：

制约当今生物动力学及运动数据分析研究的最大瓶颈便是数据采集技术。因此，高精度电子测量装置的研制具有重要意义。鉴于传统的GPS定位装置具有体积大、精度低、成本高、普适性差的缺点，在涉及动力学研究的各学科领域，可控的高精度多维运动数据测量与采集装置亟待研发。

高精度数据采集装置采用惯性导航系统，它是随着惯性传感器的发展而发展起来的一门导航技术，具有完全自主、不受干扰、输出信息量大、输出信息实时性强等优点，在军用航行载体和民用相关领域获得了广泛应用。惯导系统的精度、成本主要取决于陀螺仪和加速度传感器的精度和成本，尤其是陀螺仪，其漂移对惯导系统位置误差增长的影响巨大，而高精度的陀螺仪制造困难，成本很高，因此亟待寻求提高陀螺仪的精度，同时降低系统成本的有效方法。

技术实现要素：

针对现有技术的缺陷或迫切需求，本发明提供了一种运动物体的多维数据测量装置及方法，其目的在于，通过卡尔曼滤波融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据精度。

一种运动物体的多维数据测量装置，包括数据采集模块、数据处理模块和数据通信模块；

所述数据采集模块包括三轴陀螺仪、三轴加速度计、三轴磁力计；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向；

所述数据处理模块用于对被测物体的加速度和角速度进行卡尔曼融合滤波以消除误差，结合磁力计采集的磁场强度和方向进行角速度补偿，然后进行姿态解算得到角度；

所述数据通信模块用于将消除误差后的加速度、角速度和角度传送给上位机。

进一步地，所述数据处理模块包括卡尔曼滤波子模块、角速度校正子模块和姿态解算子模块；

所述卡尔曼滤波子模块用于融合角速度和加速度进行卡尔曼滤波以消除误差；

所述角速度校正子模块用于将采集的磁场记为若使X轴对准北方，那么b_y＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为b_x，在参考坐标系XOY平面中的投影为则b_z＝h_z；将转换到自身坐标系中得到 然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝K_Pe+K_I∫e,K_p为比例系数，K_I为积分系数；修正角速度w＝w_a+δ；

姿态解算子模块，用于将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。

进一步地，所述卡尔曼滤波子模块采用角速度积分得到的角度和陀螺仪常值偏差E作为状态向量。

进一步地，修正后的角速度以四元数形式输出：

[q₀,q₁,q₂,q₃]^T为四元数，t为采样周期，Δt为采样间隔，w_x,w_y,w_z分别是修正后的角速度w在x、y、z轴的分量。

进一步地，所述姿态解算子模块用于将修正后的角速度以四元数形式输出[x,y,z,w]^T＝[q₀,q₁,q₂,q₃]^T，依据四元数形式的角速度计算角度： α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。

进一步地，所述数据通信模块采用蓝牙无线传输方式，蓝牙传输采用板载陶瓷天线。

进一步地，还包括连接数据处理模块的存储模块。

一种运动物体的多维数据测量方法，包括以下步骤；

利用三轴陀螺仪采集被测物体的三轴角速度，利用三轴加速度计采集被测物体的三轴加速度，利用三轴磁力计采集磁场的强度和方向；

对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度；

将消除误差后的加速度、角速度和角度传送给上位机。

进一步地，所述对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度的具体实现方式为；

融合角速度和加速度进行卡尔曼滤波以消除误差；

将采集的磁场记为若使X轴对准北方，那么b_y＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为b_x，在参考坐标系XOY平面中的投影为则 b_z＝h_z；将转换到自身坐标系中得到然后再将 与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝K_Pe+K_I∫e,K_p为比例系数，K_I为积分系数；修正角速度w＝w_a+δ；

将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。

本发明的有益技术效果体现在：

陀螺仪用于测量设备自身的旋转运动，但不能确定设备的方位。加速计测量设备的受力情况但用来测量设备相对于地面的摆放姿势，则精确度不高。磁力计可定位设备的方位，测量出当前设备与东南西北四个方向上 的夹角。本发明通过上述传感器，测量出运动个体的加速度、角速度、以及所处位置的磁场强度和方向，通过卡尔曼滤波，融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据准确性。

作为优化，该设备采用蓝牙无线传输模式，减少设备之间的连线，通过将设备附着与运动个体表面，随之运动，采用蓝牙低功耗模式，耗电量少，在同规格独立电源供电情况下，续航时间久。同时，若超出范围或通讯收到影响时，可采用离线模式，将数据存储于内部SRAM，记录、传输方式可靠。

附图说明

图1为本发明运动物体的多维数据测量装置实例结构图；

图2为本发明测量流程图；

图3为本发明上位机接收数据显示的效果图；

具体实施方式

为了使本发明的目的，技术方案及优点更加清楚明白，以下结合附图对本发明进行进一步详细说明。

图1给出本发明运动物体的多维数据测量装置结构图，包括：包括数据采集模块、数据处理模块和数据通信模块。所述数据采集模块包括三轴陀螺仪、三轴加速度计、三轴磁力计和角度解算模块；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向。数据处理模块对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，并结合磁力计采集的磁场强度和方向对角速度进行补偿，通过补偿后的角速度更新四元数，然后进行姿态解算得到角度。数据通信模块用于将消除误差后的加速度、角速度和角度传送给上位机。

数据处理模块包括卡尔曼滤波子模块、角速度矫正子模块和角度解算 模块。下面对各子模块详细说明。

(1)卡尔曼滤波子模块

加速度计的输出实际包括运动方向的加速度和重力加速度分量的叠加。这些干扰信号叠加在测量信号中会使输出信号无法得出准确的角度，在测量时，输出的结果在实际倾角附近波动。同时加速度计输出和陀螺仪输出是互相弥补的关系：加速度计可以克服陀螺仪输出零点受温度影响的缺陷，使积分输出信号的误差大大减少，而陀螺仪则可以减少加速度计输出噪声，大大的低了静态输出噪声。为了得到所需要的准确角度数据需要融合MPU6050的陀螺仪数据，利用陀螺仪可以测得运动个体的角速度，进而对角速度积分可以获得角度，但是这里如果只通过积分运算获得角度数据，由于角速度信号可能存在的微小偏差和漂移经过积分运算之后逐渐累积，随着时间的推移，测得的数据误差将越来越大，最后无法得到可用的数据。为了消除这个误差，在此采用卡尔曼滤波方法。

卡尔曼滤波有两类方程：预测方程和更新方程。预测方程根据之前的状态和控制量预测当前状态；更新方程表示相信传感器数据多些还是相信总体估计值多些(由卡尔曼增益Kg决定)。滤波器的工作原理为：根据预测方程预测当前状态并用更新方程检测预测结果。

预测方程：

X(k|k-1)＝AX(k-1|k-1)+BU(k)......(1)

P(k|k-1)＝AP(k-1|k-1)A^T+Q......(2)

更新方程：

K_g(k)＝P(k|k-1)H^T/(HP(k|k-1)H^T+R)......(3)

X(k|k)＝X(k|k-1)+K_g(k)(Z(k)-HX(k|k-1))......(4)

P(k|k)＝(I-K_g(k)H)P(k|k-1)......(5)

X为状态向量。U为驱动输入向量，比如通过受力驱动而产生加速度。A是状态转移矩阵，隐含了(k-1)时刻的状态会对(k)时刻的状态产生影 响。B是控制输入矩阵，指示了(k)时刻给的驱动量将如何影响(k)时刻的状态。

●X(k)：n*1矩阵，表示k时刻的状态向量；

●Z(k)：n*1矩阵，表示Kalman滤波器k+1时刻的预测观测向量输入；

●X(k|k-1)：用观测向量X在k时刻以前的数据进行对k时刻的估计结果；●X(k|k)：用观测向量X在k时刻及k时刻以前的数据对k时刻的估计结果，这也是Kalman滤波器的输出；

●P(k|k-1)：n*n矩阵，表示观测向量X的最小预测均方误差矩阵；

●P(k|k)：n*n矩阵，表示观测向量X的修正后最小均方误差矩阵；

●Kg(k)：n*m矩阵，表示误差增益因子；

●H：m*n观测模型矩阵；即把真实状态空间映射到观测空间；

●Q：n*n过程噪声协方差矩阵；

●R：m*m过程噪声协方差矩阵；

●I：单位矩阵；

在本发明中，上述公式中的状态向量X将会融合加速度和角速度。因为角度和角速度存在导数关系，则这里利用采集的角速度计算得到角度，采用计算的角度作为一个状态向量，然后用加速度计估计出陀螺仪常值偏差E，并以此偏差作为另一个状态向量，得到相应的状态方程和观测方程。

ω_ge:带有偏差的角速度；

由加速度数据处理得到的角度

ω_g：陀螺仪测量噪声；

ω_a：加速度计噪声；

E：陀螺仪漂移误差；

根据K-1时刻的角度值，由预测得到K时刻的角度值，进而算出K时刻的噪声的方程，通过卡尔曼滤波器融合了加速度计、陀螺仪数据，进行 迭代运算直至估计出最可靠的角度值，再对估计出的最可靠角度值求导得到最可靠的角速度。

(2)角速度补偿子模块

若只通过加速度计、陀螺仪融合数据，会存在偏航角的误差，在这里将引入磁力计的误差补偿，磁力计用于测量地球磁场的大小和方向，在这里这个方向不再与重力加速度方向一样竖直向下，与X轴、Y轴呈一夹角，与Z轴存在夹角。磁力计在自身坐标系的输出为若使X轴对准北方，那么b_y＝0，即通过转换矩阵将磁力计在自身坐标系的输出转换为在参考坐标系中的输出为所述自身坐标系到参考坐标系的转换矩阵为：

在参考坐标系XOY平面中的投影为b_x，在参考坐标系XOY平面中的投影为则b_z＝h_z；将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积 求误差计算修正参数δ＝K_Pe+K_I∫e,K_p为比例系数，K_I为积分系数；修正角速度w＝w_a+δ。

接下来利用修正后的角速度采用一阶龙格库塔法更新四元数。

这里假设有下列微分方程：

其解为

X[t+t]＝X[t]+t*f[X[t],w[t]]

t为采样周期，套用至四元数，

K_p为比例系数，K_I为积分系数，t为采样周期，Δt为采样间隔，w_x,w_y,w_z分别是修正后的角速度w在x、y、z轴的分量。

通过卡尔曼滤波，融合加速度计和陀螺仪数据，借助磁力计数据补偿可以有效抑制因积分和漂移产生的误差带来的影响，提高数据准确性。

(3)姿态解算子模块

为了实现姿态解算，这里利用三轴陀螺仪和三轴加速度计、三轴磁力计的采集数据，采用四元数的方法来进行姿态解算。

所述四元数解算的具体实现过程为：

地球的坐标系又称为地理坐标系，是固定不变的。正北，正东，正向上构成了这个坐标系的X、Y、Z轴，用坐标系R表示。在这里，量装置使用的是自身坐标系r，通过四元数就可以描述R与r坐标系之间的关系，进而可以描述物体运动情况：

这里w为一实数，为坐标向量，对应前文的[q₀,q₁,q₂,q₃]^T。下面对四元数单位化：

假设有一任意旋转轴的向量与旋转角度θ，它们满足如下关系：

x＝s*x_a

y＝s*y_a

z＝s*y_a

w＝cos(θ/2)

s＝sin(θ/2)

通过上述公式，多个旋转的组合可以通过四元数的乘法来表示。

获得采集的四元数数据，通过下述公式可以解算得角度信息即x，y，z轴的倾角。

α＝arctan2(2wz+2xy,1-2y²-2z²)(1)

θ＝arcsin(2wy-2zx)(2)

α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。

数据通信模块采用蓝牙无线传输方式，按照一种较佳的实现方式，其结构为：蓝牙传输模块采用板载陶瓷天线，有效缩小天线尺寸，并能达到隐藏天线目的。由于陶瓷本身介电常数较pcb电路板高，所以使用陶瓷天线能有效缩小天线尺寸，在介电损耗方面，陶瓷介质也比pcb电路板的介电损失小，所以非常适合低耗电率的蓝牙模块中使用。无线传输模块采用的蓝牙4.1，蓝牙4.1是蓝牙技术联盟于2013年底推出的新的规范，其最重要的特性是支持省电，支持多设备连接。在这里蓝牙模块采用从设备模式，在此模式下完全符合BLE 4.1协议同时兼容4.0协议。

本发明装置除了有实时数据传输模式，还有离线模式。设备在离线模式下，利用独立电源供电，然后将所采集的数据存储到存储模块，此时蓝牙模块处于休眠模式，采集数据一段时间之后，上位机通过USB获取存储的数据。

为了适用于于水下测量，还进行防水和防撞设计。设备采用硅胶封装，硅酸凝胶是一种高活性吸附材料，除强碱、氢氟酸外不与任何物质发生反应，不溶于水和任何溶剂，无毒无味，化学性质稳定，具有其他材料不具备的性质：吸附性能高、热稳定性好、化学性质稳定、有较高的机械强度。采用硅胶封装，可以做成所期望的形状，不需要再通过厂家进行开模生产外壳，降低成本。而且硅胶不溶于水，不易于其他物质发生反应，可以适应水下环境需求。

实施例：

数据采集模块采用MPU6050芯片2实现，MPU6050芯片自带的三轴陀螺仪和三轴加速度计以及通过I2C扩展的三轴磁力计1，经由I2C总线将传感器测量的数据传输给数据处理模块。数据处理模块采用STM32芯片3。数据通信模块采用蓝牙无线传输模块传输至上位机4。存储模块采用16GB的SRAM。

这里采用PC端作为上位机，借助开发的上位机软件以及蓝牙适配器检测到处于从设备的测量装置，通过UUID扫描周围的从设备，选择连接的从设备，测量装置在从设备模式下包含一个串口收发的Service，分别代表读和写，通过操作这两个通道可以进行数据传输。当PC和STM32通过蓝牙建立通信连接之后，STM32会一直向PC发送数据，接着PC端接收到数据之后，如图3所示将数据进行实时可视化显示，在上位机中，可以同时显示XYZ三轴的角速度、加速度、磁力计数据，并可以保存所选取时间段的数据。通过配置蓝牙连接部分，具有可视化和记录功能模块。

应用实例：请参见图2，数据测量与采集过程如下：

第一步：装置中MPU6050自带的3轴陀螺仪和3轴加速度计以及通过I2C扩展的磁力计，从加速度计、磁力计、陀螺仪相关寄存器获取的数值，将数据直接传输给STM32。

第二步：STM32在接收到I2C总线传输来的数据之后，通过卡尔曼滤波，融合加速度计和陀螺仪数据，降低数据误差、抑制陀螺仪零漂。接着利用磁力计数据对融合后的角速度进行补偿，更新四元数。进而进行姿态解算。

第三步：STM32对数据进行解算之后，采用蓝牙通信方式将数据传输给PC端。

除了实时传输数据，该设备还具有SRAM(Static Random Access Memory)，通过电源独立供电，可用于离线数据存储。

最后，加上电源模块，整套装置尺寸为35*16*4mm，重量为4g(不带独立电源)，采用硅胶密封。具有体积小，重量轻，防水防摔等特性。

试验结果表明：旋转角度在所有轴量程都为360°，分辨率达到0.01°，精度为0.5°(静态)，动态为2°。对于原始的加速度计、陀螺仪、磁力计数据精度分别达到±2g(16bit)、±500dps(16bit)、±4gauss(16bit)。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。