CN114994672A - 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置 - Google Patents

毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置 Download PDF

Info

Publication number
CN114994672A
CN114994672A CN202210925083.XA CN202210925083A CN114994672A CN 114994672 A CN114994672 A CN 114994672A CN 202210925083 A CN202210925083 A CN 202210925083A CN 114994672 A CN114994672 A CN 114994672A
Authority
CN
China
Prior art keywords
point cloud
millimeter wave
wave radar
human body
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210925083.XA
Other languages
English (en)
Other versions
CN114994672B (zh
Inventor
陈昶昊
潘献飞
何晓峰
李方言
褚超群
毛军
范晨
张礼廉
胡小平
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202210925083.XA priority Critical patent/CN114994672B/zh
Publication of CN114994672A publication Critical patent/CN114994672A/zh
Application granted granted Critical
Publication of CN114994672B publication Critical patent/CN114994672B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30196—Human being; Person
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Geometry (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Software Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Graphics (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置，该方法包括：获取惯性数据与点云数据；基于惯性数据得到人体的姿态信息，并基于人体运动学的约束与惯性数据进行人体的步长估计，并确定人体的位置信息；对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；通过人体的姿态信息与位置信息将稠密化点云数据映射到全局坐标系下构建地图。本发明应用于导航定位领域，针对应急救援人员运动复杂多变的特点，基于人体运动学的约束，通过惯性器件检测步态、计算出位置和姿态，实现导航定位；再通过位置和姿态信息将点云数据映射到全局坐标系下构建地图，实现人员在烟雾环境下的实时定位与建图定位。

Description

毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置
技术领域
本发明涉及导航定位技术领域，具体是一种毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置。
背景技术
应急救援人员经常暴露在危险恶劣的环境中，当需要从燃烧、倒塌的建筑物中救援受害者时，伴随着建筑物内因燃烧而产生的大量浓烟，建筑内通道狭窄、错综复杂，对应急救援人员获取位置和环境地图，都充满了困难和挑战，对他们的安全造成威胁。当前主流的导航建图系统，如ORB-SLAM、VINS、LOAM等，常使用激光雷达或RGB相机，辅助以惯性传感器来生成精确的室内地图。当空气中存在的颗粒物（如灰尘、雾、烟雾等），这些光学传感器的效果会受到严重限制。在存在浓烟或光线复杂的应急救援场景中，视觉传感器很难获取到特征点；激光雷达在狭窄通道存在盲区，并且受到烟、雾颗粒的影响，致使激光雷达传感器信号被严重干扰。
面向上述具有挑战性情况下测绘精确地图的需求，考虑采用小型单通道毫米波雷达和惯性器件组合，构建行人烟雾环境下的建图系统。毫米波雷达受灰尘、雾和烟雾等的影响较小，可以透过烟雾，帮助救援人员识别障碍物和构建环境地图。此外，与激光雷达等传感器相比，小型单通道毫米波雷达重量更轻、体积更小，能减少人员负担。但毫米波雷达受限于其单通道的结构，生成的点云噪声大、点云密度极其稀疏（一次扫描仅有几百个点）。人员的运动较为复杂和多变，仅依靠单通道毫米波雷达来实现定位和建图较为困难。
毫米波雷达建图的研究主要集中在车载领域。Guan et al.等基于模拟毫米波雷达的输入进行深度学习模型训练，生成车载图像的深度信息。Engelhardt et al.等采用车载毫米波雷达与激光雷达、深度相机同时采集数据，用激光雷达与深度相机的数据作为真实标签训练学习模型，实现了毫米波雷达的室外环境建图。2019年，Brodeski et al.等人提出从毫米波的回波信息出发，建立回波信息到实际点云的端到端网络RadarNet，以取代传统雷达进行回波信号处理，在无杂波干扰的隔离室中，通过采集多个角度和位置数据训练神经网络，提升了毫米波雷达点云建图精度。国内相关领域的研究较少，多数研究仍是将毫米波雷达与其他传感器进行数据融合建立地图。例如，将毫米波雷达与激光雷达结合建图，将毫米波雷达与视觉传感器融合进行车道级定位。毫米波雷达目前多应用于室外自动驾驶的避障领域，往往通过与激光雷达、TOF深度相机结合，配合神经网络训练，从而达到建图效果。当前，适用于人员在复杂室内环境下的建图，尤其是烟、雾等恶劣环境下的毫米波雷达建图的相关研究较少，仍未形成成熟的方案和产品。
发明内容
针对上述现有技术中的不足，本发明提供一种毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置，实现应急救援人员在火场烟雾环境下的实时定位与建图定位。
为实现上述目的，本发明提供一种毫米波雷达惯性组合的火场烟雾场景定位建图方法，将惯性器件与毫米波雷达搭载在进入火场烟雾场景的人体上，所述定位建图方法包括如下步骤：
步骤1，获取惯性数据与毫米波雷达点云数据；
步骤2，基于惯性数据得到人体的姿态信息，并基于人体运动学的约束与惯性数据进行人体的步长估计，并确定人体的位置信息；
步骤3，对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；
步骤4，通过人体的姿态信息与位置信息将稠密化点云数据映射到全局坐标系下构建地图，实现人体在烟雾环境下的实时定位与建图定位。
为实现上述目的，本发明还提供一种毫米波雷达惯性组合的火场烟雾场景定位建图装置，包括：
惯性器件与毫米波雷达，搭载在人体的胸部位置；
数据获取模块，与惯性器件、毫米波雷达相连，用于获取惯性数据与点云数据；
姿态信息计算模块，与数据获取模块相连，用于计算得到人体的姿态信息；
步长估计模块，与数据获取模块相连，用于计算得到人体的步长估计；
位置信息计算模块，与姿态信息计算模块、步长估计模块相连，用于计算确定人体的位置信息；
点云提升模块，与数据获取模块相连，用于对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；
地图构建模块，与点云提升模块、姿态信息计算模块、位置信息计算模块相连，用于将点云数据映射到全局坐标系下构建地图。
本发明提供的一种毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置，针对应急救援人员运动复杂多变的特点，基于人体运动学的约束，通过惯性器件检测步态、计算出位置和姿态，实现导航定位；再通过位置和姿态信息将点云数据映射到全局坐标系下构建地图，实现人员在烟雾环境下的实时定位与建图定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例中定位建图方法的流程图；
图2为本发明实施例中定位建图装置的佩戴示意图；
图3为本发明实施例中定位建图装置的模块图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
实施例1
本实施例公开的一种毫米波雷达惯性组合的火场烟雾场景定位建图方法，该方法针对应急救援人员运动复杂多变的特点，基于人体运动学的约束，通过惯性器件检测步态、计算出位置和姿态，实现导航定位；再通过位置和姿态信息将点云数据映射到全局坐标系下构建地图，实现人员在烟雾环境下的实时定位与建图定位。
参考图1，该定位建图方法具体包括如下步骤：
步骤1，将惯性器件与毫米波雷达搭载在进入火场烟雾场景的应急救援人员人体的胸部位置，即图2所示。相比于搭载在脚上，装置安装在使用者胸部会降低人体运动对装置数据采集的影响，提高定位和建图的精度。实时获取惯性数据与毫米波雷达点云数据；
步骤2，基于惯性数据得到人体的姿态信息，并基于人体运动学的约束与惯性数据进行人体的步长估计，并确定人体的位置信息。
基于惯性数据得到人体的姿态信息的过程为：
基于惯性数据得到人体在两个时刻帧之间的姿态变化量
Figure 478709DEST_PATH_IMAGE001
，为：
Figure 855464DEST_PATH_IMAGE002
Figure 256358DEST_PATH_IMAGE003
式中，
Figure 145686DEST_PATH_IMAGE004
为单位矩阵，
Figure 382763DEST_PATH_IMAGE005
为旋转矢量，
Figure 543967DEST_PATH_IMAGE006
为惯性数据中的角速度信息，
Figure 346838DEST_PATH_IMAGE007
为单位时 间，
Figure 141488DEST_PATH_IMAGE008
为旋转矢量
Figure 115129DEST_PATH_IMAGE009
的叉乘矩阵，即：
Figure 302528DEST_PATH_IMAGE010
其中，
Figure 740331DEST_PATH_IMAGE011
、
Figure 708812DEST_PATH_IMAGE012
、
Figure 717219DEST_PATH_IMAGE013
分别为旋转矢量
Figure 160839DEST_PATH_IMAGE014
的三个元素，即
Figure 938302DEST_PATH_IMAGE015
、
Figure 605913DEST_PATH_IMAGE016
、
Figure 836037DEST_PATH_IMAGE017
三个方向的旋转变 化量；
基于姿态变化量更新姿态矩阵，为：
Figure 880085DEST_PATH_IMAGE018
式中，
Figure 777634DEST_PATH_IMAGE019
为
Figure 636654DEST_PATH_IMAGE020
时刻惯性器件相对于全局坐标系的姿态矩阵，
Figure 619653DEST_PATH_IMAGE021
为
Figure 201813DEST_PATH_IMAGE022
时刻惯性 器件相对于全局坐标系的姿态矩阵；
根据姿态矩阵
Figure 688289DEST_PATH_IMAGE023
得到在
Figure 963282DEST_PATH_IMAGE024
时刻人体的航向角
Figure 167998DEST_PATH_IMAGE025
，依次类推得到人体 在任意时刻的航向角，即姿态信息。
基于人体运动学的约束与惯性数据进行人体的步长估计的过程为：
获取惯性数据得到人体运动过程中的加速度值序列，将其转换为加速度值变化曲线，将加速度值变化曲线中两个波峰之间对应的区域记为一步；
计算得到任意一步的步长估计值
Figure 553849DEST_PATH_IMAGE026
，为：
Figure 615870DEST_PATH_IMAGE027
式中，
Figure 546917DEST_PATH_IMAGE028
为与人体身高相关的比例系数，
Figure 488197DEST_PATH_IMAGE029
为这一步对应的加速度值序列中的 最大值，
Figure 428471DEST_PATH_IMAGE030
为这一步对应的加速度值序列中的最小值。
最后，根据当前位置、航向角和步长，得到下一时刻的位置信息，为：
Figure 138807DEST_PATH_IMAGE031
Figure 506335DEST_PATH_IMAGE032
式中，
Figure 669332DEST_PATH_IMAGE033
为人体在
Figure 147717DEST_PATH_IMAGE034
时刻的位置信息，
Figure 178472DEST_PATH_IMAGE035
为人 体在
Figure 716900DEST_PATH_IMAGE036
时刻的位置信息，
Figure 367194DEST_PATH_IMAGE037
为人体在
Figure 383691DEST_PATH_IMAGE038
时刻的航向角，
Figure 68619DEST_PATH_IMAGE039
为东向坐标，
Figure 512370DEST_PATH_IMAGE040
为北向坐标。
步骤3，对稀疏高噪声的毫米波雷达点云数据进行提升处理，其过程为：
首先设计基于编码解码结构（Encoder-Decoder）的点云数据稠密提升网络
Figure 915538DEST_PATH_IMAGE041
，将稀 疏的毫米波雷达点云数据
Figure 735727DEST_PATH_IMAGE042
输入神经网络模型，输出生成的稠密化点云
Figure 278091DEST_PATH_IMAGE043
，即：
Figure 158322DEST_PATH_IMAGE044
采集单通道的毫米波雷达点云数据和激光雷达点云数据构建数据集，以稠密的激 光雷达点云作为标签训练该神经网络模型，使神经网络模型能够将稀疏的毫米波雷达点云 映射到稠密化的点云，尤其是采集烟雾场景下的毫米波雷达数据，使神经网络模型能够适 应烟雾场景下的点云稠密化过程。采集的激光雷达稠密点云记为
Figure 783208DEST_PATH_IMAGE045
，与基于毫米波雷达数 据生成的稠密点云
Figure 141508DEST_PATH_IMAGE046
比较它们之间的差值，设为点云重建损失函数
Figure 801028DEST_PATH_IMAGE047
，为：
Figure 586582DEST_PATH_IMAGE048
为了生成更真实的稠密点云，去除噪声，引入对抗生成损失函数。另外训练一个辨 别网络
Figure 698763DEST_PATH_IMAGE049
，用于判定输入的点云是否为稠密点云，可以得到如下点云对抗重建损失函数
Figure 860754DEST_PATH_IMAGE050
， 为：
Figure 637430DEST_PATH_IMAGE051
Figure 593885DEST_PATH_IMAGE052
优化总的损失函数
Figure 193363DEST_PATH_IMAGE053
，训练网络。将训练好的模型用于部署，实时输入测试稀疏毫 米波雷达数据，输出稠密化的点云用于步骤4中的地图构建。
步骤4，通过人体的姿态信息与位置信息将点云数据映射到全局坐标系下构建地图，实现人体在烟雾环境下的实时定位与建图定位。本实施例中，所构建的地图为占据格栅地图，其构建过程为：
读取一帧点云数据以及该帧对应的使用者的姿态信息与位置信息。通过位姿信息，首先计算出该时刻使用者所在的栅格位置。然后遍历该时刻由步骤3得到的稠密化点云数据，得到点云中每一个点所在的栅格位置以及栅格序号，将点云中点所在格栅标记为占用；
对于每一个点云中的点，从当前使用者所在的栅格位置出发，直线连接点云中的点栅格位置，将划线所经过的栅格标记为空，记录位置编码在集合中；
由每一个时刻采集到的点云数据生成空余栅格集合，将点云所在的栅格标记为占据，同时在空栅格集合中，将之前被加入占据栅格集合中的点云所在栅格删除，完成地图的更新。
实施例2
如图3所示为本实施例所公开的一种毫米波雷达惯性组合的火场烟雾场景定位建图装置，其主要由惯性器件、毫米波雷达、数据获取模块、姿态信息计算模块、步长估计模块、位置信息计算模块、点云提升模块与地图构建模块组成。具体地：
惯性器件与毫米波雷达搭载在人体的胸部位置，用于采集惯性数据与点云数据；
数据获取模块与惯性器件、毫米波雷达相连，用于获取惯性数据与点云数据；
姿态信息计算模块与数据获取模块相连，用于计算得到人体的姿态信息；
步长估计模块与数据获取模块相连，用于计算得到人体的步长估计；
位置信息计算模块与姿态信息计算模块、步长估计模块相连，用于计算确定人体的位置信息；
点云提升模块与数据获取模块相连，用于对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；
地图构建模块与点云提升模块、姿态信息计算模块、位置信息计算模块相连，用于将点云数据映射到全局坐标系下构建地图。
上述各个模块的具体计算过程与实施例1相同，因此本实施例中不再对其进行赘述。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (9)

1.一种毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，将惯性器件与毫米波雷达搭载在进入火场烟雾场景的人体上，所述定位建图方法包括如下步骤：
步骤1，获取惯性数据与毫米波雷达点云数据；
步骤2，基于惯性数据得到人体的姿态信息，并基于人体运动学的约束与惯性数据进行人体的步长估计，并确定人体的位置信息；
步骤3，对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；
步骤4，通过人体的姿态信息与位置信息将稠密化点云数据映射到全局坐标系下构建地图，实现人体在烟雾环境下的实时定位与建图定位。
2.根据权利要求1所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤2中，所述基于惯性数据得到人体的姿态信息，具体为：
基于惯性数据得到人体在两个时刻帧之间的姿态变化量
Figure 89230DEST_PATH_IMAGE001
，为：
Figure 944053DEST_PATH_IMAGE002
Figure 114003DEST_PATH_IMAGE003
式中，
Figure 696163DEST_PATH_IMAGE004
为单位矩阵，
Figure 448219DEST_PATH_IMAGE005
为旋转矢量，
Figure 460562DEST_PATH_IMAGE006
为惯性数据中的角速度信息，
Figure 930857DEST_PATH_IMAGE007
为单位时间，
Figure 316708DEST_PATH_IMAGE008
为旋转矢量
Figure 923270DEST_PATH_IMAGE009
的叉乘矩阵；
基于姿态变化量更新姿态矩阵，为：
Figure 103584DEST_PATH_IMAGE010
式中，
Figure 998859DEST_PATH_IMAGE011
为
Figure 922822DEST_PATH_IMAGE012
时刻惯性器件相对于全局坐标系的姿态矩阵，
Figure 383890DEST_PATH_IMAGE013
为
Figure 21193DEST_PATH_IMAGE014
时刻惯性器件 相对于全局坐标系的姿态矩阵；
根据姿态矩阵
Figure 466081DEST_PATH_IMAGE015
得到在
Figure 928155DEST_PATH_IMAGE016
时刻人体的航向角
Figure 961839DEST_PATH_IMAGE017
，依次类推得到人体在任 意时刻的航向角，即姿态信息。
3.根据权利要求2所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤2中，所述基于人体运动学的约束与惯性数据进行人体的步长估计，具体为：
获取惯性数据得到人体运动过程中的加速度值序列，将其转换为加速度值变化曲线，将加速度值变化曲线中两个波峰之间对应的区域记为一步；
计算得到任意一步的步长估计值
Figure 234688DEST_PATH_IMAGE018
，为：
Figure 416140DEST_PATH_IMAGE019
式中，
Figure 167058DEST_PATH_IMAGE020
为与人体身高相关的比例系数，
Figure 120495DEST_PATH_IMAGE021
为这一步对应的加速度值序列中的最大 值，
Figure 33088DEST_PATH_IMAGE022
为这一步对应的加速度值序列中的最小值。
4.根据权利要求3所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤2中，所述确定人体的位置信息，具体为：
Figure 170677DEST_PATH_IMAGE023
Figure 990865DEST_PATH_IMAGE024
式中，
Figure 530300DEST_PATH_IMAGE025
为人体在
Figure 676110DEST_PATH_IMAGE026
时刻的位置信息，
Figure 300996DEST_PATH_IMAGE027
为人体在
Figure 659296DEST_PATH_IMAGE028
时刻的位置信息，
Figure 315886DEST_PATH_IMAGE029
为人体在
Figure 367019DEST_PATH_IMAGE030
时刻的航向角，
Figure 479200DEST_PATH_IMAGE031
为东向坐标，
Figure 641191DEST_PATH_IMAGE032
为北 向坐标。
5.根据权利要求1至4任一项所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤3中，所述对稀疏高噪声的毫米波雷达点云数据进行提升处理，具体为：
构建基于编码解码结构的点云数据稠密提升网络
Figure 358481DEST_PATH_IMAGE033
，采集单通道的毫米波雷达点云数 据和激光雷达点云数据构建数据集，以稠密的激光雷达点云作为标签训练该点云数据稠密 提升网络，使点云数据稠密提升网络能够将稀疏的毫米波雷达点云映射到稠密化的点云， 其中，采集的激光雷达稠密点云记为
Figure 298624DEST_PATH_IMAGE034
，与基于毫米波雷达数据生成的稠密点云
Figure 104293DEST_PATH_IMAGE035
比较它 们之间的差值，设为点云重建损失函数
Figure 804396DEST_PATH_IMAGE036
，为：
Figure 641771DEST_PATH_IMAGE037
为了生成更真实的稠密点云，去除噪声，引入对抗生成损失函数，另外训练一个辨别网 络
Figure 487236DEST_PATH_IMAGE038
，用于判定输入的点云是否为稠密点云，可以得到如下点云对抗重建损失函数
Figure 324742DEST_PATH_IMAGE039
，为：
Figure 77803DEST_PATH_IMAGE040
Figure 317155DEST_PATH_IMAGE041
优化总的损失函数
Figure 861750DEST_PATH_IMAGE042
，训练点云数据稠密提升网络，并将训练好的点云数据稠密提升网 络
Figure 904661DEST_PATH_IMAGE043
用于部署，实时输入测试稀疏毫米波雷达数据，输出稠密化的点云，为：
Figure 946566DEST_PATH_IMAGE044
式中，
Figure 24113DEST_PATH_IMAGE045
为稀疏的毫米波雷达点云数据，
Figure 945801DEST_PATH_IMAGE046
为稠密化点云数据。
6.根据权利要求5所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，所采集的单通道的毫米波雷达点云数据为火场烟雾场景下的毫米波雷达点云数据，以使得点云数据稠密提升网络能够适应烟雾场景下的点云稠密化过程。
7.根据权利要求1至4任一项所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，步骤4中，所构建的地图为占据格栅地图，其构建过程为：
读取一帧点云数据以及该帧对应的使用者的姿态信息与位置信息，通过位姿信息，首先计算出该时刻使用者所在的栅格位置，然后遍历该时刻由步骤3得到的稠密化点云数据，得到稠密化点云数据中每一个点所在的栅格位置以及栅格序号，将稠密化点云数据中点所在格栅标记为占用；
对于每一个稠密化点云数据中的点，从当前使用者所在的栅格位置出发，直线连接点云中的点栅格位置，将划线所经过的栅格标记为空，记录位置编码在集合中；
由每一个时刻采集到的稠密化点云数据生成空余栅格集合，将稠密化点云数据所在的栅格标记为占据，同时在空栅格集合中，将之前被加入占据栅格集合中的点云所在栅格删除，完成地图的更新。
8.根据权利要求1至4任一项所述的毫米波雷达惯性组合的火场烟雾场景定位建图方法，其特征在于，惯性器件与毫米波雷达搭载在人体的胸部位置。
9.一种毫米波雷达惯性组合的火场烟雾场景定位建图装置，其特征在于，包括：
惯性器件与毫米波雷达，搭载在人体的胸部位置；
数据获取模块，与惯性器件、毫米波雷达相连，用于获取惯性数据与点云数据；
姿态信息计算模块，与数据获取模块相连，用于计算得到人体的姿态信息；
步长估计模块，与数据获取模块相连，用于计算得到人体的步长估计；
位置信息计算模块，与姿态信息计算模块、步长估计模块相连，用于计算确定人体的位置信息；
点云提升模块，与数据获取模块相连，用于对稀疏高噪声的毫米波雷达点云数据进行提升处理，得到稠密化点云数据；
地图构建模块，与点云提升模块、姿态信息计算模块、位置信息计算模块相连，用于将点云数据映射到全局坐标系下构建地图。
CN202210925083.XA 2022-08-03 2022-08-03 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置 Active CN114994672B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210925083.XA CN114994672B (zh) 2022-08-03 2022-08-03 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210925083.XA CN114994672B (zh) 2022-08-03 2022-08-03 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置

Publications (2)

Publication Number Publication Date
CN114994672A true CN114994672A (zh) 2022-09-02
CN114994672B CN114994672B (zh) 2022-10-25

Family

ID=83022665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210925083.XA Active CN114994672B (zh) 2022-08-03 2022-08-03 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置

Country Status (1)

Country Link
CN (1) CN114994672B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150015869A1 (en) * 2013-01-15 2015-01-15 Raytheon Company Ladar backtracking of wake turbulence trailing an airborne target for point-of-origin estimation and target classification
CN109974696A (zh) * 2019-05-08 2019-07-05 东北大学 基于slam与步态imu融合的室内人员自主定位方法
CN110456343A (zh) * 2019-07-22 2019-11-15 深圳普捷利科技有限公司 一种基于fmcw毫米波雷达的即时定位方法及系统
CN112346048A (zh) * 2020-09-25 2021-02-09 深圳捷豹电波科技有限公司 一种基于毫米波的火灾探测搜救系统及方法
CN113189575A (zh) * 2021-07-01 2021-07-30 长沙莫之比智能科技有限公司 一种用于烟雾场景下人员定位的探测方法和装置
CN113255504A (zh) * 2021-05-19 2021-08-13 燕山大学 一种基于深度学习的路侧视角超视距全域融合感知系统
CN113843788A (zh) * 2021-06-03 2021-12-28 安徽师范大学 一种基于毫米波与惯性融合导航的安防机器人
CN114782636A (zh) * 2022-05-17 2022-07-22 江苏集萃深度感知技术研究所有限公司 三维重建方法、装置及系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150015869A1 (en) * 2013-01-15 2015-01-15 Raytheon Company Ladar backtracking of wake turbulence trailing an airborne target for point-of-origin estimation and target classification
CN109974696A (zh) * 2019-05-08 2019-07-05 东北大学 基于slam与步态imu融合的室内人员自主定位方法
CN110456343A (zh) * 2019-07-22 2019-11-15 深圳普捷利科技有限公司 一种基于fmcw毫米波雷达的即时定位方法及系统
CN112346048A (zh) * 2020-09-25 2021-02-09 深圳捷豹电波科技有限公司 一种基于毫米波的火灾探测搜救系统及方法
CN113255504A (zh) * 2021-05-19 2021-08-13 燕山大学 一种基于深度学习的路侧视角超视距全域融合感知系统
CN113843788A (zh) * 2021-06-03 2021-12-28 安徽师范大学 一种基于毫米波与惯性融合导航的安防机器人
CN113189575A (zh) * 2021-07-01 2021-07-30 长沙莫之比智能科技有限公司 一种用于烟雾场景下人员定位的探测方法和装置
CN114782636A (zh) * 2022-05-17 2022-07-22 江苏集萃深度感知技术研究所有限公司 三维重建方法、装置及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
YASIN ALMALIOGLU ET AL.: "Milli-RIO: Ego-Motion Estimation With Low-Cost Millimetre-Wave Radar", 《IEEE SENSORS JOURNAL》 *
祁嘉然: "浅析森林火情快速高分辨毫米波监测技术", 《消防科学与技术》 *

Also Published As

Publication number Publication date
CN114994672B (zh) 2022-10-25

Similar Documents

Publication Publication Date Title
KR102001728B1 (ko) 스테레오 카메라 드론을 활용한 무기준점 3차원 위치좌표 취득 방법 및 시스템
US7516039B2 (en) Mapping systems and methods
CN110262546A (zh) 一种隧道智能无人机巡检系统及方法
CN102620737B (zh) 用于生成建筑物地图的便携式外壳
US9091755B2 (en) Three dimensional image capture system for imaging building facades using a digital camera, near-infrared camera, and laser range finder
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN107783545B (zh) 基于ooda环多传感器信息融合的灾后救援旋翼无人机避障系统
CN108139758A (zh) 基于显著性特征的载运工具定位
CN208953962U (zh) 一种机器人感知系统及机器人
CN106197429A (zh) 一种多信息融合定位设备及系统
CN111915128B (zh) 一种对地震诱发的次生滑坡灾后评估与救援辅助系统
CN102288176A (zh) 基于信息融合的煤矿救灾机器人导航系统及方法
CN106643739A (zh) 一种室内环境人员定位方法及系统
CN107607091A (zh) 一种测量无人机飞行航迹的方法
CN109579832B (zh) 一种人员高度自主定位算法
Xing et al. Multi-UAV cooperative system for search and rescue based on YOLOv5
CN109764869A (zh) 一种双目相机和惯导融合的自主巡检机器人定位与三维地图构建方法
CN113075686B (zh) 一种基于多传感器融合的电缆沟智能巡检机器人建图方法
CN107783119A (zh) 应用在避障系统中的决策融合方法
CN106096207A (zh) 一种基于多目视觉的旋翼无人机抗风评估方法及系统
CN105738909B (zh) 一种适用于低空植保无人直升机作业边界提取方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN116339337A (zh) 基于红外成像、激光雷达和声音定向探测的目标智能定位控制系统及其方法
Sternberg et al. Precise indoor mapping as a basis for coarse indoor navigation
CN113358117B (zh) 一种利用地图的视觉惯性室内定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant