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

技术特征：

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{\Σ}}_{i=1}^n{\mathrm{\ϵ}}^2\right(i\left)\right)={\mathrm{\Σ}}_{i=1}^n{\[m^T\left(i\right)A-\left|\right|m_0\left(i\right)|{|}^2\]}^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}={\[{\mathrm{MM}}^T\]}^{-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{\→}{m_b}={\stackrel{\→}{Q_e^b}}^{*}\⊗\stackrel{\→}{m_e}\⊗\stackrel{\→}{Q_e^b}$

$\stackrel{\→}{a_b}={\stackrel{\→}{Q_e^b}}^{*}\⊗\stackrel{\→}{a_e}\⊗\stackrel{\→}{Q_e^b}$

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

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

$e_{\mathrm{\θ},\mathrm{\φ}}=\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]\×\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{\→}{m_b}$

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

$e_{\mathrm{\ψ}}=C_e^b\left[\begin{array}{c}{h}_x\\ h_y\\ h_z\end{array}\right]\×\stackrel{\→}{m_b}$

系统姿态向量的误差为：

e＝e_θ,φ+e_ψ

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

$w_0=w_m+{\mathrm{\λK}}_{p}e\left(t\right)+{\mathrm{\λK}}_i{\mathrm{\Σ}}_{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时刻重力补偿后的加速度。通过计算手部运动的实时速度和位移，即可获取对手部的实时追踪。