CN106767804B - 一种运动物体的多维数据测量装置及方法 - Google Patents
一种运动物体的多维数据测量装置及方法 Download PDFInfo
- Publication number
- CN106767804B CN106767804B CN201611237854.7A CN201611237854A CN106767804B CN 106767804 B CN106767804 B CN 106767804B CN 201611237854 A CN201611237854 A CN 201611237854A CN 106767804 B CN106767804 B CN 106767804B
- Authority
- CN
- China
- Prior art keywords
- angular speed
- angle
- axis
- magnetometer
- acceleration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 19
- 238000000034 method Methods 0.000 title claims abstract description 19
- 230000001133 acceleration Effects 0.000 claims abstract description 35
- 238000001914 filtration Methods 0.000 claims abstract description 27
- 230000008030 elimination Effects 0.000 claims abstract description 6
- 238000003379 elimination reaction Methods 0.000 claims abstract description 6
- 238000012545 processing Methods 0.000 claims description 12
- 230000005540 biological transmission Effects 0.000 claims description 10
- 238000004891 communication Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 7
- 239000000919 ceramic Substances 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 5
- 230000004927 fusion Effects 0.000 claims description 3
- 241000208340 Araliaceae Species 0.000 claims 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 claims 1
- 235000003140 Panax quinquefolius Nutrition 0.000 claims 1
- 235000008434 ginseng Nutrition 0.000 claims 1
- 239000011159 matrix material Substances 0.000 description 7
- 230000033001 locomotion Effects 0.000 description 5
- VYPSYNLAJGMNEJ-UHFFFAOYSA-N Silicium dioxide Chemical compound O=[Si]=O VYPSYNLAJGMNEJ-UHFFFAOYSA-N 0.000 description 4
- 239000000463 material Substances 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 239000000741 silica gel Substances 0.000 description 4
- 229910002027 silica gel Inorganic materials 0.000 description 4
- 230000003068 static effect Effects 0.000 description 3
- KRHYYFGTRYWZRS-UHFFFAOYSA-N Fluorane Chemical compound F KRHYYFGTRYWZRS-UHFFFAOYSA-N 0.000 description 2
- 238000010521 absorption reaction Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000004806 packaging method and process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- RFHAOTPXVQNOHP-UHFFFAOYSA-N fluconazole Chemical compound C1=NC=NN1CC(C=1C(=CC(F)=CC=1)F)(O)CN1C=NC=N1 RFHAOTPXVQNOHP-UHFFFAOYSA-N 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 239000000499 gel Substances 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002574 poison Substances 0.000 description 1
- 231100000614 poison Toxicity 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000007789 sealing Methods 0.000 description 1
- RMAQACBXLXPBSY-UHFFFAOYSA-N silicic acid Chemical compound O[Si](O)(O)O RMAQACBXLXPBSY-UHFFFAOYSA-N 0.000 description 1
- 235000012239 silicon dioxide Nutrition 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 239000002904 solvent Substances 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 230000009967 tasteless effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/06—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving measuring of drift angle; involving correction for drift
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供了一种运动物体的多维数据测量装置及方法，属于运动数据测量技术领域。本发明利用三轴陀螺仪采集被测物体的三轴角速度，利用三轴加速度计采集被测物体的三轴加速度，利用三轴磁力计采集磁场的强度和方向；对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度；将消除误差后的加速度、角速度和角度传送给上位机。本发明通过卡尔曼滤波，融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据准确性。
Description
技术领域
本发明属于运动数据测量技术领域，更具体地，涉及一种运动物体的多维数据测量装置及方法。
背景技术
制约当今生物动力学及运动数据分析研究的最大瓶颈便是数据采集技术。因此，高精度电子测量装置的研制具有重要意义。鉴于传统的GPS定位装置具有体积大、精度低、成本高、普适性差的缺点，在涉及动力学研究的各学科领域，可控的高精度多维运动数据测量与采集装置亟待研发。
高精度数据采集装置采用惯性导航系统，它是随着惯性传感器的发展而发展起来的一门导航技术，具有完全自主、不受干扰、输出信息量大、输出信息实时性强等优点，在军用航行载体和民用相关领域获得了广泛应用。惯导系统的精度、成本主要取决于陀螺仪和加速度传感器的精度和成本，尤其是陀螺仪，其漂移对惯导系统位置误差增长的影响巨大，而高精度的陀螺仪制造困难，成本很高，因此亟待寻求提高陀螺仪的精度，同时降低系统成本的有效方法。
发明内容
针对现有技术的缺陷或迫切需求，本发明提供了一种运动物体的多维数据测量装置及方法，其目的在于，通过卡尔曼滤波融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据精度。
一种运动物体的多维数据测量装置，包括数据采集模块、数据处理模块和数据通信模块；
所述数据采集模块包括三轴陀螺仪、三轴加速度计、三轴磁力计；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向；
所述数据处理模块用于对被测物体的加速度和角速度进行卡尔曼融合滤波以消除误差，结合磁力计采集的磁场强度和方向进行角速度补偿，然后进行姿态解算得到角度；
所述数据通信模块用于将消除误差后的加速度、角速度和角度传送给上位机。
进一步地，所述数据处理模块包括卡尔曼滤波子模块、角速度校正子模块和姿态解算子模块；
所述卡尔曼滤波子模块用于融合角速度和加速度进行卡尔曼滤波以消除误差；
所述角速度校正子模块用于将采集的磁场记为若使X轴对准北方，那么by＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为bx，在参考坐标系XOY平面中的投影为则bz＝hz；将转换到自身坐标系中得到 然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝KPe+KI∫e,Kp为比例系数，KI为积分系数；修正角速度w＝wa+δ；
姿态解算子模块，用于将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。
进一步地，所述卡尔曼滤波子模块采用角速度积分得到的角度和陀螺仪常值偏差E作为状态向量。
进一步地，修正后的角速度以四元数形式输出：
[q0,q1,q2,q3]T为四元数，t为采样周期，Δt为采样间隔，wx,wy,wz分别是修正后的角速度w在x、y、z轴的分量。
进一步地，所述姿态解算子模块用于将修正后的角速度以四元数形式输出[x,y,z,w]T＝[q0,q1,q2,q3]T，依据四元数形式的角速度计算角度：α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。
进一步地，所述数据通信模块采用蓝牙无线传输方式，蓝牙传输采用板载陶瓷天线。
进一步地，还包括连接数据处理模块的存储模块。
一种运动物体的多维数据测量方法，包括以下步骤；
利用三轴陀螺仪采集被测物体的三轴角速度，利用三轴加速度计采集被测物体的三轴加速度，利用三轴磁力计采集磁场的强度和方向；
对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度；
将消除误差后的加速度、角速度和角度传送给上位机。
进一步地，所述对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度的具体实现方式为；
融合角速度和加速度进行卡尔曼滤波以消除误差；
将采集的磁场记为若使X轴对准北方，那么by＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为bx，在参考坐标系XOY平面中的投影为则 bz＝hz；将转换到自身坐标系中得到然后再将 与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝KPe+KI∫e,Kp为比例系数，KI为积分系数；修正角速度w＝wa+δ；
将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。
本发明的有益技术效果体现在：
陀螺仪用于测量设备自身的旋转运动，但不能确定设备的方位。加速计测量设备的受力情况但用来测量设备相对于地面的摆放姿势，则精确度不高。磁力计可定位设备的方位，测量出当前设备与东南西北四个方向上 的夹角。本发明通过上述传感器，测量出运动个体的加速度、角速度、以及所处位置的磁场强度和方向，通过卡尔曼滤波，融合加速度计和陀螺仪数据可以有效抑制因积分和漂移产生的误差带来的影响，并借助磁力计数据补偿提高数据准确性。
作为优化，该设备采用蓝牙无线传输模式，减少设备之间的连线，通过将设备附着与运动个体表面，随之运动，采用蓝牙低功耗模式，耗电量少，在同规格独立电源供电情况下，续航时间久。同时，若超出范围或通讯收到影响时，可采用离线模式，将数据存储于内部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)AT+Q......(2)
更新方程：
Kg(k)＝P(k|k-1)HT/(HP(k|k-1)HT+R)......(3)
X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))......(4)
P(k|k)＝(I-Kg(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轴对准北方，那么by＝0，即通过转换矩阵将磁力计在自身坐标系的输出转换为在参考坐标系中的输出为所述自身坐标系到参考坐标系的转换矩阵为：
在参考坐标系XOY平面中的投影为bx，在参考坐标系XOY平面中的投影为则bz＝hz；将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积 求误差计算修正参数δ＝KPe+KI∫e,Kp为比例系数，KI为积分系数；修正角速度w＝wa+δ。
接下来利用修正后的角速度采用一阶龙格库塔法更新四元数。
这里假设有下列微分方程：
其解为
X[t+t]＝X[t]+t*f[X[t],w[t]]
t为采样周期，套用至四元数，
Kp为比例系数，KI为积分系数，t为采样周期，Δt为采样间隔，wx,wy,wz分别是修正后的角速度w在x、y、z轴的分量。
通过卡尔曼滤波，融合加速度计和陀螺仪数据，借助磁力计数据补偿可以有效抑制因积分和漂移产生的误差带来的影响，提高数据准确性。
(3)姿态解算子模块
为了实现姿态解算，这里利用三轴陀螺仪和三轴加速度计、三轴磁力计的采集数据，采用四元数的方法来进行姿态解算。
所述四元数解算的具体实现过程为：
地球的坐标系又称为地理坐标系，是固定不变的。正北，正东，正向上构成了这个坐标系的X、Y、Z轴，用坐标系R表示。在这里，量装置使用的是自身坐标系r，通过四元数就可以描述R与r坐标系之间的关系，进而可以描述物体运动情况：
这里w为一实数，为坐标向量，对应前文的[q0,q1,q2,q3]T。下面对四元数单位化：
假设有一任意旋转轴的向量与旋转角度θ，它们满足如下关系：
x＝s*xa
y＝s*ya
z＝s*ya
w＝cos(θ/2)
s＝sin(θ/2)
通过上述公式，多个旋转的组合可以通过四元数的乘法来表示。
获得采集的四元数数据，通过下述公式可以解算得角度信息即x，y，z轴的倾角。
α＝arctan2(2wz+2xy,1-2y2-2z2)(1)
θ＝arcsin(2wy-2zx)(2)
α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。
数据通信模块采用蓝牙无线传输方式，按照一种较佳的实现方式，其结构为：蓝牙传输模块采用板载陶瓷天线，有效缩小天线尺寸，并能达到隐藏天线目的。由于陶瓷本身介电常数较pcb电路板高，所以使用陶瓷天线能有效缩小天线尺寸，在介电损耗方面，陶瓷介质也比pcb电路板的介电损失小，所以非常适合低耗电率的蓝牙模块中使用。无线传输模块采用的蓝牙4.1，蓝牙4.1是蓝牙技术联盟于2013年底推出的新的规范，其最重要的特性是支持省电，支持多设备连接。在这里蓝牙模块采用从设备模式，在此模式下完全符合BLE4.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)。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (8)
1.一种运动物体的多维数据测量装置，其特征在于，包括数据采集模块、数据处理模块和数据通信模块；
所述数据采集模块包括三轴陀螺仪、三轴加速度计、三轴磁力计；三轴陀螺仪用于采集被测物体的三轴角速度，三轴加速度计用于采集被测物体的三轴加速度，三轴磁力计用于采集磁场的强度和方向；
所述数据处理模块用于对被测物体的加速度和角速度进行卡尔曼融合滤波以消除误差，结合磁力计采集的磁场强度和方向进行角速度补偿，然后进行姿态解算得到角度；
所述数据通信模块用于将消除误差后的加速度、角速度和角度传送给上位机；所述数据处理模块包括卡尔曼滤波子模块、角速度校正子模块和姿态解算子模块；
所述卡尔曼滤波子模块用于融合角速度和加速度进行卡尔曼滤波以消除误差；
所述角速度校正子模块用于将采集的磁场记为若使X轴对准北方，那么by＝0，即设磁力计在参考坐标系中的输出为在参考坐标系XOY平面中的投影为bx，在参考坐标系XOY平面中的投影为则bz＝hz；将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝KPe+KI∫e,Kp为比例系数，KI为积分系数；修正角速度w＝wa+δ，ωa为加速度计噪声；
姿态解算子模块，用于将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。
2.根据权利要求1所述的运动物体的多维数据测量装置，其特征在于，所述卡尔曼滤波子模块采用角速度积分得到的角度和陀螺仪常值偏差E作为状态向量。
3.根据权利要求1所述的运动物体的多维数据测量装置，其特征在于，修正后的角速度以四元数形式输出：
[q0,q1,q2,q3]T为四元数，t为采样周期，Δt为采样间隔，wx,wy,wz分别是修正后的角速度w在x、y、z轴的分量。
4.根据权利要求3所述的运动物体的多维数据测量装置，其特征在于，所述姿态解算子模块用于将修正后的角速度以四元数形式输出[x,y,z,w]T＝[q0,q1,q2,q3]T，依据四元数形式的角速度计算角度：α:z轴偏航角，θ:y轴俯仰角，x轴滚转角。
5.根据权利要求1所述的运动物体的多维数据测量装置，其特征在于，所述数据通信模块采用蓝牙无线传输方式，蓝牙传输采用板载陶瓷天线。
6.根据权利要求1所述的运动物体的多维数据测量装置，其特征在于，还包括连接数据处理模块的存储模块。
7.一种运动物体的多维数据测量方法，其特征在于，包括以下步骤；
利用三轴陀螺仪采集被测物体的三轴角速度，利用三轴加速度计采集被测物体的三轴加速度，利用三轴磁力计采集磁场的强度和方向；
对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度；
将消除误差后的加速度、角速度和角度传送给上位机；所述对被测物体的加速度和角速度进行卡尔曼滤波以消除误差，进而结合磁力计采集的磁场强度和方向进行姿态解算得到角度的具体实现方式为；
融合角速度和加速度进行卡尔曼滤波以消除误差；
将采集的磁场记为若使X轴对准北方，那么by＝0，即设磁力计在参考坐标系中的输出为 在参考坐标系XOY平面中的投影为bx，在参考坐标系XOY平面中的投影为则bz＝hz；将转换到自身坐标系中得到然后再将与磁力计自身坐标系输出做向量积求误差计算修正参数δ＝KPe+KI∫e,Kp为比例系数，KI为积分系数；修正角速度w＝wa+δ，ωa为加速度计噪声；
将修正后的角速度以四元数形式输出，依据四元数形式的角速度计算角度。
8.根据权利要求7所述的运动物体的多维数据测量方法，其特征在于，所述卡尔曼滤波采用角速度积分得到的角度和陀螺仪常值偏差E作为状态向量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611237854.7A CN106767804B (zh) | 2016-12-28 | 2016-12-28 | 一种运动物体的多维数据测量装置及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611237854.7A CN106767804B (zh) | 2016-12-28 | 2016-12-28 | 一种运动物体的多维数据测量装置及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106767804A CN106767804A (zh) | 2017-05-31 |
| CN106767804B true CN106767804B (zh) | 2018-03-27 |
Family
ID=58925108
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611237854.7A Active CN106767804B (zh) | 2016-12-28 | 2016-12-28 | 一种运动物体的多维数据测量装置及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106767804B (zh) |
Families Citing this family (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107561462B (zh) * | 2017-09-05 | 2024-09-13 | 重庆金山医疗技术研究院有限公司 | 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 |
| CN107898466B (zh) * | 2017-10-17 | 2020-12-11 | 深圳大学 | 一种基于惯性传感器的肢体运动捕捉系统和方法 |
| TWI680277B (zh) * | 2017-10-31 | 2019-12-21 | 芬蘭商亞瑪芬體育數字服務公司 | 用於確定物體的運動方向的方法和系統 |
| CN107576319A (zh) * | 2017-11-01 | 2018-01-12 | 黄海 | 无管全向自修正实时位移测量管 |
| CN108037658A (zh) * | 2017-11-15 | 2018-05-15 | 东莞市松迪智能机器人科技有限公司 | 一种基于导航系统的机器人运行误差的纠偏方法 |
| CN107677325B (zh) * | 2017-11-21 | 2024-02-23 | 成都瑞嘉和科技有限公司 | 黄磷炉电极偏斜、震动检测系统及检测方法 |
| CN108225370B (zh) * | 2017-12-15 | 2024-01-30 | 路军 | 一种运动姿态传感器的数据融合与解算方法 |
| CN108426559B (zh) * | 2018-02-27 | 2020-07-24 | 北京环境特性研究所 | 一种天线姿态检测装置及方法 |
| CN109258059B (zh) * | 2018-07-17 | 2021-06-22 | 南京航空航天大学 | 一种割草机位置的确定方法和装置 |
| CN109101019B (zh) * | 2018-07-31 | 2021-07-30 | 安徽灵翔智能机器人技术有限公司 | 一种基于信息融合的确定智能割草机行走位置的方法 |
| CN109945848B (zh) * | 2019-04-08 | 2023-04-18 | 深圳市智微智能科技股份有限公司 | 一种解决陀螺仪漂移的方法 |
| CN110174949A (zh) * | 2019-05-28 | 2019-08-27 | 欣旺达电子股份有限公司 | 虚拟现实设备和姿态感知及触觉再现控制方法 |
| CN110339542A (zh) * | 2019-06-06 | 2019-10-18 | 清华大学 | 一种可检测力量大小及方向的轮椅冰壶推杆 |
| CN110873889B (zh) * | 2019-11-19 | 2021-09-24 | 中国人民解放军海军工程大学 | 贯穿辐射应急探测模拟系统及方法 |
| CN112902954A (zh) * | 2021-03-05 | 2021-06-04 | 上海竹格智能传感技术有限公司 | 塔架晃动传感器和塔架晃动角度测量方法 |
| CN113080879A (zh) * | 2021-05-06 | 2021-07-09 | 陕西福音假肢有限责任公司 | 一种连续动态测量关节活动角度的电路及其方法 |
| CN114098716B (zh) * | 2021-11-15 | 2023-11-21 | 北京航空航天大学 | 一种获取运动姿态的方法及装置 |
| CN113865616B (zh) * | 2021-12-01 | 2022-04-05 | 成都九鼎科技(集团)有限公司 | 车辆悬架姿态测量方法和测量系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102221365A (zh) * | 2010-04-19 | 2011-10-19 | 霍尼韦尔国际公司 | 用于确定惯性导航系统故障的系统和方法 |
| CN102707305A (zh) * | 2012-06-06 | 2012-10-03 | 山东大学 | 摔倒检测定位系统及方法 |
| CN103175529A (zh) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | 基于室内磁场特征辅助的行人惯性定位系统 |
| CN103267524A (zh) * | 2013-04-24 | 2013-08-28 | 华中科技大学 | 一种穿戴式的人员步态检测室内定位系统及方法 |
| CN104020846A (zh) * | 2014-04-29 | 2014-09-03 | 诺力科技有限公司 | 一种主机运动感测方法、组件及运动感测系统 |
| CN104147770A (zh) * | 2014-07-24 | 2014-11-19 | 燕山大学 | 基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法 |
| CN104757976A (zh) * | 2015-04-16 | 2015-07-08 | 大连理工大学 | 一种基于多传感器融合的人体步态分析方法和系统 |
-
2016
- 2016-12-28 CN CN201611237854.7A patent/CN106767804B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102221365A (zh) * | 2010-04-19 | 2011-10-19 | 霍尼韦尔国际公司 | 用于确定惯性导航系统故障的系统和方法 |
| CN102707305A (zh) * | 2012-06-06 | 2012-10-03 | 山东大学 | 摔倒检测定位系统及方法 |
| CN103175529A (zh) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | 基于室内磁场特征辅助的行人惯性定位系统 |
| CN103267524A (zh) * | 2013-04-24 | 2013-08-28 | 华中科技大学 | 一种穿戴式的人员步态检测室内定位系统及方法 |
| CN104020846A (zh) * | 2014-04-29 | 2014-09-03 | 诺力科技有限公司 | 一种主机运动感测方法、组件及运动感测系统 |
| CN104147770A (zh) * | 2014-07-24 | 2014-11-19 | 燕山大学 | 基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法 |
| CN104757976A (zh) * | 2015-04-16 | 2015-07-08 | 大连理工大学 | 一种基于多传感器融合的人体步态分析方法和系统 |
Non-Patent Citations (2)
| Title |
|---|
| 基于MIMU和磁力计的姿态更新算法研究;米刚 等;《传感技术学报》;20150131;第28卷(第1期);43-48页 * |
| 多传感信息融合的改进扩展卡尔曼滤波定姿;邹波 等;《计算机应用研究》;20140430;第31卷(第4期);1035-1038页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106767804A (zh) | 2017-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106767804B (zh) | 一种运动物体的多维数据测量装置及方法 | |
| CN103776451B (zh) | 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 | |
| CN102980577B (zh) | 一种微型捷联航姿系统及其工作方法 | |
| CN102707725B (zh) | 固定翼自动导航飞行控制系统及其使用方法 | |
| EP2557394B1 (en) | System for processing pulse signals within an inertial navigation system | |
| CN101413800B (zh) | 导航/稳瞄一体化系统的导航、稳瞄方法 | |
| CN101059384B (zh) | 一种捷联mems惯性测量单元及安装误差标定方法 | |
| CN109141410B (zh) | Agv组合导航的多传感器融合定位方法 | |
| CN110007354B (zh) | 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法 | |
| CN202974288U (zh) | 一种微型捷联航姿系统 | |
| CN104075699B (zh) | 三维固态电子罗盘及其传感器的零点和比例系数核正方法 | |
| CN201561759U (zh) | 惯性姿态方位测量装置 | |
| CN103900571B (zh) | 一种基于惯性坐标系旋转型捷联惯导系统的载体姿态测量方法 | |
| CN104316080B (zh) | 一种姿态陀螺仪倾斜补偿四元数解算装置 | |
| CN106052682A (zh) | 一种混合式惯性导航系统及导航方法 | |
| CN202771262U (zh) | 固定翼自动导航飞行控制系统 | |
| CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
| CN103868648A (zh) | 三轴气浮仿真实验平台的质心测量方法 | |
| CN104697521A (zh) | 一种采用陀螺冗余斜交配置方式测量高速旋转体姿态和角速度的方法 | |
| CN103017764A (zh) | 高速列车自主导航及姿态测量装置 | |
| CN116125789A (zh) | 一种基于四元数的姿态算法参数自动匹配系统及方法 | |
| RU2256881C2 (ru) | Способ определения параметров ориентации и навигации и бесплатформенная инерциальная навигационная система для быстровращающихся объектов | |
| CN117804437A (zh) | 一种微型全姿定向装置及其测量方法 | |
| CN205317213U (zh) | 一种面向房地一体的不动产单元实地调查测量装置 | |
| CN113984036A (zh) | 一种便携式三轴光纤陀螺测姿系统及其控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20170531 Assignee: JIUJIANG DIVISION OF NO.707 INSTITUTE, CHINA SHIPBUILDING INDUSTRY Corp. Assignor: HUAZHONG University OF SCIENCE AND TECHNOLOGY Contract record no.: X2022420000075 Denomination of invention: A device and method for measuring multidimensional data of moving objects Granted publication date: 20180327 License type: Common License Record date: 20220824 |
|
| EE01 | Entry into force of recordation of patent licensing contract | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20170531 Assignee: Guangzhou Shipyard International Co.,Ltd. Assignor: HUAZHONG University OF SCIENCE AND TECHNOLOGY Contract record no.: X2024980000874 Denomination of invention: A multidimensional data measurement device and method for moving objects Granted publication date: 20180327 License type: Common License Record date: 20240122 |
|
| EE01 | Entry into force of recordation of patent licensing contract |