[发明专利]一种运动员动态姿态识别方法、装置、系统及存储介质

权利要求书

1.一种运动员动态姿态识别方法，所述方法包括：

步骤S101：采集运动员若干身体指定部位p_i的加速度{ax_i,ay_i,az_i}、角速度{gx_i,gy_i,gz_i}和磁场方向{mx_i,my_i,mz_i}，1≤i≤n，n为指定部位的总数；

步骤S102：基于每一个身体指定部位p_i的采集数据，根据四元数与姿态阵间的关系，计算该身体指定部位对应的全姿态角信息，包括航向角、俯仰角、横滚角，所述四元数记为{q₀,q₁,q₂,q₃}，q₀为该四元数的标量部分，q₁、q₂、q₃为该四元数的矢量部分信 息；即能够通过四元数求出一个物体相对于一个坐标系旋转后的坐标信息；

步骤S103：基于各个身体指定部位对应的全姿态角信息，获取该运动员的全身整体全姿态角信息；

其中，所述步骤S102包括：

步骤S1021：对身体指定部位p_i的采集数据中的加速度{ax_i,ay_i,az_i}、磁场方向{mx_i,my_i,mz_i}进行归一化处理，将三维向量转换为单位向量；

步骤S1022：根据四元数换算方向余弦矩阵与欧拉角换算方向余弦矩阵的等效关系，获取重力分量{Vx_i,Vy_i,Vz_i}、地磁分量{Wx_i,Wy_i,Wz_i}，所述重力分量表示重力加速度g随着位移X、Y、Z变化的情况，包括：

计算地球磁场的参考方向bx_i,bz_i：

hx_i＝2.0f*(mx_i*(0.5f-q_2iq_2i-q_3iq_3i)+my_i*(q_1iq_2i-q_0iq_3i)+mz_i*(q_1iq_3i+q_0iq_2i))

hy_i＝2.0f*(mx_i*(q_1iq_2i+q_0iq_3i)+my_i*(0.5f-q_1iq_1i-q_3iq_3i)+mz_i*(q_2iq_3i-q_0iq_1i))

bz_i＝2.0f*(mx_i*(q_1iq_3i-q_0iq_2i)+my_i*(q_2iq_3i+q_0iq_1i)+mz_i(0.5f-q_1iq_1i-q_2iq_2i))

其中，hx_i为地磁北向分量，hy_i为地磁东向分量，bx_i为水平分量，bz_i为垂直分量,f为浮点数标志；

分离重力分量：

vx_i＝q_1iq_3i-q_0iq_2i

vy_i＝q_0iq_1i+q_2iq_3i

vz_i＝q_0iq_0i-0.5f+q_3iq_3i (6)

分离地磁分量：

wx_i＝bx_i(0.5f-q_2iq_2i-q_3iq_3i)+bz_i(q_1iq_3i-q_0iq_2i)

wy_i＝bx_i(q_1iq_2i+q_0iq_3i)+bz_i(q_0iq_1i-q_2iq_3i)

wz_i＝bx_i(q_0iq_2i+q_1iq_3i)+bz_i(0.5f-q_1iq_1i-q_2iq_2i) (7)

通过向量叉乘的方式计算姿态误差{ex_i,ey_i,ez_i}：

ex_i＝(ay_i*vz_i-az_i*vy_i)+(my_i*wz_i-mz_i*wy_i)

ey_i＝(az_i*vx_i-ax_i*vz_i)+(mz_i*wx_i-mx_i*wz_i)

ez_i＝(ax_i*vy_i-ay_i*vx_i)+(mx_i*wy_i-my_i*wx_i) (8)

对姿态误差{ex_i,ey_i,ez_i}进行积分，消除误差，积分后的结果为{accex_i,accey_i,accez_i}；

其中，accex_i,accey_i,accez_i分别对应在x,y,z轴的误差；

将姿态误差补偿到角速度上，修正角速度{gx_i,gy_i,gz_i}：

gx_i＝gx_i+Kp_i*ex_i*accex_i

gy_i＝gy_i+Kp_i*ey_i*accey_i

gz_i＝gz_i+Kp_i*ez_i*accez_i (10)

其中，Kp_i为互补滤波系数；

对修正后的角速度{gx_i,gy_i,gz_i}进行归一化处理，得到归一化后的修正角速度{gx'_i,gy'_i,gz'_i}：

四元数的初始值为q_0i＝1，q_1i＝0，q_2i＝0，q_3i＝0；建立部位p_i所在坐标b_i相对于地磁坐标E的姿态改变速率的四元数微积分方程，计算该四元数微积分方程；根据归一化后的修正角速度{gx'_i,gy'_i,gz'_i}更新四元数，Δt为角速度更新周期时间；

对更新后的四元数进行归一化处理，得到归一化后的四元数：

根据所述归一化后的四元数，解算部位p_i的全姿态角信息：

γ_i＝-arcsin(2(q'_1iq'_3i-q'_0iq'_2i))