CN106679649A - 一种手部运动追踪系统及追踪方法 - Google Patents

Abstract

本发明公开了一种手部运动追踪系统及追踪方法。包括基于加速度计、陀螺仪和磁传感器的航姿参考系统和基于该航姿参考系统的手部运动追踪方法。首先获取加速度计测量的三轴加速度、陀螺仪测量的三轴角速度和磁传感器测量的三轴磁场分量，上位机在接收到传感数据后，采用最小二乘法对磁传感器进行误差补偿建立误差模型，通过窗口低通滤波器来消除三轴加速度的高频噪声，并对陀螺仪建立误差模型对其随机漂移进行误差补偿；之后通过改进型自适应互补滤波算法将陀螺仪、加速度计和磁传感器有效地融合得到姿态角和航向角，最后通过对加速度信号进行重力补偿和离散数字积分，得到手部运动的速度和轨迹。本发明可应用于人机交互系统，操作方便，体验感强。

Description

一种手部运动追踪系统及追踪方法

技术领域

本发明涉及惯性导航技术领域，具体的说是涉及一种基于航姿参考系统的手部运动追踪方法。

技术背景

运动追踪是检测记录运动目标的动作和其轨迹，将其转化为数字化的“抽象运动”。通过运动追踪技术，用户就可以使用体态、手势等模态向计算机发出指令等，因此其是新一代人机交互中的关键技术之一。而随着MEMS技术的发展，MEMS传感器由于体积小、功耗低、性能优异而逐渐被应用在导航和运动捕获等领域。

基于陀螺仪、加速度计和磁传感器的惯性式运动追踪系统，不仅有价格和尺寸大小的优势，而且能够提供更细致的运动参数数据。MEMS陀螺仪利用科里奥利力——旋转物体在有径向运动时所受到的切向力，用于测量物体运动的角速度，其瞬时精度高，但在长期运动情况下偏移较大，且误差随时间累积逐渐增大。加速度传感器主要类型有压阻式、电容式、力平衡式和谐振式，主要用于测量物体运动的加速度和重力加速度，其在运动过程中会受到载体振动的影响。磁传感器用于测量当地地磁场强度，虽然短期精度不如惯性器件，但不随运动时间而积累误差，可以采用合适的软件补偿消除噪声干扰。因此，如何融合多传感器的数据，滤除外部干扰，得到高可靠性高精度的姿态数据，是一项非常具有挑战性的工作。扩展卡尔曼滤波器是一种高精度的应用非常广泛的姿态解算算法，但是其计算量很大，对处理器的运算速度和精度要求很高。互补滤波器算法简单可靠，对惯性器件的精度要求较低，在运动姿态解算中的应用愈加广泛。但传统的互补滤波器算法在高速运动状态下，姿态角响应有一定的延迟，其动态性能不如卡尔曼滤波器。本发明设计了基于改进的自适应互补滤波器算法，实现了长时间稳定地输出高精度姿态数据，从而提升运动追踪的准确性。

发明内容

本发明的目的是克服现有技术中的不足，提供一种基于加速度、陀螺仪和磁传感器等MEMS传感器的手部运动追踪系统即追踪方法。该系统通过对传感器建立校正模型，减小原始传感数据的误差，并通过改进的自适应互补滤波算法提供准确的姿态角度，根据手部的不同运动状态在小范围内完成对手部运动轨迹的精确记录和追踪。

为实现上述目的，本发明通过以下技术方案实现：一种手部运动追踪系统，包括加速度计、陀螺仪、磁传感器、MCU、电源模块和上位机，加速度计、陀螺仪、磁传感器分别与MCU通过IIC总线相连，MCU与上位机相连；加速度计、陀螺仪、磁传感器、MCU和上位机均通过电源模块供电；所述加速度计测量手部的三轴加速度，陀螺仪测量手部的三轴角速度，磁传感器测量手部的三轴磁场分量；上位机通过MCU获取上述三类原始传感数据(三轴加速度、三轴角速度、三轴磁场分量)，分别对三类原始传感数据进行误差补偿，并通过改进型自适应互补滤波算法更新姿态，并结合姿态矩阵对手部的加速度信号进行重力补偿和离散数字积分，得到该时间段内的手部位移和其运动轨迹，实现手部运动的追踪。

一种手部运动追踪方法，具体包括以下步骤：

(1)加速度计测量手部的三轴加速度，陀螺仪测量手部的三轴角速度，磁传感器测量手部的三轴磁场分量；

(2)MCU通过IIC总线接收加速度计测量到的三轴加速度、陀螺仪测量到的三轴角速度和磁传感器测量的三轴磁场分量，并发送给上位机；

(3)上位机先对三轴磁场分量通过最小二乘法建立误差模型，消除环境中的磁场干扰；其次对三轴加速度采用窗口低通滤波，消除高频噪声信号，最后对三轴角速度进行误差建模，并对其随机偏移进行误差补偿；

(4)将步骤3得到的窗口低通滤波后的加速度信号通过三轴分量的关系求解手部的初始横滚角和俯仰角。将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿后，计算出初始航向角。根据上述初始横滚角、俯仰角和航向角可以得到姿态四元数，从而根据姿态四元数建立四元数姿态矩阵；

(5)重复步骤1-3，获得下一时刻的校正后的三轴磁场分量、三轴加速度、三轴角速度。将三轴磁场分量、三轴加速度和步骤4得到的四元数姿态矩阵的对应分量做向量叉积，从而修正校准陀螺仪偏移；并根据一阶龙格库塔方程得到更新的姿态四元数，根据姿态四元数得到姿态角和航向角；

(6)由步骤5得到的惯性传感器的姿态角和航向角，可获得惯性传感器的最佳姿态矩阵将步骤5得到的加速度计的三轴加速度通过姿态矩阵从手部坐标系下转换到地理坐标系下，再对三轴加速度进行重力补偿,得到补偿后的加速度。最后对补偿后的加速度进行离散数字积分，得到手部的位置和速度信息，在屏幕上定位手部所移动过的位置，从而实现运动追踪。

进一步地，所述步骤3中，上位机对三轴磁场分量通过最小二乘法建立误差模型，具体如下：

建立三轴磁场分量误差模型方程如下：

m_x0＝k_xm_x+b_x

m_y0＝k_yγm_x+k_ym_y+b_y

m_z0＝k_zβm_x+k_zαm_y+k_zm_z+b_z

其中，k_x，k_y，k_z是标度系数误差，γ，β，α是三轴垂直度误差，b_x，b_y，b_z是三轴零偏误差，m_x，m_y，m_z是磁传感器测得的三轴磁场分量，m_x0，m_y0，m_z0是三轴磁场分量的理想值。

通过将上述模型变换到到椭球曲面方程形式，利用最小二乘法，得到k_x，k_y，k_z，γ，β，α，b_x，b_y，b_z，从而获得三轴磁场分量误差模型。

进一步地，所述椭球曲面方程的最小二乘法求解方法具体为：

假设手部所在地球空间的地磁场强度为m₀，则有：

||m₀||²＝m_x0 ²+m_y0 ²+m_z0 ²

将权利要求3中所述的三轴磁场分量误差模型代入上式，可得：

||m₀||²＝k_xm_x ²+k_ym_y ²+k_zm_z ²+2k_y ²γm_xm_y+2k_z ²βm_xm_z+2k_z ²αm_ym_z+2k_xb_xm_x+2k_yb_ym_y+2k_zb_zm_z

该方程对应为一般的椭球曲面方程，其中

A＝[k_x,k_y,k_z,2k_y ²γ,2k_z ²β,2k_z ²α,2k_xb_x,2k_yb_y,2k_zb_z]^T，表示待求椭球的曲面参数向量，

m＝[m_x ²,m_y ²,m_z ²,2m_xm_y,2m_xm_z,2m_ym_z,2m_x,2m_y,2m_z]^T，表示测量数据的运算组合向量。获取n组数据对上述方程进行求解，n≥9；

将上述模型改写成矩阵形式如下：

||m₀||²＝m^TA

模型残差可写为：ε(i)＝m(i)A-||m₀(i)||²

其中i表示第i组实验数据。m表示磁传感器测得的三轴磁场分量，m₀表示三轴磁场分量的理想值。

最小二乘原理是寻求使得模型残差平方和最小的被估参量，即：

将方程组中的向量都改写成矩阵形式，模型残差可用M^TA-M₀下式表示：

其中定义：

最小化残差平方和，令可得曲面参数向量A的最小二乘估计值是：

根据实验数据M，M₀即可以得到曲面参数向量A的估计值，从而完成磁传感器误差模型的建立。

进一步地，所述步骤3中对三轴加速度信号进行窗口低通滤波，步骤如下：

本发明中对加速度信号用中值滤波器滤除数据中的高频噪声，设滤波器窗口A的长度l，观测值为x₁，x₂，…，x_N，N为观测值个数，即当窗口A在原始序列上移动时，中值滤波器设计如下：

在任意时刻窗口内的观测数据重新按照数值大小排序，当长度l为奇数时，取中间数据作为本次滤波器的输出。当长度l为偶数时，取中间两个数据的平均值作为本次滤波器的输出。

进一步地，所述步骤3中，对三轴角速度进行误差建模，并对其随机偏移进行误差补偿，步骤具体如下：

在一定测量时间内，陀螺仪的误差模型可用下式表示：

w_m＝w₀+ε+ε₀+δn(t)

w_m是陀螺仪的实际测量值,w₀是陀螺仪的理论值，ε是时变漂移，ε₀是常值零偏，n(t)是高斯白噪声，其强度为δ。

通过陀螺仪的数字可编程的低通滤波器，滤掉ε和δn(t)，而对于常值零偏ε₀，将陀螺仪静止一段时间，取静态下陀螺仪的均值输出作为常值零偏ε₀。

因此，陀螺仪校正后输出w₀可由下式来表示：

w₀＝w_m-ε₀。

进一步地，所述获取初始姿态角和四元数姿态矩阵的处理步骤具体如下：

首先，利用步骤3得到的窗口低通滤波后的加速度信号(a_x,a_y,a_z)通过三轴分量的关系求解手部的初始横滚角θ和俯仰角φ：

φ＝-arcsin(a_x)

θ＝-arctan2(a_y,a_z)

将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿，将手部坐标系上的磁场分量转换到地理坐标系，可得手部的初始航向角ψ如下：

ψ＝-arctan2(m_y0*cosθ+m_x0*sinθ*sinφ-m_z0*sinθ*cosφ,m_x0*cosφ+m_z0*sinφ)

在获取初始姿态角和航向角以后，可以得到初始四元数的四个分量如下：

q₀＝cos(0.5θ)*cos(0.5φ)*cos(0.5ψ)+sin(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₁＝sin(0.5θ)*cos(0.5φ)*cos(0.5ψ)-cos(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₂＝cos(0.5θ)*sin(0.5φ)*cos(0.5ψ)+sin(0.5θ)*cos(0.5φ)*sin(0.5ψ)

q₃＝cos(0.5θ)*cos(0.5φ)*sin(0.5ψ)-sin(0.5θ)*sin(0.5φ)*cos(0.5ψ)

根据上述的四元数，可以得到四元数形式的姿态矩阵如下：

进一步地，所述步骤5具体为：

设在地理坐标系下，手部对应的地磁场的矢量为表示为(m_exm_eym_ez)，重力场的矢量是在手部坐标系下，手部的地磁场的矢量是重力场的矢量是表示为(a_bxa_bya_bz)；将地理坐标系下的地磁场矢量和重力场矢量通过姿态矩阵转换到手部坐标系下，如果是在姿态矩阵理想无误差的情况下，则可得：

但由于姿态矩阵存在误差，因此在和和之间存在误差，和间的误差对应俯仰角和横滚角的误差e_θ,φ，和之间的误差对应航向角的误差。向量之间的误差可以用叉积表示。

对于重力场，加速度传感器测量的重力加速度在地理坐标系中只有竖直向下的方向，因此可设地理坐标下的重力场矢量将其通过姿态矩阵转换到手部坐标系下，即对应姿态矩阵的第三列元素。将该矢量与手部坐标系下加速度的测量值间的误差表示如下：

对于地磁场，地理坐标下磁场传感器对应的地磁向量可以表示为：

手部运动的航向角定义在地理水平面x-y，因此在地理坐标下的地磁矢量h＝(h_xh_y h_z)^T，其中h_y＝0，h_z＝m_ez。航向角的误差为地理坐标下地磁向量和观测向量的叉积：

系统姿态向量的误差为：

e＝e_θ,φ+e_ψ

用磁传感器和加速度计补偿得到的系统姿态向量误差来作为PI调节器的输入来校准陀螺仪的偏差，t时刻校准后的陀螺仪角速度向量为：

其中，e(t)为t时刻系统姿态向量的误差，λ和采样频率有关系，一般设为采样周期的大小，w_m为陀螺仪输出的角速度向量，w₀为校准后的角速度向量。比例增益K_p控制低频段和高频段之间的截止频率，积分增益K_i控制滤波器消除误差的时间。通常，K_p和K_i的选择与截止频率f和阻尼系数ξ有关：

K_p＝2ξf

K_i＝f²

一般情况下，可选择阻尼系数ξ＝0.707，将手部运动形态划分为以下三种不同状态，根据不同的运动状态自适应调整截止频率：

1)静止状态:此时陀螺仪测量值w_m<0.02rad/s(1.7°/s)，设截止频率f＝0.05rad/s；

2)弱机动运动状态:此时0.02rad/s(1.7°/s)<w_m<0.524rad/s(30°/s)，可设截止频率f＝0.005rad/s；

3)高机动运动状态:此时w_m>0.524rad/s(30°/s)，截止频率设为0。

在获取校准后的陀螺仪角速度后，地理坐标系相对手部坐标系转动的姿态四元数变化率可表示为：

当前时刻t的姿态四元数可由上一时刻t-1的姿态四元数加上姿态四元数变化率的积分而获得：

之后可根据当前姿态四元数获取传感节点的姿态和四元数形式的姿态矩阵：

φ＝-arcsin2(q₁q₃-q₀q₂)

进一步地，所述获取手部运动位置和速度，通过以下步骤实现：

将步骤5得到的加速度计的三轴加速度表示为(a_bxa_bya_bz)，通过姿态矩阵从手部坐标系下转换到地理坐标系下,并进行重力加速度补偿,得到补偿后的加速度表示为(a_xa_ya_z)如下：

根据加速度和速度、位移之间的关系，假设从初始时刻t₀时刻开始采样，采样周期为Δt，可得速度和位移如下：

其中，v₀和s₀分别是初始速度和初始位置，v_t、s_t分别是t时刻的速度、位移。之后采用Simpson积分法，对补偿后的加速度进行离散数字积分，得到速度和位移，计算方法如下：

其中，a_t表示t时刻重力补偿后的加速度。通过计算手部运动的实时速度和位移，即可获取对手部的实时追踪。

与现有技术相比，本发明的技术方案具有以下优势：

1.本发明的改进型自适应互补滤波算法根据手部运动的不同状态，将运动分为三类：静止或匀速运动、弱机动运动状态和高机动运动状态，从而动态调整互补滤波系数，在不增加复杂度的情况下使得姿态推算在动态和静态下都大为改善。

2.本系统融合了加速度、角速度和磁场信息，且所采用的传感器均为低成本器件，但通过数据的良好滤波以及姿态算法的良好设计准确地解算出手部姿态，描绘出手部运动信息。

3.运动过程中，可不间断进行重置，消除了加速度计算位移的累计误差，提高追踪准确性。

附图说明

图1为本发明基于航姿参考系统的手部运动追踪流程图；

图2为静止状态下，航姿参考系统解算的航向角；

图3为低速运动状态下(11.25°/s)，航姿参考系统解算的航向角；

图4为高速运动状态下(45°/s)，航姿参考系统解算的航向角。

具体实施方式

下面结合附图对本发明作进一步的说明。

如图1所示，本发明中运动追踪方法的技术实现具体包括以下步骤：

(1)加速度计测量手部的三轴加速度，陀螺仪测量手部的三轴角速度，磁传感器测量手部的三轴磁场分量；

(2)MCU通过IIC总线接收加速度计测量到的三轴加速度、陀螺仪测量到的三轴角速度和磁传感器测量的三轴磁场分量，并发送给上位机；

(3)上位机先对三轴磁场分量通过最小二乘法建立误差模型，消除环境中的磁场干扰；其次对三轴加速度采用窗口低通滤波，消除高频噪声信号，最后对三轴角速度进行误差建模，并对其随机偏移进行误差补偿；

上述对三轴磁场分量通过最小二乘法建立误差模型的步骤具体如下：

建立三轴磁场分量误差模型方程如下：

m_x0＝k_xm_x+b_x

m_y0＝k_yγm_x+k_ym_y+b_y

m_z0＝k_zβm_x+k_zαm_y+k_zm_z+b_z

其中，k_x，k_y，k_z是标度系数误差，γ，β，α是三轴垂直度误差，b_x，b_y，b_z是三轴零偏误差，m_x，m_y，m_z是磁传感器测得的三轴磁场分量，m_x0，m_y0，m_z0是三轴磁场分量的理想值。

假设手部所在地球空间的地磁场强度为m₀，则有：

||m₀||²＝m_x0 ²+m_y0 ²+m_z0 ²

将权利要求3中所述的三轴磁场分量误差模型代入上式，可得：

||m₀||²＝k_xm_x ²+k_ym_y ²+k_zm_z ²+2k_y ²γm_xm_y+2k_z ²βm_xm_z+2k_z ²αm_ym_z+2k_xb_xm_x+2k_yb_ym_y+2k_zb_zm_z

该方程对应为一般的椭球曲面方程，其中

A＝[k_x,k_y,k_z,2k_y ²γ,2k_z ²β,2k_z ²α,2k_xb_x,2k_yb_y,2k_zb_z]^T，表示待求椭球的曲面参数向量，

m＝[m_x ²,m_y ²,m_z ²,2m_xm_y,2m_xm_z,2m_ym_z,2m_x,2m_y,2m_z]^T，

表示测量数据的运算组合向量。获取n组数据对上述方程进行求解，n≥9；

将上述模型改写成矩阵形式如下：

||m₀||²＝m^TA

模型残差可写为：ε(i)＝m(i)A-||m₀(i)||²

其中i表示第i组实验数据。m表示磁传感器测得的三轴磁场分量，m₀表示三轴磁场分量的理想值。

最小二乘原理是寻求使得模型残差平方和最小的被估参量，即：

将方程组中的向量都改写成矩阵形式，模型残差可用M^TA-M₀下式表示：

其中定义：

最小化残差平方和，令可得曲面参数向量A的最小二乘估计值是：

根据实验数据M，M₀即可以得到曲面参数向量A的估计值，从而完成磁传感器误差模型的建立。

上述对三轴加速度信号进行窗口低通滤波，步骤具体如下：

本发明中对加速度信号用中值滤波器滤除数据中的高频噪声，设滤波器窗口A的长度l，观测值为x₁，x₂，…，x_N，N为观测值个数，即当窗口A在原始序列上移动时，中值滤波器输出为：

在任意时刻窗口内的观测数据重新按照数值大小排序，当长度l为奇数时，取中间数据作为本次滤波器的输出。当长度l为偶数时，取中间两个数据的平均值作为本次滤波器的输出。

对三轴角速度进行误差建模，并对其随机偏移进行误差补偿，步骤具体如下：

一般来说，陀螺的误差主要是随机漂移，但由于各种电路噪声等外部环境因素的影响，实际测量的数据还包括白噪声。在一定测量时间内，陀螺仪的误差模型可用下式表示：

w_m＝w₀+ε+ε₀+δn(t)

w_m是陀螺仪的实际测量值,w₀是陀螺仪的理论值，ε是时变漂移，ε₀是常值零偏，n(t)是高斯白噪声，其强度为δ。

通过陀螺仪的数字可编程的低通滤波器，滤掉ε和δn(t)，而对于常值零偏ε₀，将陀螺仪静止一段时间，取静态下陀螺仪的均值输出作为常值零偏ε₀。

因此，陀螺仪校正后输出w₀可由下式来表示：

w₀＝w_m-ε₀

(4)将步骤3得到的窗口低通滤波后的加速度信号通过三轴分量的关系求解手部的初始横滚角和俯仰角。将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿后，计算出初始航向角。根据上述初始横滚角、俯仰角和航向角可以得到姿态四元数，从而根据姿态四元数建立四元数姿态矩阵；

上述获取初始姿态角、航向角和四元数姿态矩阵的处理步骤具体如下：

首先，利用步骤3得到的窗口低通滤波后的加速度信号(a_x,a_y,a_z)通过三轴分量的关系求解手部的初始横滚角θ和俯仰角φ：

φ＝-arcsin(a_x)

θ＝-arctan2(a_y,a_z)

之后，将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿，将手部坐标系上的磁场分量转换到地理坐标系，可得手部的初始航向角ψ如下：

ψ＝-arctan2(m_y0*cosθ+m_x0*sinθ*sinφ-m_z0*sinθ*cosφ,m_x0*cosφ+m_z0*sinφ)

在获取初始姿态角和航向角以后，可以得到初始四元数的四个分量如下：

q₀＝cos(0.5θ)*cos(0.5φ)*cos(0.5ψ)+sin(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₁＝sin(0.5θ)*cos(0.5φ)*cos(0.5ψ)-cos(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₂＝cos(0.5θ)*sin(0.5φ)*cos(0.5ψ)+sin(0.5θ)*cos(0.5φ)*sin(0.5ψ)

q₃＝cos(0.5θ)*cos(0.5φ)*sin(0.5ψ)-sin(0.5θ)*sin(0.5φ)*cos(0.5ψ)

根据上述的四元数，可以得到四元数形式的姿态矩阵如下：

(5)重复步骤1-3，获得下一时刻的校正后的三轴磁场分量、三轴加速度、三轴角速度。将三轴磁场分量、三轴加速度和步骤4得到的四元数姿态矩阵的对应分量做向量叉积，从而修正校准陀螺仪偏移；并根据一阶龙格库塔方程得到更新的姿态四元数，根据姿态四元数得到姿态角和航向角；

陀螺仪测量载体坐标系下的三轴角速度分量，将角速度经过积分得到姿态角，短时精度高。但因为陀螺仪自身存在漂移误差，虽然已对陀螺仪的常值零偏进行过校准，但随着时间的积累偏移会逐渐变化。因此长时间的精度较差。与之相反，加速度计和磁传感器短期精度较差，但其测量误差不会随时间的积累而增加。利用不同传感器在频率上的互补关系，采用互补滤波算法对姿态数据进行融合能提高姿态的测量精度和动态响应。

上述改进型自适应互补滤波器的处理步骤具体如下：

设在地理坐标系下，手部对应的地磁场的矢量为表示为(m_exm_eym_ez)，重力场的矢量是在手部坐标系下，手部的地磁场的矢量是重力场的矢量是表示为(a_bxa_bya_bz)；将地理坐标系下的地磁场矢量和重力场矢量通过姿态矩阵转换到手部坐标系下，如果是在姿态矩阵理想无误差的情况下，则可得：

但由于姿态矩阵存在误差，因此在和和之间存在误差，和间的误差对应俯仰角和横滚角的误差e_θ,φ，和之间的误差对应航向角的误差。向量之间的误差可以用叉积表示。

对于重力场，加速度传感器测量的重力加速度在地理坐标系中只有竖直向下的方向，因此可设地理坐标下的重力场矢量将其通过姿态矩阵转换到手部坐标系下，即对应姿态矩阵的第三列元素。将该矢量与手部坐标系下加速度的测量值间的误差表示如下：

对于地磁场，地理坐标下磁场传感器对应的地磁向量可以表示为：

手部运动的航向角定义在地理水平面x-y，因此在地理坐标下的地磁矢量h＝(h_xh_y h_z)^T，其中h_y＝0，h_z＝m_ez。航向角的误差为地理坐标下地磁向量和观测向量的叉积：

系统姿态向量的误差为：

e＝e_θ,φ+e_ψ

用磁传感器和加速度计补偿得到的系统姿态向量误差来作为PI调节器的输入来校准陀螺仪的偏差，t时刻校准后的陀螺仪角速度向量为：

其中，e(t)为t时刻系统姿态向量的误差，λ和采样频率有关系，一般设为采样周期的大小，w_m为陀螺仪输出的角速度向量，w₀为校准后的角速度向量。比例增益K_p控制低频段和高频段之间的截止频率，积分增益K_i控制滤波器消除误差的时间。截止频率较低时，滤波结果主要依赖于陀螺的测量，截止频率较高时，滤波结果主要依赖于加速度计和磁传感器的测量。

通常，K_p和K_i的选择与截止频率f和阻尼系数ξ有关：

K_p＝2ξf

K_i＝f²

然而，在手部运动过程中，可能会出现多种运动形态，固定的截止频率并不适用于整个运动追踪过程，因此本发明提出了基于运动状态监测的自适应参数调整。一般情况下，可选择阻尼系数ξ＝0.707，将手部运动形态划分为以下三种不同状态，根据不同的运动状态自适应调整截止频率：

1)静止状态:此时陀螺仪测量值w_m<0.02rad/s(1.7°/s)，此时加速度计和磁传感器可以得到较好的测量，并赋予其较大的权重，此时可设截止频率f＝0.05rad/s；

2)弱机动运动状态:此时0.02rad/s(1.7°/s)<w_m<0.524rad/s(30°/s)，加速度计和磁传感器的测量值将为姿态估计带来部分偏差，因此应适当降低加速度和磁传感器的权重，增加陀螺仪测量值的权重，降低加速度和磁传感器的权重，此时可设截止频率f＝0.005rad/s；

3)高机动运动状态:此时w_m>0.524rad/s(30°/s)，此时陀螺仪的动态性能较好，因此应该赋予其较大的权重。而此时加速度计和磁传感器的动态性能比较差，因此截止频率设为0。

在获取校准后的陀螺仪角速度后，地理坐标系相对手部坐标系转动的姿态四元数变化率可表示为：

当前时刻t的姿态四元数可由上一时刻t-1的姿态四元数加上姿态四元数变化率的积分而获得：

之后可根据当前姿态四元数获取传感节点的姿态和四元数形式的姿态矩阵：

φ＝-arcsin2(q₁q₃-q₀q₂)

(6)由步骤5得到的惯性传感器的姿态角和航向角，可获得惯性传感器的最佳姿态矩阵将步骤5得到的加速度计的三轴加速度通过姿态矩阵从手部坐标系下转换到地理坐标系下，再对三轴加速度进行重力补偿,得到补偿后的加速度。最后对补偿后的加速度进行离散数字积分，得到手部的位置和速度信息，在屏幕上定位手部所移动过的位置，从而实现运动追踪。

上述获取手部运动位置和速度的处理步骤具体如下：

将步骤5得到的加速度计的三轴加速度表示为(a_bxa_bya_bz)，通过姿态矩阵从手部坐标系下转换到地理坐标系下,并进行重力加速度补偿,得到补偿后的加速度表示为(a_xa_ya_z)如下：

根据加速度和速度、位移之间的关系，假设从初始时刻t₀时刻开始采样，采样周期为Δt，可得速度和位移如下：

其中，v₀和s₀分别是初始速度和初始位置，v_t、s_t分别是t时刻的速度、位移。

对加速度进行积分的方法有时域二次积分法和频域二次积分法，本发明中采用时域中的离散数字积分法。离散数字积分方法一般常用方法有梯形积分法、Simpson积分法，其中梯形积分法计算简单、计算时间短，但误差比较大；而Simpson积分法计算结果精确，但计算时间稍长。本发明中运动追踪算法运行在上位机，处理能力强，因此本发明中采用Simpson积分法。

之后采用Simpson积分法，对补偿后的加速度进行离散数字积分，得到速度和位移，计算方法如下：

其中，a_t表示t时刻重力补偿后的加速度。通过计算手部运动的实时速度和位移，即可获取对手部的实时追踪。

考虑到加速度二次积分的累积误差，在系统运行一段时间以后，对系统进行一次重新初始化操作。重新初始化只需要将手部与地平面保持水平，并保持静止3s左右。此时系统检测到传感模块的俯仰角和横滚角均为0，且陀螺仪输出为0，系统会将起始速度和位移重置为0，从而避免长时间运行的累积误差。

下面通过实验结果以航向角为例来对本发明的提出的改进型自适应互补滤波解算方法的综合性能进行验证和说明。图2是静止状态下手部航向角解算结果，其均方根误差仅为0.0657°；图3是将航姿参考模块放置在低速运转的电机下(11.25°/s)旋转一周，解算出来的航向角与理论值的对比，其均方根误差仅为1.0231°；图4是将航姿参考模块放置在高速运转的电机下(45°/s)旋转一周，解算出来的航向角与理论值的对比，其均方根误差仅为1.2515°。从以上解算结果可看出，该运动追踪系统无论在低速运动状态或高速运动状态下均具有良好的性能，解算精度较高。

Claims (9)

1.一种手部运动追踪系统，其特征在于，包括加速度计、陀螺仪、磁传感器、MCU、电源模块和上位机，加速度计、陀螺仪、磁传感器分别与MCU通过IIC总线相连，MCU与上位机相连；加速度计、陀螺仪、磁传感器、MCU和上位机均通过电源模块供电；所述加速度计测量手部的三轴加速度，陀螺仪测量手部的三轴角速度，磁传感器测量手部的三轴磁场分量；上位机通过MCU获取上述三类原始传感数据(三轴加速度、三轴角速度、三轴磁场分量)，分别对三类原始传感数据进行误差补偿，并通过改进型自适应互补滤波算法更新姿态，并结合姿态矩阵对手部的加速度信号进行重力补偿和离散数字积分，得到该时间段内的手部位移和其运动轨迹，实现手部运动的追踪。

2.一种权利要求1所述的手部运动追踪系统的追踪方法，其特征在于，具体包括以下步骤：

(1)加速度计测量手部的三轴加速度，陀螺仪测量手部的三轴角速度，磁传感器测量手部的三轴磁场分量；

(2)MCU通过IIC总线接收加速度计测量到的三轴加速度、陀螺仪测量到的三轴角速度和磁传感器测量的三轴磁场分量，并发送给上位机；

(3)上位机先对三轴磁场分量通过最小二乘法建立误差模型，消除环境中的磁场干扰；其次对三轴加速度采用窗口低通滤波，消除高频噪声信号，最后对三轴角速度进行误差建模，并对其随机偏移进行误差补偿；

(4)将步骤3得到的窗口低通滤波后的加速度信号通过三轴分量的关系求解手部的初始横滚角和俯仰角。将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿后，计算出初始航向角。根据上述初始横滚角、俯仰角和航向角可以得到姿态四元数，从而根据姿态四元数建立四元数姿态矩阵；

(5)重复步骤1-3，获得下一时刻的校正后的三轴磁场分量、三轴加速度、三轴角速度。将三轴磁场分量、三轴加速度和步骤4得到的四元数姿态矩阵的对应分量做向量叉积，从而修正校准陀螺仪偏移；并根据一阶龙格库塔方程得到更新的姿态四元数，根据姿态四元数得到姿态角和航向角；

(6)由步骤5得到的惯性传感器的姿态角和航向角，可获得惯性传感器的最佳姿态矩阵将步骤5得到的加速度计的三轴加速度通过姿态矩阵从手部坐标系下转换到地理坐标系下，再对三轴加速度进行重力补偿,得到补偿后的加速度。最后对补偿后的加速度进行离散数字积分，得到手部的位置和速度信息，在屏幕上定位手部所移动过的位置，从而实现运动追踪。

3.根据权利要求2所述的方法，其特征在于，所述步骤3中，上位机对三轴磁场分量通过最小二乘法建立误差模型，具体如下：

建立三轴磁场分量误差模型方程如下：

m_x0＝k_xm_x+b_x

m_y0＝k_yγm_x+k_ym_y+b_y

m_z0＝k_zβm_x+k_zαm_y+k_zm_z+b_z

其中，k_x，k_y，k_z是标度系数误差，γ，β，α是三轴垂直度误差，b_x，b_y，b_z是三轴零偏误差，m_x，m_y，m_z是磁传感器测得的三轴磁场分量，m_x0，m_y0，m_z0是三轴磁场分量的理想值。

通过将上述模型变换到到椭球曲面方程形式，利用最小二乘法，得到k_x，k_y，k_z，γ，β，α，b_x，b_y，b_z，从而获得三轴磁场分量误差模型。

4.根据权利要求3所述的方法，其特征在于，所述椭球曲面方程的最小二乘法求解方法具体为：

假设手部所在地球空间的地磁场强度为m₀，则有：

||m₀||²＝m_x0 ²+m_y0 ²+m_z0 ²

将权利要求3中所述的三轴磁场分量误差模型代入上式，可得：

||m₀||²＝k_xm_x ²+k_ym_y ²+k_zm_z ²+2k_y ²γm_xm_y+2k_z ²βm_xm_z+2k_z ²αm_ym_z+2k_xb_xm_x+2k_yb_ym_y+2k_zb_zm_z

该方程对应为一般的椭球曲面方程，其中

A＝[k_x,k_y,k_z,2k_y ²γ,2k_z ²β,2k_z ²α,2k_xb_x,2k_yb_y,2k_zb_z]^T，表示待求椭球的曲面参数向量，

m＝[m_x ²,m_y ²,m_z ²,2m_xm_y,2m_xm_z,2m_ym_z,2m_x,2m_y,2m_z]^T，表示测量数据的运算组合向量。获取n组数据对上述方程进行求解，n≥9；

将上述模型改写成矩阵形式如下：

||m₀||²＝m^TA

模型残差可写为：ε(i)＝m(i)A-||m₀(i)||²

其中i表示第i组实验数据。m表示磁传感器测得的三轴磁场分量，m₀表示三轴磁场分量的理想值。

最小二乘原理是寻求使得模型残差平方和最小的被估参量，即：

$J=min\left({\mathrm{\&Sigma;}}_{i=1}^n{\mathrm{\&epsiv;}}^2\right(i\left)\right)={\mathrm{\&Sigma;}}_{i=1}^n{\&lsqb;m^T\left(i\right)A-\left|\right|m_0\left(i\right)|{|}^2\&rsqb;}^2$

将方程组中的向量都改写成矩阵形式，模型残差可用M^TA-M₀下式表示：

其中定义：

$M^T=\left[\begin{array}{c}{m}^T\left(1\right)\\ m^T\left(2\right)\\ ...\\ m^T\left(n\right)\end{array}\right],M_0=\left[\begin{array}{c}\left|\right|m_0\left(1\right)|{|}^2\\ \left|\right|m_0\left(2\right)|{|}^2\\ ...\\ \left|\right|m_0\left(n\right)|{|}^2\end{array}\right]$

最小化残差平方和，令可得曲面参数向量A的最小二乘估计值是：

$\hat{A}={\&lsqb;{\mathrm{MM}}^T\&rsqb;}^{-1}{\mathrm{MM}}_0$

根据实验数据M，M₀即可以得到曲面参数向量A的估计值，从而完成磁传感器误差模型的建立。

5.根据权利要求2所述的方法，其特征在于，所述步骤3中，对三轴加速度信号进行窗口低通滤波，步骤具体如下：

本发明中对加速度信号用中值滤波器滤除数据中的高频噪声，设滤波器窗口A的长度l，观测值为x₁，x₂，…，x_N，N为观测值个数，即当窗口A在原始序列上移动时，中值滤波器设计如下：

在任意时刻窗口内的观测数据重新按照数值大小排序，当长度l为奇数时，取中间数据作为本次滤波器的输出。当长度l为偶数时，取中间两个数据的平均平均值作为本次滤波器的输出。

6.根据权利要求2所述的方法，其特征在于，所述步骤3中，对三轴角速度进行误差建模，并对其随机偏移进行误差补偿，步骤具体如下：

在一定测量时间内，陀螺仪的误差模型可用下式表示：

w_m＝w₀+ε+ε₀+δn(t)

w_m是陀螺仪的实际测量值,w₀是陀螺仪的理论值，ε是时变漂移，ε₀是常值零偏，n(t)是高斯白噪声，其强度为δ。

通过陀螺仪的数字可编程的低通滤波器，滤掉ε和δn(t)，而对于常值零偏ε₀，将陀螺仪静止一段时间，取静态下陀螺仪的均值输出作为常值零偏ε₀。

因此，陀螺仪校正后输出w₀可由下式来表示：

w₀＝w_m-ε₀。

7.根据权利要求2所述的方法，其特征在于，所述获取初始姿态角和四元数姿态矩阵的处理步骤具体如下：

首先，利用步骤3得到的窗口低通滤波后的加速度信号(a_x,a_y,a_z)通过三轴分量的关系求解手部的初始横滚角θ和俯仰角φ：

φ＝-arcsin(a_x)

θ＝-arctan2(a_y,a_z)

将步骤3处理得到的三轴磁场分量通过上述初始姿态角(横滚角和俯仰角)进行倾斜补偿，将手部坐标系上的磁场分量转换到地理坐标系，可得手部的初始航向角ψ如下：

ψ＝-arctan2(m_y0*cosθ+m_x0*sinθ*sinφ-m_z0*sinθ*cosφ,

m_x0*cosφ+m_z0*sinφ)

在获取初始姿态角和航向角以后，可以得到初始四元数的四个分量如下：

q₀＝cos(0.5θ)*cos(0.5φ)*cos(0.5ψ)+sin(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₁＝sin(0.5θ)*cos(0.5φ)*cos(0.5ψ)-cos(0.5θ)*sin(0.5φ)*sin(0.5ψ)

q₂＝cos(0.5θ)*sin(0.5φ)*cos(0.5ψ)+sin(0.5θ)*cos(0.5φ)*sin(0.5ψ)

q₃＝cos(0.5θ)*cos(0.5φ)*sin(0.5ψ)-sin(0.5θ)*sin(0.5φ)*cos(0.5ψ)

根据上述的四元数，可以得到四元数形式的姿态矩阵如下：

$\begin{array}{c}{C}_e^b\\ =\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-2q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]\end{array}$

8.根据权利要求2所述的方法，其特征在于，所述步骤5具体为：

设在地理坐标系下，手部对应的地磁场的矢量为表示为(m_exm_eym_ez)，重力场的矢量是在手部坐标系下，手部的地磁场的矢量是重力场的矢量是表示为(a_bxa_bya_bz)；将地理坐标系下的地磁场矢量和重力场矢量通过姿态矩阵转换到手部坐标系下，如果是在姿态矩阵理想无误差的情况下，则可得：

$\stackrel{\&RightArrow;}{m_b}={\stackrel{\&RightArrow;}{Q_e^b}}^{*}\&CircleTimes;\stackrel{\&RightArrow;}{m_e}\&CircleTimes;\stackrel{\&RightArrow;}{Q_e^b}$

$\stackrel{\&RightArrow;}{a_b}={\stackrel{\&RightArrow;}{Q_e^b}}^{*}\&CircleTimes;\stackrel{\&RightArrow;}{a_e}\&CircleTimes;\stackrel{\&RightArrow;}{Q_e^b}$

但由于姿态矩阵存在误差，因此在和和之间存在误差，和间的误差对应俯仰角和横滚角的误差e_θ,φ，和之间的误差对应航向角的误差。向量之间的误差可以用叉积表示。

对于重力场，加速度传感器测量的重力加速度在地理坐标系中只有竖直向下的方向，因此可设地理坐标下的重力场矢量将其通过姿态矩阵转换到手部坐标系下，即对应姿态矩阵的第三列元素。将该矢量与手部坐标系下加速度的测量值间的误差表示如下：

$e_{\mathrm{\&theta;},\mathrm{\&phi;}}=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_0{q}_1+q_2{q}_3)\\ 2(\frac{1}{2}-q_1^2-q_2^2)\end{array}\right]\&times;\left[\begin{array}{c}{a}_{bx}\\ a_{by}\\ a_{bz}\end{array}\right]$

对于地磁场，地理坐标下磁场传感器对应的地磁向量可以表示为：

${\left(m_{ex}{m}_{ey}{m}_{ez}\right)}^T=C_e^{b^{-1}}\stackrel{\&RightArrow;}{m_b}$

手部运动的航向角定义在地理水平面x-y，因此在地理坐标下的地磁矢量h＝(h_x h_yh_z)^T，其中h_y＝0，h_z＝m_ez。航向角的误差为地理坐标下地磁向量和观测向量的叉积：

$e_{\mathrm{\&psi;}}=C_e^b\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right]\&times;\stackrel{\&RightArrow;}{m_b}$

系统姿态向量的误差为：

e＝e_θ,φ+e_ψ

用磁传感器和加速度计补偿得到的系统姿态向量误差来作为PI调节器的输入来校准陀螺仪的偏差，t时刻校准后的陀螺仪角速度向量为：

$w_0=w_m+{\mathrm{\&lambda;K}}_{p}e\left(t\right)+{\mathrm{\&lambda;K}}_i{\mathrm{\&Sigma;}}_{j=0}^{t}e\left(j\right)$

其中，e(t)为t时刻系统姿态向量的误差，λ和采样频率有关系，一般设为采样周期的大小，w_m为陀螺仪输出的角速度向量，w₀为校准后的角速度向量。比例增益K_p控制低频段和高频段之间的截止频率，积分增益K_i控制滤波器消除误差的时间。通常，K_p和K_i的选择与截止频率f和阻尼系数ξ有关：

K_p＝2ξf

K_i＝f²

一般情况下，可选择阻尼系数ξ＝0.707，将手部运动形态划分为以下三种不同状态，根据不同的运动状态自适应调整截止频率：

1)静止状态:此时陀螺仪测量值w_m<0.02rad/s(1.7°/s)，设截止频率f＝0.05rad/s；

2)弱机动运动状态:此时0.02rad/s(1.7°/s)<w_m<0.524rad/s(30°/s)，可设截止频率f＝0.005rad/s；

3)高机动运动状态:此时w_m>0.524rad/s(30°/s)，截止频率设为0。

在获取校准后的陀螺仪角速度后，地理坐标系相对手部坐标系转动的姿态四元数变化率可表示为：

${\stackrel{\&CenterDot;}{Q}}_e^b=\frac{1}{2}{Q}_e^b\&CircleTimes;w_0$

当前时刻t的姿态四元数可由上一时刻t-1的姿态四元数加上姿态四元数变化率的积分而获得：

$Q_{e,t}^b=Q_{e,t-1}^b+{\stackrel{\&CenterDot;}{Q}}_e^b\mathrm{\&Delta;}t$

之后可根据当前姿态四元数获取传感节点的姿态和四元数形式的姿态矩阵：

$\mathrm{\&theta;}=\arctan 2(q_2{q}_3+q_0{q}_1)/(q_0^2-q_1^2-q_2^2+q_3^2)$

φ＝-arcsin2(q₁q₃-q₀q₂)

$\mathrm{\&psi;}=\arctan 2(q_1{q}_2+q_0{q}_3)/(q_0^2+q_1^2-q_2^2+q_3^2).$

9.根据权利要求2所述的方法，其特征在于，所述获取手部运动位置和速度的处理步骤具体如下：

将步骤5得到的加速度计的三轴加速度表示为(a_bxa_bya_bz)，通过姿态矩阵从手部坐标系下转换到地理坐标系下,并进行重力加速度补偿,得到补偿后的加速度表示为(a_xa_ya_z)如下：

$\stackrel{\&RightArrow;}{a}=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=C_e^{b^{-1}}\left[\begin{array}{c}{a}_{bx}\\ a_{by}\\ a_{bz}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$

根据加速度和速度、位移之间的关系，假设从初始时刻t₀时刻开始采样，采样周期为Δt，可得速度和位移如下：

$v_t=v_0+{\&Integral;}_0^t{a}_{t}dt$

$s_t=s_0+{\&Integral;}_0^t{v}_{t}dt$

其中，v₀和s₀分别是初始速度和初始位置，v_t、s_t分别是t时刻的速度、位移。之后采用Simpson积分法，对补偿后的加速度进行离散数字积分，得到速度和位移，计算方法如下：

$v_t=v_{t-1}+\frac{a_{t-1}+4a_t+a_{t+1}}{6}\mathrm{\&Delta;}t$

$s_t=s_{t-1}+\frac{v_{t-1}+4v_t+v_{t+1}}{6}\mathrm{\&Delta;}t$

其中，a_t表示t时刻重力补偿后的加速度。通过计算手部运动的实时速度和位移，即可获取对手部的实时追踪。