人机协作中人体实时室内定位与运动姿态捕捉方法与系统

人机协作中人体实时室内定位与运动姿态捕捉方法与系统

摘要：一种人机协作中人体实时室内定位与运动姿态捕捉方法及系统，该方法包括以下步骤：(1)获取人体在地理坐标系下的位姿信息，并将UWB传回的位置坐标转换到地理坐标系下；(2)根据足部触地情况，使用正运动学树推算跟点坐标；(3)将基于惯性传感器网络计算的跟点坐标和UWB传回的标签坐标进行数据融合；(4)根据融合后的跟点坐标值计算全身躯干姿态信息和位置信息；该系统包括数据接收模块、坐标转换模块、数据融合模块和姿态推算模块。本发明由超宽带定位技术实时获得固定在跟点的标签三维坐标信息，通过无迹卡尔曼滤波融合得到了更加平滑准确的跟点坐标，使距离和位置的精准确定，实现了人体运动姿态的实时准确跟踪和显示。

专利类型：发明专利

专利申请号：CN202110133657.5

专利申请（专利权）人：山东大学
权利人地址：山东省济南市历下区经十路17923号

专利发明（设计）人：周乐来,杨苗,李贻斌,田新诚,李雪

主权利要求:人机协作中人体实时室内定位与运动姿态捕捉方法与系统
技术领域
[0001] 本发明涉及人机协作技术领域，具体而言是在人机协作过程中对测试人员的三维定位以及姿态跟踪。
背景技术
[0002] 在人机协作过程中，对人体动作姿态的精确捕捉和定位尤为重要，直接关乎到协作过程中人员的安全问题。现有的动作捕捉技术分为光学式、机械式、电磁式、声学式以及惯性传感式，光学式运动捕捉技术最为成熟，但价格高昂，且受光照影响严重；机械式捕捉系统采用机械结构，较为笨重，且很难完成校准；电磁运动捕捉系统通过所在位置的场强和相位确定其所在位置，容易受到空间内磁场干扰，降低估计精度；超声波动作捕捉系统通过超声波飞行时间计算各传感器空间位置，但需在视距范围内，易受传播环境和路径的影响；惯性传感器由于其不需要外部辅助设备、质量轻、体积小，不怕遮挡，对光、环境要求比较低，因而受到广泛应用。基于惯性传感器的动作捕捉系统只需要在身体指定躯干佩戴惯性传感器就可以完成姿态数据的采集，通过对加速度的二重积分获得位移。虽然惯性传感器能够非常精确地确定每个关节的旋转，但是随着使用时间的增加，加速度积分产生的累计误差将越来越大，并不能实现人体准确位移。

[0003] 目前已有的定位方式中，包括了超声波定位、蓝牙定位、红外线定位、GPS定位、超宽带定位以及惯导定位等等。在上述的定位方式中超声波依据到达时间原理实现定位，定位实时性较差，蓝牙定位的功耗一般比较小而且易于与当前设备集成，但是其定位精度稳定性较差，容易被干扰。红外线定位限制了被定位目标必须处在视距范围内，同时容易受到环境光的影响。GPS定位作为目前应用范围最广的定位方式，具有全天候，实时，连续的优点。但是相应的GPS无法在室内环境使用，在高动态运动状态下会丢失信号从而失去目标。此外GPS成本高，不利于多锚节点部署。

发明内容
[0004] 针对现有上述技术问题，本发明提供一种使用超宽带(Ultra‑wideband,简称UWB)和惯性传感器相结合的方式实现人机协作中人体实时室内定位与运动姿态捕捉的方法，以及实现该方法的系统。惯性传感器获得人体各躯干上的姿态信息，并通过触地脚计算跟点实时坐标，由超宽带定位技术实时获得固定在跟点的标签三维坐标信息，通过无迹卡尔曼滤波融合得到更加平滑准确的跟点坐标，以此推算全身姿态信息，将得到的姿态信息和位置信息赋值给软件中的人体模型，重现人体姿态运动。
[0005] 本发明的人机协作中人体实时室内定位与运动姿态捕捉方法，包括以下步骤：
[0006] (1)获取人体在地理坐标系下的位姿信息，并将超宽带定位系统传回的位置坐标数据转换到地理坐标系下；
[0007] (2)根据足部触地情况，使用正运动学树推算跟点(盆骨)坐标；
[0008] (3)将基于惯性传感器网络计算的跟点坐标和UWB传回的标签坐标进行数据融合；
[0009] (4)根据融合后的跟点坐标值计算全身躯干姿态信息和位置信息。
[0010] 所用到的坐标系：
[0011] 地理坐标系：以初始位置为原点，X轴正方向指东方，Y轴正方向指北方，Z轴正方形按右手定则指向天，也即东北天坐标系；
[0012] 传感器坐标系：坐标原点为传感器中心，X轴正方向平行传感器长边向右，Y轴正方向平行传感器短边向前，Z轴正方向垂直传感器向上；
[0013] 基体坐标系：人体各关节坐标系与跟点坐标系同向，跟点为基体坐标系，X轴正方向指向人体左手边，Y轴正方向指向人体后侧，Z轴正方向按右手定则垂直向上；
[0014] UWB局部定位系统坐标系：根据基站放置情况确定局部坐标系，选择基站A0为坐标系原点，X轴正方向从基站A0指向基站A3，Y轴正方向从基站A0指向基站A1且与X轴垂直，Z轴垂直XY轴平面且指向天。
[0015] 所述步骤(1)中获取人体在地理坐标系下的位姿信息的具体过程是：
[0016] 在人体上固定惯性测量单元(IMU)套装，通过惯性传感器自动捕捉与采集人体接触部位的动作数据(选用12个无线惯性测量单元，分别固定在上身躯干、下身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚和右脚)，选择NoopLoop提供的LinkTrack模块作为超宽带定位系统(UWB)，使用4个模块作为基站(A0,A1,A2,A3)，1个模块作为标签(T0)放置在人体上身躯干，基站模块放置于水平地面。
[0017] 惯性测量单元将处理得到的角速度或四元数传到上位机，同时采集UWB系统中标签坐标，一是选取人员在行走过程中，根据接触地面那只脚的坐标推算跟点坐标，与UWB传回的标签位置信息进行融合，得到更高定位精度的盆骨坐标信息，二是选取盆骨为跟点，通过惯性测量单元实时捕捉各躯干运动信息，根据树型结构和正运动学函数求解各躯干姿态信息和位置信息，将实时求取的位姿信息赋值给人体三维模型，驱动三维人体模型运动；
[0018] 所述步骤(1)中将UWB传回的位置坐标(所得数据)转换到地理坐标系下的具体过程是：
[0019] 假定运动过程中传感器坐标系和基体坐标系之间的关系保持不变，确定基体坐标系与传感器坐标系之间的关系， 通过直接余弦矩阵算法计算传感器坐标系在地理坐标系下的表示， 根据坐标系之间的坐标变换，求得基体坐标系在地理坐标系下的表示，具体转换关系如下：
[0020]
[0021] 使用单位化后的四元数q＝[q0q1q2q3]表示旋转矩阵 公式如下：
[0022]
[0023] 将局部定位系统坐标系下求得的坐标值也转换到地理坐标系下。
[0024] 所述步骤(2)中推算跟点(盆骨)坐标的具体过程是：
[0025] 当左脚触地时，人员跟点(骨盆)坐标获取的具体过程为：
[0026] 由于人体肢体长度是已知的，故初始时刻左脚的坐标可知，以此推算跟点坐标，设左膝关节在以左踝关节为坐标系中的坐标为 其转换矩阵写为：
[0027]
[0028] 表示左大腿的旋转矩阵，同理得 左脚触地时跟点相对左脚踝的位移表示为：

[0029]
[0030] 若此时左脚踝坐标表示为 则跟点实时坐标为：
[0031]
[0032] 同时根据跟点坐标估计右脚踝坐标值,右脚踝相对跟点坐标表示为：
[0033]
[0034] 右脚踝实时坐标表示为：
[0035]
[0036] 右脚触地推算跟点坐标和左脚踝坐标的过程同上；
[0037] 双脚触地时，以左脚踝触地和以右脚踝触地分别计算跟点坐标，计算跟点坐标，对求得的这两个坐标值求均值作为双脚触底时的跟点坐标。
[0038] 所述步骤(3)中进行数据融合，是使用无迹卡尔曼滤波(UKF)算法融合XY轴方向的数据，卡尔曼滤波(KF)算法对基于IMU计算的Z轴位置坐标进行滤波估计；
[0039] 使用惯性传感器获得的数据用于建立系统状态方程，UWB获得的数据用于建立系统观测方程，具体融合算法流程如下：
[0040] (1)假设初始状态估计和方差估计；
[0041] (2)以比例修正对称采样策略选取Sigma点集；
[0042] (3)对Sigma点集进行一步预测；
[0043] (4)利用预测结果计算均值和协方差；
[0044] (5)根据预测值，再一次使用UT变换，得到新的Sigma点集
[0045] (6)利用非线性观测函数预测观测值；
[0046] (7)计算观测预测均值和协方差；
[0047] (8)计算滤波增益；
[0048] (9)更新状态和方差。
[0049] 基于IMU计算的Z轴位置坐标进行滤波估计，具体滤波算法流程如下：
[0050] (1)假设初始状态估计和方差估计；
[0051] (2)计算先验状态估计值；
[0052] (3)计算先验误差协方差；
[0053] (4)更新滤波增益；
[0054] (5)更新观测值；
[0055] (6)更新误差协方差。
[0056] 所述步骤(4)中计算全身躯干姿态信息和位置信息的具体过程是：
[0057] 建立以盆骨为中心的局部坐标系，左肩关节在盆骨坐标系中的相对坐标已知(为躯干长度，且长度固定)，利用旋转矩阵计算运动过程中的相对位移，进而计算左肩关节坐标；
[0058] 以左肩关节为坐标系原点，左肘关节坐标在左肩关节坐标系中的相对坐标已知(为躯干长度，且长度固定)，利用旋转矩阵计算运动过程中的相对位移，进而计算左肘关节坐标；
[0059] 以左肘关节为坐标系原点，左手腕关节在左肘关节坐标系中的相对坐标已知(为躯干长度，且长度固定)，利用旋转矩阵计算运动过程中的相对位移，进而计算左手腕关节坐标；
[0060] 以同样的推导方式计算右肩关节、右肘关节、右手腕坐标；
[0061] 接下来计算下半身各关节坐标：
[0062] 以盆骨为跟点坐标系原点，左髋关节坐标在跟点坐标系中的相对位置已知(为躯干长度，且长度固定)，根据固定在下半身的惯性传感器计算旋转矩阵，获得左髋关节坐标；
[0063] 以左髋关节为局部坐标系原点，左膝关节坐标在左髋关节坐标系中的相对位置已知(为躯干长度，且长度固定)，根据固定在大腿上的惯性传感器计算旋转矩阵，获得左膝关节坐标；
[0064] 以左膝关节为局部坐标系原点，左踝关节坐标在左膝关节坐标系中的相对位置已知(为躯干长度，且长度固定)，根据固定在小腿上的惯性传感器计算旋转矩阵，获得左踝关节坐标；
[0065] 以同样的方法计算右髋关节、右膝关节、右踝关节坐标。
[0066] 实现上述人机协作中人体实时室内定位与运动姿态捕捉方法的系统，具体包括：
[0067] 数据接收模块：接收IMU传回的四元数和角速度，以及UWB传回的位置信息；
[0068] 坐标转换模块：将人体基坐标系下的姿态坐标信息和UWB传回的位置信息转换到地理坐标系下；
[0069] 数据融合模块：以触地脚为原点，采用位姿矩阵计算跟点坐标，与UWB传回的标签坐标进行融合，IMU传感器获得的数据用于建立系统状态方程，UWB获得的数据用于建立系统观测方程。
[0070] 姿态推算模块：依据融合后的跟点坐标计算全身各躯干位置信息和姿态信息，提交给软件的运动姿态显示模块，用于人体运动模型实时重建。
[0071] 本发明通过惯性传感器获得人体各躯干上的姿态信息，并通过触地脚计算跟点实时坐标，由超宽带定位技术实时获得固定在跟点的标签三维坐标信息，通过无迹卡尔曼滤波融合得到了更加平滑准确的跟点坐标，实现人体准确位移，使距离和位置的精准确定，可重现人体姿态运动。
附图说明
[0072] 图1为惯性测量单元在人员身上的放置示意图。
[0073] 图2为结合UWB的测试环境图。
[0074] 图3为人体左脚触地时跟点坐标推算顺序图。
[0075] 图4为人体右脚触地时跟点坐标推算顺序图。
[0076] 图5为以跟点为中心点推算全身各躯干姿态和坐标示意图。
具体实施方式
[0077] 以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
[0078] 本发明所需测试环境分为两部分，第一部分是惯性测量单元(Inertialmeasurementunit，简称IMU)套装，该模块体积小、质量轻、续航能力强，绑在人体身上并不影响人体运动，配置灵活，可对全身动作进行捕捉，具体放置位置如图1所示。本发明选用12个无线惯性测量单元(XSENSMTwAwinda),分别固定在上身躯干、下身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚和右脚。第二部分是超宽带(Ultra‑wideband，简称UWB)定位系统，选择NoopLoop提供的LinkTrack模块，可同时作为标签、基站和控制台，为提高人员三维定位精度，使用4个模块作为基站，1个模块作为标签放置在测试人员上身躯干，基站模块通过三脚架放置于水平地面，整个测试系统如图2所示。在整个测试过程中，IMU将处理得到的角速度或四元数传到上位机软件，同时采集UWB系统中标签坐标，后续数据解算均在软件内进行，具体概括为两部分：一是选取人员在行走过程中，根据接触地面那只脚的坐标推算跟点坐标，与UWB传回的标签位置信息进行融合，得到更高定位精度的盆骨坐标信息。二是选取盆骨为跟点，通过多惯性测量单元系统实时捕捉各躯干运动信息，根据树型结构和正运动学函数求解各躯干姿态信息和位置信息，将实时求取的位姿信息赋值给人体三维模型，驱动三维人体模型运动。
[0079] 本实例所用到的坐标系：
[0080] 地理坐标系：一般以初始位置为原点，X轴正方向指东方，Y轴正方向指北方，Z轴正方形按右手定则指向天，也即东北天坐标系；
[0081] 传感器坐标系：坐标原点为传感器中心，X轴正方向平行传感器长边向右，Y轴正方向平行传感器短边向前，Z轴正方向垂直传感器向上；
[0082] 基体坐标系：人体各关节坐标系与跟点坐标系同向，跟点为基体坐标系，X轴正方向指向人体左手边，Y轴正方向指向人体后侧，Z轴正方向按右手定则垂直向上；
[0083] UWB局部定位系统坐标系：根据基站放置情况确定局部坐标系，一般选择基站A0为坐标系原点，X轴正方向从基站A0指向基站A3，Y轴正方向从基站A0指向基站A1且与X轴垂直，Z轴垂直XY轴平面且指向天。
[0084] 假定运动过程中传感器坐标系和基体坐标系之间的关系保持不变，确定基体坐标系与传感器坐标系之间的关系， 通过直接余弦矩阵算法计算传感器坐标系在地理坐标系下的表示， 根据坐标系之间的坐标变换，可求得基体坐标系在地理坐标系下的表示，具体转换关系如下：
[0085]
[0086] 使用单位化后的四元数q＝[q0q1q2q3]表示旋转矩阵 公式如下：
[0087]
[0088] 将局部定位系统坐标系下求得的坐标值也转换到地理坐标系下。
[0089] 根据触地脚位置计算盆骨坐标。
[0090] 人在行走过程中，总会有一只脚触地或两只脚同时触地的情况，对此我们分别进行讨论。
[0091] 通过固定在脚上的传感器传回的角速度判断哪只脚触地，哪只脚在空中，不同的脚触地会选择不同的正运动学树来估算跟点坐标。每个关节为一个局部坐标系，并于基坐标系保持一致。
[0092] 当左脚触地时，如图3，人员姿态获取的具体过程为：
[0093] 由于人体肢体长度是已知的，故初始时刻左脚的坐标可知，以此推算跟点坐标。设左膝关节在以左踝关节为坐标系中的坐标为 其平移矩阵可写为：
[0094]
[0095] 表示左大腿的旋转矩阵。同理可得 左脚触地时跟点相对左脚踝的位移可表示为：

[0096]
[0097] 若此时左脚踝坐标表示为 则跟点实时坐标为：
[0098]
[0099] 同时根据跟点坐标估计右脚踝坐标值,右脚踝相对跟点坐标表示为：
[0100]
[0101] 右脚踝实时坐标表示为：
[0102]
[0103] 右脚触地推算跟点坐标和左脚踝触地方法相同，如图4，这里不再赘述。
[0104] 双脚触地时，若以左脚踝触地考虑可计算跟点坐标，若以右脚踝触地考虑也可计算跟点坐标，但实际上这两种推算方法计算的跟点坐标并不能保证是相等的，因此本发明使用一种简单的方法来解决这一矛盾，即对求得的这两个坐标值求均值作为双脚触底时的跟点坐标。
[0105] UWB传回标签位置坐标。
[0106] 将基站安装在三脚架上，天线朝上，标签放置于测试人员盆骨位置，打开各模块开关，标签与基站实时通信，传回标签实时位置坐标。在与基于IMU计算的跟点坐标进行融合前，首先对UWB传回的坐标值进行预处理。由于UWB易受多径效应和非视距的影响，容易出现突变值和连续不变值。若某时刻的值为突变值，使用前一时刻值作为该时刻值；若某值连续多次出现，对出现超过5次的值直接忽略。
[0107] 跟点坐标信息融合定位算法。
[0108] 由于UWB在Z轴方向的坐标变化误差太大，所以在数据融合中，使用无迹卡尔曼滤波(UKF)算法融合IMU和UWB计算的X轴、Y轴方向的位置数据，利用样本的后验概率密度对非线性函数概率密度分布进行近似，解决了扩展卡尔曼滤波中的计算复杂度和成本，有效提高参数估计精度和系统稳定性。由于UWB在Z轴方向的坐标变化误差太大，所以在数据融合中，只对基于IMU计算的Z轴位置坐标进行简单的卡尔曼滤波(KF)滤波估计。
[0109] XY轴方向的位置坐标数据融合。
[0110] 使用IMU传感器获得的数据用于建立系统状态方程，UWB获得的数据用于建立系统观测方程，具体融合算法流程如下：
[0111] (10)假设初始状态估计和协方差估计；
[0112] (11)以比例修正对称采样策略选取Sigma点集；
[0113] (12)对Sigma点集进行一步预测；
[0114] (13)利用预测结果计算均值和协方差；
[0115] (14)根据预测值，再一次使用UT变换，得到新的Sigma点集
[0116] (15)利用非线性观测函数预测观测值；
[0117] (16)计算观测预测均值和协方差；
[0118] (17)计算滤波增益；
[0119] (18)更新状态和方差。
[0120] 初始状态向量为X(k)＝[xx(k)xy(k)],表示基于惯性传感器网络计算的跟点在X轴、Y轴方向的坐标，所测数据本身就包含过程噪声，故系统状态方程简化为X(k+1)＝FX(k),F表示系统状态转移矩阵，本例中为单位阵。观测向量为Z(k)＝[zx(k)zy(k)],表示UWB定位系统中标签在X轴、Y轴方向的坐标，所测数据本身就包含了测量噪声，故观测方程简化为Z(k+1)＝HZ(k)，H为观测矩阵，此处为单位阵，在数据融合过程中，UWB测量值实时修正IMU的计算值，获得更加平滑准确的跟点坐标。
[0121] 使用卡尔曼滤波算法对基于IMU计算的Z轴位置坐标进行滤波估计。
[0122] 具体滤波算法流程如下：
[0123] (7)假设初始状态估计和方差估计；
[0124] (8)计算先验状态估计值；
[0125] (9)计算先验误差协方差；
[0126] (10)更新滤波增益；
[0127] (11)更新观测值；
[0128] (12)更新误差协方差。
[0129] 由于此时卡尔曼滤波函数的状态变量只有IMU的Z轴坐标，故将滤波方程简化，其中系统的状态方程为X(k)＝AX(k‑1)，A为状态转移矩阵，且A＝[1]，观测方程为Z(k)＝HX(k‑1)，H为状态转移矩阵，且H＝[1]，误差协方差P(k)＝P(k‑1)+Q，卡尔曼增益K(k+1)＝P(k)/(P(k)+R)，状态更新估计X(k+1)＝X(k)+K(k+1)*(Z(k)‑X(k))，更新误差协方差P(k+1)＝(1‑K(k+1))*P(k)。
[0130] 至此，盆骨的中心点坐标已经得到，进而可推算全身躯干姿态信息和位置信息,如图5所示。对惯性传感器传回的四元数进行单位化，即可表示各躯干姿态信息。
[0131] 根据融合后的跟点坐标推算全身各躯干位置姿态信息。
[0132] 建立以盆骨为中心的局部坐标系，左肩关节在盆骨坐标系中的相对坐标已知，为将固定在上半身的IMU传回的四元数转换为旋转矩阵Rup，则左肩关节坐标可表示为：
[0133]
[0134] 以左肩关节为坐标系原点，左肘关节坐标在左肩关节坐标系中的相对坐标已知，为 将固定在左大臂上的IMU传回的四元数转换为旋转矩阵Rarm1，则左肘关节坐标可表示为：
[0135]
[0136] 以左肘关节为坐标系原点，左手腕关节在左肘关节坐标系中的相对坐标已知，为将固定在左小臂上的IMU传回的四元数转换为旋转矩阵Rarm2,则左手腕关节坐标可表示为：
[0137]
[0138] 以同样的推导方式计算右肩关节、右肘关节、右手腕坐标。
[0139] 接下来计算下半身各关节坐标。
[0140] 以盆骨为跟点坐标系原点，左髋关节坐标在跟点坐标系中的相对位置已知，记为根据固定在下半身的惯性传感器计算旋转矩阵Rdown，获得左髋关节坐标为：
[0141]
[0142] 以左髋关节为局部坐标系原点，左膝关节坐标在左髋关节坐标系中的相对位置已知，记为 根据固定在大腿上的惯性传感器计算旋转矩阵Rleg1，获得左膝关节坐标为：
[0143]
[0144] 以左膝关节为局部坐标系原点，左踝关节坐标在左膝关节坐标系中的相对位置已知,记为 根据固定在小腿上的惯性传感器计算旋转矩阵Rleg2，获得左踝关节坐标为：
[0145]
[0146] 以同样的方法计算右髋关节、右膝关节、右踝关节坐标。
[0147] 至此，将人员三维位置信息和姿态信息传到软件中的虚拟人体模型上，便可实现对人员运动姿态的实时跟踪。
[0148] 相应地，在该实例中，具体包括：
[0149] 数据接收模块：接收IMU传回的四元数和角速度，以及UWB传回的位置信息；
[0150] 坐标转换模块：将人体基坐标系下的姿态坐标信息和UWB传回的位置信息转换到地理坐标系下；
[0151] 数据融合模块：以触地脚为原点，采用位姿矩阵计算跟点坐标，与UWB传回的标签坐标进行融合，IMU传感器获得的数据用于建立系统状态方程，UWB获得的数据用于建立系统观测方程。
[0152] 姿态推算模块：依据融合后的跟点坐标计算全身各躯干位置信息和姿态信息，提交给软件的运动姿态显示模块，用于人体运动模型实时重建。

专利地区：山东

专利申请日期：2021-02-01

专利公开日期：2022-10-18

专利公告号：CN112957033B