CN104596504A - 应急救援场景下快速构建地图辅助室内定位方法及系统 - Google Patents

Abstract

本发明提供一种应急救援场景下快速构建地图辅助室内定位方法，包括：构建目标建筑物的三维地图；获取救援人员进入目标建筑物的初始位置坐标信息；读取传感数据，处理传感数据；通过预存的推算算法来推算当前时刻救援人员行走位置坐标信息；以救援人员双脚中的一只脚为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，修正存在的计算误差和位置计算累计误差，输出修正后的当前时刻救援人员行走位置坐标信息，若否，输出推算出的当前时刻救援人员行走位置坐标信息；将当前时刻救援人员行走位置坐标信息标注在目标建筑物的三维地图中。本发明实现了动态实时跟踪救援人员及为应急救援的整体指挥调度与救援人员的生命保障提供了辅助。

Description

应急救援场景下快速构建地图辅助室内定位方法及系统

技术领域

本发明属于室内定位领域，涉及一种定位方法及系统，特别是涉及一种应急救援场景下快速构建地图辅助室内定位方法及系统。

背景技术

当前成熟的室内定位方法主要基于WIFI、Bluetooth、WLAN、RFID、UWB等无线技术，以及基于惯性传感器的航位推算以及惯性导航方法等。其中，不同的室内定位技术适用于不同的应用，所有的基于无线技术进行定位的方法均依赖于事前部署相关的基站设备，并要求实现建立信号特征数据库，但是在应急救援的场景下，通常无法保证救援场所安装有以上所需设施，及时有也无法获知其具体位置，同时也不能保证这些设备在正常工作。所以基于无线技术的室内定位技术无法完成应急救援场景下的室内定位需求，不能保障救援人员的生命安全。

那么在应急救援等场景下就需要一种无需预先布置的自完备的定位技术，即基于惯性传感器的室内定位技术。此方法通过MEMS(Micro-Electro-Mechanical System，微机电系统)传感器，利用航位推算方法计算得到行人位置坐标信息。常用的计算方法主要有两种：一种方式是计算行人的步频乘以估计的步长得到位置信息，另一种方式是对加速度二次积分，并结合陀螺仪得到的方位得到位置信息。但是此两种方法均存在累计误差发散的问题。为了克服误差无限累积的缺点，可以利用零速更新、无线辅助组合定位以及如地图辅助定位等方法。

实际上，在应急救援情况下预先获得救援场所的室内地图也是不切实际的。但是应用研究指出，应急救援是人员定位精度达到建筑物内的房间位置即可。综合应用需求、定位技术的计算效率与精度。

因此，如何提供一种应急救援场景下快速构建地图辅助室内定位方法及系统以解决现有技术中在应急救援过程中无法动态实时跟踪救援人员以导致无法为应急救援的整体指挥调度与救援人员的生命保障提供辅助的现象，且无法满足现有应急救援的需求等种种缺陷，实已成为本领域从业者亟待解决的技术问题。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种应急救援场景下快速构建地 图辅助室内定位方法及系统，用于解决现有技术中在应急救援过程中无法动态实时跟踪救援人员以导致无法为应急救援的整体指挥调度与救援人员的生命保障提供辅助的现象，且无法满足现有应急救援的需求的问题。

为实现上述目的及其他相关目的，本发明一方面提供一种应急救援场景下快速构建地图辅助室内定位方法，所述应急救援场景发生在一目标建筑物内，该定位方法通过救援人员执行，所述救援人员佩戴传感模块，所述应急救援场景下快速构建地图辅助室内定位方法包括：步骤一，构建所述目标建筑物的三维地图；步骤二，令所述救援人员在进入所述目标建筑物之前加载所述目标建筑物的三维地图，并获取所述救援人员进入所述目标建筑物的初始位置坐标信息；步骤三，读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据，并对所述传感数据进行相应处理；所述传感数据中存在测量误差；步骤四，根据所述传感数据通过预存的航位推算算法推算当前时刻所述救援人员行走位置坐标信息；其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差和位置计算累计误差；步骤五，以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，则修正所述步骤四中存在的计算误差和位置计算累计误差以获取修正后的当前时刻所述救援人员所述救援人员行走位置坐标信息，输出修正后的当前时刻所述救援人员行走位置坐标信息，转入下一步骤，若否，则输出步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息，转入下一步骤；步骤六，将修正后的当前时刻救援人员行走位置坐标信息或步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息的标注在所述目标建筑物的三维地图中；步骤七，判断是否终止推算所述救援人员行走位置坐标信息；若是，将修正后的当前时刻救援人员行走位置坐标信息或步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息作为下一时刻所述救援人员的初始位置坐标信息，转入步骤三，直至救援结束；若否，结束进程。

可选地，所述步骤一包括以下步骤：通过地理信息获取软件搜索所述目标建筑物；获取所述目标建筑物的轮廓和所述目标建筑物的位置坐标信息；通过所述地理信息获取软件的测距功能分别测量所述目标建筑物的平面尺寸；利用几何测距法计算所述目标建筑物的高度以估算所述目标建筑物的楼层数信息；根据所述目标建筑物的平面尺寸及楼层数信息构建所述目标建筑物的三维地图。

可选地，在利用几何测距法计算所述目标建筑物的高度的步骤中需要通过测量人员完成，并且需要在所述测量人员和所述目标建筑物之间设置一标杆，且根据相似三角形定理计算所述目标建筑物的高度；在计算所述目标建筑物的高度时需要所述标杆高度，所述测量人员到 标杆的距离，测量人员到目标建筑物的距离，及测量人员高度。

可选地，计算所述目标建筑物的高度的公式为：目标建筑物的高度＝(标杆高度/测量人员到标杆的距离)×测量人员到目标建筑物的距离+测量人员高度。

可选地，所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

可选地，所述步骤三中对所述传感数据进行相应处理具体包括以下步骤：对所述角速度消除静态漂移；对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度，将旋转角度、与加速度和磁感应强度进行互补滤波以获取自校准后的旋转角度；利用旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转换以实现加速度从传感模块坐标系下转换成世界坐标系下；从转换后的世界坐标系下的加速度减去重力方向的重力加速度；对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度；对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。

可选地，所述步骤五中利用零速更新和卡尔曼滤波算法消除所述步骤四中存在的计算误差。

可选地，所述步骤五中继续判断所述参照脚是否处于脚步静止的最后一刻，若否，则输出步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息，若是，则基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正，且在所述步骤五中还包括判断所述救援人员是否接收到无线定位信号，若是，则利用无线定位信号进一步修正位置计算累计误差，若否，则输出利用粒子滤波算法对位置计算累计误差进行修正后的当前时刻所述救援人员行走位置坐标信息。

本发明另一方面还提供一种应急救援场景下快速构建地图辅助室内定位系统，所述应急救援场景发生在一目标建筑物内，该定位系统通过救援人员执行，所述救援人员佩戴传感模块，所述应急救援场景下快速构建地图辅助室内定位系统，手持终端、及远程控制中心配合完成应急救援，其中，所述应急救援场景下快速构建地图辅助室内定位系统包括：三维地图构建模块，用于构建所述目标建筑物的三维地图；初始位置获取模块，用于获取所述救援人员进入所述目标建筑物的初始位置坐标信息；数据读取模块，用于读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据，数据处理模块，用于对所述传感数据进行相应处理；所述传感数据中存在测量误差；推算模块，用于根据所述传感数据通过预存 的推算算法推算当前时刻所述救援人员行走位置坐标信息；其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差和位置计算累计误差；第一判断模块，用于以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，则调用用于消除所述推算模块在推算过程中产生的计算误差的第一校准模块，在修正计算误差和位置计算累计误差以获取修正后的当前时刻所述救援人员所述救援人员行走位置坐标信息，并调用用于输出修正后的当前时刻所述救援人员行走位置坐标信息的输出模块，若否，则调用所述输出模块，所述输出模块用于输出所述推算模块所推算出的当前时刻所述救援人员行走位置坐标信息；标注模块，用于将修正后的当前时刻救援人员行走位置坐标信息或推算出的当前时刻所述救援人员行走位置坐标信息的标注在所述目标建筑物的三维地图中；第三判断模块，用于判断是否终止推算所述救援人员行走位置坐标信息；若是，将调用用于将修正后的当前时刻救援人员行走位置坐标信息或所述推算模块在推算过程中中推算出的当前时刻所述救援人员行走位置坐标信息作为所述救援人员下一时刻的初始位置坐标信息的初始化模块；若否，调用用于终止执行应急救援的终止模块。

可选地，所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

可选地，所述数据处理模块包括：偏移单元，用于对所述角速度消除静态偏移；第一积分单元，对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度；滤波单元，用于将第一积分单元计算的旋转角度、与加速度和磁感应强度进行互补滤波以获取自校准后的旋转角度；坐标转换单元，用于利用旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转换以实现加速度从传感模块坐标系下转换成世界坐标系下；减除单元，用于从转换后的世界坐标系下的加速度减去重力方向的重力加速度；第二积分单元，用于对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度；第三积分单元，用于对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。

可选地，所述应急救援场景下快速构建地图辅助室内定位系统还包括第二判断模块，所述第二判断模块用于判断所述参照脚是否处于离地时刻，若是，则调用所述第二校准模块对所述位置计算累计误差进行修正，若否，则调用所述输出模块输出所述推算模块推算的当前时刻所述救援人员行走位置坐标信息。

如上所述，本发明所述的应急救援场景下快速构建地图辅助室内定位方法及系统，具有 以下有益效果：

1、本发明所述的应急救援场景下快速构建地图辅助室内定位方法及系统通过现场快速构建目标建筑物三维地图，作为参考物，并在该地图上根据移动轨迹实时的绘制救援人员的移动轨迹和高度位置。在此过程中实现了室内外定位的无缝衔接，且无需预先安装基础支撑设备，是一种自完备的定位方法。

2、同时，本发明所述的应急救援场景下快速构建地图辅助室内定位方法及系统避免了传统的地图辅助定位中复杂的算法实现和无法获取室内地图的尴尬，实现了动态实时跟踪救援人员及为应急救援的整体指挥调度与救援人员的生命保障提供辅助。

附图说明

图1显示为本发明的应急救援场景下快速构建地图辅助室内定位方法流程示意图示意图。

图2显示为本发明的步骤S1的具体步骤流程示意图。

图3显示为本发明的步骤S3的具体步骤流程示意图。

图4显示为基于三轴加速度与三轴陀螺仪采集的数据计算的零速度区间与零速度结束最后一刻的计算效果示意图。图5显示为本发明的应急救援场景下快速构建地图辅助室内定位系统原理结构示意图。

元件标号说明 

1       应急救援场景下快速构建地图辅助室内定位系统

11      三维地图构建模块

12      指令输出模块

13      初始位置获取模块

14      数据读取模块

15      数据处理模块

16      推算模块

17      第一判断模块

18      第一校准模块

19      第二判断模块

20      第二校准模块

21      输出模块

22      标注模块

23      第三判断模块

24      终止模块

25      初始化模块

S1～S14 步骤

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅附图。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

下面结合实施例和附图对本发明进行详细说明。

实施例一

本实施例提供一种应急救援场景下快速构建地图辅助室内定位方法，所述应急救援场景发生在一目标建筑物内，该应急救援场景下快速构建地图辅助室内定位方法通过救援人员执行，所述救援人员佩戴传感模块。请参阅图1，显示为应急救援场景下快速构建地图辅助室内定位方法流程示意图，如图1所示，所述应急救援场景下快速构建地图辅助室内定位方法包括：

S1，构建所述目标建筑物的三维地图。在本实施例中通过谷歌地图搜索目标建筑物，并获取目标建筑物的轮廓。通过谷歌地图测距的功能测量目标建筑物的各个边的长度。根据上述信息绘制目标建筑物的平面地图。利用几何测距的办法或激光测距设备快速测量建筑物的高度。并通过目测或者咨询熟悉人员获得建筑物的楼层数。结合上述的平面地图和建筑物的高度和楼层数，可以绘制出目标建筑物的简易三维地图。通过谷歌地图获取目标建筑物轮廓的GPS数据(以一定的采样率)，并保存，目的是为了后面的救援人员位置估算提供校准的限制条件。在实际应用中，地图系统不仅仅包括谷歌地图，常用的ArcGIS,百度地图，高德 地图等等GIS软件系统均可以作为本发明的基础平台。

请参阅图2，显示为步骤S1的具体步骤流程图。如图2所示，所述步骤S1具体包括以下步骤：

S11，通过地理信息获取软件，即通过谷歌地图搜索所述目标建筑物。

S12，获取所述目标建筑物的轮廓和所述目标建筑物的位置坐标信息，例如GPS数据。

S13，通过所述地理信息获取软件的测距功能，例如，谷歌地图中测距功能分别测量所述目标建筑物的平面尺寸。

S14，根据先验知识，例如，根据目测所述目标建筑物的层数，利用几何测距法计算所述目标建筑物的高度以估算所述目标建筑物的楼层数信息。在利用几何测距法计算所述目标建筑物的高度中需要通过测量人员完成，并且需要在所述测量人员和所述目标建筑物之间设置一标杆，且根据相似三角形定理计算所述目标建筑物的高度；在计算所述目标建筑物的高度时需要所述标杆高度，所述测量人员到标杆的距离，测量人员到目标建筑物的距离，及测量人员高度。其中，计算所述目标建筑物的高度的公式为：

目标建筑物的高度＝(标杆高度/测量人员到标杆的距离)×

测量人员到目标建筑物的距离+测量人员高度   公式(1)换言之，凡是能够得到所述目标建筑物的高度的方法均适用于本发明。

S15，根据所述目标建筑物的平面尺寸及楼层数信息构建所述目标建筑物的三维地图。

S2，令所述救援人员在进入所述目标建筑物之前加载所述目标建筑物的三维地图，并获取所述救援人员进入所述目标建筑物的初始位置坐标信息。

S3，读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据，并对所述传感数据进行相应处理，所述传感数据中存在测量误差。请参阅图3，显示为步骤S3的具体步骤流程图。如图3所示，所述步骤S3具备包括以下步骤：

S31，读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据。所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

S32，对所述角速度消除静态漂移。

S33，对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度。

S34,将步骤S33计算的旋转角度，与加速度数据和磁感应强度进行互补滤波以获取自校准后的旋转角度得到更加精确的旋转角度信息。

S35，利用自校准的旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转 换以实现加速度从传感模块坐标系下转换成世界坐标系下。

S36，从转换后的世界坐标系下的加速度减去重力方向的重力加速度，即用转换后的世界坐标系下的加速度减去Z轴的重力加速度值以消除重力加速度的影响。

S37，对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度。

S38，对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。通过本步骤可以得到所述救援人员的水平面的坐标位置和高度。同时高度可以通过与楼层的高度做比较来修正其误差。例如设楼层高2.6米，从加速度积分计算的高度小于1.3米时可以判定救援人员没有楼层高度的变化，当计算结果大于1.3米时可以判定救援人员上了一层楼。

步骤S3中对所述传感数据进行相应处理还包括：

对加速度进行FIR低通滤波实现行进计步,并基于三轴加速度计测量信息的步长在线标定方法与步态分类识别。最后利用步数与步长来进行行走距离的估算，同时基于方向余弦矩阵将测量到的地磁的磁感应强度投影到当地地理系后计算磁航向，并结合陀螺仪输出的角速度进行校准，从而计算出运动方向。这样就实现了位置估计。

S4，根据所述传感数据通过预存的推算算法推算当前时刻所述救援人员行走位置坐标信息。其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差和位置计算累计误差。所述计算误差来源于传感模块本身的测量误差，对于这些测量误差的2次积分后，会导致计算误差以时间的3次方扩展。在本实施例中，所述推算算法包括航位推算算法和校准框架算法。换言之，凡是能够推算出当前时刻所述救援人员行走位置坐标信息的方法均适用于本发明。

S5，以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚的当前时刻是否处于静止状态，若是，则执行步骤S6，若否，则执行步骤S11，即输出步骤S4所推算的当前时刻所述救援人员行走位置坐标信息。在本步骤中，判断该参照脚的当前时刻是否处于静止状态依据包括：

判断条件1：其中，α表示加速度，k表示当前时刻，X，Y，Z为世界坐标系下的三个坐标。

判断条件2：其中，ω表示角速度，i表示X，Y，Z为世界坐标系下的三个坐标中一个轴

判断条件3：其中，n为3，5，7，9…的奇数时刻，S表示前n 个时刻的方差，

零速度判断条件利用逻辑与结合上述三个条件来判断。

S6，修正所述步骤S4中存在的计算误差。在本步骤中，为了减少世界坐标系下加速度、速度和位置误差的积累，直观地讲就是为了将步骤S4的估计的计算误差降低到以步数的1次方扩展。在本实施例中，利用零速更新的算法，检查所述救援人员在当前时刻脚接触地面的时间利用间接卡尔曼滤波器对速度位置和方向角进行滤波。在修正计算误差后，仍存在位置计算累计误差，在本实施例中还需对所述位置计算累计误差进行修正。换言之，凡是能够同零速更新算法和卡尔曼滤波一样消除所述步骤S4中的计算误差的方法均适用于本发明。在本步骤中，零速更新的算法是利用逻辑与结合上述三个条件来判断

S7，判断所述参照脚是否处于脚步静止的最后一刻，若是，则执行步骤S8，若否，则执行步骤S11，输出步骤S4所推算的当前时刻所述救援人员行走位置坐标信息。脚步静止的最后一刻判断条件如下：

请参阅图4，显示为基于三轴加速度与三轴陀螺仪采集的数据计算的零速度区间与零速度结束最后一刻的计算效果示意图，图4依次展现了三轴加速度数据,三轴陀螺仪数据和基于这2个原始数据计算的零速度结果,其中第三张图中,1表示为零速度时刻,0表示非零速度时刻，(零速度即脚步静止时刻),其中图中的点A即零速度结束时刻，在这个时刻触发利用粒子滤波算法，结合地图信息来校准计算的位置结果。点A的获取方法如下：

若((零速度_k-1＝＝1)&&(零速度_k＝＝0))，则时刻k是零速度结束时刻。

S8，基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新的有效粒子仍然失败，那么就将其标记为无效粒子，粒子的权值如下：

粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设混入到一步步长的噪声v_l和一步方位变化的噪声v_ψ都服从零均值的高斯分布。以此列写出水平位置(x，y)和方位ψ的状态转移方程：

$\left\{\begin{array}{c}{x}_k^i=x_{k-1}^i+l_k^i\cos {\mathrm{\ψ}}_{k-1}^i\\ y_k^i=y_{k-1}^i+l_k^i\sin {\mathrm{\ψ}}_{k-1}^i\\ {\mathrm{\ψ}}_k^i={\mathrm{\ψ}}_{k-1}^i+\mathrm{\δ}{\mathrm{\ψ}}_k^i\end{array}\right.$

在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子   的基础上由重要性分布    采样得到。这可以通过先采样得到粒子和然后代入状态转移方程分别得到    换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于本发明。

S9，判断所述救援人员在救援过程是否接收到可靠的无线定位信号，若是，则执行步骤S10，若否，则执行S11，即输出步骤S4所推算的当前时刻所述救援人员行走位置坐标信息。在本实施例中，所述无线定位信号包括窗口GPS，无线信号等。

S10，利用所述无线定位信号进一步修正位置计算累计误差。在室内定位情况下，一旦确认接收可靠无线定位信号(窗口GPS，无线信标)，对惯性导航算法进行初始化并结合自适应方法实现惯性导航算法参数的修正，从而提高定位精度。

S11，此时输出经过粒子滤波算法或经过所述无线定位信号进行修正后的当前时刻所述救援人员行走位置坐标信息。

S12，接收经过粒子滤波算法和经过所述无线定位信号进行修正后的当前时刻所述救援人员行走位置坐标信息或步骤S4所推算的当前时刻所述救援人员行走位置坐标信息，并将接收的经过粒子滤波算法和经过所述无线定位信号进行修正后的当前时刻所述救援人员行走位置坐标信息或步骤S4所推算的当前时刻所述救援人员行走位置坐标信息标注在所述目标建筑物的三维地图中以便远程控制中心及时获悉救援人员的位置。

S13，判断是否终止推算所述救援人员的行走位置坐标信息，若是，结束进程；若否，则执行步骤S14。

S14，将利用粒子滤波算法对位置计算累计误差进行修正后的当前时刻所述救援人员行走位置坐标信息、或利用所述无线定位信号进行修正后的当前时刻所述救援人员行走位置坐标信息、或步骤S4所推算的当前时刻所述救援人员行走位置坐标信息作为下一时刻所述救援人员的初始位置坐标信息，并转入步骤S3，依次循环，直至救援行动结束。

本实施例所述的应急救援场景下快速构建地图辅助室内定位方法，通过现场快速构建目标建筑物三维地图，作为参考物。并在该地图上根据移动轨迹实时的绘制救援人员的移动轨迹和高度位置。在此过程中实现了室内外定位的无缝衔接，且无需预先安装基础支撑设备，是一种自完备的定位方法。同时，也避免了传统的地图辅助定位中复杂的算法实现和无法获取室内地图的尴尬，实现了动态实时跟踪救援人员及为应急救援的整体指挥调度与救援人员的生命保障提供辅助。

实施例二

本实施例提供一种应急救援场景下快速构建地图辅助室内定位系统，所述应急救援场景发生在一目标建筑物内，该定位系统通过救援人员执行，所述救援人员佩戴传感模块，所述应急救援场景下快速构建地图辅助室内定位系统，手持终端、及远程控制中心配合完成应急救援。请参阅图5，显示为应急救援场景下快速构建地图辅助室内定位系统原理结构示意图。如图4所示，所述应急救援场景下快速构建地图辅助室内定位系统1包括：三维地图构建模块11、指令输出模块12、初始位置获取模块13、数据读取模块14、数据处理模块15、推算模块16、第一判断模块17、第一校准模块18、第二判断模块19、第二校准模块20、输出模块21、标注模块22、第三判断模块23、终止模块24、及初始化模块25。

所述三维地图构建模块11用于构建所述目标建筑物的三维地图。在本实施例中通过谷歌地图搜索目标建筑物，并获取目标建筑物的轮廓。通过谷歌地图测距的功能测量目标建筑物的各个边的长度。根据上述信息绘制目标建筑物的平面地图。利用几何测距的办法或激光测距设备快速测量建筑物的高度。并通过目测或者咨询熟悉人员获得建筑物的楼层数。结合上述的平面地图和建筑物的高度和楼层数，可以绘制出目标建筑物的简易三维地图。通过谷歌地图获取目标建筑物轮廓的GPS数据(以一定的采样率)，并保存，目的是为了后面的救援人员位置估算提供校准的限制条件。在实际应用中，地图系统不仅仅包括谷歌地图，常用的ArcGIS,百度地图，高德地图等等GIS软件系统均可以作为本发明的基础平台。

所述三维地图构建模块11具体包括以下各单元：

搜索单元用于通过地理信息获取软件，即通过谷歌地图搜索所述目标建筑物。

获取单元用于获取所述目标建筑物的轮廓和所述目标建筑物的位置坐标信息，例如GPS数据。

测量单元用于通过所述地理信息获取软件的测距功能，例如，谷歌地图中测距功能分别测量所述目标建筑物的平面尺寸。

计算单元用于根据先验知识，例如，根据目测所述目标建筑物的层数，利用几何测距法计算所述目标建筑物的高度以估算所述目标建筑物的楼层数信息。在利用几何测距法计算所述目标建筑物的高度中需要通过测量人员完成，并且需要在所述测量人员和所述目标建筑物之间设置一标杆，且根据相似三角形定理计算所述目标建筑物的高度；在计算所述目标建筑物的高度时需要所述标杆高度，所述测量人员到标杆的距离，测量人员到目标建筑物的距离，及测量人员高度。其中，计算所述目标建筑物的高度的公式为：

目标建筑物的高度＝(标杆高度/测量人员到标杆的距离)×

测量人员到目标建筑物的距离+测量人员高度   公式(1)

构建单元用于根据所述目标建筑物的平面尺寸及楼层数信息构建所述目标建筑物的三维地图。

所述指令输出模块12用于令所述救援人员在进入所述目标建筑物之前给所述手持终端和远程控制中心加载所述目标建筑物的三维地图。

所述初始位置获取模块13用于获取所述救援人员进入所述目标建筑物的初始位置坐标信息。

所述数据读取模块14用于读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据。所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

所述数据处理模块15用于对所述传感数据进行相应处理。所述传感数据中存在测量误差。所述数据处理模块具体包括以下各单元：

去噪单元用于去掉所述救援人员的加速度和角速度中高频噪声；

偏移单元用于对所述角速度消除静态偏移；

第一积分单元对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度；

滤波单元用于利用所述第一积分单元计算的旋转角度，与加速度数据和磁感应强度进行互补滤波以获取自校准后的旋转角度得到更加精确的旋转角度信息。

坐标转换单元用于利用旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转换以实现加速度从传感模块坐标系下转换成世界坐标系下；

减除单元用于从转换后的世界坐标系下的加速度减去重力方向的重力加速度；

第二积分单元用于对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度；

第三积分单元用于对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。

所述推算模块16用于根据所述传感数据通过预存的推算算法推算当前时刻所述救援人员行走位置坐标信息，在本实施例中，所述推算模块16中预存的推算算法包括航位推算算法和校准框架算法。其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差 和位置计算累计误差。所述计算误差来源于传感模块本身的测量误差，对于这些测量误差的2次积分后，会导致计算误差以时间的3次方扩展。

第一判断模块17用于以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，则调用用于修正所述推算模块16在推算过程中产生的计算误差的第一校准模块18，在修正计算误差后，仍存在位置计算累计误差，调用用于判断所述参照脚是否处于脚步静止的最后一刻的第二判断模块19，若是，则调用第二校准模块20对所述位置计算累计误差进行修正以获取修正后的当前时刻所述救援人员行走位置坐标信息，并调用所述输出模块21输出修正后的当前时刻所述救援人员行走位置坐标信息，若否，则调用所述输出模块21，即输出所述推算模块16推算的当前时刻所述救援人员行走位置坐标信息；若否，同样调用所述输出模块21，输出所述推算模块16推算的当前时刻所述救援人员行走位置坐标信息。为了减少世界坐标系下加速度、速度和位置误差的积累，直观地讲就是为了将推算模块16的估计的计算误差降低到以步数的1次方扩展。在本实施例中，利用零速更新的算法，检查所述救援人员在当前时刻脚接触地面的时间利用间接卡尔曼滤波器对速度位置和方向角进行滤波。在降低计算误差后，所述测量误差中仍存在位置计算累计误差，在本实施例中还需对所述位置计算累计误差进行修正。

所述第二校准模块20用于基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新的有效粒子仍然失败，那么就将其标记为无效粒子，粒子的权值如下：

粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设混入到一步步长的噪声v_l和一步方位变化的噪声v_ψ都服从零均值的高斯分布。以此列写出水平位置(x，y)和方位ψ的状态转移方程：

$\left\{\begin{array}{c}{x}_k^i=x_{k-1}^i+l_k^i\cos {\mathrm{\ψ}}_{k-1}^i\\ y_k^i=y_{k-1}^i+l_k^i\sin {\mathrm{\ψ}}_{k-1}^i\\ {\mathrm{\ψ}}_k^i={\mathrm{\ψ}}_{k-1}^i+\mathrm{\δ}{\mathrm{\ψ}}_k^i\end{array}\right.$

在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子的基础上由重要性分布    采样得到。这可以通过先采样得到粒子和然后代入状态转移方程分别得到    

所述第二校准模块19还用于判断所述救援人员在救援过程是否接收到无线定位信号，若是，则利用所述无线定位信号进一步修正位置计算累计误差，并调用所述输出模块21，此时输出的当前时刻所述救援人员行走位置坐标信息是经过粒子滤波算法和无线定位信号修正过的位置坐标信息，即修正后的当前时刻所述救援人员行走位置坐标信息。在室内定位情况下，一旦确认接收可靠无线定位信号(窗口GPS，无线信标)，对惯性导航算法进行初始化并结合自适应方法实现惯性导航算法参数的修正，从而提高定位精度，若否，则调用所述输出模块21输出所述推算模块16推算的当前时刻所述救援人员行走位置坐标信息。

所述标注模块22用于接收输出模块21输出的当前时刻所述救援人员行走位置坐标信息并将其标注在所述目标建筑物的三维地图中以便远程控制中心及时获悉救援人员的位置。

所述第三判断模块23用于判断是否终止所述救援人员的行走位置坐标信息，若是，则调用用于终止执行应急救援的终止模块24；若否，则调用用于将所述输出模块21输出的当前时刻所述救援人员行走位置坐标信息作为下一时刻所述救援人员的初始位置坐标信息的初始化模块25及数据读取模块14，依次循环，直至救援行动结束。

本发明所述的应急救援场景下快速构建地图辅助室内定位方法及系统，通过现场快速构建目标建筑物三维地图，作为参考物。并在该地图上根据移动轨迹实时的绘制救援人员的移动轨迹和高度位置。在此过程中实现了室内外定位的无缝衔接，且无需预先安装基础支撑设备，是一种自完备的定位方法。同时，也避免了传统的地图辅助定位中复杂的算法实现和无法获取室内地图的尴尬，实现了动态实时跟踪救援人员及为应急救援的整体指挥调度与救援人员的生命保障提供辅助。

综上所述，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (12)

1.一种应急救援场景下快速构建地图辅助室内定位方法，所述应急救援场景发生在一目标建筑物内，该定位方法通过救援人员执行，所述救援人员佩戴传感模块，其特征在于，所述应急救援场景下快速构建地图辅助室内定位方法包括：

步骤一，构建所述目标建筑物的三维地图；

步骤二，令所述救援人员在进入所述目标建筑物之前加载所述目标建筑物的三维地图，并获取所述救援人员进入所述目标建筑物的初始位置坐标信息；

步骤三，读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据，并对所述传感数据进行相应处理；所述传感数据中存在测量误差；

步骤四，根据所述传感数据通过预存的航位推算算法推算当前时刻所述救援人员行走位置坐标信息；其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差和位置计算累计误差；

步骤五，以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，则修正所述步骤四中存在的计算误差和位置计算累计误差以获取修正后的当前时刻所述救援人员所述救援人员行走位置坐标信息，输出修正后的当前时刻所述救援人员行走位置坐标信息，转入下一步骤，若否，则输出步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息，转入下一步骤；

步骤六，将修正后的当前时刻救援人员行走位置坐标信息或步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息的标注在所述目标建筑物的三维地图中；

步骤七，判断是否终止推算所述救援人员行走位置坐标信息；若是，将修正后的当前时刻救援人员行走位置坐标信息或步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息作为下一时刻所述救援人员的初始位置坐标信息，转入步骤三，直至救援结束；若否，结束进程。

2.根据权利要求1所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：所述步骤一包括以下步骤：

通过地理信息获取软件搜索所述目标建筑物；

获取所述目标建筑物的轮廓和所述目标建筑物的位置坐标信息；

通过所述地理信息获取软件的测距功能分别测量所述目标建筑物的平面尺寸；

利用几何测距法计算所述目标建筑物的高度以估算所述目标建筑物的楼层数信息；

根据所述目标建筑物的平面尺寸及楼层数信息构建所述目标建筑物的三维地图。

3.根据权利要求2所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：在利用几何测距法计算所述目标建筑物的高度的步骤中需要通过测量人员完成，并且需要在所述测量人员和所述目标建筑物之间设置一标杆，且根据相似三角形定理计算所述目标建筑物的高度；在计算所述目标建筑物的高度时需要所述标杆高度，所述测量人员到标杆的距离，测量人员到目标建筑物的距离，及测量人员高度。

4.根据权利要求3所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：计算所述目标建筑物的高度的公式为：目标建筑物的高度＝(标杆高度/测量人员到标杆的距离)×测量人员到目标建筑物的距离+测量人员高度。

5.根据权利要求1所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

6.根据权利要求5所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：所述步骤三中对所述传感数据进行相应处理具体包括以下步骤：

对所述角速度消除静态漂移；

对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度，将旋转角度、与加速度和磁感应强度进行互补滤波以获取自校准后的旋转角度；

利用旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转换以实现加速度从传感模块坐标系下转换成世界坐标系下；

从转换后的世界坐标系下的加速度减去重力方向的重力加速度；

对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度；

对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。

7.根据权利要求1所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：所述步骤五中利用零速更新和卡尔曼滤波算法消除所述步骤四中存在的计算误差。

8.根据权利要求1所述的应急救援场景下快速构建地图辅助室内定位方法，其特征在于：所述步骤五中继续判断所述参照脚是否处于脚步静止的最后一刻，若否，则输出步骤四中所推算出的当前时刻所述救援人员行走位置坐标信息，若是，则基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正，且在所述步骤五中还包括判断所述救援人员是否接收到无线定位信号，若是，则利用无线定位信号进一步修正位置计算累计误差，若否，则输出利用粒子滤波算法对位置计算累计误差进行修正后的当前时刻所述救援人员行走位置坐标信息。

9.一种应急救援场景下快速构建地图辅助室内定位系统，所述应急救援场景发生在一目标建筑物内，该定位系统通过救援人员执行，所述救援人员佩戴传感模块，其特征在于，所述应急救援场景下快速构建地图辅助室内定位系统，手持终端、及远程控制中心配合完成应急救援，其中，所述应急救援场景下快速构建地图辅助室内定位系统包括：

三维地图构建模块，用于构建所述目标建筑物的三维地图；

初始位置获取模块，用于获取所述救援人员进入所述目标建筑物的初始位置坐标信息；

数据读取模块，用于读取所述救援人员佩戴的传感模块感应到的当前时刻所述救援人员的传感数据，

数据处理模块，用于对所述传感数据进行相应处理；所述传感数据中存在测量误差；

推算模块，用于根据所述传感数据通过预存的推算算法推算当前时刻所述救援人员行走位置坐标信息；其中，推算出的当前时刻所述救援人员行走位置坐标信息中包括计算误差和位置计算累计误差；

第一判断模块，用于以所述救援人员双脚中的一只脚作为参照脚，判断该参照脚当前时刻是否处于静止状态，若是，则调用用于消除所述推算模块在推算过程中产生的计算误差的第一校准模块，在修正计算误差和位置计算累计误差以获取修正后的当前时刻所述救援人员所述救援人员行走位置坐标信息，并调用用于输出修正后的当前时刻所述救援人员行走位置坐标信息的输出模块，若否，则调用所述输出模块，所述输出模块用于输出所述推算模块所推算出的当前时刻所述救援人员行走位置坐标信息；

标注模块，用于将修正后的当前时刻救援人员行走位置坐标信息或推算出的当前时刻所述救援人员行走位置坐标信息的标注在所述目标建筑物的三维地图中；

第三判断模块，用于判断是否终止推算所述救援人员行走位置坐标信息；若是，将调用用于将修正后的当前时刻救援人员行走位置坐标信息或所述推算模块在推算过程中中推算出的当前时刻所述救援人员行走位置坐标信息作为所述救援人员下一时刻的初始位置坐标信息的初始化模块；若否，调用用于终止执行应急救援的终止模块。

10.根据权利要求9所述的应急救援场景下快速构建地图辅助室内定位系统，其特征在于：所述救援人员佩戴传感模块包括三轴加速度计、三轴陀螺仪、三轴磁力计、及大气压力计：所述传感数据包括当前时刻所述救援人员的加速度、角速度、磁感应强度、及大气压力。

11.根据权利要求10所述的应急救援场景下快速构建地图辅助室内定位系统，其特征在于：所述数据处理模块包括：

偏移单元，用于对所述角速度消除静态偏移；第一积分单元，对所述救援人员的角速度进行积分以获取当前时刻所述救援人员的旋转角度；

滤波单元，用于将第一积分单元计算的旋转角度、与加速度和磁感应强度进行互补滤波以获取自校准后的旋转角度；

坐标转换单元，用于利用旋转角度获取旋转矩阵，将获取的旋转矩阵左乘加速度进行坐标系转换以实现加速度从传感模块坐标系下转换成世界坐标系下；

减除单元，用于从转换后的世界坐标系下的加速度减去重力方向的重力加速度；

第二积分单元，用于对所述世界坐标系下的X轴，Y轴，Z轴的加速度进行积分以获取所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度；

第三积分单元，用于对所述救援人员在世界坐标系下X轴，Y轴，Z轴的速度进行积分，获取所述救援人员水平面的坐标位置和高度。

12.根据权利要求9所述的应急救援场景下快速构建地图辅助室内定位系统，其特征在于：所述应急救援场景下快速构建地图辅助室内定位系统还包括第二判断模块，所述第二判断模块用于判断所述参照脚是否处于离地时刻，若是，则调用所述第二校准模块对所述位置计算累计误差进行修正，若否，则调用所述输出模块输出所述推算模块推算的当前时刻所述救援人员行走位置坐标信息。