CN106813677B - 一种可多台并行工作的多维运动学数据测量装置 - Google Patents

Abstract

本发明提供一种可多台并行工作的多维运动学数据测量装置，属于运动数据采集技术领域。装置包括多个数据采集单元、路由器和上位机，数据采集单元、路由器和上位机构成一个无线局域网；所述数据处理模块用于将角度信息、磁场强度和方向、加速度、角速度通过数据通信模块传送给上位机。本发明多个数据采集单元并行采集工作，实时传送给上位机显示，相比传统的单线采集，效率倍增；通过卡尔曼滤波，融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据准确性。

Description

一种可多台并行工作的多维运动学数据测量装置

技术领域

本发明属于电子测量装置制造领域，更具体地，涉及一种可多台并行工作的多维运动学数据测量装置。

背景技术

从人群到飞鸟、游鱼、昆虫、细菌，自然界中广泛存在着丰富的群体运动现象，相互联系而不断运动的个体所组成的系统涌现出丰富多彩而高度协调的群体运动行为。自然界群集动力学行为所展现出的惊人的魅力，为工业、社会群体的认识和优化提供了丰富的思想源泉。从应用角度出发，作为当今物理科学、自动化科学、系统科学、计算机科学、等交叉领域的热点研究方向，深入理解自然界群集构型演化机理或将推动群体机器人协同技术的深刻变革，对于无人系统控制、区块链技术应用、人体群体行为调控疏导等领域提供十分可观的应用价值。近年来，广泛的研究关注于大规模个体组成的群体产生的群集运动行为，研究重点在于理解群集运动的产生机理和内部潜在机制，而研究的基础在于，需要大量准确的实验数据，因此建立完善的实验装置、数据采集与处理系统对于群集运动的分析有着举足轻重的作用。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种可多台并行工作的多维运动学数据测量装置，其目的在于，通过多个数据采集单元并行采集工作，实时传送给上位机显示，相比传统的单线采集，效率倍增。

一种可多台并行工作的多维运动学数据测量装置，包括多个数据采集单元、路由器和上位机，数据采集单元、路由器和上位机构成一个无线局域网；多个数据采集单元用于并行地采集群集运动目标的运动数据，路由器用于将采集的运动数据转发给上位机；上位机用于显示运动数据；

所述数据采集单元包括三轴陀螺仪、三轴加速度计、三轴磁力计；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向；所述数据处理模块用于将角度信息、磁场强度和方向、加速度、角速度通过数据通信模块传送给上位机。

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

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

所述角速度校正子模块用于将采集的磁场记为若使X轴对准北方，那么b_y＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为b_x，在参考坐标系XOY平面中的投影为则将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝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轴的分量。

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

进一步地，所述数据通信模块采用WI-FI传输方式。

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

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益技术效果：

(1)本发明多个数据采集单元与上位机、路由器构成无线局域网，多个数据采集单元并行采集工作，实时传送给上位机显示，相比传统的单线采集，效率倍增。

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

(3)这里优选采用的无线传输方式为WI-FI，WI-FI是一种允许电子设备连接到一个无线局域网(WLAN)的技术，通常使用2.4G UHF或5G SHF ISM射频频段。连接到无线局域网可以有密码保护，也可是开放的，无线局域网允许在WLAN范围内的传感器设备进行连接，并可以在网络中进行数据的交换。通过无线传感器网络感知、采集和处理网络覆盖区域中被感知对象(水生运动个体)的运动信息，特别适用于获取水下生物运动信息。

(4)优选采用WI-FI无线传输模式，减少设备之间的连线，通过将设备附着与运动个体表面，随之运动，采用WI-FI无线传输方式，利于多台设备数据同时传输以及多台设备组网。同时，若超出范围或通讯收到影响时，可采用离线模式，将数据存储于内部SRAM，记录、传输方式可靠。

附图说明

图1是本发明可多台并行工作的多维运动学数据测量装置的一个实施例结构图；

图2是本发明数据采集单元的一个实施例结构图；

图3是本发明测量流程图；

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

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是本发明可多台并行工作的多维运动学数据测量装置的一个实施例结构图。装置包括电源1、多个数据采集单元2、路由器3和上位机4。电源1为多个数据采集单元2供电，多个数据采集单元2并行地采集群集运动目标的运动数据，路由器3将采集的运动数据转发给上位机。

数据采集单元包括采集模块、数据处理模块和数据通信模块。所述数据采集模块包括三轴陀螺仪、三轴加速度计、三轴磁力计；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向。所述数据处理模块用于将角度信息、磁场强度和方向、加速度、角速度通过数据通信模块传送给上位机。

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

(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平面中的投影为则将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝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轴滚转角。

数据通信模块采用WI-FI无线传输方式，按照一种较佳的实现方式，其结构为：WI-FI传输模块采用板载PCB天线，有效缩小天线尺寸，在无遮挡的情况下覆盖距离达到400m，在较小尺寸封装中集成了Tensilica L106超低功耗32位微型MCU，可配合STM32使用。。

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

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

实例：

图2是本发明数据采集单元的一个实施例结构图。数据采集模块采用MPU6050芯片22实现，MPU6050芯片自带的三轴陀螺仪和三轴加速度计以及通过I2C扩展的三轴磁力计21，经由I2C总线将传感器测量的数据以四元数的形式传输给数据处理模块。数据处理模块采用STM32芯片23。采用WI-FI无线传输模块传输至上位机4。存储模块采用16GB的SRAM。

如图2所示，这里采用PC端作为上位机，借助开发的上位机软件以及WI-FI模块检测到处于从设备的测量装置，通过SSID扫描周围的从设备，选择连接的从设备。当PC和STM32通过WI-FI连接路由器之后，STM32会一直向PC发送数据，接着PC端接收到数据之后，将数据进行实时可视化显示，在上位机中，可以同时显示XYZ三轴的角速度、加速度、磁力计数据，并可以保存所选取时间段的数据。一般架设无线网络的基本配备为无线网卡和一台AP，即可组成无线局域网组网。只要所部署的无线传感器没有超出局域网的感知半径，那么传感器设备便可不用固定为一特定位置，从而可以伴随运动个体一同运动，并通过WI-FI在局域网内传输测量信息。这种传输方式具有极强的灵活性和移动性。在传统的有线网络中，网络设备的安放位置固定，而在无线局域网内，只要处于信号覆盖区域内，设备都可接入网络。无线局域网最大的优点在于其移动性，同时连接到无线局域网的用户也可自由移动且能同时与网络保持连接。形成无线局域网之后，路由器作为AP将可以连接多个传感器设备，同时，路由器在无线中继器模式下，也起到了对信号的放大和重新发送，这样无线网络范围将进一步扩大。

一般无线路由器均有DHCP(动态主机设置协议)，DHCP是一个局域网的网络协议，用于内部网络或网络服务供应商自动分配IP地址给用户，以及用于内部网络管理员作为对所有连接设备进行中央管理的手段。本发明中通过DHCP配置网络中的局域网中设备的IP、网关等相关设置。通过路由器判断STM32传来的数据时间间隔，若时间间隔大于20ms，则认为一帧结束；否则，一直到接收数据达到上限值2KB，认为一帧结束。在ESP8266模块判断UART传来的数据一帧结束后，通过WI-FI接口将数据转发出去。整个过程ESP8266只负责将数据传到目标地址，不对数据进行处理，发送方和接收方的数据内容、长度完全一致，传输过程拥有良好的透明性。

在传输数据之前，需要通过路由器建立起传感器设备和PC端之间的联系。此时，我们需要配置路由器，例如网络名称(SSID)、安全模式、密钥(Password)。配置好路由器之后，将PC和传感器设备接入路由器，然后通过WEB端查看已连接的设备。设备连接进网络之后，对传感器设备的WI-FI模块进行目的IP、目的端口、串口参数进行设置，然后开始传输数据。作为目的设备的PC将对路由器转发的数据包进行解析，最后将数据通过软件进行可视化显示。理论上WI-FI连接设备数目由IP地址数目决定，一个地址池有多少IP地址就可以连接多少无线设备。通过IP地址的合理划分，WI-FI就能够连接大量传感器设备，可以用于处理多台并行工作的传感器设备数据传输问题。

参见图3，数据测量与采集过程如下：

第一步：使用硅胶密封传感器设备，以捆绑的方式将设备附着于运动个体。

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

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

第四步：STM32对数据进行解算之后，采用WI-FI通信方式将数据传输给PC端。

第五步：在传输数据之前，将PC和传感器设备接入路由器，然后通过WEB端查看已连接的设备。设备连接进网络之后，对传感器设备的WI-FI模块进行目的IP、目的端口、串口参数进行设置。

第六步：设置动态主机设置协议(Dynamic Host Configuration Protocol，DHCP)来给连接入路由器的设备自动分配IP地址并可对所有设备进行中央管理。UDP为面向无连接的通信(速度快)，只向指定的IP(每一台电脑都有自己的IP地址)地址发数据，数据即可发送到指定的电脑。

第七步：上位机从指定的端口接收到数据如图4所示。

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

Claims (6)

1.一种可多台并行工作的多维运动学数据测量装置，其特征在于，包括多个数据采集单元、路由器和上位机，数据采集单元、路由器和上位机构成一个无线局域网；多个数据采集单元用于并行地采集群集运动目标的运动数据，路由器用于将采集的运动数据转发给上位机；上位机用于接收和显示运动数据；

所述数据采集单元包括三轴陀螺仪、三轴加速度计、三轴磁力计、数据处理模块和数据通信模块；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向；所述数据通信模块用于将角度信息、磁场强度和方向、加速度、角速度通过数据通信模块传送给上位机；

所述上位机包括卡尔曼滤波子模块、角速度校正子模块和姿态解算子模块；

所述卡尔曼滤波子模块用于通过预测方程预测当前状态，并用更新方程检测预测结果，融合加速度计数据和陀螺仪数据，进行迭代运算直至估计出最可靠的角度值，再对估计出的最可靠角度值求导得到最可靠的角速度；

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

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

2.根据权利要求1所述的可多台并行工作的多维运动学数据测量装置，其特征在于，所述卡尔曼滤波子模块采用角速度积分得到的角度和陀螺仪常值偏差E作为状态向量。

3.根据权利要求1所述的可多台并行工作的多维运动学数据测量装置，其特征在于，修正后的角速度以四元数形式输出：

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

4.根据权利要求3所述的可多台并行工作的多维运动学数据测量装置，其特征在于，所述姿态解算子模块用于将修正后的角速度以四元数形式输出[x,y,z,w]^T＝[q₀,q₁,q₂,q₃]^T，依据四元数形式的角速度计算角度：α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。

5.根据权利要求1所述的可多台并行工作的多维运动学数据测量装置，其特征在于，所述数据通信模块采用WI-FI无线传输方式。

6.根据权利要求1所述的可多台并行工作的多维运动学数据测量装置，其特征在于，还包括连接数据处理模块的存储模块。