基于惯性动作捕捉技术的人体运动数字孪生的构建方法

1.本发明属于电子信息工程和计算机科学领域的人体运动数字孪生的构建方法，尤其设计一种基于惯性动作捕捉技术的人体运动数字孪生的构建方法。


背景技术：

2.在工业领域，数字孪生通过在信息空间中构建物理实体的虚拟模型，运用实时更新数据和历史状态数据，模拟物理实体在真实空间中的行为，用于监控、诊断、预测和控制物理实体在真实环境中的工作过程和状态。随着关键使能技术的推进和发展，数字孪生在物理系统和网络系统之间的持续性交互和连接应用上显示出巨大的潜力，逐渐渗透至其他领域中。
3.人类既是制造生产和服务应用活动中最具能动性和活力的关键因素，也是各种活动的最终服务目标，“以人为本”的制造生产和服务应用正逐渐引起学术界和工业界的普遍关注。目前传统的数字孪生系统的研究对象主要围绕机械物理系统而展开，缺少对“人”的关注和研究。
4.不同于传统的数字孪生系统，人体数字孪生旨在从基因、生理和行为等多维度构建人体的数字孪生体，动态跟踪物理世界中的人体变化，为监控分析人体和实现人机交互提供指导作用。目前人体数字孪生的研究多从遗传学和生物学的角度出发，用于模拟人体内的器官或微结构，例如心血管数字孪生，心脏数字孪生，提供个性化的医疗服务，缺少对人体的生理和行为状态的研究。人体运动数字孪生是从生理行为、人因工程的角度刻画人体的数字孪生模型，旨在通过数据采集设备获取人体在物理空间中的行为运动状态，信息空间的数字孪生模型映射跟踪人体的运动，分析人体关节参数，实现数字孪生模型的实时动态演化更新。


技术实现要素：

5.为了解决背景技术中存在的问题和需求，本发明提供了一种基于惯性动作捕捉技术的人体运动数字孪生的构建方法，采用的技术方案如下：
6.1)将多节点惯性动作捕捉设备穿戴于人体并利用多节点惯性动作捕捉设备采集人体各个关节对应传感器的原始传感数据，利用自适应扩展卡尔曼滤波算法对各个关节对应传感器的原始传感数据进行处理后，获得人体各个关节对应传感器的传感器姿态，将各个关节对应传感器的原始传感数据和对应的传感器姿态作为各个关节的传感数据并无线发送给上位机；
7.2)根据各个关节的传感数据、多节点惯性动作捕捉设备的安装位置和人体的骨架结构，上位机进行人体运动数字孪生模型的构建；基于各个关节的传感数据，再利用人体姿态校准方法计算获得各个关节的姿态校准矩阵，接着利用各个关节的姿态校准矩阵对对应关节的传感器姿态进行关节校准，获得各个关节的姿态，由各个关节的姿态组成人体姿态，最后根据人体姿态驱动上位机中的人体运动数字孪生模型；
8.3)多节点惯性动作捕捉设备实时采集并向上位机发送各个关节的实时传感数据，根据各个关节的实时姿态校准矩阵和实时传感数据计算人体关节参数，将关节参数与对应的参数阈值进行比较，根据比较结果迭代演化人体运动数字孪生模型，实现人体运动数字孪生模型的动态更新。
9.所述步骤1)中，多节点惯性动作捕捉设备中每个子节点的传感器采集人体关节对应传感器的角速度、加速度和磁场强度，由角速度、加速度和磁场强度组成当前关节对应传感器的原始传感数据。
10.所述步骤1)中，利用自适应扩展卡尔曼滤波算法对各个关节对应传感器的原始传感数据进行处理后，获得人体各个关节的传感器姿态，具体为：
11.s1：根据每个关节对应传感器的原始传感数据确定加速度-磁场强度参数，基于当前关节的加速度-磁场强度参数构建地理坐标系与当前关节的传感器坐标系之间的误差函数和代价函数，利用高斯-牛顿算法对当前关节的误差函数和代价函数进行求解后获得地理坐标系和当前关节的传感器坐标系之间的姿态四元数，并将地理坐标系和当前关节对应的传感器坐标系之间的姿态四元数作为当前关节对应的传感器初始姿态；
12.s2：将当前关节对应的传感器初始姿态与角速度作为自适应扩展卡尔曼滤波器的状态量并进行解算，获得当前关节对应的传感器滤波姿态；
13.s3：计算当前关节在当前时刻对应的传感器滤波姿态与其前一时刻的传感器滤波姿态之间的距离，以及当前关节在当前时刻对应的传感器滤波姿态的相反数与其前一时刻的传感器滤波姿态之间的距离，将两个距离中最小的距离对应的传感器滤波姿态作为当前关节在当前时刻的传感器姿态。
14.s4：重复s1-s3，对剩余关节的原始传感数据进行处理，获得人体各个关节的传感器姿态。
15.所述自适应扩展卡尔曼滤波器是通过对扩展卡尔曼滤波算法中量测噪声均值和量测噪声方差阵进行改进获得，引入遗忘因子和遗忘因子加权系数，动态更新量测噪声，量测噪声均值和量测噪声方差阵的公式如下：
[0016][0017][0018][0019][0020]
其中，i表示单位矩阵，表示第k个离散时间的量测噪声均值估计值，表示第k个离散时间量测噪声均值的遗忘因子加权系数，表示第k-1个离散时间量测噪声均值的遗忘因子加权系数，表示量测噪声均值的遗忘因子加权系数的初始值，表示第k-1个离散时间的量测噪声均值估计值，zk表示第k个离散时间的离散化的扩展卡尔曼滤波观测量，表示扩展卡尔曼滤波中的状态预测值，h表示扩展卡尔曼滤波中量测阵，表示第k个离散时间的量测噪声方差阵估计值，表示第k-1个离散时间的量测噪声方差阵估计
值，表示第k个离散时间量测噪声方差阵的遗忘因子加权系数，表示第k-1个离散时间量测噪声方差阵的遗忘因子加权系数，表示量测噪声方差阵的遗忘因子加权系数的初始值，e
k-i
和ek分别表示扩展卡尔曼滤波中第k-1个离散时间、第k个离散时间的量测估计误差，p
k/k-1
表示扩展卡尔曼滤波中根据第k-1个离散时间的估计均方误差推算的第k个离散时间的一步预测均方误差，b表示遗忘因子，n表示离散的时间长度，t表示矩阵转置，e表示自然常数，tr()表示矩阵的迹，c1、c2、c3表示第一、第二、第三系数，||表示绝对值运算。
[0021]
所述步骤2)中，利用人体姿态校准方法计算获得各个关节的姿态校准矩阵，具体为：
[0022]
首先，多节点惯性动作捕捉设备采集人体处于三种校准姿势下的传感数据，再根据三种校准姿势下的传感数据计算各个关节对应的初始姿态校准矩阵，接着利用各个关节的初始姿态校准矩阵对对应关节的传感器姿态进行关节校准，获得各个关节的姿态；最后，根据三种校准姿势的属性和各个关节的姿态，对各个关节对应的初始姿态校准矩阵进行校正，获得各个关节对应的姿态校准矩阵。
[0023]
所述各个关节对应的初始姿态校准矩阵均由身体坐标系的x轴向量、y轴向量和z轴向量组成，其中x轴向量的计算公式如下：
[0024]
x'
bcs
＝[acc'
x acc'
y acc'z]
[0025]
y轴向量的计算公式如下：
[0026][0027]
y”bcs
＝pca([gyr'
x
,gyr'y,gyr'z])
[0028][0029]
z轴向量的计算公式如下：
[0030][0031][0032]
z”'
bcs
＝pca([gyr”x
,gyr”y
,gyr”z
])
[0033]
其中，x'
bcs
表示身体坐标系的第一x轴向量，y'
bcs
、y”bcs
、y”'
bcs
分别表示身体坐标系的第一、第二、第三y轴向量，z'
bcs z
ꢀ”
bcs z”'
bcs
分别表示身体坐标系的第一、第二、第三z轴向量，acc'
x
、acc'y、acc'z分别表示人体处于第一校准姿势下每个关节对应的传感器测量获得的加速度在自身传感器坐标系的xyz轴的分量；acc”x
,acc”y
,acc”z
分别表示人体处于第二校准姿势下每个关节对应的传感器测量获得的加速度在自身传感器坐标系的xyz轴的分量；norm()表示归一化处理；||||表示欧几里得范数计算；pca()表示主成分分析法，gyr'
x
,gyr'y,gyr'z分别表示人体处于第二校准姿势下每个关节对应的传感器测量获得的角速度在自身传感器坐标系的xyz轴的分量；gyr”x
,gyr”y
,gyr”z
分别表示人体处于第三校准姿势下每个关节对应的传感器测量获得的角速度在自身传感器坐标系的xyz轴的分量；
[0034]
髋关节，脊柱，头部和左、右大腿的姿态校准矩阵的计算方式相同，由身体坐标系
的第一x轴向量、第一y轴向量和第一z轴向量组成；左、右小腿和左、右脚掌的姿态校准矩阵的计算方式相同，由身体坐标系的第一x轴向量、第二y轴向量和第二z轴向量组成；左、右大臂，左、右小臂和左、右手部的姿态校准矩阵的计算方式相同，由身体坐标系的第一x轴向量、第三y轴向量和第三z轴向量组成。
[0035]
所述根据三种校准姿势的属性和各个关节的初始姿态，对各个关节对应的初始姿态校准矩阵进行校正，获得各个关节对应的姿态校准矩阵，具体为：
[0036]
根据人体处于第一校准姿势下的传感数据计算左、右肘关节，脊柱和左、右膝关节的关节角，基于计算左、右肘关节，脊柱和左、右膝关节的关节角，再根据第一校准姿势的属性分别对左、右大臂与左、右小臂，头与髋关节，左、右大腿与左、右小腿的初始姿态校准矩阵进行校正，具体公式如下：
[0037]
proximalcor＝vec2mat(acc
proximal
,gravityvec)
[0038]
proximalrotm

＝proximalcor*proximalrotm-[0039]
其中，proximalcor为每个关节的加速度向量与重力向量之间的相对旋转矩阵，vec2mat()是将向量转化为矩阵形式，acc
proximal
表示每个关节的加速度向量，gravityvec表示每个关节的重力向量；proximalrotm-为每个关节的初始姿态校准矩阵，proximalrotm

为每个关节的校正后的姿态校准矩阵；
[0040]
再根据人体处于第二和第三校准姿势下的传感数据计算膝关节和肩关节的自由度，基于膝关节和肩关节的自由度，再根据第二和第三校准姿势的属性以及左、右大臂，左、右小臂，左、右大腿与左、右小腿的初始姿态对左、右大臂，左、右小臂，左、右大腿与左、右小腿的校正后的姿态校准矩阵进行补偿，获得对应关节的最终的姿态校准矩阵，具体计算公式如下：
[0041]
angleoffset＝(computeangle(proximalqua,distalqua))/2，
[0042]
proximalrotm
'
＝ang2mat(-angleoffset)*proximalrotm

[0043]
其中，angleoffset表示肩关节前屈/后伸自由度、肩关节水平内收/外展自由度或者膝关节内旋/外旋自由度的关节角度的误差，proximalqua表示左、右大臂或者左、右大腿的初始姿态，distalqua表示左、右小臂或者左、右小腿的初始姿态，computeangle()表示将姿态转化为角度形式，ang2mat()表示将角度转化为矩阵形式，proximalrotm
'
为每个关节的最终的姿态校准矩阵。
[0044]
所述三种校准姿势分别为第一校准姿势、第二校准姿势和第三校准姿势；
[0045]
第一校准姿势：躯干笔直站立，双臂自然下垂且放于身体两侧，手掌朝向身体，双脚与肩同宽，脚掌相互平行且朝向身体前方；
[0046]
第二校准姿势：保持坐姿，上肢躯干前倾，手肘和小臂平放在桌面上，手肘间距离与肩同款，小臂保持平行，大腿平放在椅子上，与地面保持平行，左右小腿分别绕膝盖同频率前后摆动；
[0047]
第三校准姿势：保持坐姿，双臂在身体左右两侧从竖直向下平举至与肩齐高，手掌朝向地面，上肢呈“t”型，大腿平放在椅子上，与地面保持平行，小腿与地面呈90
°
，双脚与肩同宽，脚掌相互平行且朝向身体前方。
[0048]
所述步骤3)中，上位机在实时驱动人体运动数字孪生模型的同时，计算人体关节参数，并将关节参数与对应的参数阈值范围进行比较，如果比较结果超出对应阈值范围，上
位机提示人体停止运动并提醒检查真实人体当前运动状态是否与正常生理运动状态相符；如果真实人体当前运动状态正常，则动态更新人体运动数字孪生模型，提示人体执行三种校准姿态，更新人体各个关节的姿态校准矩阵，重新驱动人体运动数字孪生模型，进而实现人体运动数字孪生模型的动态演化。
[0049]
本发明有益效果在于：
[0050]
本发明提供一种人体运动数字孪生构建方法，采用自适应扩展卡尔曼滤波算法，动态更新量测噪声参数，融合惯性传感数据解算传感器姿态，提高姿态解算精度；结合姿态校准姿势和关节角属性，获取人体姿态校准矩阵，减小由于校准姿势不准确或传感器穿戴位置偏移导致的误差；利用解算后的人体关节姿态同步驱动和动态更新演化人体数字模型，能够实现在人体数字模型映射人体运动的同时，同步演化更新人体运动数字孪生模型，既保证了数字模型的稳定正确运行又实现了模型的动态更新。
附图说明
[0051]
图1为本发明的算法流程示意图；
[0052]
图2为本发明的硬件框架示意图；
[0053]
图3为本发明的人体运动数字孪生模型的迭代演化流程框图。
[0054]
图4为本发明中穿戴惯性动作捕捉设备的人和人体运动数字孪生模型。
具体实施方式
[0055]
下面结合附图对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。在详述本发明实施例时，为便于说明，示意图只是实例，其在此不应限制本发明保护的范围。
[0056]
本发明的一种人体运动数字孪生系统主要包括硬件和软件两部分。惯性动作捕捉设备通过固定在头部、左右大臂、左右小臂、脊柱、髋关节、左右大腿、左右小腿和左右脚部的惯性传感子节点，采集和解算传感数据，并发送至上位机。上位机在接收传感数据前，首先基于人体骨架结构，通过计算机渲染引擎构建人体数字模型。在人体姿态校准阶段，上位机根据校准姿势的运动数据解算人体关节的姿态校准矩阵，并存储在上位机端，然后，利用姿态校准矩阵对从子节点接收的传感器姿态矩阵进行校准，计算人体关节的实时姿态矩阵，实时驱动人体运动数字孪生模型。最后，在实时驱动模型的同时，上位机基于人体关节的实时姿态矩阵和传感数据计算人体关节参数，结合参数的阈值范围，比较判断当前人体运动数字孪生模型的状态是否正常，对人体数字模型进行迭代演化，动态更新人体数字模型。
[0057]
图1为本发明实施例提供的人体运动的数字孪生系统构建方法的实现流程图，如图1所示，本发明包括以下步骤：
[0058]
利用多节点惯性动作捕捉设备采集人体关节对应传感器的传感数据，利用无线通信将人体关节对应传感器的传感数据传输至上位机中，上位机中构建人体数字模型，利用解算后的人体关节姿态数据驱动人体数字模型，具体地：
[0059]
1)将多节点惯性动作捕捉设备穿戴于人体并利用多节点惯性动作捕捉设备采集人体各个关节对应传感器的原始传感数据，利用自适应扩展卡尔曼滤波算法对各个关节对应传感器的原始传感数据进行处理后，获得人体各个关节对应传感器的传感器姿态，将各个关节对应传感器的原始传感数据和对应的传感器姿态作为各个关节的传感数据并以tcp协议无线发送给上位机；
[0060]
具体实施中，多节点惯性动作捕捉设备包括一个主节点和多个子节点，主节点设置在髋关节，各个子节点对应各个人体关节的传感器，主节点通过向子节点下达命令，实现数据开启/关闭接收、节点重启、特定数据接收等功能，各个关节对应传感器的传感数据封装为传感数据帧后发送给主节点，主节点再通过无线通信将汇总后的所有子节点的传感数据传输至上位机；
[0061]
步骤1)中，多节点惯性动作捕捉设备中每个子节点的传感器中陀螺仪、加速度计和磁力计采集人体关节对应传感器的角速度、加速度和磁场强度，由角速度、加速度和磁场强度组成当前关节对应传感器的原始传感数据。
[0062]
步骤1)中，利用自适应扩展卡尔曼滤波算法对各个关节对应传感器的原始传感数据进行处理后，获得人体各个关节的传感器姿态，具体为：
[0063]
s1：根据每个关节对应传感器的原始传感数据确定加速度-磁场强度参数，加速度-磁场强度参数记为z
mea
，满足z
mea
＝[acc
x acc
y acc
z mag
x mag
y magz]，其中，acc
x
，accy，accz分别为每个关节对应的传感器测量获得的加速度在自身传感器坐标系的xyz轴的分量，mag
x
，magy，magz分别为每个关节对应的传感器测量获得的磁场强度在自身传感器坐标系的xyz轴的分量。基于当前关节的加速度-磁场强度参数构建地理坐标系与当前关节的传感器坐标系之间的误差函数和代价函数，利用高斯-牛顿算法对当前关节的误差函数和代价函数进行求解后获得地理坐标系和当前关节的传感器坐标系之间的姿态四元数，将地理坐标系和当前关节对应的传感器坐标系之间的姿态四元数作为当前关节对应的传感器初始姿态；
[0064]
本发明中使用四元数表示各个关节的传感器姿态，已知g表示重力加速度，m
ex
，m
ey
，m
ez
表示地球磁场强度在地理坐标系的x，y，z坐标轴上的分量，则地理坐标系下地球重力加速度-磁场强度参数z
earth
表示为：
[0065]zearth
＝[0 0g m
ex m
ey m
ez
]
[0066]
每个关节通过加速度计和磁力计测量的加速度和磁场强度在自身传感器坐标系的xyz轴的分量分别为acc
x
，accy，accz和mag
x
，magy，magz，当前关节对应的加速度-磁场强度参数z
mea
的公式如下：
[0067]zmea
＝[acc
x acc
y acc
z mag
x mag
y magz]
[0068]
根据当前关节对应的加速度-磁场强度参数z
mea
构建误差函数ε(q)和代价函数f(q)，公式如下：
[0069][0070][0071]
其中，q为地理坐标系和当前关节的传感器坐标系之间的姿态四元数，r(q)表示姿态四元数q对应的姿态旋转矩阵，表示四元数乘法，t表示矩阵转置。
[0072]
再采用高斯-牛顿算法解算获取地理坐标系和传感器坐标系之间的姿态四元数。
[0073]
s2：将当前关节对应的传感器初始姿态与角速度作为自适应扩展卡尔曼滤波器的状态量并进行解算，获得当前关节对应的传感器滤波姿态；
[0074]
自适应扩展卡尔曼滤波器是通过对扩展卡尔曼滤波算法中量测噪声均值和量测噪声方差阵进行改进获得，引入遗忘因子和遗忘因子加权系数，动态更新量测噪声，量测噪声均值和量测噪声方差阵的公式如下：
[0075][0076][0077][0078][0079]
其中，i表示单位矩阵，表示第k个离散时间的量测噪声均值估计值，表示第k个离散时间量测噪声均值的遗忘因子加权系数，表示第k-1个离散时间量测噪声均值的遗忘因子加权系数，表示量测噪声均值的遗忘因子加权系数的初始值，表示第k-1个离散时间的量测噪声均值估计值，zk表示第k个离散时间的离散化的扩展卡尔曼滤波观测量，表示扩展卡尔曼滤波中的状态预测值，h表示扩展卡尔曼滤波中量测阵，表示第k个离散时间的量测噪声方差阵估计值，表示第k-1个离散时间的量测噪声方差阵估计值，表示第k个离散时间量测噪声方差阵的遗忘因子加权系数，表示第k-1个离散时间量测噪声方差阵的遗忘因子加权系数，表示量测噪声方差阵的遗忘因子加权系数的初始值，e
k-i
和ek分别表示扩展卡尔曼滤波中第k-1个离散时间、第k个离散时间的量测估计误差，p
k/k-1
表示扩展卡尔曼滤波中根据第k-1个离散时间的估计均方误差推算的第k个离散时间的一步预测均方误差，b表示遗忘因子，n表示离散的时间长度，t表示矩阵转置，e表示自然常数，tr()表示矩阵的迹，c1、c2、c3表示第一、第二、第三系数，为常数，||表示绝对值运算。
[0080]
具体地：
[0081]
基于加速度和磁场强度，本发明估算地理坐标系和传感器坐标系之间的传感器初始姿态，结合测量的角速度，以传感器初始姿态与角速度作为状态量，构建自适应扩展卡尔曼滤波器。
[0082]
系统状态方程：选择在时间t测量的角速度和基于高斯-牛顿算法解算的姿态四元数构建系统的状态向量x(t)和状态向量的微分
[0083][0084]
其中，ω＝[ω
x ω
y ωz]，q＝[q
0 q
1 q
2 q3]，w，τ＝[τ
x τ
y τz]依次表示角速度，四元数，高斯白噪声和模型参数，表示四元数乘法。本发明中取状态向量中角速度的初值为
[0.001 0.001 0.001]，四元数的初值为通过高斯-牛顿算法迭代更新后的四元数初始值，模型参数τ＝[0.04 0.04 0.04]。
[0085]
对上述非线性系统进行线性化和离散化，首先对状态向量围绕其最优状态估计进行线性化，因此真轨迹x(t)与标称轨迹的偏差δx(t)为：
[0086][0087]
将非线性物理系统的状态方程在状态最优估计附近展开为泰勒级数，获取对应的偏差函数为：
[0088][0089][0090]
δt表示离散化的采样时间间隔，表示标称轨迹下的角速度，表示标称轨迹下的四元数，e表示自然常数，
[0091]
并对上式进行离散化。
[0092]
观测方程：以角速度和四元数作为观测量，构建扩展卡尔曼滤波器的观测量z(t)，并离散化为zk，
[0093]
zk＝hxk vk[0094]
其中，ω(t)为时间t时的角速度，q(t)为时间t时的四元数，具体实施中，h为单位矩阵，xk为离散时间k时的状态量，vk为离散时间k时的量测噪声。
[0095]
状态参量初始化和估计：根据确定的系统状态方程和观测方程，获取滤波估计的一步预测均方误差p
k/k-1
及其估计均方误差pk为，
[0096][0097]
其中，φ
k,k-1
为第k-1个离散时间至第k个离散时间的状态转移矩阵，q
k-1
为第k-1个离散时间的系统噪声的方差阵，i为单位矩阵，hk为第k个离散时间的量测阵，kk为第k个离散时间的滤波增益，rk为第k个离散时间的量测噪声的方差阵，t表示转置。初始值p0为对称矩阵且正定，设定其初值为：
[0098]
p0＝diag(0.0001 0.0001 0.0001 0.001 0.001 0.001 0.001)
[0099]
扩展卡尔曼滤波增益kk为：
[0100][0101]
各传感子节点的系统噪声参数相对较为稳定，通过建模确定相应的参数，
[0102][0103]
其中，m为采样周期，a2为3
×
4的零矩阵，a3为4
×
3的零矩阵，a4为4
×
4的零矩阵，系统噪声均值参数q初始化为7
×
1的零矩阵。
[0104]
量测噪声主要由外部环境导致，易发生变化，因此采用改进的自适应算法处理，量测噪声协方差初始值，
[0105][0106]
均值参数r初始化为7
×
1的零矩阵。
[0107]
鉴于随机系统的模型误差会影响输出，因此，在进行状态估计的同时，通过量测输出实时估计噪声参数。由于系统噪声取决于传感系统自身，参数相对较为稳定，因此通过建模确定相应的参数，而量测噪声主要由外部环境导致，容易发生变化。因此本发明对量测噪声采用改进的自适应算法处理，基于上述滤波，由第k个离散时间的一步预测状态量代替第k个离散时间的真实状态量xk，引起量测的估计误差ek为：
[0108][0109]
其中，zk为第k个离散时间的系统量测量，为基于第k-1个离散时间的预测状态量推算的第k个离散时间的估计量测量。
[0110]
考虑实际噪声参数处于缓慢变化的状态，对噪声参数采用指数渐消记忆加权平均算法，引入遗忘因子b和遗忘因子加权系数，并针对量测噪声的均值rk，量测噪声的方差阵rk采用特定的遗忘因子，rk对应的遗忘因子加权系数为rk对应的遗忘因子加权系数为
[0111][0112]
其中，为离散时间k、k-1时量测噪声的均值估计值，
[0113]
由于与zk密切相关，容易受到测量信息的干扰，因此本算法中对其进行平滑估计：
[0114][0115]
其中，为离散时间k、k-1时量测噪声的方差阵估计值，
[0116]
b的取值决定了权重的不同分配，当b越小，则新近数据的权重越大，陈旧数据的权重越小，为了捕捉惯性传感单元在不同运动状态下(高速运动和低速运动)的姿态，本算法采样自适应遗忘因子，估计不同动态环境下的量测噪声。将通过新息获取的协方差与理论协方差进行比较，用于判断异常值的出现和噪声估计的稳定性，即：
[0117][0118]
为保证滤波器的稳定性，当量测噪声稳定的时候，遗忘因子b应取较大值，当量测噪声不稳定的时候，b应取较小值，c3为常数，因此，本发明设定自适应遗忘因子为：
[0119][0120]
c1和c2的取值可以动态调整。
[0121]
s3：四元数与其相反数表示的是相同的姿态信息，即[q
0 q
1 q
2 q3]和[-q0ꢀ‑
q1ꢀ‑
q2ꢀ‑
q3]表示为相同的姿态。为平滑任意相邻两个时刻的四元数间的变换轨迹，计算当前关节在当前时刻对应的传感器滤波姿态与其前一时刻的传感器滤波姿态之间的距离，以及当前关节在当前时刻对应的传感器滤波姿态的相反数与其前一时刻的传感器滤波姿态之间的距离，将两个距离中最小的距离对应的传感器滤波姿态作为当前关节在当前时刻的传感器姿态。
[0122]
s4：重复s1-s3，对剩余关节的原始传感数据进行处理，获得人体各个关节的传感器姿态。
[0123]
2)上位机按照传感数据帧格式解析传感数据，获得各个关节的传感数据；根据各个关节的传感数据、多节点惯性动作捕捉设备的安装位置和人体的结构，上位机进行人体运动数字孪生模型的构建；基于各个关节的传感数据，再利用人体姿态校准方法计算获得各个关节的姿态校准矩阵，并存储在本地；接着利用各个关节的姿态校准矩阵对对应关节的传感器姿态进行关节校准，获得各个关节的姿态(即实时姿态矩阵)，将每个关节的姿态校准矩阵与对应关节的实时传感器姿态相乘即为关节的实时姿态，由各个关节的姿态组成人体姿态，最后根据人体姿态驱动上位机中的人体运动数字孪生模型；
[0124]
步骤2)中，利用人体姿态校准方法计算获得各个关节的姿态校准矩阵，具体为：
[0125]
人体的各个关节包括头部，左、右大臂、左、右小臂，左、右手部，脊柱，髋关节，左、右大腿，左、右小腿和左、右脚掌。
[0126]
人体的身体坐标系的建立方式：
[0127]
以髋关节为原点，当人体保持第一校准姿势时，x轴在人体矢状面上，与垂直轴重合，竖直向上；y轴在人体冠状面上，与冠状轴重合，指向人体右侧；z轴在人体水平面上，与矢状轴重合，指向人体前侧，身体坐标系的xyz轴指向会跟随人体的运动动态变化。
[0128]
首先，多节点惯性动作捕捉设备采集人体处于三种校准姿势下的传感数据，再根据三种校准姿势下的传感数据计算各个关节对应的初始姿态校准矩阵，三种校准姿势分别为第一校准姿势、第二校准姿势和第三校准姿势；
[0129]
第一校准姿势：躯干笔直站立，双臂自然下垂且放于身体两侧，手掌朝向身体，双脚与肩同宽，脚掌相互平行且朝向身体前方；
[0130]
第二校准姿势：保持坐姿，上肢躯干前倾，手肘和小臂平放在桌面上，手肘间距离与肩同款，小臂保持平行，大腿平放在椅子上，与地面保持平行，左右小腿分别绕膝盖同频率前后摆动；
[0131]
第三校准姿势：保持坐姿，双臂在身体左右两侧从竖直向下平举至与肩齐高，手掌朝向地面，上肢呈“t”型，大腿平放在椅子上，与地面保持平行，小腿与地面呈90
°
，双脚与肩同宽，脚掌相互平行且朝向身体前方。
[0132]
各个关节对应的初始姿态校准矩阵均由身体坐标系的x轴向量、y轴向量和z轴向量组成，其中x轴向量的计算公式如下：
[0133]
x'
bcs
＝[acc'
x acc'
y acc'z]
[0134]
y轴向量的计算公式如下：
[0135][0136]
y”bcs
＝pca([gyr'
x
,gyr'y,gyr'z])
[0137][0138]
z轴向量的计算公式如下：
[0139][0140][0141]
z”'
bcs
＝pca([gyr”x
,gyr”y
,gyr”z
])
[0142]
其中，x'
bcs
表示身体坐标系的第一x轴向量，y'
bcs
、y”bcs
、y”'
bcs
分别表示身体坐标系的第一、第二、第三y轴向量，z'
bcs z”bcs z”'
bcs
分别表示身体坐标系的第一、第二、第三z轴向量，acc'
x
、acc'y、acc'z分别表示人体处于第一校准姿势下每个关节对应的传感器测量获得的加速度在自身传感器坐标系的xyz轴的分量；acc”x
,acc”y
,acc”z
分别表示人体处于第二校准姿势下每个关节对应的传感器测量获得的加速度在自身传感器坐标系的xyz轴的分量；norm()表示归一化处理；||||表示欧几里得范数计算；pca()表示主成分分析法，gyr'
x
,gyr'y,gyr'z分别表示人体处于第二校准姿势下每个关节对应的传感器测量获得的角速度在自身传感器坐标系的xyz轴的分量；gyr”x
,gyr”y
,gyr”z
分别表示人体处于第三校准姿势下每个关节对应的传感器测量获得的角速度在自身传感器坐标系的xyz轴的分量；
[0143]
髋关节，脊柱，头部和左、右大腿的姿态校准矩阵的计算方式相同，由身体坐标系的第一x轴向量、第一y轴向量和第一z轴向量组成；左、右小腿和左、右脚掌的姿态校准矩阵的计算方式相同，由身体坐标系的第一x轴向量、第二y轴向量和第二z轴向量组成；左、右大臂，左、右小臂和左、右手部的姿态校准矩阵的计算方式相同，由身体坐标系的第一x轴向量、第三y轴向量和第三z轴向量组成。
[0144]
接着利用各个关节的初始姿态校准矩阵对对应关节的传感器姿态进行关节校准，
获得各个关节的初始姿态；最后，根据三种校准姿势的属性和各个关节的初始姿态，对各个关节对应的初始姿态校准矩阵进行校正，获得各个关节对应的姿态校准矩阵，具体为：
[0145]
根据人体处于第一校准姿势下的传感数据计算左、右肘关节，脊柱和左、右膝盖的关节角，基于计算的左、右肘关节，脊柱和左、右膝盖的关节角，再根据第一校准姿势的属性分别对左、右大臂与左、右小臂，头与髋关节，左、右大腿与左、右小腿的初始姿态校准矩阵进行校正，肘关节的关节角由大臂与小臂组成，脊柱的关节角由头和髋关节组成，膝盖的关节角由大腿和小腿组成。具体公式如下：
[0146]
proximalcor＝vec2mat(acc
proximal
,gravityvec)
[0147]
proximalrotm

＝proximalcor*proximalrotm-[0148]
其中，proximalcor为每个关节的加速度向量与重力向量之间的相对旋转矩阵，vec2mat()是将向量转化为矩阵形式，acc
proximal
表示每个关节的加速度向量，为[acc
x acc
y accz]，gravityvec表示每个关节的重力向量；proximalrotm-为每个关节的初始姿态校准矩阵，proximalrotm

为每个关节的校正后的姿态校准矩阵；
[0149]
第一校准姿势的属性即人体处于直立状态时，肘关节、脊柱、膝盖处关节角的理论值为0
°
。
[0150]
再根据人体处于第二和第三校准姿势下的传感数据计算膝关节和肩关节的自由度，基于膝关节和肩关节的自由度，再根据第二和第三校准姿势的属性以及左、右大臂，左、右小臂，左、右大腿与左、右小腿的初始姿态对左、右大臂，左、右小臂，左、右大腿与左、右小腿的校正后的姿态校准矩阵进行补偿，获得对应关节的最终的姿态校准矩阵，具体计算公式如下：
[0151]
angleoffset＝(computeangle(proximalqua,distalqua))/2，
[0152]
proximalrotm
'
＝ang2mat(-angleoffset)*proximalrotm

[0153]
其中，angleoffset表示肩关节前屈/后伸自由度、肩关节水平内收/外展自由度或者膝关节内旋/外旋自由度的关节角度的误差，proximalqua表示左、右大臂或者左、右大腿的初始姿态，distalqua表示左、右小臂或者左、右小腿的初始姿态，computeangle()表示将姿态转化为角度形式，ang2mat()表示将角度转化为矩阵形式，proximalrotm
'
为每个关节的最终的姿态校准矩阵。
[0154]
在第二校准姿势和第三校准姿势中，将手臂绕肩的旋转运动和小腿绕膝盖的旋转运动简化为单一自由度运动，即膝关节的屈伸和肩关节的外展内收。膝关节的屈伸和肩关节其他自由度的角度理论值为0
°
，膝关节的其他自由度包括内旋/外旋，肩关节的其他自由度包括前屈后伸和水平内收/外展。
[0155]
3)多节点惯性动作捕捉设备实时采集并向上位机发送各个关节的实时传感数据，根据人体关节的实时姿态校准矩阵和传感数据计算人体关节参数，将关节参数与对应的参数阈值进行比较，根据比较结果迭代演化人体运动数字孪生模型，实现人体运动数字孪生模型的动态更新。人体关节参数主要包括角度，位移，角位移，速度，角速度，加速度和角加速度等。
[0156]
步骤3)中，上位机在实时驱动人体运动数字孪生模型的同时，计算人体关节参数，并将关节参数与对应的参数阈值范围进行比较，如果比较结果超出对应阈值范围，可能是人体自身运动姿态不当或者关节遭遇外力干扰，上位机提示人体停止运动并提醒检查真实
人体当前运动状态是否与正常生理运动状态相符；如果真实人体当前运动状态正常，即反馈结果排除人为或者外力干扰导致的异常，则说明当前人体数字模型无法正确反映真实人体运动状态，则动态更新人体运动数字孪生模型，提示人体执行三种校准姿态，更新人体各个关节的姿态校准矩阵，重新驱动人体运动数字孪生模型，进而实现人体运动数字孪生模型的动态演化，继续执行上述步骤，直至退出人体运动数字孪生系统。
[0157]
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
[0158]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。