本发明涉及室内导航技术，具体地，涉及一种融合imu和视觉传感器的室内高精度轨迹重建方法及系统。
背景技术：
：近年来，行人导航已成为导航定位领域一个活跃的分支。在城市、室内、地下等卫星信号受限的环境中，行人导航系统可实时监测人员的位置信息，有效地解决“我在哪里”、“我应该在哪里”、“如何到达目的地”等问题，快速准确地为行人提供导航与定位服务。因此，行人导航系统研究对复杂环境中的消防抢险、突发事件中的紧急救援、未知环境中的自我定位及目标找寻等都具有重要的意义。在室外环境下，gps能够提供精确的定位导航服务,但在室内环境中,由于建筑内部构造的复杂性和屏蔽性，gps定位导航无法满足用户室内精准定位的需求。目前，室内定位和导航大多依赖于室内平面图的约束,而其构造往往需要大量的人工勘测(如室内公共场所：医院、大型商场、火车站等),大大降低室内定位导航系统的可扩展性。而且，传统室内定位技术主要依靠wifi或路标的导航方案，其采集信息单一，后期维护成本高，且容易受到室内墙体影响，比如因信号衰变而导致的较大定位误差。imu因具备体积小、质量小、可操作性强、抗干扰性好等优点，正逐步在室内外定位和导航工程中得到了广泛应用。通常情况下，imu平台包含有一个3轴的加速度计和一个3轴的陀螺仪，加速度计传感器可以实时的反馈行人的运动中的加速度信息，陀螺仪测量的是运动物体的角速度，通过积分可以得出运动物体的角度。通过这两个传感器在理想的情况下可以直接测得行人的轨迹，但是由于imu存在漂移和积分误差，导致定位结果很不稳定，特别是垂直方向，由于重力加速度的影响，导致高度向位移的解算存在很大的偏差。针对该问题，常规解决方案是使用imu结合气压计的方法计算楼梯高度以实现垂直方向定位，但气压计容易受温度、气流强度等因素影响，在火灾等恶劣环境中难以实现垂直高度向的定位精度。技术实现要素：有鉴于此，本发明实施例期望提供一种融合imu和视觉传感器的室内高精度轨迹重建方法及系统，在基于imu和视觉传感器的信息融合方案基础上，实现室内行人轨迹的精准重建。为达到上述目的，本发明实施例的技术方案是这样实现的：本发明实施例提供一种融合imu和视觉传感器的室内高精度轨迹重建方法，所述方法包括：接收imu发送的实时采集数据；根据所述实时采集数据对行人的运动轨迹进行预测并优化；接收视觉传感器采集到的室内环境信息；根据所述室内环境信息与所述实时采集数据对行人的运动轨迹进行定位纠正，重建获得最终的行人的运动轨迹。优选地，所述根据所述实时采集数据对行人的运动轨迹进行预测的具体过程包括：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述实时采集数据对行人速度、位置、姿态角及旋转矩阵进行更新；根据所述行人速度、位置、姿态角及旋转矩阵对行人的运动轨迹进行预测。优选地，根据所述实时采集数据对行人的运动轨迹进行优化的具体过程包括：预先设置行人的身高和体重比；根据所述预先设置的行人的身高和体重比在预定距离内采用imu多次检测出行人的运动轨迹；将多次检测获得的行人的运动距离与实际距离进行对比；筛选出检测获得的行人的运动距离等于实际距离时所对应的预先设置的行人的身高和体重比作为优化参数对行人的运动轨迹进行优化。优选地，所述接收视觉传感器采集到的室内环境信息的具体过程包括：在二维码中导入室内环境信息；将所述二维码设置在所述视觉传感器的采集区域；接收所述视觉传感器采集所述二维码获得的室内环境信息。优选地，所述方法还包括在接收imu发送的实时采集数据之前对所述imu进行零偏校正的步骤。本发明实施例还提出了一种融合imu和视觉传感器的室内高精度轨迹重建系统，所述系统包括：imu，佩戴在行人身上，用于检测行人的运动姿态；视觉传感器，用于采集辅助定位装置中预设的室内环境信息；辅助定位装置，设置在视觉传感器的采集区域中，用于视觉传感器获取所述辅助定位装置中预设的室内环境信息；处理器，其被配置具有处理器可执行指令以执行操作，使得：接收imu发送的实时采集数据；根据所述实时采集数据对行人的运动轨迹进行预测并优化；接收视觉传感器采集到的室内环境信息；根据所述室内环境信息与所述实时采集数据对行人的运动轨迹进行定位纠正，重建获得最终的行人的运动轨迹。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述实时采集数据对行人速度、位置、姿态角及旋转矩阵进行更新；根据所述行人速度、位置、姿态角及旋转矩阵对行人的运动轨迹进行预测。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：预先设置行人的身高和体重比；根据所述预先设置的行人的身高和体重比在预定距离内采用imu多次检测出行人的运动轨迹；将多次检测获得的行人的运动距离与实际距离进行对比；筛选出检测获得的行人的运动距离等于实际距离时所对应的预先设置的行人的身高和体重比作为优化参数对行人的运动轨迹进行优化。优选地，所述辅助定位装置采用预设有室内环境信息的二维码实现。优选地，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在接收imu发送的实时采集数据之前对所述imu进行零偏校正。本发明的有益效果如下：本发明提出应用视觉传感器解析二维码的方式，来弥补imu传感器的累积漂移误差。由于imu传感器工作特性会产生累积漂移误差，导致imu获取的相关测量数据无法准确计算行人定位信息。常规的定位辅助传感器实施维护成本高，定位可靠性也存在不足，因此本发明提出使用简易二维码(蕴含基础位置信息)作为定位辅助路标，利用该路标可以有效弥补imu的累积漂移误差，在经过长时间工作后，仍旧可以实现行人轨迹的精准定位和重建。附图说明图1为本实施例所述的融合imu和视觉传感器的室内高精度轨迹重建方法的流程图；图2为本实施例所述的根据所述实时采集数据对行人的运动轨迹进行预测的流程图；图3为本实施例所述的根据所述实时采集数据对行人的运动轨迹进行优化的流程图；图4为本实施例所述的优化前真实轨迹和重建轨迹的示意图；图5为本实施例所述的优化后真实轨迹和重建轨迹的示意图；图6为基于imu数据的参数优化轨迹重建效果图；图7为融合imu信息和二维码视觉信息的轨迹重建效果图。具体实施方式为了使本发明实施例中的技术方案及优点更加清楚明白，以下结合附图对本发明的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本发明的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。如图1所示，本实施例提出了一种融合imu和视觉传感器的室内高精度轨迹重建方法，所述方法具体包括：s101，对imu进行零速矫正。具体的，利用零速矫正原理，提取出行人刚开始有动作时所产生的数据位置，将该数据位置之前的所有数据做平均处理,并记录下该值的结果。该计算值为表示多次数据测量求均情况下的传感器测量偏差，故该点位置之后的所有测量点的数据均需要消除该值，以减少传感器漂移的影响。s102，根据所述实时采集数据对行人的运动轨迹进行预测并优化。进一步的，如图2所示，所述s102中所述的根据所述实时采集数据对行人的运动轨迹进行预测具体包括如下步骤：s1021，根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态。具体的，应用零速检测算法对行人的状态进行分析，使用公式(1)对静止或运动状态进行判别。其中，w代表滑动窗口的大小，δa，δw分别代表加速度计和陀螺仪的噪声方差，代表测得的加速度的值，分别代表这一段加速度计和陀螺仪的采样数据的均值，g代表当地的重力加速度，n代表该采样点的时刻。当t(zn)＞η时，说明此刻行人处于运动状态，否则为静止状态。这里的η是预先设定的阈值，根据真实实验效果进行筛选得到，首先看t(zn)分布的范围是多大，接着选取一些合适阈值参数进行测试。s1022，根据所述实时采集数据确定行人初始姿态角及旋转矩阵，根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述实时采集数据对行人速度、位置、姿态角及旋转矩阵进行更新。具体的，加速度计在水平静止状态下只会受到重力g的影响，因此利用此特性，在静止的状态下读出加速度计的3个轴上的值，利用如下公式得出初始姿态角：phi代表横滚角，theta代表俯仰角，初始情况下设定航向角为0。通过上述姿态角利用四元数的方法可以得到姿态矩阵，计算方法如下：设定一个初始的q＝[q0q1q2q3]t，四元数通过如下公式进行更新：其中wx,wy,wz代表陀螺仪三个轴上的角速度，即公式(3)可以换做如下公式：这里采用毕卡逼近的方法对公式(3)进行解算，可以得到如下迭代公式,进而得出旋转矩阵其中，δθx,δθy,和δθz通过如下公式计算得出：δθx＝wx×δt,δθy＝wy×δt,δθz＝wz×δt。最后就可以得到旋转矩阵的值如下：s1023，根据所述行人速度、位置、姿态角及旋转矩阵对行人的运动轨迹进行预测。具体的，本发明中状态方程模型如下：其中括号里面分别代表x，y和z三个方向的位移误差、速度误差、姿态角误差。kk＝pk,k-1ht(rk+hpk,k-1ht)-1(8)pk,k-1＝ak-1pk-1,k-1ak-1t+qk-1(9)pk,k＝(i-kkh)pk,k-1(10)为待估计量，kk为卡尔曼滤波增益，h为观测矩阵，zk为测量数据，rk为协方差矩阵，pk,k-1为协方差状态估计，这里代表k时刻的转移矩阵其中qw为噪声协方差矩阵。在该发明中主要通过如上的常用的卡尔曼滤波模型对位置进行估计，f(1)，f(2)，f(3)及h矩阵的取值在下文里面会提到。卡尔曼滤波器的使用就是利用人体脚落地时速度为零这一特性，设定(7)式中z的观测值为0，利用该特性对人体的位移、速度和姿态时刻进行矫正。位移计算公式如下：f代表导航系下加速度三个方向的大小(f(1)，f(2)，f(3))，其中代表姿态转换矩阵，可以将载体系下的数据转换到导航系下。基于公式(14)的位移计算公式，公式(15)是我们通过使用卡尔曼滤波和参数矫正方法对人体的位移进行精确计算。进一步的，如图3所示，所述s102中所述的根据所述实时采集数据对行人的运动轨迹进行优化的具体过程包括：s102a、预先设置行人的身高和体重比。s102b、根据所述预先设置的行人的身高和体重比在预定距离内采用imu多次检测出行人的运动轨迹；s102c、将多次检测获得的行人的运动距离与实际距离进行对比；s102d、筛选出检测获得的行人的运动距离等于实际距离时所对应的预先设置的行人的身高和体重比作为优化参数对行人的运动轨迹进行优化。具体的，设置人体的身高和体重为(λ，β)，根据测量实际情况取值为(0.8,1.7)，该方法采用事先标定的方式，在一定距离内使用imu去测试行人轨迹数据，将计算出的距离和实际的距离进行对比，经过重复试验和调整参数后，使得将计算出的距离贴近实际值。参数校正主要是对位移和姿态矩阵进行矫正，其中公式(17)就是利用参数β矫正n时刻的转移矩阵。s103，接收视觉传感器采集到的室内环境信息，根据所述室内环境信息与所述实时采集数据对行人的运动轨迹进行定位纠正，重建获得最终的行人的运动轨迹。具体的，本发明使用视觉传感器采集到二维码的信息，读取相应位置信息后，和imu信息相互融合，对行人轨迹进行重新构建。但利用视觉传感器对二维码进行解析之前，需要将视觉传感器和二维码做一个标定，利用标定好的数据来获取视觉传感器到二维码的距离和角度信息，继而解析出二维码上载有的地理位置信息，在此基础上，通过全局空间坐标转换，计算出行人所在定位信息。最后，需要将视觉解析出的定位信息和imu解算出的位置信息进行加权融合，具体加权融合实现方法如下：本发明中使用姿态角变化进行判定是否需要视觉解析出的定位信息进行矫正，即本发明中通过对比3秒之前的姿态角信息和现在的姿态角信息进行比较，如果姿态角的变化超过3度就使用摄像头进行矫正。数据融合采用按照比重分配的方式进行数据融合，如公式(18)其中，pos_truth代表融合后的位置信息，cam代表摄像头解析出的位置信息，imu_p代表经过imu解算出的位置信息。下面以两个具体事例说明本实施例所述方法的优越性。实验一、利用人体参数对行人轨迹进行优化实验。imu平台包含有一个3轴的加速度计和一个3轴的陀螺仪，加速度计传感器可以实时的反馈行人的运动中的加速度大小，陀螺仪测量的是运动物体的角速度，通过积分可以得出运动物体的角度。通过这两个传感器在理想的情况下可以直接测得行人的轨迹。选定一个长15米的直线，经过多次行走计算出解算出的位置信息和实际的值比较，查看哪个更接近合适的值。经过多次的测试的结果列出如表1所示。表1阈值取值0.1×1050.2×1050.3×1050.5×1051×105估计出的距离14.74m14.74m14.79m14.74m14.2m错误率1.7％1.7％1.4％1.7％5.3％在阈值选定的情况下，对本发明的参数优化算法进行实验验证。实施实验的环境场景信息为一条长走廊，该实验行走距离相对长度为39.6米，并将测得的数据带入到本发明的提出的算法中，解算出人体行走轨迹，这里实际轨迹由激光测距仪进行定点计算。通过观测图4和图5的对比情况(其中标号为1的线代表实际轨迹，标号为2的线代表经过imu进行解算得出的轨迹)，其中图4图代表未经参数优化得出的轨迹，图5代表经过参数优化之后得出的轨迹，对比两个图即可看出经过参数优化的轨迹比未经过参数优化的轨迹更加接近实际值。实验二、在室内环境下进行视觉传感器加imu的轨迹重建实验。一个惯性传感器(imu)和一个可见光摄像机(camera)；辅助定位装置采用带有室内环境位置信息的二维码。需要设置某室内环境的基础位置点，并将其相关定位信息导入到二维码，将二维码粘贴到对应位置墙面可见处。实验开始在事先标定好的(0,0)位置开始出发，向南行进7米，然后向东转向90°并前进41米后结束行程。实测数据选用了10803组实验测量数据对本发明进行计算。本次实验中，时间步长选为0.01s，通过图6和图7可以看出经过视觉传感器进行融合矫正后，本实验的轨迹误差能够控制在0.8米之内，而且也未发生明显方向偏转的现象。单用imu和卡尔曼滤波参数优化算法解算出的轨迹可以参见图6，其中标号为a的线代表实际的轨迹值，标号为b的线代表只用imu和参数优化得卡尔曼滤波解算出的值。融合视觉传感器矫正后的轨迹图看以参见图7，其中蓝色的线代表实际的轨迹值，红色的代表经过视觉传感器加imu同时处理过后得到的轨迹。通过对比这两个轨迹图，可以明显的看到本发明提出的算法加入视觉传感器矫正后对人体轨迹的定位精度更高更贴近实际。对应的，本实施例还提供了一种融合imu和视觉传感器的室内高精度轨迹重建系统，该系统包括：imu，佩戴在行人身上，用于检测行人的运动姿态；视觉传感器，用于采集辅助定位装置中预设的室内环境信息；辅助定位装置，设置在视觉传感器的采集区域中，用于视觉传感器获取所述辅助定位装置中预设的室内环境信息；处理器，其被配置具有处理器可执行指令以执行操作，使得：接收imu发送的实时采集数据；根据所述实时采集数据对行人的运动轨迹进行预测并优化；接收视觉传感器采集到的室内环境信息；根据所述室内环境信息与所述实时采集数据对行人的运动轨迹进行定位纠正，重建获得最终的行人的运动轨迹。进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：根据所述实时采集数据标定行人运动轨迹，并根据所述行人运动轨迹判断所述行人的运动状态；根据所述实时采集数据确定行人初始姿态角及旋转矩阵；根据所述行人运动轨迹、行人运动状态、行人初始姿态角及旋转矩阵获得行人速度和位置，并根据所述实时采集数据对行人速度、位置、姿态角及旋转矩阵进行更新；根据所述行人速度、位置、姿态角及旋转矩阵对行人的运动轨迹进行预测。进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：预先设置行人的身高和体重比；根据所述预先设置的行人的身高和体重比在预定距离内采用imu多次检测出行人的运动轨迹；将多次检测获得的行人的运动距离与实际距离进行对比；筛选出检测获得的行人的运动距离等于实际距离时所对应的预先设置的行人的身高和体重比作为优化参数对行人的运动轨迹进行优化。进一步的，所述辅助定位装置采用预设有室内环境信息的二维码实现。进一步的，所述处理器还被配置具有处理器可执行指令以执行操作，使得：在接收imu发送的实时采集数据之前对所述imu进行零偏校正。本实施例提出应用视觉传感器解析二维码的方式，来弥补imu传感器的累积漂移误差。由于imu传感器工作特性会产生累积漂移误差，导致imu获取的相关测量数据无法准确计算行人定位信息。常规的定位辅助传感器实施维护成本高，定位可靠性也存在不足，因此本发明提出使用简易二维码(蕴含基础位置信息)作为定位辅助路标，利用该路标可以有效弥补imu的累积漂移误差，在经过长时间工作后，仍旧可以实现行人轨迹的精准定位和重建。当前第1页12