基于人体分布惯性节点矢量测距的三维位姿测量方法与流程

本发明属于行人惯性导航定位领域，特别涉及一种基于人体分布惯性节点矢量测距的三维位姿测量方法。

背景技术：

步态分析与步长计算是惯性导航定位领域的关键点。通过分析行人的步态信息，可以对人体的生理信息，运动行为，健康状况进行估计。通过提升步长计算精度，可以提高最终的导航定位精度。

当前，研究人员对运动估计系统做了大量的研究。主要分为三大类，一类是基于光学方法，如利用vicon系统对人体数据进行采集，采集的信息可应用于体育运动分析及生物医学研究，但是存在设备价格昂贵且需要事先布置传感器的缺点；一类则是利用视觉追踪平台，通过相应的计算机技术与图像处理技术，提取人体运动的参数信息，进而分析步态信息、计算步幅长度，如杭州电子科技大学张松等人[张松.基于深度相机的人体动作评价方法[d].杭州电子科技大学,2018.]通过kinect深度相机采集深度图像，使用kalman-meanshift跟踪方法实现了较好的深度图像人体目标跟踪效果，但是存在算法复杂，无法适用于实时操作系统的问题；一类是借助可穿戴设备，如常见的惯性器件来感知、获取人体的运动参数，通过参数的提取与分析检测行人的步态信息，并使用估计量估计行人的步长，常见的估计方法有基于步频计算步长的线性步长计算模型以及基于加速度幅值变化计算步长的非线性步长计算模型，燕山大学张雄杰等人[文献：张雄杰.基于xsensmvn惯性运动捕捉系统装备的人体运动特性研究[d].燕山大学,2016.]利用可穿戴的惯性传感器设备获取人员的运动参数，进而对人体的运动特性进行研究，南昌大学熊剑等人[文献：熊剑,徐江颖,杨祖华,etal.一种基于人体运动模式监测的行人导航方法:.]通过将双轴角速度传感器安装在行人的髋关节、膝关节和踝关节，实时测量运动过程中人员腿部的角度信息，结合行人的双腿长度信息，最终得到各关节的运动信息、位置信息及行人步长信息，但是存在算法模型参数需要根据测试人员的不同而进行手动更改，鲁棒性差的缺点。

本发明针对基于光学的运动估计系统需要事先布置传感器，基于视觉的运动估计系统算法复杂度高，基于惯性器件的运动估计系统鲁棒性差、精度低的缺点。提出一种基于人体分布惯性节点矢量测距的三维位姿测量方法，该方法以行人自身可携带的传感器完成精准步长计算无需事先布置传感器，算法计算量小可应用于实时操作系统，算法的鲁棒性强无需根据测试人员的差异改变模型参数具有极强的实用性。相比较于传统的运动估计系统，通过测距模块与惯性节点的动态组合可以实现无估计参量，无累计误差的实时三维位姿测量，并可对行人单步运动实现全过程跟踪与复现。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种基于人体分布惯性节点矢量测距的三维位姿测量方法。本发明的技术方案如下：

一种基于人体分布惯性节点矢量测距的三维位姿测量方法，其包括以下步骤：

步骤1，在行人左右腿的踝关节处安装测距模块及惯性节点，其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息，测距模块实时采集两个节点间的距离信息；

步骤2，初始阶段，利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息；

步骤3，利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计，区分运动状态与静止状态；

步骤4，对于静止状态的节点通过零速修正减小误差，对于运动状态的节点利用陀螺仪对姿态角信息进行实时更新，得到节点当前姿态信息，并结合参考姿态角信息得到姿态角变化的结果；

步骤5，利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息；

步骤6，根据测距模块提供的两节点间的距离信息以及矢量的角度信息，获取节点的实时位置信息；

步骤7、综合行人单步的运动信息，实现人员定位功能。

进一步的，所述步骤1中，通过调整测距模块的功率、选择合适的定向天线及馈线等方式来实现测距模块的近距离高精度测距功能，可以实时测量踝关节节点间的距离信息。通过惯性节点实时采集节点处的运动信息。

进一步的，所述步骤2中，初始阶段，利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息；在初始阶段，人体的左足与右足均处于支撑阶段，此时，踝关节节点处于静止状态，因此，可以通过加速度计与磁力计计算该状态下的姿态角信息，作为踝关节节点的初始姿态角即参考姿态角，如式(1)所示：

其中，θ1、γ1、ψ1分别代表俯仰角、横滚角、航向角，ax、ay、az分别代表三轴加速度信息，mx、my、mz代表三轴磁力计的强度信息。

进一步的，所述步骤3中，利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计，区分运动状态与静止状态，具体包括：通过阈值条件，对足部的运动状态进行判断，判断方法如式(2)所示：

其中，ath代表区分运动状态与静止状态的阈值，anorm代表合加速度，当合加速度anorm大于阈值ath时，代表节点处于运动状态；当合加速度anorm小于阈值时ath，代表节点处于静止状态。

进一步的，所述步骤4中，当踝关节节点处于静止状态时，通过卡尔曼滤波技术进行数据融合，估计系统的误差并利用误差的估计值对系统参数进行校正，如式(3)所示：

其中，f是误差模型与状态量所构成的系统矩阵，w为系统随机过程噪声序列，v是系统观测噪声序列；

当踝关节节点处于运动状态，通过陀螺仪对节点的姿态角信息进行更新，如式(4)所示：

其中，q0、q1、q2、q3是四元数信息，θ2、γ2、ψ2是实时姿态角信息，通过陀螺仪对四元数进行更新，可以得到实时的姿态角信息即节点的姿态信息，将实时姿态角信息与参考姿态角信息作差，得到姿态角的变换信息，如式(5)所示：

进一步的，所述步骤5中，利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息，具体包括：在实际运动过程中，三轴加速度计可以敏感到比力在载体坐标系的三个方向上的分力，通过分力的比例关系可以得到在载体坐标系下的矢量的角度信息，结合步骤4中姿态角的变化信息与载体坐标系下的矢量的角度信息，计算得到最终的相对于初始姿态角的矢量的角度信息，在行人的矢状面，通过矢量关系可以得到方向角度信息，如式(6)、式(7)所示：

θy＝arccos(ay/anorm)(6)

θsum＝θy+θ(7)

其中，θy是载体坐标系的y轴加速度ay与合加速度anorm的夹角，θ是当前姿态角相比于初始姿态角的变化信息，θsum是经过姿态角变化补偿后的矢量在行人矢状面的角度变化信息。

进一步的，所述步骤6中，根据测距模块提供的两节点间的距离信息以及矢量的角度信息获取节点的实时位置信息，具体包括：在步骤5中可以得到每个时间片段内的矢量的角度信息，结合测距模块提供的两节点间的距离信息可以得到矢量的全部信息，最终，通过函数关系可以获取节点实时的位置信息，如式(8)所示：

其中，l为测距模块测量的两节点间的距离信息，即矢量长度信息，α为矢量在矢状面的角度信息，即矢量的角度信息，y为矢状面的步进距离信息，h为矢状面的步高信息，整个过程中，需要对矢量的角度信息即α不断进行更新，在运动过程中，运动节点相对静止节点从后向前摆动即完成一个跨步的过程可以分为三个阶段，第一阶段是运动节点在静止节点的后方，第二阶段是运动节点从静止节点的后方摆动到静止节点的前方，第三阶段是运动节点在静止节点的前方，通过矢量的角度信息可以对三个阶段进行区分。

进一步的，所述步骤7中，综合行人单步的运动信息，实现人员定位功能，具体包括：通过步骤1到步骤6可以精确计算单步的步长信息及单步内的姿态信息，综合各单步的运动信息，实现三维空间下人员的定位功能，计算公式如下：

其中，xstep是单次跨步的水平位移距离，ystep是单次跨步的垂直位移距离，l是单步的步长信息，ψ是节点提供的航向信息，xsum是上一次测量条件下相对于初始坐标的水平位移距离，ysum是上一次测量条件下相对于初始坐标的垂直位移距离，x是本次测量条件下相对于初始坐标的水平位移距离，y是本次测量条件下相对于初始坐标的垂直位移距离。

本发明的优点及有益效果如下：

本发明通过矢量信息与姿态信息监测行人在三维空间中的实时位姿信息。本发明的主要创新点是步骤(5)与步骤(6)，具有以下益处：

(1)自主性好：该方法仅依靠行人自身携带的传感器完成精确步长计算、姿态估计，无需借助外部传感器。

(2)运动信息丰富：该方法可对行人单步运动实现全过程跟踪与复现，可以提供单步运动过程中的各个位置的步进信息与步高信息，并提供该位置处的姿态信息。

(3)精度高：该方法可以完成对行人在三维空间中位姿信息的实时监测，计算参数直观可测，针对行人航迹推算算法中步长计算使用估计量造成精度低且误差累计的不足进行改进。

(4)实时性好：该方法的计算量小，参数直观可测，适用于实时操作系统。

(5)实用性强：该方法无需根据测试人员的差异改变计算模型的参数，鲁棒性强。

附图说明

图1是本发明提供优选实施例载体坐标系下的矢量关系表示

图2是姿态角补偿后矢量关系表示

图3是步态周期分解示意图

图4是步进、步高计算示意图

图5是运动节点在静止节点后方示意图

图6是运动节点从静止节点后方跨越至前方示意图

图7是运动节点在静止节点前方示意图

图8是算法整体框架流程图

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

如图8所示，一种基于人体分布惯性节点矢量测距的三维位姿测量方法。该方法在行人左右腿的踝关节均安装测距模块与惯性节点，每个惯性节点内部集成三轴加速度计、角速度计及磁力计。利用测距模块测量行人踝关节间的实时距离信息；利用惯性节点计算出踝关节节点处的姿态角信息。通过运动过程中三个轴向上的加速度信息及姿态角信息并结合之前的测距信息，得到各时间片段内的矢量信息。通过矢量信息及姿态角信息可以实时追踪行人在三维空间域中的位置与姿态信息，简称位姿信息。本发明的参数直观、可靠，具有自主测量、不使用估计量、不受特征参数约束的特点。

包括以下步骤：(1)在行人左右腿的踝关节处安装测距模块及惯性节点，其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息，测距模块实时采集两个节点间的距离信息；(2)初始阶段，利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息；(3)利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计，区分运动状态与静止状态；(4)对于静止状态的节点通过零速修正减小误差，提升系统精度，对于运动状态的节点利用陀螺仪对姿态角信息进行实时更新，得到节点当前姿态信息，并结合参考姿态角信息得到姿态角变化的结果；(5)利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息；(6)根据测距模块提供的两节点间的距离信息以及矢量的角度信息可以获取节点的实时位置信息；(7)综合行人单步的运动信息，实现人员定位功能。本发明可以完成对行人在三维空间中位姿信息的实时监测，并可对行人单步运动实现全过程跟踪与复现，且以行人自身可携带的传感器完成精准步长计算，不使用估计量、无累计误差的位置计算和姿态测量，无需借助其他数据信息，具有极强的适用性。

一种基于人体分布惯性节点矢量测距的三维位姿测量方法，其包括以下步骤：

步骤(1)，在行人左右腿的踝关节处安装测距模块及惯性节点，其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息，测距模块实时采集两个节点间的距离信息；

步骤(2)，初始阶段，利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息；

步骤(3)，利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计，区分运动状态与静止状态；

步骤(4)，对于静止状态的节点通过零速修正减小误差，提升系统精度，对于运动状态的节点利用陀螺仪对姿态角信息进行实时更新，得到节点当前姿态信息，并结合参考姿态角信息得到姿态角变化的结果；

步骤(5)，利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息；

步骤(6)，根据测距模块提供的两节点间的距离信息以及矢量的角度信息可以获取节点的实时位置信息；

步骤(7)综合行人单步的运动信息，实现人员定位功能。

所述步骤(1)中，在行人左右腿的踝关节处安装测距模块及惯性节点，其中惯性节点实时采集安装节点的加速度信息、角速度信息及磁场强度信息，测距模块实时采集两个节点间的距离信息。惯性节点用于提供节点处的运动信息，采用降低测距模块增益、降低采样频率等方式实现测距模块的近距离高精度测距，可以精确地测量踝关节节点间的距离信息。

所述步骤(2)中，初始阶段，利用惯性节点采集到的加速度信息与磁场强度信息得到参考姿态角信息；在程序运行的初始阶段，人体的左足与右足均处于支撑阶段，此时，踝关节节点处于静止状态。因此，可以通过加速度计与磁力计计算该状态下的姿态角信息，作为踝关节节点的初始姿态角(参考姿态角)。

如式(1)所示：

其中，θ1、γ1、ψ1分别代表俯仰角、横滚角、航向角，ax、ay、az分别代表三轴加速度信息，mx、my、mz代表三轴磁力计的强度信息。

所述步骤(3)中，利用踝关节节点处的合加速度信息对两个节点的运动状态进行估计，区分运动状态与静止状态。人在行走过程中，当一只脚处于支撑状态时，另一只脚则处于摆动状态，两只脚的状态呈现出周期性的交替变换。因此，节点的运动状态也会相应的进行改变。在通过矢量估计运动节点的位置信息过程中，需要以静止节点作为基准点，因此，需要我们在计算过程中，区分运动节点与静止节点。当节点处于静止状态时，合加速度(经重力补偿)在0值附近变化；当节点处于运动状态时，合加速度会产生较大的变化。通过阈值条件，可以对足部的运动状态进行判断。判断方法如式(2)所示：

其中，ath代表区分运动状态与静止状态的阈值，anorm代表合加速度。当合加速度anorm大于阈值ath时，代表节点处于运动状态；当合加速度anorm小于阈值时ath，代表节点处于静止状态。

所述步骤(4)中，对于静止状态的节点通过零速修正减小误差，提升系统精度，对于运动状态的节点利用陀螺仪对姿态角信息进行实时更新，得到节点当前姿态信息，并结合参考姿态角信息得到姿态角变化的结果。惯性器件具有短时精度高，长时间精度下降的缺点。因此需要我们建立合适的误差修正模型，进而提升系统的精度。当踝关节节点处于静止状态时，通过卡尔曼滤波技术进行数据融合，估计系统的误差并利用误差的估计值对系统参数进行校正。如式(3)所示：

其中，f是误差模型与状态量所构成的系统矩阵，w为系统随机过程噪声序列，v是系统观测噪声序列。

当踝关节节点处于运动状态，通过陀螺仪对节点的姿态角信息进行更新，如式(4)所示：

其中，q0、q1、q2、q3是四元数信息，θ2、γ2、ψ2是实时姿态角信息，通过陀螺仪对四元数进行更新，可以得到实时的姿态角信息即节点的姿态信息。将实时姿态角信息与参考姿态角信息作差，得到姿态角的变换信息。如式(5)所示：

所述步骤(5)中，利用姿态角变化的结果以及加速度计敏感到的三个轴向上的加速度信息得到矢量的角度信息。在实际运动过程中，三轴加速度计可以敏感到比力在载体坐标系的三个方向上的分力，通过分力的比例关系可以得到在载体坐标系下的矢量的角度信息，载体坐标系下的矢量关系表示如附图1所示。结合步骤(4)中姿态角的变化信息与载体坐标系下的矢量的角度信息，计算得到最终的相对于初始姿态角的矢量的角度信息。在行人的矢状面，通过矢量关系可以得到方向角度信息，几何表示如附图2所示。计算公式如式(6)、式(7)所示：

θy＝arccos(ay/anorm)(6)

θsum＝θy+θ(7)

其中，θy是载体坐标系的y轴加速度ay与合加速度anorm的夹角，θ是当前姿态角相比于初始姿态角的变化信息，θsum是经过姿态角变化补偿后的矢量在行人矢状面的角度变化信息。

所述步骤(6)中，根据测距模块提供的两节点间的距离信息以及矢量的角度信息可以获取节点的实时位置信息。程序的执行频率为200hz，人的行走频率通常约为3-5hz。行人的整个行走过程可以被分成多个时间片段，在每个时间片段内，节点的运动可以看作矢量运动。因此，整个行走过程可以被视为多个矢量的叠加，将步态周期进行分解进而得到多个矢量的示意图如附图3所示。在步骤(5)中可以得到每个时间片段内的矢量的角度信息，结合测距模块提供的两节点间的距离信息可以得到矢量的全部信息。最终，通过函数关系可以获取节点实时的位置信息，如附图4所示。如式(8)所示：

其中，l为测距模块测量的两节点间的距离信息，即矢量长度信息，α为矢量在矢状面的角度信息，即矢量的角度信息，y为矢状面的步进距离信息，h为矢状面的步高信息。整个过程中，需要对矢量的角度信息即α不断进行更新。在运动过程中，运动节点相对静止节点从后向前摆动(完成一个跨步的过程)可以分为三个阶段，第一阶段是运动节点在静止节点的后方，如附图5所示；第二阶段是运动节点从静止节点的后方摆动到静止节点的前方，如附图6所示；第三阶段是运动节点在静止节点的前方，如附图7所示。通过矢量的角度信息可以对三个阶段进行区分，三个阶段的角度信息更新略有不同。计算公式如下：

第一个阶段：

其中，l1为前一次测量的矢量的长度信息，l2为当前测量的矢量的长度信息，l3为下一次测量的矢量的长度信息，θ1是上一次测量惯性节点所提供的角度信息，θ2为当前测量惯性节点所提供的角度信息，α为前一次测量与当前测量的矢量的夹角，α1为本次测量与下一次测量的矢量的夹角，β与γ是计算过程中用到的角度信息，通过这些参量可以完成矢量角度的更新。当计算流程结束后，需要将中间量更新，从而得到第一阶段中各时间片段内的步进信息与步高信息。

第二阶段：

其中，l1为前一次测量的矢量的长度信息，l2为当前测量的矢量的长度信息，θ1是当前测量惯性节点所提供的角度信息，α为更新后的前一次矢量的角度信息(与水平轴夹角)，α1为前一次测量与当前测量的矢量的夹角，β与ψ是计算过程中用到的角度信息，通过这些参量可以完成矢量角度的更新。当计算流程结束后，需要将中间量更新，从而得到第二阶段中各时间片段内的步进信息与步高信息。

第三阶段：

其中，l1为前一次测量的矢量的长度信息，l2为当前测量的矢量的长度信息，θ1是当前测量惯性节点所提供的角度信息，α为更新后的前一次矢量的角度信息(与水平轴夹角)，α1为前一次测量与当前测量的矢量的夹角，β与ψ是计算过程中用到的角度信息，通过这些参量可以完成矢量角度的更新。当计算流程结束后，需要将中间量更新，从而得到第三阶段中各时间片段内的步进信息与步高信息。

所述步骤(7)中，综合行人单步的运动信息，实现人员定位功能。通过步骤(1)到步骤(6)可以精确计算单步的步长信息及单步内的姿态信息。综合各单步的运动信息，可以实现三维空间下人员的定位功能。计算公式如下：

其中，xstep是单次跨步的水平位移距离，ystep是单次跨步的垂直位移距离，l是单步的步长信息，ψ是节点提供的航向信息，xsum是上一次测量条件下相对于初始坐标的水平位移距离，ysum是上一次测量条件下相对于初始坐标的垂直位移距离，x是本次测量条件下相对于初始坐标的水平位移距离，y是本次测量条件下相对于初始坐标的垂直位移距离。

算法的整体框架流程图如附图8所示。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。