CN114839658B - 定位消防员建筑物进入点的方法及系统 - Google Patents

Abstract

本发明提供了一种定位消防员建筑物进入点的方法及系统，包括：步骤S1：微惯性导航系统、卫星导航系统各自解算出位置数据；步骤S2：对位置数据进行融合，得到融合位置数据；步骤S3：采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，配对建筑物进入点成功后用数据库位置信息校正微惯性导航系统的位置信息；步骤S4：微惯性导航系统基于校正过的位置信息，从此点开始积分计算精确的位置数据。本发明提供一种基于微型3D激光雷达的融合定位方法，确定消防员建筑物进入点的精确位置，用于单兵定位系统的融合定位解算，从而解决由于现有技术无法提供一种有效的准确融合定位方法的问题。

Description

定位消防员建筑物进入点的方法及系统

技术领域

本发明涉及单兵定位系统，具体地，涉及一种定位消防员建筑物进入点的方法及系统。

背景技术

随着社会的发展，城市化和工业化越来越快，各种高楼大厦和规模庞大结构复杂的厂房越建越多。当火灾发生时，就对消防救援提出了更高的要求。消防救援，既要解救被困人员，也要关注消防员自身的安危，这都需要对火灾现场的人员位置做到精确定位，这样现场指挥调度更有效和成功。但是由于现代建筑物结构的复杂性，增加了确定人员位置的难度，同时火灾发生后，由于火苗和烟雾的影响，火灾现场能见度急剧降低，各种照明设施也会由于火灾而失效。因此，为了实时定位消防员的位置，给现场指挥员提供更好的态势信息，提高指挥调度的能力，必须采用各种消防员单兵定位系统。

目前，常用的单兵定位系统一般采用微惯性导航系统、卫星导航系统、室内无线信标等多种定位设备进行融合解算得到最终的位置输出。微惯性导航系统（Micro-INS，Micro Inertial-Navigation System)是一种不依赖外部信息，基于自身的加速度、角速度等传感器进行时间积分从而确定速度、偏航角和位置等信息，单兵室内定位基本都是以惯性导航系统为基础，但是其缺点是：误差会随着时间积累，经过长时间后最终误差会变的很大，所以需要外部其他信息源对其修正。卫星导航系统（GNSS）通过获取卫星信号，实时解算位置信息，定位数据不会随着时间积累，但是其缺点是：在室内和高楼密集区域信号被遮挡，城市环境还会有多径现象，这些都影响卫星导航系统定位精度和可用性，如果用有误差的卫星导航系统数据去跟微惯导系统融合从而修正微惯导输出，会导致微惯导积分开始的位置就是错误的。用室内无线信标融合校正微惯导输出的方法，需要预先布置大量的无线信标，增加了成本，同时火灾时这些信标极有可能被烧毁，最终导致无法使用。

专利文献CN103957507A（申请号：CN201410213934.3）公开了一种新型火场消防单兵定位系统，包括手持终端单元、第一至第三监测单元和监测终端单元；所述手持终端单元定时分别向第一至第三监测单元发送终端定位信息，第一至第三监测单元分别对接收到的终端定位信息进行解算，得到第一至第三监测单元分别与手持终端单元的距离，第一至第三监测单元分别将各自的位置信息及各自的解算结果传输给监测终端单元，由监测终端单元最终精确解算出手持终端单元的位置。但该发明没有基于微型3D激光雷达的融合定位方法，确定消防员建筑物进入点的精确位置。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种定位消防员建筑物进入点的方法及系统。

根据本发明提供的一种定位消防员建筑物进入点的方法，包括：

步骤S1：微惯性导航系统、卫星导航系统各自解算出位置数据；

步骤S2：对位置数据进行融合，得到融合位置数据；

步骤S3：采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，配对建筑物进入点成功后用数据库位置信息校正微惯性导航系统的位置信息；

步骤S4：微惯性导航系统基于校正过的位置信息，从此点开始积分计算精确的位置数据。

优选地，卫星导航系统同时采用多种导航系统解算位置数据，导航系统包括：GPS、北斗、格洛纳斯或伽利略。

优选地，在所述步骤S1中：

微惯性导航系统根据初始位置，基于自身的加速度传感器和角速度传感器的数据输出，周期性地基于固定时间间隔进行积分，得到单兵定位设备的实时位置、偏航角、速度的信息，将微惯性导航系统实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度；

微惯性导航系统和卫星导航系统定位数据进行融合定位时，具体通过以下步骤：

a、数据对准：

数据对准主要包括时间对准和空间对准，由于微惯性导航系统和卫星导航系统解算出的定位数据的时间不同，将数据外推或线性时间对准插值到相同的时刻；插值方法采用抛物时间对准插值、拉格朗日时间对准插值；

b、数据融合：

定位数据融合采用卡尔曼滤波融合算法，微惯性导航系统和卫星导航系统卡尔曼滤波的估计误差协方差矩阵的逆阵加权其估计值的卡尔曼滤波方程基础上，计算出相应的交叉协方差阵，并将交叉协方差阵用于微惯性导航系统与卫星导航系统的定位融合方程中，利用更新过的卡尔曼滤波公式迭代计算得到融合定位数据。

优选地，在所述步骤S2中：

将微惯性导航系统和卫星导航系统得到的定位数据采用集中式融合结构进行融合。

优选地，在所述步骤S3中：

采用五个模块进行匹配解算：3D激光雷达数据采集模块、3D建筑物数据库模块、地形数据配准模块、位置解算模块和位置数据校正模块；

3D激光雷达数据采集模块：3D激光雷达数据采集模块实时采集消防员周围的3D图形数据，并发送至地形数据配准模块做进一步处理；

3D建筑物数据库模块：3D建筑物数据库模块存储目标建筑物及周边区域的3D图形信息，并提供接口供地形数据配准模块读取预设区域的数据；

地形数据配准模块：地形数据配准模块利用3D激光雷达的图形数据，根据消防员位置数据，从数据库中读取周围的3D图形数据，并实时进行配准计算；到达目标建筑物预设距离内后与目标建筑物的特定进入点配准成功，转送位置解算模块继续处理；

位置解算模块：位置解算模块读取特定进入点的精确位置坐标信息，根据3D雷达的3D成像信息，利用计算机视觉算法，计算出消防员此时的精确位置；

位置数据校正模块：位置数据校正模块将得到的消防员精确位置，对微惯性导航系统和卫星导航系统的融合定位数据进行校正，并设置状态量指示精确位置校正已经完成。

优选地，在所述步骤S4中：

微惯性导航系统的位置信息被校正修正后，微惯性导航系统基于自身的加速度传感器和角速度传感器的数据输出，根据时间进行积分，从而得到单兵定位设备的实时位置、偏航角、速度的信息；在此后预设时间内，整个单兵定位设备的位置数据输出采用微惯性导航系统的数据输出；

基于微惯性导航系统的性能指标，设置一时间阈值，超过时间阈值后，微惯性导航系统的积分累计误差已经超过卫星导航系统数据的误差，如果卫星导航系统数据有输出且输出数据稳定，用卫星导航系统数据对微惯性导航系统进行校正。

根据本发明提供的一种定位消防员建筑物进入点的系统，执行所述的定位消防员建筑物进入点的方法，包括：

微惯性导航模块：解算位置数据，基于校正过的准确位置信息，积分计算精确的位置数据；

卫星导航模块：解算位置数据，与微惯性导航系统数据进行融合，得到融合位置数据；

微型3D雷达模块：采集周围环境3D数据；

建筑物3D图形库模块：建筑物3D图形数据库数据与微型3D雷达模块采集数据进行匹配解算。

优选地，微惯性导航系统为一种或两种以上系统的组合；

卫星导航系统为一种或两种以上系统的组合。

优选地，消防员的建筑物进入点包括大门、窗户或预设装置。

优选地，在所述建筑物3D图形库模块中：

建筑物3D图形库包括建筑物的3D尺寸的建模数据、建筑物预设区域的精确位置信息，所述预设区域指的是大门、窗户或预设装置；

建筑物3D图形库存储于后方综合信息处理中心，在后方完成匹配解算并利用稳定可靠的高速通信实时发送回单兵定位系统。

与现有技术相比，本发明具有如下的有益效果：

1、本发明提供一种基于微型3D激光雷达的融合定位方法，确定消防员建筑物进入点的精确位置，用于单兵定位系统的融合定位解算，从而解决由于现有技术无法提供一种有效的准确融合定位方法的问题；

2、本发明以最大程度地实时提供消防员进入建筑物后的精确位置信息，从而给现场指挥员提供更好的态势信息，提高消防救援指挥调度的能力。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为定位消防员建筑物进入点的方法的流程图；

图2为定位消防员建筑物进入点的系统框图；

图3为定位消防员建筑物进入点的3D建筑物匹配解算方案框图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1：

本发明公开了一种定位消防员建筑物进入点的方法，微惯性导航系统、卫星导航系统各自解算出位置数据；微惯性导航系统、卫星导航系统数据进行融合，得到融合位置数据；3D激光雷达实时采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，若配对建筑物进入点成功，则用数据库精确位置信息校正微惯性导航系统的位置信息；微惯性导航系统基于校正过的准确位置信息，从此点开始积分计算精确的位置数据。本发明以最大程度地实时提供消防员进入建筑物后的精确位置信息，从而给现场指挥员提供更好的态势信息，提高消防救援指挥调度的能力。

根据本发明提供的一种定位消防员建筑物进入点的方法，如图1-图3所示，包括：

步骤S1：微惯性导航系统、卫星导航系统各自解算出位置数据；

具体地，在所述步骤S1中：

微惯性导航系统根据初始位置，基于自身的加速度传感器和角速度传感器的数据输出，周期性地基于固定时间间隔进行积分，得到单兵定位设备的实时位置、偏航角、速度的信息，将微惯性导航系统实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度；

微惯性导航系统和卫星导航系统定位数据进行融合定位时，具体通过以下步骤：

a、数据对准：

数据对准主要包括时间对准和空间对准，由于微惯性导航系统和卫星导航系统解算出的定位数据的时间不同，将数据外推或线性时间对准插值到相同的时刻；插值方法采用抛物时间对准插值、拉格朗日时间对准插值；

b、数据融合：

定位数据融合采用卡尔曼滤波融合算法，微惯性导航系统和卫星导航系统卡尔曼滤波的估计误差协方差矩阵的逆阵加权其估计值的卡尔曼滤波方程基础上，计算出相应的交叉协方差阵，并将交叉协方差阵用于微惯性导航系统与卫星导航系统的定位融合方程中，利用更新过的卡尔曼滤波公式迭代计算得到融合定位数据。

步骤S2：对位置数据进行融合，得到融合位置数据；

具体地，在所述步骤S2中：

将微惯性导航系统和卫星导航系统得到的定位数据采用集中式融合结构进行融合。

步骤S3：采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，配对建筑物进入点成功后用数据库位置信息校正微惯性导航系统的位置信息；

具体地，在所述步骤S3中：

采用五个模块进行匹配解算：3D激光雷达数据采集模块、3D建筑物数据库模块、地形数据配准模块、位置解算模块和位置数据校正模块；

3D激光雷达数据采集模块：3D激光雷达数据采集模块实时采集消防员周围的3D图形数据，并发送至地形数据配准模块做进一步处理；

3D建筑物数据库模块：3D建筑物数据库模块存储目标建筑物及周边区域的3D图形信息，并提供接口供地形数据配准模块读取预设区域的数据；

地形数据配准模块：地形数据配准模块利用3D激光雷达的图形数据，根据消防员位置数据，从数据库中读取周围的3D图形数据，并实时进行配准计算；到达目标建筑物预设距离内后与目标建筑物的特定进入点配准成功，转送位置解算模块继续处理；

位置解算模块：位置解算模块读取特定进入点的精确位置坐标信息，根据3D雷达的3D成像信息，利用计算机视觉算法，计算出消防员此时的精确位置；

位置数据校正模块：位置数据校正模块将得到的消防员精确位置，对微惯性导航系统和卫星导航系统的融合定位数据进行校正，并设置状态量指示精确位置校正已经完成。

步骤S4：微惯性导航系统基于校正过的位置信息，从此点开始积分计算精确的位置数据。

具体地，在所述步骤S4中：

微惯性导航系统的位置信息被校正修正后，微惯性导航系统基于自身的加速度传感器和角速度传感器的数据输出，根据时间进行积分，从而得到单兵定位设备的实时位置、偏航角、速度的信息；在此后预设时间内，整个单兵定位设备的位置数据输出采用微惯性导航系统的数据输出；

基于微惯性导航系统的性能指标，设置一时间阈值，超过时间阈值后，微惯性导航系统的积分累计误差已经超过卫星导航系统数据的误差，如果卫星导航系统数据有输出且输出数据稳定，用卫星导航系统数据对微惯性导航系统进行校正。

具体地，卫星导航系统同时采用多种导航系统解算位置数据，导航系统包括：GPS、北斗、格洛纳斯或伽利略。

根据本发明提供的一种定位消防员建筑物进入点的系统，执行所述的定位消防员建筑物进入点的方法，包括：

微惯性导航模块：解算位置数据，基于校正过的准确位置信息，积分计算精确的位置数据；

卫星导航模块：解算位置数据，与微惯性导航系统数据进行融合，得到融合位置数据；

微型3D雷达模块：采集周围环境3D数据；

建筑物3D图形库模块：建筑物3D图形数据库数据与微型3D雷达模块采集数据进行匹配解算。

具体地，微惯性导航系统为一种或两种以上系统的组合；

卫星导航系统为一种或两种以上系统的组合。

具体地，消防员的建筑物进入点包括大门、窗户或预设装置。

具体地，在所述建筑物3D图形库模块中：

建筑物3D图形库包括建筑物的3D尺寸的建模数据、建筑物预设区域的精确位置信息，所述预设区域指的是大门、窗户或预设装置；

建筑物3D图形库存储于后方综合信息处理中心，在后方完成匹配解算并利用稳定可靠的高速通信实时发送回单兵定位系统。

实施例2：

实施例2为实施例1的优选例，以更为具体地对本发明进行说明。

为实现上述目的，本发明提供如下技术方案：一种定位消防员建筑物进入点的方法，具体步骤如下：

A、微惯性导航系统、卫星导航系统各自解算出位置数据：

微惯性导航系统根据初始位置，基于自身的加速度传感器和角速度传感器的数据输出，随着时间流逝，周期性的基于固定时间间隔进行积分，从而得到单兵定位设备的实时位置、偏航角、速度的信息，将微惯性导航系统实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到微惯性导航系统位置和速度；卫星导航系统获取卫星信号，实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度；进一步将微惯性导航系统和卫星导航系统定位数据进行融合得到最终定位数据；

卫星导航系统获取卫星信号，实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度的最终结果；

B、微惯性导航系统、卫星导航系统数据进行融合，得到融合位置数据：

将微惯性导航系统和卫星导航系统得到的定位数据采用集中式融合结构进行融合；

C、3D激光雷达实时采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，若配对建筑物进入点成功，则用数据库精确位置信息校正微惯性导航系统的位置信息；

D、微惯性导航系统基于校正过的准确位置信息，从此点开始积分计算精确的位置数据：在上一步骤C中，微惯性导航系统的位置信息被校正修正后，微惯性导航系统基于自身的加速度传感器和角速度传感器的数据输出，根据时间进行积分，从而得到单兵定位设备的实时位置、偏航角、速度的信息。在此后一段时间内，整个单兵定位设备的最终位置数据输出只采用微惯性导航系统的数据输出。

本发明的进一步改进在于：卫星导航系统可以同时采用多种导航系统，解算位置数据，所述的多种导航系统指：GPS、北斗、格洛纳斯(GLONASS)或伽利略(Galileo)。

本发明的进一步改进在于：在根据步骤A得到的微惯性导航系统和卫星导航系统定位数据进行融合定位时，具体通过以下步骤：

a、数据对准：

数据对准主要包括时间对准和空间对准，由于微惯性导航系统和卫星导航系统解算出的定位数据的时间不同，需要将数据外推或线性时间对准插值到相同的时刻，以便进行后边的融合处理；插值方法可采用抛物时间对准插值、拉格朗日时间对准插值；

b、数据融合：

定位数据融合采用卡尔曼滤波融合算法，在步骤A中微惯性导航系统和卫星导航系统卡尔曼滤波的估计误差协方差矩阵的逆阵加权其估计值的卡尔曼滤波方程基础上，计算出相应的交叉协方差阵，并将之用于微惯性导航系统与卫星导航系统的定位融合方程中，从而利用更新过的卡尔曼滤波公式迭代计算得到融合定位数据。

本发明的进一步改进在于：步骤C中采用五个模块进行匹配解算：3D激光雷达数据采集模块、3D建筑物数据库模块、地形数据配准模块、位置解算模块和位置数据校正模块；

3D激光雷达数据采集模块：3D激光雷达数据采集模块实时采集消防员周围的3D图形数据，并发送至地形数据配准模块做进一步处理；

3D建筑物数据库模块：3D建筑物数据库模块存储目标建筑物及周边区域的3D图形信息，并提供接口供地形数据配准模块读取特定区域的数据；

地形数据配准模块：地形数据配准模块利用3D激光雷达的图形数据，根据步骤B中得到消防员位置的粗略数据，从数据库中读取周围的3D图形数据，并实时进行配准计算；一旦到达目标建筑物附近，与目标建筑物的特定进入点配准成功，则转送位置解算模块继续处理；

位置解算模块：位置解算模块读取特定进入点的精确位置坐标信息，然后根据3D雷达的3D成像信息，利用计算机视觉算法，计算出消防员此时的精确位置；

位置数据校正模块：位置数据校正模块将得到的消防员精确位置，立即对微惯性导航系统和卫星导航系统的融合定位数据进行校正，并设置状态量指示精确位置校正已经完成。

本发明的进一步改进在于：步骤D 中，基于微惯性导航系统的性能指标，设置一时间门限，超过时间门限后，可以认为微惯性导航系统的积分累计误差已经超过卫星导航系统数据的误差，为保证整个系统可用性，此时如果卫星导航系统数据有输出且输出数据稳定，用卫星导航系统数据对微惯性导航系统进行校正。

一种定位消防员建筑物进入点的系统，包括消防员单兵定位系统，消防员单兵定位系统包含一个微惯性导航系统、卫星导航系统、一个微型3D雷达和一个带有精确位置信息的建筑物3D图形库。

本发明的进一步改进在于：微惯性导航系统可以为一种或两种以上系统的组合；卫星导航系统可为一种或两种以上系统的组合。

本发明的进一步改进在于：消防员的建筑物进入点包括但不限于大门、窗户或特定装置。

本发明的进一步改进在于：建筑物3D图形库除了包含建筑物的3D尺寸的建模数据，还必须包括建筑物特定区域的精确位置信息，所述特定区域指的是大门、窗户或特定装置。

本发明的进一步改进在于：建筑物3D图形库可以存储于后方综合信息处理中心，在后方完成匹配解算并利用稳定可靠的高速通信实时发送回单兵定位系统。

实施例3：

实施例3为实施例1的优选例，以更为具体地对本发明进行说明。

本实施例提供一种技术方案：一种定位消防员建筑物进入点的方法，具体步骤如下：

A、微惯性导航系统、卫星导航系统各自解算出位置数据；

B、微惯性导航系统、卫星导航系统数据进行融合，得到融合位置数据；

C、3D激光雷达实时采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，若配对建筑物进入点成功，则用数据库精确位置信息校正微惯性导航系统的位置信息；

D、微惯性导航系统基于校正过的准确位置信息，从此点开始积分计算精确的位置数据。

在步骤A中，微惯性导航系统(INS)、卫星导航系统(GNSS)各自解算出位置数据：在具体实施中，此步骤包含了两独立的功能单元，微惯性导航系统(INS)和卫星导航系统(GNSS)。微惯性导航系统(INS)根据初始位置，基于自身的加速度传感器和角速度传感器的数据输出，随着时间流逝，周期性的基于固定时间间隔进行积分，从而得到单兵定位设备的实时位置、偏航角、速度等信息。将微惯性导航系统实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到微惯性导航系统位置和速度的最终结果。卫星导航系统(GNSS)获取卫星信号，实时解算每一时刻的位置信息。将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度的最终结果。其中，卫星导航系统(GNSS)可以同时采用多种导航系统，解算位置数据。此处的多种导航系统指：GPS、北斗、格洛纳斯(GLONASS)或伽利略(Galileo)。卡尔曼滤波可采用扩展卡尔曼滤波或其他卡尔曼滤波形式。

在步骤B中，微惯性导航系统(INS)、卫星导航系统(GNSS)数据进行融合得到融合位置数据：在具体实施中，微惯性定位数据和卫星导航数据的融合采用集中式融合结构。将微惯性导航系统和卫星导航系统得到的定位数据输入到同一中心融合处理器，在此完成数据对准、关联、融合等功能。在根据步骤A得到的微惯性导航系统和卫星导航系统定位数据进行融合定位时，具体通过以下步骤：a、数据对准：数据对准主要包括时间对准和空间对准。由于微惯性导航系统和卫星导航系统解算出的定位数据的时间不同，需要将数据外推或线性时间对准插值（二点插值）到相同的时刻，以便进行后边的融合处理。优选的，插值方法可采用抛物时间对准插值（三点插值）、拉格朗日时间对准插值（n点插值）。b、数据融合：定位数据融合采用卡尔曼滤波融合算法。在步骤A中微惯性导航系统和卫星导航系统卡尔曼滤波的估计误差协方差矩阵的逆阵加权其估计值的卡尔曼滤波方程基础上，计算出相应的交叉协方差阵，并将之用于微惯性导航系统与卫星导航系统的定位融合方程中，从而利用更新过的卡尔曼滤波公式迭代计算得到融合定位数据。

在步骤C中，3D激光雷达实时采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，若配对建筑物进入点成功，则用数据库精确位置信息校正微惯性导航系统的位置信息。

在具体实施中，此步骤包括5个功能模块共同完成此。

1、3D激光雷达数据采集模块：3D激光雷达数据采集模块实时采集消防员周围的3D图形数据，并送地形数据配准模块做进一步处理。

2、3D建筑物数据库模块：3D建筑物数据库模块存储目标建筑物及周边区域的3D图形信息，并提供接口供地形数据配准模块读取特定区域的数据。

3、地形数据配准模块：地形数据配准模块利用3D激光雷达的图形数据，根据步骤B中得到消防员位置的粗略数据，从数据库中读取周围的3D图形数据，并实时进行配准计算。一旦到达目标建筑物附近，与目标建筑物的特定进入点配准成功，则转送位置解算模块继续处理。

4、位置解算模块：位置解算模块读取特定进入点的精确位置坐标信息（经纬度，大地坐标系系等），然后根据3D雷达的3D成像信息，利用计算机视觉算法，计算出消防员此时的精确位置。

5、位置数据校正模块：位置数据校正模块将得到的消防员精确位置，立即对微惯性导航系统和卫星导航系统的融合定位数据进行校正，并设置状态量指示精确位置校正已经完成。

在步骤D中，微惯性导航系统(INS)基于校正过的准确位置信息，从此点开始积分计算精确的位置数据。在具体实施中，在上一步骤C中微惯性导航系统(INS)的位置信息被校正修正后，微惯性导航系统(INS)基于自身的加速度传感器和角速度传感器的数据输出，根据时间进行积分，从而得到单兵定位设备的实时位置、偏航角、速度等信息。在此后一段时间内，整个单兵定位设备的最终位置数据输出只采用微惯性导航系统的数据输出。

优选的，系统基于微惯性导航系统的性能指标，设置一时间门限。超过时间门限后，可以认为微惯性导航系统的积分累计误差已经超过卫星导航系统(GNSS)数据的误差，为保证整个系统可用性，此时如果卫星导航系统(GNSS)数据有输出且输出数据稳定，用卫星导航系统(GNSS)数据对微惯性导航系统进行校正。

本发明主要通过引入微型3D雷达和建筑物3D图形库，利用配准的建筑物进入点来校正微惯性导航系统和卫星导航系统定位数据，从而解决目前消防员单兵定位系统利用微惯性导航系统、卫星导航系统在复杂城市和厂房环境下定位不精确的问题。

一种定位消防员建筑物进入点的系统，包括消防员单兵定位系统，消防员单兵定位系统包含一个微惯性导航系统、卫星导航系统、一个低成本微型3D雷达和一个带有精确位置信息的建筑物3D图形库。

优选的，微惯性导航系统可以为一种或两种以上系统的组合，卫星导航系统可为一种或两种以上系统的组合。

消防员的建筑物进入点包括但不限于大门、窗户、特定装置。

建筑物3D图形库除了包含建筑物的3D尺寸等建模数据，还必须包括建筑物特定区域的精确位置信息，这些特定区域指的是大门、窗户、特定装置等。

3D建筑物匹配解算采用自适应智能化调度策略，模块持续利用微惯性导航系统和卫星导航系统的融合定位数据判定消防员的实时位置数据，基于此位置数据智能控制3D雷达启动和3D图形配准程序执行，以降低系统功耗，延长系统的工作时间。具体调度策略为，微型3D雷达初始处于上电待机状态，微惯性导航和卫星导航持续进行融合计算得到融合定位数据，地形数据配准模块利用融合定位数据计算消防员在2D地图的实时位置数据，当地形数据配准模块判定消防员到达目标建筑物附近时，微型3D雷达才启动3D图形数据采集和3D图形配准计算。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (9)

1.一种定位消防员建筑物进入点的方法，其特征在于，包括：

步骤S1：微惯性导航系统、卫星导航系统各自解算出位置数据；

步骤S2：对位置数据进行融合，得到融合位置数据；

步骤S3：采集周围环境3D数据，并跟建筑物3D图形数据库数据匹配解算，配对建筑物进入点成功后用数据库位置信息校正微惯性导航系统的位置信息；

步骤S4：微惯性导航系统基于校正过的位置信息，从此点开始积分计算精确的位置数据；

在所述步骤S3中：

采用五个模块进行匹配解算：3D激光雷达数据采集模块、3D建筑物数据库模块、地形数据配准模块、位置解算模块和位置数据校正模块；

3D激光雷达数据采集模块：3D激光雷达数据采集模块实时采集消防员周围的3D图形数据，并发送至地形数据配准模块做进一步处理；

3D建筑物数据库模块：3D建筑物数据库模块存储目标建筑物及周边区域的3D图形信息，并提供接口供地形数据配准模块读取预设区域的数据；

地形数据配准模块：地形数据配准模块利用3D激光雷达的图形数据，根据消防员位置数据，从数据库中读取周围的3D图形数据，并实时进行配准计算；到达目标建筑物预设距离内后与目标建筑物的特定进入点配准成功，转送位置解算模块继续处理；

位置解算模块：位置解算模块读取特定进入点的精确位置坐标信息，根据3D雷达的3D成像信息，利用计算机视觉算法，计算出消防员此时的精确位置；

位置数据校正模块：位置数据校正模块将得到的消防员精确位置，对微惯性导航系统和卫星导航系统的融合定位数据进行校正，并设置状态量指示精确位置校正已经完成。

2.根据权利要求1所述的定位消防员建筑物进入点的方法，其特征在于：卫星导航系统同时采用多种导航系统解算位置数据，导航系统包括：GPS、北斗、格洛纳斯或伽利略。

3.根据权利要求1所述的定位消防员建筑物进入点的方法，其特征在于，在所述步骤S1中：

微惯性导航系统根据初始位置，基于自身的加速度传感器和角速度传感器的数据输出，周期性地基于固定时间间隔进行积分，得到单兵定位设备的实时位置、偏航角、速度的信息，微惯性导航系统实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到微惯性导航系统位置和速度；卫星导航系统获取卫星信号，实时解算每一时刻的位置信息，将实时解算出的每一时刻的定位数据，经过卡尔曼滤波处理后，得到卫星导航位置和速度；进一步将微惯性导航系统和卫星导航系统定位数据进行融合得到最终定位数据；

微惯性导航系统和卫星导航系统定位数据进行融合定位时，具体通过以下步骤：

a、数据对准：

数据对准主要包括时间对准和空间对准，由于微惯性导航系统和卫星导航系统解算出的定位数据的时间不同，将数据外推或线性时间对准插值到相同的时刻；插值方法采用抛物时间对准插值、拉格朗日时间对准插值；

b、数据融合：

定位数据融合采用卡尔曼滤波融合算法，微惯性导航系统和卫星导航系统卡尔曼滤波的估计误差协方差矩阵的逆阵加权其估计值的卡尔曼滤波方程基础上，计算出相应的交叉协方差阵，并将交叉协方差阵用于微惯性导航系统与卫星导航系统的定位融合方程中，利用更新过的卡尔曼滤波公式迭代计算得到融合定位数据。

4.根据权利要求1所述的定位消防员建筑物进入点的方法，其特征在于，在所述步骤S2中：

将微惯性导航系统和卫星导航系统得到的定位数据采用集中式融合结构进行融合。

5.根据权利要求1所述的定位消防员建筑物进入点的方法，其特征在于，在所述步骤S4中：

微惯性导航系统的位置信息被校正修正后，微惯性导航系统基于自身的加速度传感器和角速度传感器的数据输出，根据时间进行积分，从而得到单兵定位设备的实时位置、偏航角、速度的信息；在此后预设时间内，整个单兵定位设备的位置数据输出采用微惯性导航系统的数据输出；

基于微惯性导航系统的性能指标，设置一时间阈值，超过时间阈值后，微惯性导航系统的积分累计误差已经超过卫星导航系统数据的误差，如果卫星导航系统数据有输出且输出数据稳定，用卫星导航系统数据对微惯性导航系统进行校正。

6.一种定位消防员建筑物进入点的系统，其特征在于，执行权利要求1所述的定位消防员建筑物进入点的方法，包括：

微惯性导航模块：解算位置数据，基于校正过的准确位置信息，积分计算精确的位置数据；

卫星导航模块：解算位置数据，与微惯性导航系统数据进行融合，得到融合位置数据；

微型3D雷达模块：采集周围环境3D数据；

建筑物3D图形库模块：建筑物3D图形数据库数据与微型3D雷达模块采集数据进行匹配解算。

7.根据权利要求6所述的定位消防员建筑物进入点的系统，其特征在于：

微惯性导航系统为一种或两种以上系统的组合；

卫星导航系统为一种或两种以上系统的组合。

8.根据权利要求6所述的定位消防员建筑物进入点的系统，其特征在于：

消防员的建筑物进入点包括大门、窗户或预设装置。

9.根据权利要求6所述的定位消防员建筑物进入点的系统，其特征在于，在所述建筑物3D图形库模块中：

建筑物3D图形库包括建筑物的3D尺寸的建模数据、建筑物预设区域的精确位置信息，所述预设区域指的是大门、窗户或预设装置；

建筑物3D图形库存储于后方综合信息处理中心，在后方完成匹配解算并利用稳定可靠的高速通信实时发送回单兵定位系统。

Images