一种三维空间定位方法、系统、装置与存储介质与流程

1.本技术涉及三维空间定位技术领域，尤其是一种三维空间定位方法、系统、装置与存储介质。

背景技术：

2.行人导航系统可实时监测定位执行任务人员的位置，并使指挥中心了解执行任务人员的运动状态，从而极大地提高抢险搜救人员的快速反应能力和指挥中心的指挥能力，因此，开展实时、精确的行人导航定位技术具有广泛的应用前景。
3.近年来，微机电(mems)传感器的技术逐渐成熟，基于mems技术的行人导航技术得以快速发展，该技术通常把mems传感器固定在足部，通过陀螺仪与加速度计积分获取位置信息及方位信息，再经人体步态特征的零速修正降低积分的漂移误差。但是，消费级的mems传感器一般精度较低，使用该方法是无法有效地估计及修正积分的漂移误差，积分过程中加速度误差引起的位置误差更加无法精确估计，无法来回精确定位人体的空间三维位置；因此，亟需一种新的三维空间定位方法。

技术实现要素：

4.本技术的目的在于至少一定程度上解决现有技术中存在的技术问题之一。
5.为此，本技术实施例的一个目的在于提供一种三维空间定位方法、系统、装置与存储介质，该方法可以可以减少人体骨骼三维空间定位的误差，提高三维空间定位的准确度。
6.为了达到上述技术目的，本技术实施例所采取的技术方案包括：获取人体骨骼惯性动捕的第一姿态数据；根据所述第一姿态数据确定人体虚拟模型的运动状态；确定人体虚拟模型处于第一运动状态，获取人体虚拟模型的静止骨骼节点的第一位置信息；根据所述第一位置信息以及所述第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；确定人体虚拟模型处于第二运动状态，获取人体虚拟模型的运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；根据所述第二位置信息以及第三位置信息，确定目标骨骼节点的目标位置信息；根据所述目标位置信息，确定人体骨骼所有节点的位置信息。
7.另外，根据本发明中上述实施例的一种三维空间定位的方法，还可以有以下附加的技术特征：
8.进一步地，本技术实施例中，所述根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型目标骨骼节点的第二位置信息这一步骤，具体包括：根据所述第一姿态数据，确定第一姿态矩阵；根据所述第一位置信息、第一姿态矩阵以及位移迭代公式，确定人体虚拟模型目标骨骼节点的第二位置信息；其中所述位移迭代公式包括：
9.pos[j]＝pos[i]-rotm[i]
×
(intp[i]-intp[j])；
[0010]
上式中，其中intp[i]和intp[j]表示虚拟模型骨架节点的原始坐标信息，pos[i]为静止骨骼节点的第一位置信息，pos[j]目标骨骼节点的第二位置信息；rotm[i]为第一姿
态矩阵。
[0011]
进一步地，本技术实施例中，所述根据所述第一姿态数据，确定第一姿态矩阵这一步骤，具体包括：对所述第一姿态数据进行姿态标定，得到第二姿态数据；根据所述第二姿态数据以及矩阵计算公式，确定第一姿态矩阵；其中所述矩阵计算公式为
[0012][0013]
上述矩阵计算公式中，q[j]表示标定后的第二姿态数据，rotm[j]表示对应的姿态矩阵，j代表虚拟模型的关键骨骼所属序号。
[0014]
进一步地，本技术实施例中，所述根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息这一步骤，具体包括：获取连续两帧数据中运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；根据所述速度信息以及节点位置公式，得到目标骨骼节点的第三位置信息；其中，所述节点位置公式为
[0015][0016]
上式中，acc[i]为当前帧的去重力加速度，_acc[i]表示上一帧的去重力加速度，vel[i]表示当前帧的速度，_vel[i]表示上一帧的速度，初始时刻_vel[i]为0，δt表示一帧的时间，_pos[i]表示上一帧的目标节点的位置信息，pos[i]表示当前帧的目标节点的位置信息。
[0017]
进一步地，本技术实施例中，还包括步骤：确定所述人体虚拟模型处于第三运动状态，调整人体骨骼所有节点的位置信息。
[0018]
进一步地，本技术实施例中，所述对所述第一姿态数据进行姿态标定，得到第二姿态数据这一步骤，具体包括：获取第一姿态数据；根据所述第一姿态数据以及标定公式，得到第二姿态数据；其中所述标定公式为
[0019]
q[j]＝calim[j]
×qint
[j]；
[0020]
上式中，q
int
[j]表示惯性动捕模块输出的初始姿态数据，calim[j]表示惯性动捕模块的标定矩阵。
[0021]
进一步地，本技术实施例中，所述调整人体骨骼所有节点的位置信息这一步骤，具体包括：确定人体骨骼节点变化的高度差；根据所述高度差，调整人体骨骼所有节点的位置信息。
[0022]
另一方面，本技术实施例还提供一种三维空间定位系统，包括：
[0023]
第一获取单元，用于人体骨骼惯性动捕的第一姿态数据；
[0024]
第一处理单元，用于根据所述第一姿态数据确定人体虚拟模型的运动状态；
[0025]
第二获取单元，用于获取人体虚拟模型的静止骨骼节点的第一位置信息；
[0026]
第二处理单元，用于根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；
[0027]
第三获取单元，用于获取人体虚拟模型的运动骨骼节点的速度信息；
[0028]
第三处理单元，用于根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；
[0029]
第四处理单元，用于根据所述第二位置信息以及第三位置信息，确定目标骨骼节
点的目标位置信息；
[0030]
第五处理单元，用于根据所述目标位置信息，确定人体骨骼所有节点的位置信息。
[0031]
另一方面，本技术还提供一种三维空间定位装置，包括：
[0032]
至少一个处理器；
[0033]
至少一个存储器，用于存储至少一个程序；
[0034]
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如发明内容中任一项所述一种三维空间定位方法。
[0035]
此外，本技术还提供一种存储介质，其中存储有处理器可执行的指令，所述处理器可执行的指令在由处理器执行时用于执行如上述任一项所述一种三维空间定位方法。
[0036]
本技术的优点和有益效果将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到：
[0037]
本技术可以根据人体骨骼惯性动捕的第一姿态数据确定人体虚拟模型的不同运动状态，根据不同运动状态下得到的目标节点的位置信息确定最终的目标节点的位置信息，再通过目标节点的位置信息确定人体骨骼所有节点的位置信息，可以减少人体骨骼三维空间定位的误差，提高三维空间定位的准确度。
附图说明
[0038]
图1为本发明中一种具体实施例中一种三维空间定位方法的步骤示意图；
[0039]
图2为本发明中一种具体实施例中惯性动捕系统结构示意图；
[0040]
图3为本发明中一种具体实施例中根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型目标骨骼节点的第二位置信息的步骤示意图；
[0041]
图4为本发明中一种具体实施例中根据所述第一姿态数据，确定第一姿态矩阵的步骤示意图；
[0042]
图5为本发明中一种具体实施例中对所述第一姿态数据进行姿态标定，得到第二姿态数据的步骤示意图；
[0043]
图6为本发明中一种具体实施例中根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息的步骤示意图；
[0044]
图7为本发明中一种具体实施例中一种三维空间定位系统的结构示意图；
[0045]
图8为本发明中一种具体实施例中一种三维空间定位装置的结构示意图；
[0046]
图9为本发明中一种具体实施例中零速信息的波形示意图。
具体实施方式
[0047]
下面结合附图详细描述本发明的实施例对本发明实施例中的三维空间定位方法、系统、装置和存储介质的原理和过程作以下说明。
[0048]
参照图1，本发明一种三维空间定位方法，包括以下步骤：
[0049]
s1、获取人体骨骼惯性动捕的第一姿态数据；
[0050]
在本技术的一些实施例中，可以在人体关键的17个部位都佩戴一个惯性动捕模块，编号1到17，如图2所示：每一个惯性动捕模块由9轴传感器(陀螺仪、加速度计、磁力计)和微处理器组成，惯性动捕模块的微处理器与9轴传感器分别连接，用于9轴传感器校准并
进行9轴数据融合姿态解算，生成第一姿态数据；并可以把第一姿态数据传入控制器模块；控制器模块包含电池、数据通信模块和微处理器，电池对整个惯性动捕硬件供电，而微处理器把各惯性动捕模块传入的姿态数据合成帧，由数据通信模块发送到数据终端。数据终端根据发送过来的姿态数据在经过标定后，即可通过虚拟模型对佩戴者的各种运动进行实时显示，也可对运动数据进行保存；所佩戴的部位分别为：腰部、右大腿、右小腿、右脚、左大腿、左小腿、左脚、背部、头部、右肩膀、右大臂、右小臂、右手、左肩膀、左大臂、左小臂和左手。对于行人导航，一般是双脚在运动，只需将惯性动捕模块1-7号分别佩戴在腰部、右大腿、右小腿、右脚、左大腿、左小腿以及左脚。
[0051]
s2、根据所述第一姿态数据确定人体虚拟模型的运动状态；
[0052]
在本技术的一些实施例中，可以采集行走时下肢惯性动捕模块的数据，也可以采集其他的惯性动捕模块的数据，统计分析行人的步态特征，通过经神经网络模型输出，可以得到人体虚拟模型不同的运动状态，其中运动状态可以包括且不局限于静止状态、左右脚同时离开地面状态、左右脚交替向前的运动状态以及左右脚同时离开地面大于一定高度差的运动状态。
[0053]
s3、确定人体虚拟模型处于第一运动状态，获取人体虚拟模型的静止骨骼节点的第一位置信息；
[0054]
在本技术的一些实施例中，第一运动状态是指人体处于左右脚交替向前的运动状态，结合具体的应用场景，人体在向前运动时，在某一时刻其静止的骨骼节点可以是左脚或者是右脚，而第一位置信息对应的可以是静止节点在三维坐标中的位置信息，因此确定人体虚拟模型处于向前运动时，可以获取骨骼节点可以是左脚或者是右脚的位置信息。具体的获取方式包括但不局限于通过无线传输以及设备与上位机的有线传输数据等
[0055]
s4、根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；
[0056]
在本技术的一些实施例中，根据人体骨骼的每一段骨骼第一姿态数据可以得到获得对应的姿态矩阵，根据第一位置信息以及姿态矩阵可以得到第二位置信息；在本技术实施例中第二位置信息是通过人体骨骼静止节点以及姿态矩阵得到的目标骨骼节点的位置信息，在人体骨骼节点中可以选择任意一个节点为目标骨骼节点，对应地，也可以选择任意的静止节点的位置信息作为第一位置信息，为了便于后续的计算，目标节点可以选择与其他节点连接最多的节点也就是腰部节点作为优选的目标节点，静止节点可以选择人体骨骼下肢中静止的节点。
[0057]
s5、确定人体虚拟模型处于第二运动状态，获取人体虚拟模型的运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；
[0058]
在本技术的一些实施例中，第二运动状态可以指人体左右脚同时离开地面的运动状态，如跳跃状态，确定人体虚拟模型处于人体左右脚同时离开地面的运动状态后，可以获取人体虚拟模型的运动骨骼节点的速度信息，在人体左右脚同时离开地面的运动状态，人体骨骼全身均可以是运动骨骼节点，如可以获取左右脚节点的速度信息，也可以获取腰部节点速度信息，其中速度信息可以包括去重力加速度以及运动速度，去重力加速度以及运动速度可以通过采集惯性动捕模块记录的数据获得。
[0059]
s6、根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；
[0060]
在本技术的一些实施例中，可以通过运动节点连续两帧的速度信息可以确定人体虚拟模型目标骨骼节点的第三位置信息；第三位置信息可以是通过惯性动捕模块采集目标节点连续两帧的速度信息推算得到的目标节点的位置信息；第三位置信息可以是任意目标节点的位置信息，而目标节点可以是人体虚拟模型目标骨骼节点的任意节点，为了后续人体骨骼其他节点的位置解算，目标节点可以选用腰部节点，腰部节点与人体骨骼其他节点的连接最多，可以腰部节点解人体上肢的节点的位置信息。
[0061]
s7、根据所述第二位置信息以及第三位置信息，确定目标骨骼节点的目标位置信息；
[0062]
在本技术的一些实施例中，得到通过人体骨骼静止节点以及姿态矩阵得到的目标骨骼节点的第二位置信息以及通过惯性动捕模块采集目标节点连续两帧的速度信息推算得到的目标节点的位置信息后，可以通过加权重融合以及滤波处理，获得更加精确的目标位置信息。
[0063]
s8、根据所述目标位置信息，确定人体骨骼所有节点的位置信息。
[0064]
在本技术的一些实施例中，得到目标节点的目标位置信息，可以通过预设的算法确定人体骨骼其他节点的位置信息，进而得到人体所有骨骼节点的位置信息。
[0065]
进一步地，参照图3，所述根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型目标骨骼节点的第二位置信息这一步骤，具体可以包括：
[0066]
s101、根据所述第一姿态数据，确定第一姿态矩阵；
[0067]
s102、根据所述第一位置信息、第一姿态矩阵以及位移迭代公式，确定人体虚拟模型目标骨骼节点的第二位置信息；
[0068]
其中所述位移迭代公式包括：
[0069]
pos[j]＝pos[i]-rotm[i]
×
(intp[i]-intp[j])；
[0070]
上式中，其中intp[i]和intp[j]表示虚拟模型骨架节点的原始坐标信息，pos[i]为静止骨骼节点的第一位置信息，pos[j]目标骨骼节点的第二位置信息；rotm[i]为第一姿态矩阵；
[0071]
具体地，在本技术的一些实施例中，可以根据第一姿态数据，确定第一姿态矩阵，将第一矩阵以及第一位置信息输入到位移迭代公式，可以确定目标节点的第二位置信息，其中位移迭代公式为：
[0072]
pos[j]＝pos[i]-rotm[i]
×
(intp[i]-intp[j])
[0073]
其中intp[i]和intp[j]表示虚拟模型骨架节点的原始坐标信息，在本技术中，虚拟模型骨架节点的原始坐标信息为一开始确定的虚拟模型的坐标信息，pos[i]为连续帧中上一帧对应的骨骼节点的第一位置信息，而pos[j]为根据pos[i]确定的当前帧的除pos[i]以为其他节点的位置信息；rotm[i]为第一姿态矩阵，具体地，以上一帧为左脚为运动，右脚静止；当前帧的运动状态为左脚为静止，右脚运动为例；此时，当前帧中作为起始节点的左脚的位置信息为上一帧以右脚为起始节点推算后得到的位置信息，而当前帧推断得到的右脚位置信息可以在后续帧以此为节点推断后续帧中左脚的位置信息。
[0074]
进一步地，参照图4，所述根据所述第一姿态数据，确定第一姿态矩阵这一步骤，具体可以包括：
[0075]
s201、对所述第一姿态数据进行姿态标定，得到第二姿态数据；
[0076]
s202、根据所述第二姿态数据以及矩阵计算公式，确定第一姿态矩阵；其中所述矩阵计算公式为
[0077][0078]
上述矩阵计算公式中，q[j]表示标定后的第二姿态数据，rotm[j]表示对应的姿态矩阵，j代表虚拟模型的关键骨骼所属序号；
[0079]
具体地，在申请实施例中，获取第一姿态数据后，需要对第一姿态数据进行标定，得到标定后的第二姿态数据，将第二姿态数据输入计算矩阵可以得到第一姿态矩阵，第一姿态矩阵可以用于确定目标节点的位置信息。
[0080]
进一步地，参照图5，所述对所述第一姿态数据进行姿态标定，得到第二姿态数据这一步骤，具体包括：
[0081]
s301、获取第一姿态数据；
[0082]
s302、根据所述第一姿态数据以及标定公式，得到第二姿态数据；其中所述标定公式为
[0083]
q[j]＝calim[j]
×qint
[j]；
[0084]
上式中，q
int
[j]表示惯性动捕模块输出的初始姿态数据，calim[j]表示惯性动捕模块的标定矩阵；
[0085]
具体地，在标定的过程中，需要将得到的第一姿态数据输入标定公式，标定公式为
[0086]
q[j]＝calim[j]
×qint
[j]；
[0087]
其中，q
int
[j]表示惯性动捕模块输出的初始姿态数据，calim[j]表示惯性动捕模块的标定矩阵，两个数据均可通过预先对惯性动捕模块进行参数设置得到。
[0088]
进一步地，参照图6，所述根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息这一步骤，具体可以包括
[0089]
s401、获取连续两帧数据中运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；
[0090]
s402、根据所述速度信息以及节点位置公式，得到目标骨骼节点的第三位置信息；
[0091]
其中，所述节点位置公式为
[0092][0093]
上式中，acc[i]为当前帧的去重力加速度，_acc[i]表示上一帧的去重力加速度，vel[i]表示当前帧的速度，_vel[i]表示上一帧的速度，初始时刻_vel[i]为0，δt表示一帧的时间，_pos[i]表示上一帧的节点位置，pos[i]表示当前帧的节点位置。
[0094]
具体地，在本技术实施例中，可以通过惯性动捕模块获取包括当前帧在内的连续两帧数据中运动骨骼节点的速度信息，其中速度信息可以包括去重力加速度度以及运动速度，根据下述公式可以得到目标骨骼节点的第三位置信息；
[0095][0096]
上式中，acc[i]为连续帧数据中当前帧的去重力加速度，_acc[i]表示连续帧中上一帧的去重力加速度，vel[i]表示当前帧的速度，_vel[i]表示上一帧的速度，初始时刻_
vel[i]为0，δt表示一帧的时间，__pos[i]表示上一帧的节点位置，pos[i]表示当前帧的节点位置。
[0097]
进一步地，还包括步骤：确定所述人体虚拟模型处于第三运动状态，调整人体骨骼所有节点的位置信息；
[0098]
具体地，由于人体虚拟模型可以处于第三运动状态，第三运动状态为左右脚同时离开地面大于一定高度差的运动状态，如人体虚拟模型上下楼梯的过程的运动状态，当人体虚拟模型处于第三运动状态，可以调整人体骨骼所有节点的位置信息。
[0099]
进一步地，所述调整人体骨骼所有节点的位置信息这一步骤，具体包括：
[0100]
s501、确定人体骨骼节点变化的高度差；
[0101]
s502、根据所述高度差，调整人体骨骼所有节点的位置信息；
[0102]
具体地，可以通过惯性动捕模块确定人体骨骼节点变化的高度差，通过高度差可以调整人体骨骼所有节点的位置信息，具体地，若前后两帧的高度差为正数，则所有人体骨骼节点的位置信息均增加高度差数值，若前后两帧的高度差为负数，则所有人体骨骼节点的位置信息均增加高度差数值。
[0103]
下面以目标节点为腰部节点为例，下面对本技术的三维空间定位方法进行说明：
[0104]
首先在进行三维空间定位之前，在人体关键的17个部位都佩戴一个惯性动捕模块，编号1到17，每一个惯性动捕模块由9轴传感器(陀螺仪、加速度计、磁力计)和微处理器组成，惯性动捕模块的微处理器与9轴传感器分别连接，用于9轴传感器校准并进行9轴数据融合姿态解算，生成姿态信息，并把姿态数据传入控制器模块；控制器模块包含电池、数据通信模块和微处理器，电池对整个惯性动捕硬件供电，而微处理器把各惯性动捕模块传入的姿态数据合成帧，由数据通信模块发送到数据终端。数据终端根据发送过来的姿态数据在经过标定后，即可通过虚拟模型对佩戴者的各种运动进行实时显示，也可对运动数据进行保存；所佩戴的部位分别为：腰部、右大腿、右小腿、右脚、左大腿、左小腿、左脚、背部、头部、右肩膀、右大臂、右小臂、右手、左肩膀、左大臂、左小臂和左手。而具体地，对于行人导航，一般是双脚在运动，需将惯性动捕模块1-7号分别佩戴在腰部、右大腿、右小腿、右脚、左大腿、左小腿以及左脚。
[0105]
实现行人导航具体步骤如下：
[0106]
步骤一：惯性动捕模块的姿态输出；
[0107]
统计分析每个惯性动捕模块各个方位9轴数据，建立零偏、正交因子的最小二乘椭圆拟合模型，校准惯性动捕模块，以获得准确的9轴实时理论值，用于卡尔曼滤波姿态估计，最后输出惯性动捕模块姿态。
[0108]
步骤二：虚拟模型姿态标定；
[0109]
因佩戴者的差异性及每次佩戴部位的不一致性，需要对惯性动捕模块输出的姿态数据进行标定，只有通过标定，惯性动捕模块的姿态数据才能驱动虚拟模型，标定公式为：
[0110]
q[j]＝calim[j]
×qint
[j]
[0111]
其中，q
int
[j]表示每一个惯性动捕模块输出的初始姿态数据，calim[j]表示每一个惯性动捕模块的标定矩阵，q[j](q1，q2，q3，q4)表示虚拟模型的某一段关键骨骼的姿态数据，j代表1到17号某一段关键骨骼的序号，虚拟模型的骨骼不局限于此，可扩展为标准的23段骨骼，采用相邻两关键骨骼的四元数进行插值即可获得中间段骨骼的四元数。
[0112]
步骤三：运动状态的获取；
[0113]
采集行走时下肢惯性动捕模块的数据，统计分析行人的步态特征，经神经网络模型输出，即可获得实时准确的零速信息，通过零速信息可以确定人体虚拟模型的运动状态。如图9所示：zupt_rf表示右脚零速状态，zupt_lf表示左脚零速状态，zupt_rf为0时表示右脚处于运动状态，zupt_rf为1时表示右脚处于静止状态，zupt_lf为0时表示左脚处于运动状态，zupt_lf为1时表示左脚处于静止状态，此时人体虚拟模型处于第一运动状态，如图中时间片段2、时间片段4时间片段6时间片段8和时间片段10；当zupt_rf和zupt_lf同时为1时表示行人双脚触地，此时人体虚拟模型处于短暂静止状态，如图9中时间片段1、时间片段3、时间片段5、时间片段7、时间片段9和时间片段12；当zupt_rf为0和zupt_lf同时为0时表示行人双脚离地，左右脚都在移动，处于跳跃状态，如图9中时间片段11；一般行人行走的过程中双脚换脚时会有同时触地的短暂瞬间，如图9中时间片段3、时间片段5、时间片段7、时间片段9，利用这双脚触地的过程，可对位姿进行修正。
[0114]
步骤四：三维空间定位解算；
[0115]
根据步骤二中获得的每一段关键骨骼姿态数据，获得对应的姿态矩阵，公式如下：
[0116][0117]
其中，q[j]表示虚拟模型每一关键骨骼姿态四元数，rotm[j]表示对应的姿态矩阵，j代表虚拟模型的关键骨骼所属序号[1-17]。
[0118]
利用步骤三中零速区间的获取，根据刚体的运动学规律，可获得虚拟模型的每个关节点的位置信息，初始位置可以通过，公式如下：
[0119]
pos[j]＝pos[i]-rotm[i]
×
(intp[i]-intp[j])
[0120]
上述公式称为位移迭代公式，其中intp[i]和intp[j]表示已知的虚拟模型骨架节点坐标信息(初始节点)，可手动编辑模型骨架信息跟真实佩戴者相当，pos[i]和pos[j]表示实时驱动的虚拟模型每一个关节节点位置信息，i代表上一关节(父节点)序号，j代表下一关节(子节点)序号。
[0121]
当zupt_rf为1并且zupt_lf为1时，双脚处于静止状态，选择左脚或者右脚作为初始解算根节点，若选择右脚为初始解算根节点，把上一帧的pos[4]赋予当前帧pos[4]，i值为[4、3、2、1、5、6]，j值为[3、2、1、5、6、7]，完成从右脚节点解算到右小腿节点，再从右小腿节点到右大腿节点，再从右大腿节点到腰部节点，再从腰部节点到左大腿节点，再从左大腿节点到左小腿节点，再从左小腿节点到左脚节点，从而完成整条运动链的节点坐标解算；亦可选择左脚为初始解算根节点，把上一帧的pos[7]赋予当前帧pos[7]，i值为[7、6、5、1、2、3]，j值为[6、5、1、2、3、4]，完成从左脚到右脚整条运动链的节点坐标解算，亦可双向同时进行融合解算。
[0122]
当zupt_rf为1并且zupt_lf为0时，右脚处于静止状态，左脚处于运动状态，把右脚为初始解算根节点，把连续帧的上一帧的pos[4]赋予当前帧pos[4]，则i值为[4、3、2、1、5、6]，j值为[3、2、1、5、6、7]，完成从右脚节点解算到右小腿节点，再从右小腿节点到右大腿节点，再从右大腿节点到腰部节点，再从腰部节点到左大腿节点，再从左大腿节点到左小腿节点，再从左小腿节点到左脚节点，从而完成整条链的节点坐标解算；或者，当zupt_rf为0并且zupt_lf为1时，右脚处于运动状态，左脚处于静止状态，把左脚为初始解算根节点，把上
一帧的pos[7]赋予当前帧pos[7]，则i值为[7、6、5、1、2、3]，j值为[6、5、1、2、3、4]，从左脚节点解算到左小腿节点，再从左小腿节点到左大腿节点，再从左大腿节点到腰部节点，再从腰部节点到右大腿节点，再从右大腿节点到右小腿节点，再从右小腿节点到右脚节点，从而完成整条链的节点坐标解算。
[0123]
当zupt_rf为0并且zupt_lf为0时，双脚处于运动状态，分别计算左右脚的三维空间位置，公式如下：
[0124][0125]
其中，acc[i]取自惯性动捕模块的输出，表示当前帧的去重力加速度，i值为1表示腰部，i值为4表示右脚，i值为7表示左脚，_acc[i]表示上一帧的去重力加速度，vel[i]表示当前帧的速度，_vel[i]表示上一帧的速度，初始时刻_vel[i]为0，δt表示一帧的时间，_pos[i]表示上一帧的节点位置，pos[i]表示当前帧的节点位置。由于惯性动捕模块积分误差的存在，结合左右腿7个节点的信息进一步融合，从左脚的位置按照位移迭代公式迭代到腰部节点，获得一个腰部的位置信息pos1[1]，再从右脚的位置按照位移迭代公式迭代到腰部节点，又获得一个腰部的位置信息pos2[1]，近而对pos[1]、pos1[1]和pos2[1]三个信息加权重融合滤波处理，获得更加精确的位置信息，最后通过腰部的精确的位置信息，得到人体骨骼所有节点的位置信息。
[0126]
步骤五：三维空间定位修正；
[0127]
在执行上述步骤四三维空间定位解算的同时，判断行人在行走过程中换脚时的左右脚最低点高度差δh的特征，区分行人是在平面行走还是在上下楼梯；若是换脚时δh小于阈值β，则表示在平面行走，地面高度h
ground
不做更新；反之处于上下楼梯过程中，判断是上楼梯还是下楼梯，若是上楼梯则更新地面高度h
ground
＝h
ground
+h
stair
，h
stair
为台阶高度；若是下楼梯则更新地面高度h
ground
＝h
ground-h
stair
；此外，在上下楼梯的过程中，还对上下台阶的高度h
stair
及台阶数stairs进行配对统计，使得上下某一台阶的时候，根据这一台阶的记录信息来精确的控制换脚时三维空间的垂直高度信息。
[0128]
此外、参照图7，与图1的方法相对应，本技术的实施例中还提供一种三维空间定位系统，包括：第一获取单元1001，用于人体骨骼惯性动捕的第一姿态数据；第一处理单元1002，用于根据所述第一姿态数据确定人体虚拟模型的运动状态；第二获取单元1003，用于获取人体虚拟模型的静止骨骼节点的第一位置信息；第二处理单元1004，用于根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；第三获取单元1005，用于获取人体虚拟模型的运动骨骼节点的速度信息；第三处理单元1006，用于根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；第四处理单元1007，用于根据所述第二位置信息以及第三位置信息，确定目标骨骼节点的目标位置信息；第五处理单元1008，用于根据所述目标位置信息，确定人体骨骼所有节点的位置信息。
[0129]
上述方法实施例中的内容均适用于本装置实施例中，本装置实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。
[0130]
与图8的方法相对应，本技术实施例还提供了一种三维空间定位装置，其具体结构可参照图x，包括：
[0131]
至少一个处理器1011；
[0132]
至少一个存储器1012，用于存储至少一个程序；
[0133]
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现所述的三维空间定位方法。
[0134]
上述方法实施例中的内容均适用于本装置实施例中，本装置实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。
[0135]
与图1的方法相对应，本技术实施例还提供了一种存储介质，其中存储有处理器可执行的指令，所述处理器可执行的指令在由处理器执行时用于执行所述的三维空间定位方法。
[0136]
在一些可选择的实施例中，在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如，取决于所涉及的功能/操作，连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外，在本技术的流程图中所呈现和描述的实施例以示例的方式被提供，目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的，其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。
[0137]
此外，虽然在功能性模块的背景下描述了本技术，但应当理解的是，除非另有相反说明，功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中，或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是，有关每个模块的实际实现的详细讨论对于理解本技术是不必要的。更确切地说，考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下，在工程师的常规技术内将会了解该模块的实际实现。因此，本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本技术。还可以理解的是，所公开的特定概念仅仅是说明性的，并不意在限制本技术的范围，本技术的范围由所附权利要求书及其等同方案的全部范围来决定。
[0138]
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干程序用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-on ly memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0139]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行程序的定序列表，可以具体实现在任何计算机可读介质中，以供程序执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从程序执行系统、装置或设备取程序并执行程序的系统)使用，或结合这些程序执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供程序执行系统、装置或设备或结合这些程序执行系统、装置或设备而使用的装置。
[0140]
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0141]
应当理解，本技术的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的程序执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0142]
在本说明书的上述描述中，参考术语“一个实施方式/实施例”、“另一实施方式/实施例”或“某些实施方式/实施例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本技术的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
[0143]
尽管已经示出和描述了本技术的实施方式，本领域的普通技术人员可以理解：在不脱离本技术的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型，本技术的范围由权利要求及其等同物限定。
[0144]
以上是对本技术的较佳实施进行了具体说明，但本技术并不限于所述实施例，熟悉本领域的技术人员在不违背本技术精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本技术权利要求所限定的范围内。

技术特征：

1.一种三维空间定位方法，其特征在于，包括以下步骤：获取人体骨骼惯性动捕的第一姿态数据；根据所述第一姿态数据确定人体虚拟模型的运动状态；确定人体虚拟模型处于第一运动状态，获取人体虚拟模型的静止骨骼节点的第一位置信息；根据所述第一位置信息以及所述第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；确定人体虚拟模型处于第二运动状态，获取人体虚拟模型的运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；根据所述第二位置信息以及第三位置信息，确定目标骨骼节点的目标位置信息；根据所述目标位置信息，确定人体骨骼所有节点的位置信息。2.根据权利要求1所述一种三维空间定位方法，其特征在于，所述根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型目标骨骼节点的第二位置信息这一步骤，具体包括：根据所述第一姿态数据，确定第一姿态矩阵；根据所述第一位置信息、第一姿态矩阵以及位移迭代公式，确定人体虚拟模型目标骨骼节点的第二位置信息；其中所述位移迭代公式包括：pos[j]＝pos[i]-rotm[i]
×
(intp[i]-intp[j])；上式中，其中intp[i]和intp[j]表示虚拟模型骨架节点的原始坐标信息，pos[i]为静止骨骼节点的第一位置信息，pos[j]目标骨骼节点的第二位置信息；rotm[i]为第一姿态矩阵。3.根据权利要求2所述一种三维空间定位方法，其特征在于，所述根据所述第一姿态数据，确定第一姿态矩阵这一步骤，具体包括：对所述第一姿态数据进行姿态标定，得到第二姿态数据；根据所述第二姿态数据以及矩阵计算公式，确定第一姿态矩阵；其中所述矩阵计算公式为上述矩阵计算公式中，q[j]表示标定后的第二姿态数据，rotm[j]表示对应的姿态矩阵，j代表虚拟模型的关键骨骼所属序号。4.根据权利要求1所述一种三维空间定位方法，其特征在于，所述根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息这一步骤，具体包括：获取连续两帧数据中运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度；根据所述速度信息以及节点位置公式，得到目标骨骼节点的第三位置信息；其中，所述节点位置公式为
上式中，acc[i]为当前帧的去重力加速度，_acc[i]表示上一帧的去重力加速度，vel[i]表示当前帧的速度，_vel[i]表示上一帧的速度，初始时刻_vel[i]为0，δt表示一帧的时间，_pos[i]表示上一帧的目标节点的位置信息，pos[i]表示当前帧的目标节点的位置信息。5.根据权利要求3所述一种三维空间定位方法，其特征在于，所述对所述第一姿态数据进行姿态标定，得到第二姿态数据这一步骤，具体包括：获取第一姿态数据；根据所述第一姿态数据以及标定公式，得到第二姿态数据；其中所述标定公式为q[j]＝calim[j]
×
q
int
[j]；上式中，q
int
[j]表示惯性动捕模块输出的初始姿态数据，calim[j]表示惯性动捕模块的标定矩阵。6.根据权利要求1所述一种三维空间定位方法，其特征在于，还包括步骤：确定所述人体虚拟模型处于第三运动状态，调整人体骨骼所有节点的位置信息。7.根据权利要求6所述一种三维空间定位方法，其特征在于，所述调整人体骨骼所有节点的位置信息这一步骤，具体包括：确定人体骨骼节点变化的高度差；根据所述高度差，调整人体骨骼所有节点的位置信息。8.一种三维空间定位系统，其特征在于，包括：第一获取单元，用于人体骨骼惯性动捕的第一姿态数据；第一处理单元，用于根据所述第一姿态数据确定人体虚拟模型的运动状态；第二获取单元，用于获取人体虚拟模型的静止骨骼节点的第一位置信息；第二处理单元，用于根据所述第一位置信息以及第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；第三获取单元，用于获取人体虚拟模型的运动骨骼节点的速度信息；第三处理单元，用于根据所述速度信息确定人体虚拟模型目标骨骼节点的第三位置信息；第四处理单元，用于根据所述第二位置信息以及第三位置信息，确定目标骨骼节点的目标位置信息；第五处理单元，用于根据所述目标位置信息，确定人体骨骼所有节点的位置信息。9.一种三维空间定位装置，其特征在于包括：至少一个处理器；至少一个存储器，用于存储至少一个程序；当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如权利要求1-7任一项所述一种三维空间定位方法。10.一种存储介质，其中存储有处理器可执行的指令，其特征在于，所述处理器可执行的指令在由处理器执行时用于执行如权利要求1-7任一项所述一种三维空间定位方法。

技术总结

本申请公开了一种三维空间定位方法、系统、装置和存储介质，其中方法包括以下步骤：获取人体骨骼惯性动捕的第一姿态数据；根据所述第一姿态数据确定人体虚拟模型的运动状态；确定人体虚拟模型处于第一运动状态，获取人体虚拟模型的静止骨骼节点的第一位置信息；根据所述第一位置信息以及所述第一姿态数据，确定人体虚拟模型的目标骨骼节点的第二位置信息；确定人体虚拟模型处于第二运动状态，获取人体虚拟模型的运动骨骼节点的速度信息；所述速度信息包括去重力加速度以及运动速度。本方法可以减少人体骨骼三维空间定位的误差，提高三维空间定位的准确度。本申请可广泛应用于三维空间定位技术领域内。定位技术领域内。定位技术领域内。