本发明涉及室内导航技术，具体地，涉及一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法及系统。
背景技术：
：近年来，行人导航已成为导航定位领域一个活跃的分支。在城市、室内、地下等卫星信号受限的环境中，行人导航系统可实时监测人员的位置信息，有效地解决“我在哪里”、“我应该在哪里”、“如何到达目的地”等问题，快速准确地为行人提供导航与定位服务。因此，行人导航系统研究对复杂环境中的消防抢险、突发事件中的紧急救援、未知环境中的自我定位及目标找寻等都具有重要的意义。在室外环境下，gps能够提供精确的定位导航服务,但在室内环境中,由于建筑内部构造的复杂性和屏蔽性，gps定位导航无法满足用户室内精准定位的需求。目前，室内定位和导航大多依赖于室内平面图的约束,而其构造往往需要大量的人工勘测(如室内公共场所：医院、大型商场、火车站等),大大降低室内定位导航系统的可扩展性。而且，传统室内定位技术主要依靠wifi或路标的导航方案，其采集信息单一，后期维护成本高，且容易受到室内墙体影响，比如因信号衰变而导致的较大定位误差。imu因具备体积小、质量小、可操作性强、抗干扰性好等优点，正逐步在室内外定位和导航工程中得到了广泛应用。通常情况下，imu平台包含有一个3轴的加速度计和一个3轴的陀螺仪，加速度计传感器可以实时的反馈行人的运动中的加速度信息，陀螺仪测量的是运动物体的角速度，通过积分可以得出运动物体的角度。通过这两个传感器在理想的情况下可以直接测得行人的轨迹，但是由于imu存在漂移和积分误差，导致定位结果很不稳定，特别是垂直方向，由于重力加速度的影响，导致高度向位移的解算存在很大的偏差。针对该问题，常规解决方案是使用imu结合气压计的方法计算楼梯高度以实现垂直方向定位，但气压计容易受温度、气流强度等因素影响，在火灾等恶劣环境中难以实现垂直高度向的定位精度。技术实现要素：有鉴于此，本发明实施例期望提供一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法及系统，能够提供水平向和垂直高度向的高精度轨迹导航服务，以解决复杂环境中的消防抢险、突发事件中的紧急救援、未知环境中的自我定位及目标找寻位置预知等关键问题。为达到上述目的，本发明实施例的技术方案是这样实现的：本发明实施例提供一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法，所述方法包括：接收imu发送的实时采集数据；根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵；根据所述实时采集数据判别行人的步态，并根据所述步态获得行人分别在平面和高度方向运动的距离和位置；根据所述行人分别在平面和高度方向运动的距离和位置以及行人的速度和位置获得行人的行走轨迹。优选地，所述根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵的具体过程包括：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述采集数据对行人速度、位置、姿态角及旋转矩阵进行更新。优选地，所述行人的运动状态包括行走状态或静止状态。优选地，所述方法还包括在根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置之前对行人的运动轨迹进行零速修正的步骤。优选地，所述方法还包括在接收imu发送的实时采集数据之后对所述imu进行零偏校正的步骤。本发明实施例还提出了一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪系统，所述系统包括：imu，佩戴在行人身上，用于检测行人的运动姿态；处理器，其被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵；根据所述实时采集数据判别行人的步态，并根据所述步态获得行人分别在平面和高度方向运动的距离和位置；根据所述行人分别在平面和高度方向运动的距离和位置以及行人的速度和位置获得行人的行走轨迹。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述采集数据对行人速度、位置、姿态角及旋转矩阵进行更新。优选地，所述行人的运动状态包括行走状态或静止状态。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置之前对行人的运动轨迹进行零速修正。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在接收imu发送的实时采集数据之后对所述imu进行零偏校正。本发明的有益效果如下：本发明针对imu存在漂移和积分误差，特别是重力加速度的影响方面，对imu数据进行零偏校正，进而得到较为准确的水平向的位移。然后根据行人的步态特征，区分上下楼和走平地的不同特点，进而判断行人是在走平地、上楼还是下楼。本发明只需使用惯性传感器即可在行人室内定位中得到精确地位置信息，避免了多源传感器进行数据处理和融合的复杂性和工作冗余，以解决复杂环境中的消防抢险、突发事件中的紧急救援、未知环境中自我定位及目标找寻无法预知自身位置等关键问题。附图说明图1为本实施例所述的基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法的流程图；图2为本实施例所述的接收imu发送的实时采集数据，根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵的流程图；图3为本实施例所述的基于零速检测的步态识别结果图；图4为本实施例所述的基于参数优化后的步态识别结果图；图5为本实施例所述的优化前真实轨迹和重建轨迹的示意图；图6为本实施例所述的优化后真实轨迹和重建轨迹的示意图；图7为本实施例所述的使用线性步态判别函数后的纯惯导三维定位结果图；图8为本实施例所述的只采用纯惯导三维室内定位结果图；图9为本实施例所述的使用气压计加惯性传感器的室内定位结果图。具体实施方式为了使本发明实施例中的技术方案及优点更加清楚明白，以下结合附图对本发明的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本发明的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。如图1所示，本实施例提出了一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法，所述方法具体包括：s101，对imu进行零偏校正。具体的，在行人打开设备行走之前，会有10s至20s静止的阶段，利用零速检测原理，找到行人刚开始有动作时产生的数据位置，将该数据位置之前的所有数据做平均，记录下平均值的大小为b，将此均值b视为传感器测量数据本身带有的偏差，因此需要将此点位置之后的点所产生的数据均减去b，这样可以减少误差的影响。s102，接收imu发送的实时采集数据，根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵。如图2所示，所述s102还具体包括如下步骤：s1021，根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态。具体的，在行人导航领域，步态检测的主要目的在于检测步态周期中的支撑相，即脚落地的阶段，本实施例采用改进的零速检测算法的同时基于加速度和角速度数据的广义似然比检测算法。所述广义似然比检测公式如下：其中，tk表示k时刻的检测统计量，w代表滑动窗口的大小，δa代表加速度计的噪声方差，δw代表陀螺仪的噪声方差，代表测得的加速度的值，代表窗口w内加速度计的采样数据的均值，代表窗口w内陀螺仪的采样数据的均值，g代表当地的重力加速度，n代表该采样点的时刻。对行人的状态处于静止还是运动做出判别。如果公式(1)中的tk＞td,说明此刻行人为运动状态，否则为静止状态，这里的td是预先设定的阈值，它的选取选择根据实验的效果进行筛选得到。阈值的筛选方法：首先看tk分布的范围，然后选取一些合适阈值参数进行测试。测试原则：选定一个长50米的直线，通过每个阈值行走五组，解算出每个阈值对应的五组实验的平均值，查看哪个更接近真实的值。经过多次的测试，列出如表1所示。表1阈值取值0.1×1050.2×1050.3×1050.4×1050.5×1051×105估计出的距离48..64m49.03m49.48m49.01m48.74m48.43m准确率97.28％98.06％98.96％98.0297.48％96.86％根据测试数据可以看出，选取第三组实验的阈值比较合适，即td＝0.3×105。本实施例所述的改进的零速检测算法是在上述广义似然比检测算法的基础上增加了一个预设值时间阈值tl。由于支撑相内测量局部下行波动而诱发的伪摆动相，其往往持续时间较短，于是采用增加时间阈值的检测方法，该方法将所有持续时间小于tl的摆动相均视为伪摆动相(如图3中窄的为零的部分)，将其从候选摆动相中删除变成支撑相即脚运动的阶段。传统的步态检测结果如图3所示。图中标号为1、2和3的分别代表x、y和z方向上的位移，标号为4的线条代表步态检测结果，等于1表示支撑相即静止阶段，等于0表示摆动相即脚运动阶段。修改后的步态检测结果图如图4所示。从图3可以看出，步态检测结果中存在很多伪摆动相，这会导致位置解算很不准确。改进的步态检测算法将步态有效地划分开来。s1022，根据所述实时采集数据确定行人初始姿态角及旋转矩阵。具体的，初始对准是惯性导航系统的关键技术之一，初始对准精度和速度直接影响导航系统的工作性能。初始速度和初始位置的误差并不大，可不予考虑，初始对准的主要目的在于确定初始时刻的姿态角和旋转矩阵。对于捷联惯导系统而言，姿态矩阵起到了平台的作用，进行初始对准相当于建立系统的数学平台。加速度计在水平静止状态下只会受到重力g的影响，因此，利用此特性，在静止的状态下独处加速度计的3个轴上的值，利用如下公式获得初始姿态角：phi代表横滚角，theta代表俯仰角，初始情况下设定航向角为0。通过上述姿态角利用四元数的方法可以得到姿态矩阵，计算方法如下：设定一个初始的q＝[q0q1q2q3]t；四元数通过如下公式进行更新：其中wxwywz代表陀螺仪三个轴上的角速度，即可以换做如下公式：本实施例采用毕卡逼近的方法，可以得到如下迭代公式,进而得出旋转矩阵其中，δθx,δθy,和δθz通过如下公式计算得出：δθx＝wx×δt,δθy＝wy×δt,δθz＝wz×δt。最后就可以得到旋转矩阵的值如下：s1023，根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述采集数据对行人速度、位置、姿态角及旋转矩阵进行更新。具体的，捷联惯导解算行人的速度和位置，利用卡尔曼滤波器和零速更新对行人的速度、位移和姿态角进行更新，具体过程如下：1、捷联惯导结算行人的速度和位移：上述公式(7)求解出了旋转矩阵由旋转矩阵和载体系的加速度求出导航系的加速度。其中，f为导航系下三个轴的加速度(f(1)，f(2)，f(3))，accx、accy、accz分别代表载体系下三个轴的加速度。其中，代表姿态转换矩阵，可以将载体系下的数据转换到导航系下。根据捷联惯导中的积分可以求出位移和速度如下公式(9)、(10)vk＝vk-1+f×δt(9)其中vk为k时刻的速度，vk-1为k-1时刻的速度，δt为采样间隔，sk为k时刻的位移，sk-1为k-1时刻的位移。2、应用于卡尔曼滤波器的系统过程模型如下：公式(11)为状态方程，其中括号里面分别代表x，y和z三个方向的位移误差、速度误差、姿态角误差。kk＝pk,k-1ht(rk+hpk,k-1ht)-1(13)pk,k-1＝ak-1pk-1,k-1ak-1t+qk-1(14)pk,k＝(i-kkh)pk,k-1(15)为待估计量，kk为卡尔曼滤波增益，h为观测矩阵，zk为测量数据，pk,k-1为预测协方差，pk,k为更新协方差，这里代表k时刻的转移矩阵并且其中qw为噪声协方差矩阵。f(1)，f(2)，f(3)由公式(8)得到。卡尔曼滤波器的使用就是利用行人脚落地时速度为零这一特性，设定公式(12)中zk的取值为0，利用该特性对人体的位移、速度进行矫正。具体的矫正和更新方程如下公式(16)、(17)、(18)。dk＝kk(0-vk)(16)其中dk为k时刻位移、速度、姿态角的偏差，是9维向量，dk(4-6)表示速度的偏差，dk(1-3)表示位移的偏差，为k时刻更新后的速度，为k时刻更新后的位移，λ是我们设定的优化参数。下面介绍姿态角的更新过程，利用dk(7-9)的数据构造反对称矩阵如下：其中为k时刻更新后的旋转矩阵，此即姿态角度的更新。s103，根据所述实时采集数据判别行人的步态，并根据所述步态获得行人分别在平面和高度方向运动的距离和位置。具体的，本实施例提出了一种线性步态判别法，通过线性步态判别函数根据行人每一步行走的特性判别行人上下楼梯还是走平地；如果是爬楼梯，则进一步判断是上楼梯还是下楼梯。根据判别结果做出相应的策略。具体方法如下：通过捷联惯导解算可以求出当前时刻预测的位置和速度，经过实际测量发现，当一成年人走平地时，每一步水平方向的位移为0.9m-1.15m，上下楼梯时，如果一步登一个台阶，水平方向位移为0.3m-0.38m，如果一步登两个台阶，水平方向的位移为0.6m-0.75m,可以看出，这两种场景在步长上有很大的区别，因此，提取步长使得步态特征具有较大的区分度。我们设置阈值ds＝0.8m，通过比较水平方向的位移，区分上下楼梯和走平地。将上下楼梯标记为p＝1，走平地标记为p＝0，每一步的计算公式如下其中dxy为每一步的位移，xend为行走过程中x方向脚摆动的最后一个点的位置，xstart为行走过程中x方向脚摆动的第一个点之前的点位置即运动的初始点；y方向同理。如果判断为上下楼梯，则我们需要进一步区分是上楼梯还是下楼梯。这里通过利用捷联惯导解算的z轴位移来判断，众所周知，纯惯导定位中，利用z轴的重力加速度解算z轴的位移的具体数值很不准确，因为z轴重力加速度的影响使结果产生漂移，并且随着时间的增加漂移越来越严重。这就是纯惯导在高度方向解算的弊端。但通过大量实验我们发现，虽然每次都会产生漂移，但解算的结果还是能准确的反应上升和下降的特征，每一步z轴高度的计算公式如下dz＝zend-zstart(21)其中dz为每一步垂直方向的位移，zstart和zend同x、y方向。综上，可以得到线性步态判别函数：如果判断为上楼梯则进一步设置阈值判断是登一个台阶还是两个，设置另一个阈值为dsl＝0.5m，若dz＞dsl把脚摆动的最后一个点的位移赋值为初始点位移加楼梯高度的2倍，即zend＝zstart+2h；若dz＜dsl把脚摆动的最后一个点的位移赋值为初始点位移加楼梯高度的1倍，即zend＝zstart+h。若是下楼，则同理处理为zend＝zstart-2h，zend＝zstart-h如果判断为走平地则zend＝zstart，其中h为一阶楼梯台阶的高度h。台阶高度h的计算方法如下，经过多组数据测试，刚开始上楼梯的时候，楼梯高度的解算还是准确的，此时的漂移是微弱的，所以当第一次判断为爬楼梯时，并且判断出是一个台阶还是两个台阶，若是一个台阶则h＝dz1＝zend1-zstart1，若判断为两个台阶，则2h＝dz1＝zend1-zstart1，其中dz1为第一次登台阶的高度。把第一次计算的台阶高度h用于以后每次的计算中。初始台阶高度的验证实验如下表，选取三种不同的台阶高度0.15m、0.16m和0.165m，每组做20组实验验证，计算平均初始台阶高度如表2所示。表2台阶高度0.15m0.16m0.165m计算出的平均初始高度h0.1503m0.1603m0.1646m误差0.2％0.19％0.24％s104，根据所述行人分别在平面和高度方向运动的距离和位置以及行人的速度和位置获得行人的行走轨迹。下面以具体的实例来说明本实施例所述方法的优越性。选取某大学教学楼的其中一个楼层进行试验，在该楼层中再分别选取平地和楼梯处两个场景，实验人员身上装备有惯性传感器，其内部含有三轴加速度计和三轴陀螺仪以及三轴磁力计，可以通过rs232接口或者usb接口与计算机相连，该惯性传感器的采样频率为0.01s。实验一、对于水平方向轨迹的优化在室内进行多组试验，从图5和图6的对比中可以看出本实施例所提出的使用零偏校正和优化的零速检测算法更加接近实际值，在水平方向已经得到比较准确的解。实验一的测量数据如表3所示。表3实验二、通过实验一我们得到了水平方向上比较准确的解，在实验二中，采用本实施例提出的线性步态判别法，根据水平方向每一步的解算结果特性，利用线性判别函数，得到垂直方向的位移。然后得到室内的三维轨迹图。经实际测量，每层楼高3.5米，三层楼高10.5米，实验结果解算出楼层高度为10.34米，误差为1.5％，实验进行30组，误差均在3％以内。实验定位结果图如图7至图9所示，途中起始点均为(0,0,0)点，图7使用线性步态判别函数后的纯惯导三维定位结果图，图8为只采用纯惯导三维室内定位结果图，可以看出误差很大，已经漂移到14米。图9为使用气压计加惯性传感器的结算结果。图7，图8采用同一数据，实验数据如表4所示，图9加气压计的数据只是多了一列气压计的测量数据。气压计的数据如表5所示，包括序号和数据列。气压计的采样频率为0.01s,气压计计算高度的公式如下：其中，h为计算的当前高度值，p是当前时刻的气压值，p0初始气压值。加气压计的高度解算结果为10.74米，可以看出气压计的解算精度和改进的使用线性步态判别法的结果精度差不多，但使用气压计的缺点是使用多个传感器，增加数据处理量；气压计由于不稳定在走平地时会出现波动；在特殊环境如火灾等场景会受温度和气流影响导致结果很不准确。而改进的方法只利用纯惯导，既避免了使用多种传感器进行数据处理的麻烦，又解决复杂环境中的消防抢险、突发事件中的紧急救援及未知环境中的自我定位及目标找寻无法预知自身位置的关键问题。表4表5对应的，本发明实施例还提出了一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪系统，所述系统包括：imu，佩戴在行人身上，用于检测行人的运动姿态；处理器，其被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据确定并实时更新行人的速度、位置、姿态角及旋转矩阵；根据所述实时采集数据判别行人的步态，并根据所述步态获得行人分别在平面和高度方向运动的距离和位置；根据所述行人分别在平面和高度方向运动的距离和位置以及行人的速度和位置获得行人的行走轨迹。进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述采集数据对行人速度、位置、姿态角及旋转矩阵进行更新。更进一步的，所述行人的运动状态包括行走状态或静止状态。更进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置之前对行人的运动轨迹进行零速修正。更进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在接收imu发送的实时采集数据之后对所述imu进行零偏校正。本实施例针对imu存在漂移和几份误差，特别是重力加速度的影响方面，对imu数据进行零偏校正，进而得到较为准确的水平向的位移。然后根据行人的步态特征，区分上下楼和走平地的不同特点，进而判断处行人是在走平地、上楼还是下楼。本发明只需使用惯性传感器即可在行人室内定位中得到精确地位置信息，避免了多源传感器进行数据处理和融合的复杂性和工作冗余，以解决复杂环境中的消防抢险、突发事件中的紧急救援、未知环境中自我定位及目标找寻无法预知自身位置等关键问题。显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。当前第1页12