CN111459166B - 一种灾后救援环境下含受困人员位置信息的情景地图构建方法 - Google Patents

一种灾后救援环境下含受困人员位置信息的情景地图构建方法 Download PDF

Info

Publication number
CN111459166B
CN111459166B CN202010319435.8A CN202010319435A CN111459166B CN 111459166 B CN111459166 B CN 111459166B CN 202010319435 A CN202010319435 A CN 202010319435A CN 111459166 B CN111459166 B CN 111459166B
Authority
CN
China
Prior art keywords
point
robot
map
coordinate system
information
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.)
Active
Application number
CN202010319435.8A
Other languages
English (en)
Other versions
CN111459166A (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.)
Beijing University of Technology
Original Assignee
Beijing University of 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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN202010319435.8A priority Critical patent/CN111459166B/zh
Publication of CN111459166A publication Critical patent/CN111459166A/zh
Application granted granted Critical
Publication of CN111459166B publication Critical patent/CN111459166B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Optics & Photonics (AREA)
  • Multimedia (AREA)
  • Computer Graphics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提出了一种灾后救援环境下含受困人员位置信息的情景地图构建方法，主要应用于灾后探测机器人上，完成灾后事故现场地图构建及在地图上标定需要营救人员的位置等任务。具体流程包括：以搭载激光雷达，里程计，陀螺仪及深度摄像头的履带式机器人为主体。通过先验地图设定环境探索路径，实现机器人自主地在环境中进行激光建图与搜索任务。建图过程中，对采集到的图像进行人员检测，根据检测结果得到对应检测目标的像素中心。并结合对应的深度信息和当前机器人的位置，通过三维定位模型推算目标人员在世界坐标系中的位置，并标定在地图上。该方法可为救援人员提供受困人员灾后救援环境的位置信息，可高效地开展求援任务，有效避免二次伤亡。

Description

一种灾后救援环境下含受困人员位置信息的情景地图构建 方法
技术领域
本发明属于灾后救援环境下的情景地图构建技术领域。具体涉及一种对灾害救援现场进行地图构建并根据受困人员位置确定人员情景认知地图的方法。
背景技术
近年来，随着各类灾害的频发，以及机器人技术的广泛应用，国家开始研制用于特种作业的救援机器人，通过针对具体的灾害现场，及时有效的了解灾害情况，从来完成搜救与探测任务，保证生命和财产安全。
灾害现场主要针对火灾、有毒有害气体，以及放射性物质的环境，在各类灾害现场下，机器人主要实现对复杂且危险的外界环境的进行探索和侦探，从城市高层建筑，到各类地下工厂，当突发灾害后，使得现场会伴随各类隐患，针对救援人员无法深入侦查和搜救的现场，且又急于了解灾害情况的时候，救援机器人将有效代替搜救人员，完成救援现场的侦查和搜救工作，从而有效的避免二次伤亡，及时了解现场情况，为下一步救援工作的实施做出准备。
在危险复杂的灾后环境下，基于激光以及视觉的灾后情景地图的构建，能有效协助求援人员完成伤员的搜救，帮助救援人员制定更为安全可靠的营救方案。
在复杂环境下，对于外界环境的认知是探测机器人最需关注的部分。在机器人操作系统(ROS)框架中的激光算法Cartographer是建图效果最为出色的。Cartographer算法通过单线激光雷达扫描环境信息，使用激光雷达SLAM技术构建地图，最后在RVIZ(ROS的3D可视化工具)中，实时显示地图建立的效果和机器人定位效果。该算法具体实现的SLAM技术，能建立起二维栅格地图，算法内部使用图优化理论和Ceres solver非线性求解器求解，对硬件的要求很低，只需普通性能的激光雷达和处理器，即可较好地完成定位和建图的工作。如果激光雷达在一个不稳定的平面安装时，可以提供IMU将雷达数据投影到地图平面上来提高雷达数据的稳定性。Cartographer算法构建的地图分辨率能达到5cm，并提供ROS接口，其整体架构是典型的前端建图(局部地图)+后端优化(回环分析)。激光雷达读取的每一帧数据叫做点云，点云本身有自己的坐标系，多帧连续的点云构成一个子图，将一帧点云融合到子图的过程叫点云匹配。每个子图，反映的是环境的局部信息，子图本身有自己的坐标系。拼接起来之后进行闭环检测，最后利用图优化的方法进行分析，减小累积误差，给出构建环境地图。
此外，基于视觉的人员检测算法常分为特征法和深度学习两种方式，但这两种检测算法中都需消耗较多的计算机资源，特征法存在检测精度低，鲁棒性差的问题，深度学习方法存在检测时间长的缺点，难以满足实际应用的需求。为保证及时迅速的对灾后环境进行情景地图的认知，对检测时间提出较为严格的要求。本方法以YOLOv3为检测模型，在此基础上进行改进，得到只识别人员信息的检测方法。此外，为快速确定人员位置，需对灾后环境建立坐标系，并提供被困人员的位置，为此提出一种基于深度信息的三维定位模型，完成检测目标的位置估计，从而完成地图标记任务，生成灾后环境下受困人员的情景地图。
发明内容
本发明的主要目的是提出一种灾后救援环境下含受困人员位置信息的情景地图构建方法，该方法可以应用于灾后现场探测机器人中，完成灾后现场的地图构建、人员探测等灾后探测任务。其中主要面临如下问题：
1.为保证构建地图的时间和精度的要求，在进行Cartographer激光建图时，需对图像进行人员检测，三维位置估计。需要使用多线程的方式来保证灾后情景地图的构建速度。
2.灾后现场对人员检测的实时性要求较高，考虑到探测机器人运算能力有效，需尽可能采用简单高效的算法达到实时性以及准确性要求。
3.通过检测含有人员信息的图像和当前机器人的位置姿态，对环境中人员的位置进行估计，并在地图上进行标记和位置保存。
为了解决以上技术问题，本发明提出了一种灾后救援环境下含受困人员位置信息的情景地图构建方法，主要应用于灾后探测机器人上，完成灾后事故现场地图构建及在地图上标定需要营救人员的位置等探测任务。具体流程包括：以搭载激光雷达，里程计，陀螺仪以及深度摄像头的履带式机器人为主体。首先通过先验地图经验设定环境探索路径，实现机器人自主地有目的性地在环境下进行激光建图与探测搜索任务。然后，在建图的过程中，读取摄像头采集到的图像数据，并进行人员检测，根据检测结果得对应检测目标的像素位置。后结合对应的深度信息，根据三维定位模型和当前机器人的位置，推算目标人员在世界坐标系中的位置，并在地图中进行标定。本发明方法的具体工作流程如下：
S1开启探测机器人进行激光建图
针对许多建筑物已经存在有平面布局图的先验经验这一特点，为保证机器人更为快速的、自主的完成环境探索任务。首先基于已存在的先验地图信息，以机器人启动时的位置点为原点并建立世界坐标系，依据先验环境地图的尺寸大小，对机器人需要遍历的关键导航点位置进行设定，确保连续且有间隔直到最终需要到达的终点。然后分别启动机器人激光建图算法，以及针对多点目标导航算法，从而实现基于连续目标点导航式的环境探索任务，通过判定每一个目标导航点与当前机器人的位置上的距离大小，是否满足设定的距离阈值，从而判定机器人是否到达这一目标点。若是达到，更换下一个导航点，否则，继续当前导航任务。通过上述方法逐步实现对目标导航点的探索，直至终点，最终完成指定关键路线周边的栅格地图生成。
S2订阅彩色图像节点进行人员检测
通过订阅机器人在进行环境探索时所采集的彩色图像，以及深度图像信息。规定尺寸大小为：512*424，以Yolov3算法为基础，将输入图像按尺度分为S×S个单元格进行处理，若待检测目标的中心在某一单元格之中，那么对应的单元格将负责这一目标的检测任务。对于每个默认单元格，网络预测出4个坐标值以及1个置信度。最后，将网络预测的人员框经过非极大值抑制得到最终的预测结果。从而完成对待检测的物体进行分类，并实现对检测后框选得到的物体进行像素位置的输出。
S3使用检测得到的人员图像进行三维估计
根据检测目标模型可以得到每一帧图像检测到的结果，根据检测结果来推算是否进行三维目标估计。若当前帧图像未存在目标，即没有检测结果的输出，则跳转到下帧图像进行检测。若存在，则找出框选对象的中心点坐标值，以及对应帧深度图像的的值，得到距离。最终根据三维位置估算模型以及当前机器人位姿得检测目标在空间内的坐标信息。
S4根据检测目标的位置在地图标记并巡检
由每一帧带有检测目标的图像可以得到该目标的三维坐标信息，针对不同帧图像下的同一目标存在冗余信息的情况，以连续帧判定结果的平均值为最终取值，从而得到待检目标的空间坐标信息。将以上坐标点信息保留后，即可作为后续机器人巡检的导航点，为救援任务提供更好的可能。
本发明具有如下优点：
本方法采用基于先验地图的经验进行路线定制，并通过多点导航的方式来使机器人完成到目标点的自主运动，相比于传统基于随机树探索的方法，能较为快速的完成机器人探测地图并生成地图的任务。同时，基于激光所建立的栅格地图，使用目标检测框架与三维定位模型相结合的方式，从而完成环境中待检测物体的空间位置推算，得到检测物体的三维空间坐标。最后，基于上述得到的坐标信息，后续可以实现机器人多点导航任务的巡检操作等任务。
附图说明
图1灾后救援环境下含受困人员位置信息的情景地图构建方法流程图；
图2常见楼层建筑设计图；
图3对应图2设计图中线框内的3D仿真环境图；
图4三维定位分析示意图，其中图(a)为求解检测点坐标示意图，图(b)为对应仿真环境下机器人采集效果图，图(c)为机器人采集到的仿真环境图像，图(d)为机器人采集到的仿真环境下的深度图像；
图5世界坐标系下目标坐标求解分析图；
图6救援探测机器人系统组成示意图；
图7仿真环境下的室内场景图；
图8含受困人员位置信息的情景地图构建效果示意图；
具体实施方式
下面结合附图与实例，针对本方法做详细说明。
常见的深度相机拍出的深度图像尺寸比较单一，基本都是512*424尺寸
oxy是图像面的坐标系，需要用到这个坐标系来介绍推算过程。
图1为灾后救援环境下含受困人员位置信息的情景地图构建方法流程图。主要分为激光建图进程和人员检测进程，当检测到人员信息后，将采用三维估算模型求解对应人员信息的位置，标记到建立好的栅格地图中。具体步骤如下：
1、开启探测机器人进行激光建图
图2为常见楼层建筑设计图，往往在大多数情况下，楼层的平面设计图是为人们所知的，根据实际环境搭建的3D仿真物理模型如图2所示。各分支算法以及具体步骤如下：
1.1针对建筑物的平面图进行路径设定
以图2中水平和垂直的两线交点为起点，建立世界平面坐标系，根据需要探索的环境的区域，根据平面设计图上的尺寸，以坐标的形式，设定一系列导航路径点。
1.2基于Cartographer的激光建图进程
探测机器人的位姿用p＝(px,py,pθ)来表示，其中(px,py)表示机器人在世界坐标系下的位置，pθ为机器人在平面上的旋转偏向角度。激光雷达所测的一帧数据包含n个点，点的坐标记为hk,k＝1…n，初始时刻雷达点云数据表示为(0,0,0)。该数据帧映射到子图的变换位姿记为T，映射到子图坐标系下如公式(1)所示。
在一段时间内连续扫描的激光雷达点云数据帧可以生成一个子图，子图采用概率栅格的地图表达模型。当新的扫描数据插入到概率栅格时，栅格的状态将会被计算，每一个栅格都将有击中(hit)和丢失(miss)两种情况。击中的栅格，将相邻的栅格插入到命中集合中，将扫描中心和扫描点连接射线上的所有相关点添加到丢失集合中。对每个之前没有观测到的栅格会设置一个概率值，对于已经观测到的栅格则会按照式(2)对其进行概率更新。
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phit))) (2)
其中Mold(x)为更新前对应栅格点x的击中概率，Mnew(x)为更新后对应栅格点x的击中概率。
最后在把激光扫描帧插入到子图之前，需要对扫描帧位姿和当前的子图通过Ceres Solver求解器进行优化，便可以将上述问题转化为求解非线性最小二乘问题。目的是求解出点云匹配过程中转化求解转换矩阵T的最优化问题，如公式(3)所示。
其中Msmooth作为双三次插值函数，为了保证经过转换之后的各个栅格点都能在概率图上计算对应的概率值。
1.3基于A*算法的多点导航进程
A*算法是静态地图做最优路径规划时最常用的算法，其可在二维平面中，从起始点A点到终点B点寻找一条成本最低的路径，设栅格地图上每一个点x对应的权值为F(x)，则从A点经过点x并最终达到点B的总路径代价如公式(4)所示。
其中ξ∈{ξ1,ξ2,...,ξn}，ξi代表其中一条可以从点A到达点B的可行路径。D(x)表示经过点x的所有节点代价之和，Q(x)作为点x到B点的水平和垂直距离的绝对值之和。通过上述方法可以实现点A到点B的最优路径的计算。这里设人为设定的导航点坐标依次为N(x,y)∈{N1(x1,y1),N2(x2,y2),...,Nn(xn,yn)}，判定机器人是否到达某一导航点的条件如公式(5)所示。
其中，R表示判定机器人是否到达导航点的阈值，通常根据实际环境设定为0.5-1.5m之间，(R是判断机器人到达目标点的阈值，如果R越低，精度越高，所需要时间也高，一般根据实际情况，0.5～1.5都是合理的。)当机器人的位置与对应某一个导航点k位置的距离小于阈值R时，则判定freach为1，表示已经抵达该导航点，将更换第k+1个点为导航点。若判定freach为0，表示未抵达该导航点，继续当前导航任务。通过上述方法可以实现对指定导航路径上的各位置区域进行探索，从而按照指定路线进行栅格地图的生成。
2、订阅彩色图像节点进行人物检测
本文采用改进的YOLOv3算法进行人员检测。具体步骤如下：
2.1人体检测网络的训练
选择COCO公开数据集为样本数据集，使用其中带有人员信息的样本图片作为训练集的正样本，标注信息使用原数据集中人员的标注信息。将所有的训练样本分批次输入网络进行训练。训练时采用误差和平方函数作为整个训练过程的损失函数，公式定义如式(6)所示。
其中为网格i中的第j个预测框检测人员，/>为网格i中的第j个预测框中没有检测到人员。/>表示网格i中检测到人员，λloss为坐标损失系数，设置为5。λnobody为没有检测到人员信息时置信度损失系数，设置为0.6。
2.2订阅机器人图像进行检测
通过训练好的网络，对机器人采集到的彩色图像进行订阅并输入到网络中，可得到一帧图像中人员的位置，并进行框选，此外，为确定被检测目标的准确位置，需从检测目标的边框信息中提取一个像素点用来定义并代表该物体，用来作为一副图像中检测目标的位置点。这里将该像素点定义为Object(x,y)。设标记框图的输入的数据为：采集到的像素大小的512*424彩色图像和512*424标配后的深度图像。
根据Yolov3检测后所标出的框图位置可以得到该框图的左上点坐标Object(xs,ys)，右下点坐标Object(xe,ye)，根据这两点由公式(7)可得代表某一检测目标的像素位置点。
Object(x,y)＝[(Object(xe,ye)-Object(xs,ys))÷2] (7)
3、使用检测得到的人员图像进行三维估计
如图4所示，以机器人搭载的深度摄像头采集点为起始点，形成平面OXY。此外检测目标所在图像平面oBCD(在oxy坐标系上)与OXY平面中的Z轴延伸线相交于点E，通过检测模块得到检测目标的中心点为E'(x',y')。以点E为起点，沿X轴做EG垂直于BD，垂足为G。同理，沿Y轴做EH垂直于CD，垂足为H。因摄像头采集图像是中心位置，所以点E是矩形oBCD的中心点，G点和H点分别是线段BD、CD的中点。以E'点分别向EG、EH做垂线，垂足分别为M、N。最终求解检测点坐标示意图如图4(a)所示，对应仿真环境下机器人采集效果如图4(b)所示。
已知检测目标上E'(x',y')的像素坐标，根据坐标找到对应该点的深度值，即可得知E'到摄像头的距离值，也即OE线段的长度，这里设为h'。α是以摄像机采集点O为起点，形成的平面OBD与平面OHE的夹角，同理，β是平面OCD与平面OEG的夹角。由于当深度相机固定在机器人上时，所拍出的视野范围大小固定，因此这两个角度的大小也是固定的，根据机器人所载相机的安装位置，通常α取值范围在50°-80°，β取值范围在40°-70°。(这个视野范围是根据摄像头安装位置不一，角度也所在不同，通常都是在这个范围内，就能满足实验要求。)所以，根据以上信息，可以得到线段EG和EH的长度如公式(8)所示。
规定采集到的一帧深度图像的尺寸是512*424，可以得到EG、EH之间的像素个数PEG、PEH，因为E'像素坐标确定，同理得PEM、PEN，为确定点E'相对于OXY坐标系的位置，结合公式(8)求得距离和像素与距离的比例关系，如公式(9)所示。其中LNE'指的是点N到点E'的长度，LME'指的是点M到点E'的长度。
设点E'相对于OXY坐标系的坐标为：(XE',YE',ZE')。
因为点E'的像素坐标在oxy坐标系下的值为(x',y')，且oBCD大小为512*424像素值，所以PEG＝256、PEM＝x'-256、PEH＝212、PEN＝y'-212，由公式(8)和公式(9)可得点E'的各分量坐标的值分别如公式(10)所示。
通过上述模型，以及图5世界坐标系下目标坐标求解分析图可知，图中oxy为世界坐标系，(px,py)表示机器人当前在世界坐标系下的位置。此外OYZ坐标系是以机器人为原点所建坐标系，点(YE',ZE')是目标在该坐标系下的位置，在此根据机器人的偏移角pθ可以求解检测目标在世界坐标系下的位置。从而将相对于机器人的坐标系坐标转变为绝对的世界二维坐标系坐标，可得到该人员对应的世界坐标位置信息的水平分量和垂直分量如公式(11)所示。
经过公式(10)和公式(11)整理后得人员信息相对于世界坐标系的横纵坐标的计算如公式(12)所示。
4、根据检测目标的位置在地图标记
通过本方法对采集到的每一帧带有检测目标的图像进行解析，后经三维模型计算可得出每一帧图像中的目标的三维坐标信息，针对不同帧图像下的同一目标存在冗余信息的情况，以连续帧判定结果的平均值为最终取值，从而得到待检目标的空间坐标信息。后在RVIZ中根据检测到的类别，进行标记操作，根据对应计算好的坐标信息，将检测到的类别在对应坐标位置插入不同模型用以区分。
如图6所示，为救援探测机器人系统组成示意图，该情景地图构建系统以探测机器人端主，以机械结构为躯干，电气系统为桥梁，完成对电机电流以及力矩的控制，从而实现机器人的移动控制。此外，探测机器人以工控机为主体，作为主控制器，并作为局域网的服务端，开启整个局域网，等待客户端的接入。任何带有无线网卡的PC机都可以控制客户端的方式进行接入到无线局域网中，实现远程操控机器人，在控制端实现传感器数据的接入，以及建图和识别算法的运行，并生成灾后的情景认知地图。
为测试本文方法的有效性，建立仿真环境下的室内场景图，如图7所示进行仿真实验。在该室内环境下，共计六个测试人员模型，分散在环境中。通过先验地图的环境信息，设定系列导航点，让机器人快速完成探索任务。
最终生成的含受困人员位置信息的情景地图构建效果示意图如图8所示，其中图中所描绘的实线为机器人所走过的路径，实线上面的方格为基于先验地图尺寸设定的各探索路径导航点，对应的人员检测结果以戴帽子的玩具模型标注出。
通过本方法可以为后续的机器人执行巡检以及导航任务提供关键的指示信息，帮助救援人员与其他机器人更好的完成各类任务。

Claims (4)

1.一种灾后救援环境下含受困人员位置信息的情景地图构建方法，其特征在于，包括如下几个步骤：
S1：利用先验地图经验，指定路线，开启探测机器人进行自主探索；
S2：对采集到的图像进行人员检测，得到图像中人员框选区域；
S3：结合当前机器人位姿以及深度图像信息，对存在人员信息的图像进行三维估计，得检测目标的三维坐标；
S4：根据检测人员位置在地图标记，最终生成带有人员信息的情景地图；所述S1的具体步骤如下；
S1.1针对建筑物的平面图进行路径设定
以机器人启动时的位置为坐标原点，建立世界平面坐标系，根据需环境探索的区域，以及平面设计图上的尺寸，以坐标的形式，设定一系列导航路径点；
S1.2基于Cartographer的激光建图进程
通过机器人搭载的激光雷达传感器和IMU传感器，完成地图构建任务；探测机器人的位姿用p＝(px,py,pθ)来表示，其中(px,py)表示机器人在世界坐标系下的位置，pθ为机器人在平面上的旋转偏向角度；通过Cartographer的方法实现将各雷达数据帧以点云匹配的方式，形成子图，再将以后的激光数据扫描帧插入到子图里去；对区域中的栅格进行概率更新，进而更新各个栅格的状态，逐步形成栅格地图；
S1.3基于A*算法的多点导航进程
通过S1.1设定的系列导航点，并配合S1.2建图进程，实现在二维平面中，从起始点A点到终点B点寻找一条成本最低的路径，设栅格地图上每一个点x对应的权值为F(x)，则从A点经过点x并最终达到点B的总路径代价如公式(1)所示；
其中ξ∈{ξ1,ξ2,...,ξn}，ξi代表其中一条可以从点A到达点B的可行路径；D(x)表示经过点x的所有节点代价之和，Q(x)作为点x到B点的水平和垂直距离的绝对值之和；实现点A到点B的最优路径的计算；
设定的导航点坐标依次为N(x,y)∈{N1(x1,y1),N2(x2,y2),...,Nn(xn,yn)}，判定机器人是否到达某一导航点的条件如公式(2)所示；
其中，R表示判定机器人是否到达导航点的阈值，设定范围为0.5-1.5m之间，当机器人的位置与对应某一个导航点k位置的距离小于阈值R时，则判定freach为1，表示已经抵达该导航点，将更换第k+1个点为导航点；若判定freach为0，表示未抵达该导航点，继续当前导航任务。
2.根据权利要求1中所述的一种灾后救援环境下含受困人员位置信息的情景地图构建方法，其特征在于，所述S2的步骤订阅机器人采集到的彩色图像并输入到检测网络中，框选出一帧图像中人员，并选择轮廓中的中心像素点来定义并代表该人员，该像素点定义为Object(x,y)；框图的左上点坐标Object(xs,ys)，右下点坐标Object(xe,ye)，根据这两点由公式(3)得代表某一检测目标的像素位置点；
Object(x,y)＝[(Object(xe,ye)-Object(xs,ys))÷2] (3)。
3.根据权利要求1中所述的一种灾后救援环境下含受困人员位置信息的情景地图构建方法，其特征在于，步骤S3具体如下；
S3.1使用检测得到的人员图像进行三维估计
以机器人搭载的深度摄像头采集点为起始点，形成平面OXY；此外检测目标所在平面oxy与OXY平面中的Z轴延伸线相交于点E，检测目标所在平面oBCD在oxy平面上；通过检测模块检测目标的中心点为E'(x',y')；以点E为起点，做EG垂直于BD，垂足为G；同理，做EH垂直于CD，垂足为H；因摄像头采集图像是中心位置，所以点E是矩形oBCD的中心点，G点和H点分别是线段BD、CD的中点；以E'点分别向EG、EH做垂线，垂足分别为M、N；
已知检测目标上E'(x',y')的像素坐标，根据坐标找到对应该点的深度值，即可得知E'到摄像头的距离值，也即OE线段的长度，设为h'；α是以摄像机采集点O为起点，形成的平面OBD与平面OHE的夹角，同理，β是平面OCD与平面OEG的夹角，由于当深度相机固定在机器人上时，所拍出的视野范围大小固定，因此这两个角度的大小也是固定的，取α为50°～80°，β为40°～70°；所以，根据以上信息，可以得到线段EG和EH的长度如公式(4)所示；
规定采集到的一帧深度图像的尺寸是512*424，得到EG、EH之间的像素个数PEG、PEH，因为E'像素坐标确定，得到EM、EN之间的像素个数PEM、PEN，为确定点E'相对于OXY坐标系的位置，求得距离和像素的比例关系，如公式(5)所示，其中LNE'指的是点N到点E'的长度，LME'指的是点M到点E'的长度；
设点E'相对于OXY坐标系的坐标为：(XE',YE',ZE')
因为点E'的像素坐标在oxy坐标系下的值为(x',y')，且oBCD大小为512*424像素值，所以PEG＝256、PEM＝x'-256、PEH＝212、PEN＝y'-212，由公式(4)和公式(5)可得点E'的各分量坐标的值分别如公式(6)所示；
S3.2将检测目标位置转为世界坐标系位置
通过S3.1的求解模型，OYZ坐标系是以机器人为原点所建坐标系，点(YE',ZE')是目标在该坐标系下的位置，在此根据机器人的偏移角pθ以及机器人在世界坐标系下的位置(px,py)，求解检测目标在世界坐标系下的位置；从而将相对于机器人的坐标系坐标转变为绝对的世界二维坐标系坐标，得到该人员对应的世界坐标位置信息的水平分量和垂直分量如公式(7)所示；
经过公式(6)和公式(7)整理后得人员信息相对于世界坐标系的横纵坐标的计算如公式(8)所示；
。
4.根据权利要求1中所述的一种灾后救援环境下含受困人员位置信息的情景地图构建方法，其特征在于，步骤S4具体为通过每一帧图像中的目标的三维坐标信息，针对不同帧图像下的同一目标存在冗余信息的情况，以连续帧判定结果的平均值为最终取值，从而得到待检目标的空间坐标信息；后在RVIZ中根据检测到的类别，进行标记操作，根据对应计算好的坐标信息，将检测到的类别在对应坐标位置插入不同模型用以区分。
CN202010319435.8A 2020-04-22 2020-04-22 一种灾后救援环境下含受困人员位置信息的情景地图构建方法 Active CN111459166B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010319435.8A CN111459166B (zh) 2020-04-22 2020-04-22 一种灾后救援环境下含受困人员位置信息的情景地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010319435.8A CN111459166B (zh) 2020-04-22 2020-04-22 一种灾后救援环境下含受困人员位置信息的情景地图构建方法

Publications (2)

Publication Number Publication Date
CN111459166A CN111459166A (zh) 2020-07-28
CN111459166B true CN111459166B (zh) 2024-03-29

Family

ID=71679698

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010319435.8A Active CN111459166B (zh) 2020-04-22 2020-04-22 一种灾后救援环境下含受困人员位置信息的情景地图构建方法

Country Status (1)

Country Link
CN (1) CN111459166B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111982114B (zh) * 2020-07-30 2022-05-13 广东工业大学 一种采用imu数据融合估计三维位姿的救援机器人
CN112068552A (zh) * 2020-08-18 2020-12-11 广州赛特智能科技有限公司 一种基于cad图纸的移动机器人自主建图方法
CN112254582B (zh) * 2020-09-17 2023-07-04 河南理工大学 一种辅助救援单兵战车及使用方法
CN112180937A (zh) * 2020-10-14 2021-01-05 中国安全生产科学研究院 一种地铁车厢消毒机器人及其自动导航方法
CN112379384B (zh) * 2020-11-10 2024-04-09 浙江华消科技有限公司 对象的位置确定方法及装置
CN112465987A (zh) * 2020-12-17 2021-03-09 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种视觉融合信息三维重建的导航地图构建方法
CN113075687A (zh) * 2021-03-19 2021-07-06 长沙理工大学 一种基于多传感器融合的电缆沟智能巡检机器人定位方法
CN113311825A (zh) * 2021-05-05 2021-08-27 温州大学 一种可视化且自定义的ros智能机器人人机交互系统及其控制方法
CN113112098A (zh) * 2021-05-12 2021-07-13 上海野戈智能科技发展有限公司 建筑物缺陷的检测方法及装置
CN113298940B (zh) * 2021-05-25 2022-08-09 中南大学 援救区域的3d拓扑地图构建方法、系统以及存储介质
CN113506344A (zh) * 2021-07-07 2021-10-15 西南科技大学 一种核辐射环境机器人高精度三维定位装置及方法
CN113933871B (zh) * 2021-10-15 2023-01-24 贵州师范学院 基于无人机和北斗定位的洪水灾情检测系统
CN114779816B (zh) * 2022-05-17 2023-03-24 成都工业学院 一种面向震后废墟环境起降的搜救无人机及其具有的系统
CN115063541B (zh) * 2022-08-18 2022-12-02 四川天启智能科技有限公司 一种大型机器人掩埋救援方法及系统
CN117636251B (zh) * 2023-11-30 2024-05-17 交通运输部公路科学研究所 一种基于机器人的灾损检测方法和系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103914068A (zh) * 2013-01-07 2014-07-09 中国人民解放军第二炮兵工程大学 一种基于栅格地图的服务机器人自主导航方法
CN106950952A (zh) * 2017-03-10 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的农田环境感知方法
CN107544504A (zh) * 2017-09-26 2018-01-05 河南科技学院 一种面向复杂环境的灾区救援机器人自主探测系统及方法
CN108801269A (zh) * 2018-08-29 2018-11-13 山东大学 一种室内云机器人导航系统及方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050149251A1 (en) * 2000-07-18 2005-07-07 University Of Minnesota Real time high accuracy geospatial database for onboard intelligent vehicle applications

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103914068A (zh) * 2013-01-07 2014-07-09 中国人民解放军第二炮兵工程大学 一种基于栅格地图的服务机器人自主导航方法
CN106950952A (zh) * 2017-03-10 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的农田环境感知方法
CN107544504A (zh) * 2017-09-26 2018-01-05 河南科技学院 一种面向复杂环境的灾区救援机器人自主探测系统及方法
CN108801269A (zh) * 2018-08-29 2018-11-13 山东大学 一种室内云机器人导航系统及方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法

Also Published As

Publication number Publication date
CN111459166A (zh) 2020-07-28

Similar Documents

Publication Publication Date Title
CN111459166B (zh) 一种灾后救援环境下含受困人员位置信息的情景地图构建方法
Kim et al. SLAM-driven robotic mapping and registration of 3D point clouds
CN111486855B (zh) 一种具有物体导航点的室内二维语义栅格地图构建方法
Zhou et al. StructSLAM: Visual SLAM with building structure lines
CN112859859B (zh) 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
CN109682381B (zh) 基于全向视觉的大视场场景感知方法、系统、介质及设备
JP2021152662A (ja) リアルタイムのマッピングと位置確認のための方法及び装置
WO2017028653A1 (zh) 一种移动机器人室内自建地图的方法和系统
CN111429574A (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
Xiao et al. 3D point cloud registration based on planar surfaces
Moura et al. Bim-based localization and mapping for mobile robots in construction
Bavle et al. S-graphs+: Real-time localization and mapping leveraging hierarchical representations
CN112015187B (zh) 一种用于智能移动机器人的语义地图构建方法及系统
CN114332385A (zh) 一种基于三维虚拟地理场景的单目相机目标检测与空间定位方法
Díaz Vilariño et al. Scan planning and route optimization for control of execution of as-designed BIM
Kim et al. As-is geometric data collection and 3D visualization through the collaboration between UAV and UGV
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
Arias et al. 3D modeling and section properties of ancient irregular timber structures by means of digital photogrammetry
Rogers et al. The DARPA SubT urban circuit mapping dataset and evaluation metric
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
Yu et al. Unmanned aircraft path planning for construction safety inspections
Tan et al. An adaptive crack inspection method for building surface based on BIM, UAV and edge computing
Sgrenzaroli et al. Indoor mobile mapping systems and (BIM) digital models for construction progress monitoring
Blaer et al. Two stage view planning for large-scale site modeling
Xiao et al. User-guided dimensional analysis of indoor building environments from single frames of RGB-D sensors

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