CN117570959A - 一种人机协同救援态势地图构建方法 - Google Patents

一种人机协同救援态势地图构建方法 Download PDF

Info

Publication number
CN117570959A
CN117570959A CN202311494443.6A CN202311494443A CN117570959A CN 117570959 A CN117570959 A CN 117570959A CN 202311494443 A CN202311494443 A CN 202311494443A CN 117570959 A CN117570959 A CN 117570959A
Authority
CN
China
Prior art keywords
target
map
detection
rescue
positioning
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.)
Pending
Application number
CN202311494443.6A
Other languages
English (en)
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.)
China South Industries Group Automation Research Institute
Original Assignee
China South Industries Group Automation Research Institute
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 China South Industries Group Automation Research Institute filed Critical China South Industries Group Automation Research Institute
Priority to CN202311494443.6A priority Critical patent/CN117570959A/zh
Publication of CN117570959A publication Critical patent/CN117570959A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • 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/20—Instruments for performing navigational calculations
    • 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/3844—Data obtained from position sensors only, e.g. from inertial navigation
    • 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/3852—Data derived from aerial or satellite images
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • G01S19/44—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method

Landscapes

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

Abstract

本发明公开了一种人机协同救援态势地图构建方法，该方法能够将救援人员携带的救援端设备、探测机器人端、救援指挥端有机结合起来。利用探测机器人搭载探测所需的感知设备、激光雷达等，进入救援区域获取多视角、多模态的目标信息，通过感知器感知到目标后，再由探测机器人身上的高效定位算法计算出检测目标所在的位置。该方法解决搜救过程中救援前线存在的有人救援风险大、有人无法进入的地方等问题。通过人机协同构建态势图，弥补单独使用AI算法存在的固有缺陷，增强AI的智能，提高构建出来的态势图的可靠性以及有效性。解决了救援前后方存在的救援信息数据少，救援信息更新不及时和交互繁琐耗时多的问题。

Description

一种人机协同救援态势地图构建方法
技术领域
本发明涉及协同救援技术领域，特别是涉及一种用于复杂环境的人机协同救援态势地图构建方法。
背景技术
当进行野外或灾后室内楼宇救援时，在崎岖非铺装和多种地面类型的复杂环境下，靠人力无法进入的危险区域，可使用机器人用于构建救援态势地图，使得救援行动能够在减少人力物力成本的情况下，提高救援作业的安全性以及救援效率。在地震、海啸等自然灾害后，会有许多容易引起二次灾害的隐含因素，可以为二次灾害防范提供构建含有更多人无法观察到的二次灾害的隐患地图。可以为一片区域的野生动物活动范围进行排察并提供分布地图，为野生动物保护人员提供更加方便以及安全的野生动物分布图。可以选择使用无人潜航器进入水下探测，提供贵重物品、需要打捞物品的信息图以及定位，构建出水下环境救援图，提高打捞的效率以及准确性。
申请号202010319435.8的专利文献中记载了一种灾后救援环境下含受困人员位置信息的情景地图构建方法，主要应用于灾后探测机器人上，完成灾后事故现场地图构建及在地图上标定需要营救人员的位置等任务。具体流程包括：以搭载激光雷达，里程计，陀螺仪及深度摄像头的履带式机器人为主体，通过先验地图设定环境探索路径,实现机器人自主地在环境中进行激光建图与搜索任务。建图过程中，对采集到的图像进行人员检测，根据检测结果得到对应检测目标的像素中心。并结合对应的深度信息和当前机器人的位置，通过三维定位模型推算目标人员在世界坐标系中的位置，并标定在地图上。该方法可为救援人员提供受困人员灾后救援环境的位置信息。
由于上述方法以履带机器人为主体，所适用的场景较为限制，只适用于地面环境上，而无法针对于例如海上等特殊场景，无法完成多场景地图构建任务。并且该方法的履带式机器人只装载深度摄像头，使得在探测侦察过程中获取到的目标信息单一，加大了对目标具体类别、救助优先级判断的难度。在建图过程中，完全依赖于机器人自身的算法对目标进行检测，该方法使用传统的YOLOv3算法进行环境感知，算法较为落后，并且在光照、距离、环境材质、算法本身性能的影响下，容易出现误检、漏检的情况。由于没有人机交互的过程，使得在机器探测出现明显错误时，人无法介入进行修改矫正，无法保证检测目标的正确率。探测机器人在建图过程较为独立，救援前线与指挥端相互割裂，没有打通救援前线与指挥端之间的实时通讯与监测，无法实时呈现多模态地图化救援态势图。
可见，现有技术中的相关领域的地图构建方法，使用场景较为限制，无法应用于更多领域，并且获取目标信息形式单一，没有多模态信息，设计的AI算法较为落后，容易出现误检漏检的情况，并且无法做到人机协同共建救援态势图，救援前线与指挥端之间的地图呈现不够及时且完整。
发明内容
鉴于上述问题，本发明提供用于克服上述问题或者至少部分地解决上述问题的一种人机协同救援态势地图构建方法。
本发明提供了如下方案：
一种人机协同救援态势地图构建方法，包括：
通过进入救援场景中的探测机器人端搭载的多种感知设备获取所述救援场景的多视角、多模态数据，以使所述探测机器人端根据所述多视角、多模态数据通过目标检测模块进行目标检测获得目标检测结果，并在确定检测到目标后结合所述多视角、多模态数据以及目标定位模型进行目标定位获得目标定位结果，并利用所述目标检测结果以及所述目标定位结果构建初步态势地图；
通过救援端设备以及指挥端设备分别接收所述探测机器人端发送的所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图并展示，以便用户根据所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图对探测结果是否有误进行判断；
通过所述救援端设备和/或所述指挥端设备接收用户输入的修正信息并将所述修正信息发送至所述探测机器人端，以使所述探测机器人端根据所述修正信息对所述初步态势地图进行修正获得目标态势地图；所述修正信息为用户确定所述探测结果有误后根据所述探测结果有误的类型所确定；
其中，多种所述感知设备至少包括相机、GNSS/RTK设备以及激光雷达；所述目标检测模块包括改进的YOLOv5目标检测网络，所述改进的YOLOv5目标检测网络在训练过程中采用分类损失、定位损失、置信度损失加权求和作为整个训练过程的损失函数；所述目标定位模型用于实现有GNSS/RTK信号、有已知地图、无GNSS/RTK信号和已知地图情况下的目标定位。
优选地：所述损失函数由下式表示：
式中：表示真实的类别标签的独热编码，/>表示预测的类别标签的概率输出，x,y,w,h分别表示预测的检测框的中心坐标以及宽度与高度，tx,ty,tw,th表示实际标注框的中心坐标以及宽度与高度，/>为实际的存在标签，表示目标是否真实存在与检测框中，为预测的存在标签，λcls,λloc,λconf分别为分类权重、定位损失权重、置信度损失的权重，用于控制各个损失的重要性。
优选地：还包括探测机器人端在确定检测到目标后结合所述多视角、多模态数据以及跟踪算法进行实时跟踪，然后将所述多视角、多模态数据送入所述目标定位模型进行位置计算，将坐标映射到世界坐标系上获得所述目标定位结果。
优选地：所述相机包括RealSense D435i RGBD相机；所述改进的YOLOv5目标检测网络用于根据输入的所述RealSense D435i RGBD相机采集到的RGB图像输出目标的位置并进行框选生成检测框；所述跟踪算法用于执行以下操作：
提取所述检测框内的特征，所述特征包括表观特征以及运动特征；
使用卡尔曼滤波公式进行先验状态预测；
利用上一帧的后验估计值，通过状态转移矩阵变换，得到当前帧的先验估计状态；
使用上一帧的后验估计协方差来计算当前帧的先验估计协方差，预测对应边框的轨迹；
将上一帧检测出的目标框与预测的框进行交并比匹配，再通过交并比匹配的结果计算其代价矩阵；
将得到的代价矩阵作为匈牙利算法的输入，得到线性匹配的若干检测结果，将对应的检测结果通过卡尔曼滤波更新对应的轨迹变量；不断重复，实现目标的持续跟踪。
优选地：所述状态转移矩阵F如下：
式中：(u,v)表示预测框的中心坐标，γ表示宽高比，h表示高度，表示前四个变量的速度信息；
所述卡尔曼滤波预测公式如下式所示：
式中：表示第k步的状态预测值，Fk表示第k步的状态转移矩阵，/>表示第k-1步的状态预测值，Bk表示输入矩阵，uk表示第k步的控制输入，P表示某一步的估计误差，Q表示系统噪声协方差矩阵；
所述交并比公式如下式所示：
式中：x1,y1,x2,y2表示第一个矩形框的左上角和右下角的坐标，x3,y3,x4,y4表示第二个矩形框的左上角和右下角的坐标，xinter1,yinter1,xinter2,yinter2表示两个矩形框的交集区域的左上角和右下角的坐标。
优选地：所述目标定位模型用于执行以下操作：
进行载体坐标系定位以便确定目标相对于所述多种感知设备的位置坐标，确定载体相对于地图坐标系的坐标，确定求得载体坐标系到地图坐标系的变换矩阵Tvm，变换矩阵Tvm由旋转矩阵Rvm和平移向量tvm组成，此时目标在地图坐标系下的坐标即可以表示为：
pm＝Tvmpv
式中：pv载体坐标系定位坐标，pm表示目标在地图坐标系下的坐标。
载体坐标系定位坐标通过激光雷达点云数据和相机图像数据计算得到，相机图像经由目标检测网络得到目标的2D框，通过下式将激光点投影到图像平面：
式中：x、y、z为某一个激光点的坐标，fx、fy、cx、cy为相机的内参，Tlc为激光雷达到相机坐标系的变换矩阵；
通过基于欧式距离的聚类算法获得多个目标点云簇，计算点云簇的重心作为目标物体的坐标。
优选地：所述目标定位模型包括激光雷达/IMU里程计定位模块、3D激光配准定位模块、GNSS/RTK定位模块以及定位解析模块；
所述激光雷达/IMU里程计定位模块用于采用LIO-SAM激光雷达/IMU里程计输出的载体到里程计起点的变换矩阵信息，并在运行时构建前端所需展示的地图信息；
所述3D激光配准定位模块用于通过当前获得的点云数据与已经构建好的初步态势地图进行配准获得定位结果；
所述GNSS/RTK定位模块用于获取模块的经纬高和朝向信息，根据与初始经纬高的经纬高差获取位置变换量即平移向量tvm，由当前朝向与正东朝向的夹角获奖旋转矩阵Rvm，并且根据初始经纬高截取地图作为前端展示所用的地图；
所述定位解析模块用于将地图坐标系的坐标转换为前端模块中地图图像上的像素坐标值，若地图中单位像素代表的距离为d米，地图图像像素为m×n，地图坐标原点在地图图像上的坐标值为(uo，vo)，则地图坐标系的物体位置(x，y)的像素坐标值为：
优选地：所述探测结果有误包括同一目标在地图上的位置不同、不同目标被算法认定为同一目标以及将目标类别判断错误。
优选地：对探测结果是否有误进行判断包括：
所述指挥端设备接收所述救援端设备获取的匹配图像，并利用目标匹配算法对所述匹配图像以及所述初步态势地图中的目标的相似度进行匹配；以便用户根据匹配结果对探测结果是否有误进行判断；
所述目标匹配算法采用无监督的ReID方法进行训练。
优选地：所述目标匹配算法采用骨干网络为ResNet50网络进行特征提取，训练阶段首先通过骨干网络和全局池化提取所有训练图像的图像特征，通过基于密度的聚类方法DBSCAN，对所有的训练图像进行聚类过滤噪声得到伪标签；
分配伪标签给各个训练图像，计算各个聚类的聚类中心作为集群字典并保存；
组成新的训练数据集，所述新的训练数据集的标签为所述伪标签，所述新的训练数据集通过ResNet50骨干网络提取特征，通过和所述集群字典计算infoNCE损失，由下式表示：
式中：c是各个聚类中心，c+标签此训练样本所属的聚类中心，τ表示温度系数；
将生成的特征*0.5加新特征所属的聚类中心特征*0.5得到新的聚类中心特征，然后进行迭代更新；
用于匹配使用时，输入两张图片，使用ResNet骨干网络和全局池化分别提取两张图片特征，根据特征计算两张图片的余弦相似度。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本申请实施例提供的一种人机协同救援态势地图构建方法，该方法能够将救援人员携带的救援端设备、探测机器人端、救援指挥端有机结合起来。利用探测机器人搭载探测所需的感知设备、激光雷达等，进入救援区域获取多视角、多模态的目标信息，通过感知器感知到目标后，再由探测机器人身上的高效定位算法计算出检测目标所在的位置。该方法解决搜救过程中救援前线存在的有人救援风险大、有人无法进入的地方等问题。通过人机协同构建态势图，弥补单独使用AI算法存在的固有缺陷，增强AI的智能，提高构建出来的态势图的可靠性以及有效性。解决了救援前后方存在的救援信息数据少，救援信息更新不及时和交互繁琐耗时多的问题。
另外，在优选的实施方式下，通过提供模块化选装搭载不同的感知设备，与检测算法、跟踪算法、定位导航算法有机结合，能够做到在多种场景、多类别、多种复杂环境下进行有效探测，提升应用范围。人机交互为AI增智、提升可靠性方法，通过AI算法的反馈，人能够把握介入时机并及时介入探测过程，对AI检测结果进行修改矫正，使得最终得到的结果是正确的、可靠的，能够有效增智。
当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种人机协同救援态势地图构建方法的整体布局结构图；
图2是本发明实施例提供的一种人机协同救援态势地图构建方法的实现步骤图；
图3是本发明实施例提供的目标定位流程图；
图4是本发明实施例提供的激光点投影到图像平面的示意图；
图5是本发明实施例提供的目标匹配算法的模型结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供的一种人机协同救援态势地图构建方法，该方法可以包括：
通过进入救援场景中的探测机器人端搭载的多种感知设备获取所述救援场景的多视角、多模态数据，以使所述探测机器人端根据所述多视角、多模态数据通过目标检测模块进行目标检测获得目标检测结果，并在确定检测到目标后结合所述多视角、多模态数据以及目标定位模型进行目标定位获得目标定位结果，并利用所述目标检测结果以及所述目标定位结果构建初步态势地图；
通过救援端设备以及指挥端设备分别接收所述探测机器人端发送的所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图并展示，以便用户根据所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图对探测结果是否有误进行判断；
通过所述救援端设备和/或所述指挥端设备接收用户输入的修正信息并将所述修正信息发送至所述探测机器人端，以使所述探测机器人端根据所述修正信息对所述初步态势地图进行修正获得目标态势地图；所述修正信息为用户确定所述探测结果有误后根据所述探测结果有误的类型所确定；
其中，多种所述感知设备至少包括相机、GNSS/RTK设备以及激光雷达；所述目标检测模块包括改进的YOLOv5目标检测网络，所述改进的YOLOv5目标检测网络在训练过程中采用分类损失、定位损失、置信度损失加权求和作为整个训练过程的损失函数；所述目标定位模型用于实现有GNSS/RTK信号、有已知地图、无GNSS/RTK信号和已知地图情况下的目标定位。
进一步的，所述损失函数由下式表示：
式中：表示真实的类别标签的独热编码，/>表示预测的类别标签的概率输出，x,y,w,h分别表示预测的检测框的中心坐标以及宽度与高度，tx,ty,tw,th表示实际标注框的中心坐标以及宽度与高度，/>为实际的存在标签，表示目标是否真实存在与检测框中，为预测的存在标签，λcls,λloc,λconf分别为分类权重、定位损失权重、置信度损失的权重，用于控制各个损失的重要性。
可以理解的是，本申请实施例提供的探测机器人端的探测机器人可以采用多种形式的机器人，例如，可以实现四足仿生机器人，也可以是履带车，还可以是适用于水下探测使用的机器人，机器人的种类可以根据使用场景不同进行选择。通过在不同类型的探测机器人上进行方便的程序移植，使得该方法能够应用于多种复杂场景下。
由于机器人端发现目标后，该目标可能是固定不动的，也可能是移动目标，为了可以对移动目标进行跟踪，本申请实施例还可以提供探测机器人端在确定检测到目标后结合所述多视角、多模态数据以及跟踪算法进行实时跟踪，然后将所述多视角、多模态数据送入所述目标定位模型进行位置计算，将坐标映射到世界坐标系上获得所述目标定位结果。
具体实现时，本申请实施例可以提供所述相机包括RealSense D435i RGBD相机；所述改进的YOLOv5目标检测网络用于根据输入的所述RealSense D435i RGBD相机采集到的RGB图像输出目标的位置并进行框选生成检测框；所述跟踪算法用于执行以下操作：
提取所述检测框内的特征，所述特征包括表观特征以及运动特征；
使用卡尔曼滤波公式进行先验状态预测；
利用上一帧的后验估计值，通过状态转移矩阵变换，得到当前帧的先验估计状态，
使用上一帧的后验估计协方差来计算当前帧的先验估计协方差，预测对应边框的轨迹；
将上一帧检测出的目标框与预测的框进行交并比匹配，再通过交并比匹配的结果计算其代价矩阵；
将得到的代价矩阵作为匈牙利算法的输入，得到线性匹配的若干检测结果，将对应的检测结果通过卡尔曼滤波更新对应的轨迹变量；不断重复，实现目标的持续跟踪。
进一步的，所述状态转移矩阵F如下：
式中：(u,v)表示预测框的中心坐标，γ表示宽高比，h表示高度，表示前四个变量的速度信息；
所述卡尔曼滤波预测公式如下式所示：
式中：表示第k步的状态预测值，Fk表示第k步的状态转移矩阵，/>表示第k-1步的状态预测值，Bk表示输入矩阵，uk表示第k步的控制输入，P表示某一步的估计误差，Q表示系统噪声协方差矩阵；
所述交并比公式如下式所示：
式中：x1,y1,x2,y2表示第一个矩形框的左上角和右下角的坐标，x3,y3,x4,y4表示第二个矩形框的左上角和右下角的坐标，xinter1,yinter1,xinter2,yinter2表示两个矩形框的交集区域的左上角和右下角的坐标。
在目标被识别出来后，需要对目标进行定位，为此本申请实施例可以提供所述目标定位模型用于执行以下操作：
进行载体坐标系定位以便确定目标相对于所述多种感知设备的位置坐标，确定载体相对于地图坐标系的坐标，确定求得载体坐标系到地图坐标系的变换矩阵Tvm，变换矩阵Tvm由旋转矩阵Rvm和平移向量tvm组成，此时目标在地图坐标系下的坐标即可以表示为：
pm＝Tvmpv
式中：pv载体坐标系定位坐标，pm表示目标在地图坐标系下的坐标。
载体坐标系定位坐标通过激光雷达点云数据和相机图像数据计算得到，相机图像经由目标检测网络得到目标的2D框，通过下式将激光点投影到图像平面：
式中：x、y、z为某一个激光点的坐标，fx、fy、cx、cy为相机的内参，Tlc为激光雷达到相机坐标系的变换矩阵；
通过基于欧式距离的聚类算法获得多个目标点云簇，计算点云簇的重心作为目标物体的坐标。所述目标定位模型包括激光雷达/IMU里程计定位模块、3D激光配准定位模块、GNSS/RTK定位模块以及定位解析模块；
所述激光雷达/IMU里程计定位模块用于采用LIO-SAM激光雷达/IMU里程计输出的载体到里程计起点的变换矩阵信息，并在运行时构建前端所需展示的地图信息；
所述3D激光配准定位模块用于通过当前获得的点云数据与已经构建好的初步态势地图进行配准获得定位结果；
所述GNSS/RTK定位模块用于获取模块的经纬高和朝向信息，根据与初始经纬高的经纬高差获取位置变换量即平移向量tvm，由当前朝向与正东朝向的夹角获奖旋转矩阵Rvm，并且根据初始经纬高截取地图作为前端展示所用的地图；
所述定位解析模块用于将地图坐标系的坐标转换为前端模块中地图图像上的像素坐标值，若地图中单位像素代表的距离为d米，地图图像像素为m×n，地图坐标原点在地图图像上的坐标值为(uo，vo)，则地图坐标系的物体位置(x，y)的像素坐标值为：
本申请实施例提供的方法中，由机器人端获得的探测结果是否存在判断错误的判断由用户完成，采用人机结合的方式可以进一步提升态势地图的可靠性与正确性。为了方便用户识别判断是否存在错误，本申请实施例还可以提供所述探测结果有误包括同一目标在地图上的位置不同、不同目标被算法认定为同一目标以及将目标类别判断错误。用户在发现上述三种结果时，即可以确定机器人端的探测结果存在错误，需要对探测结果进行校正。此时，用户可以根据经验确定需要校正的参数，通过救援端设备和/或指挥端设备发送给探测机器人端进行调整。
可以理解的是，对于一些在图像中展示比较清晰的错误，用户很容易对其是否有错误进行判断，但是对于通过肉眼无法判断目标是否相同等情况时，则指挥端可以采用匹配算法进行目标相似度匹配。为此，本申请实施例还可以提供对探测结果是否有误进行判断包括：
所述指挥端设备接收所述救援端设备获取的匹配图像，并利用目标匹配算法对所述匹配图像以及所述初步态势地图中的目标的相似度进行匹配；以便用户根据匹配结果对探测结果是否有误进行判断；
所述目标匹配算法采用无监督的ReID方法进行训练。
进一步的，所述目标匹配算法采用骨干网络为ResNet50网络进行特征提取，训练阶段首先通过骨干网络和全局池化提取所有训练图像的图像特征，通过基于密度的聚类方法DBSCAN，对所有的训练图像进行聚类过滤噪声得到伪标签；
分配伪标签给各个训练图像，计算各个聚类的聚类中心作为集群字典并保存；
组成新的训练数据集，所述新的训练数据集的标签为所述伪标签，所述新的训练数据集通过ResNet50骨干网络提取特征，通过和所述集群字典计算infoNCE损失，由下式表示：
式中：c是各个聚类中心，c+标签此训练样本所属的聚类中心，τ表示温度系数；
将生成的特征*0.5加新特征所属的聚类中心特征*0.5得到新的聚类中心特征，然后进行迭代更新；
用于匹配使用时，输入两张图片，使用ResNet骨干网络和全局池化分别提取两张图片特征，根据特征计算两张图片的余弦相似度。
本申请实施例提供的人机协同救援态势地图构建方法，该方法能够将救援人员携带的救援端设备、探测机器人端、救援指挥端有机结合起来。利用探测机器人搭载探测所需的感知设备、激光雷达等，进入救援区域获取多视角、多模态的目标信息，通过感知器感知到目标后，再由探测机器人身上的高效定位算法计算出检测目标所在的位置。该方法解决搜救过程中救援前线存在的有人救援风险大、有人无法进入的地方等问题。通过人机协同构建态势图，弥补单独使用AI算法存在的固有缺陷，增强AI的智能，提高构建出来的态势图的可靠性以及有效性。解决了救援前后方存在的救援信息数据少，救援信息更新不及时和交互繁琐耗时多的问题。
如图1所示，该方法可以通过无线电台设备，将探测机器人获取到的坐标、图像等信息传递至指挥端以及救援人员端。救援人员以及指挥端使用终端设备接收信息，并且在终端设备上实时同步构建出态势图。救援人员以及指挥端可以在终端设备上查看目标的信息，如果发现探测机器人探测的目标有重复的、错误的、漏检的，可以通过进行介入矫正。针对重复目标可以采用高效匹配算法进行去重，针对错误、漏检目标可以通过手动修正。最后得到完整的、可靠的态势图。
下面对本申请实施例提供的人机协同救援态势地图构建方法进行详细说明。
该方法通过提供模块化选装搭载不同的感知设备，与检测算法、跟踪算法、定位导航算法有机结合，能够做到在多种场景、多类别、多种复杂环境下进行有效探测，提升应用范围。
人机交互为AI增智、提升可靠性方法，通过AI算法的反馈，人能够把握介入时机并及时介入探测过程，对AI检测结果进行修改矫正，使得最终得到的结果是正确的、可靠的，能够有效增智。
所述人机协同完成探测任务的方法，在机进行探测检查时，人通过携带一定的感知设备(例如头戴式摄像机)进入探测场景，可以达到与机协同完成探测的任务，可以让机去危险或者人无法到达的区域进行探测，人去较为安全的区域进行探测，同时人也可以通过终端设备实时查看机的检测动向并具有修改的权限，能够实现人机协同进行任务，提高任务执行的效率。
所述人机共建救援态势地图的方法，当人和机一同进行探测任务时，指挥端可以实时显示人与机的动向，通过人和机不断发现到新目标，地图态势不断更新，指挥端以及救援人员端在需要介入时机查看修正，最终能够共建出一副完整的、可靠的救援态势地图。
所述的机器人端、救援人员端、指挥端三端互联，通过无线电台，构建三端互联的局域网，实现多模态的信息实时传递以及态势图的实时更新。
如图2所示，该方法具体通过如下步骤实现：
步骤1：探测机器人首先进入救援场景通过感知设备对目标进行AI算法检测，如果检测到目标，则将该目标的图像信息、受伤程度判断信息进行记录，如果目标是移动目标，则由机器人搭载的跟踪算法进行实时跟踪，然后将图像信息送入机器人搭载的定位算法进行位置计算，将坐标映射到世界坐标系上，最后将位置坐标、目标的多模态信息通过无线电台发送至终端设备进行实时显示。最终探测机器人在经过一轮探测侦察后，构建出一副初步的态势地图。
探测机器人可以搭载不同的感知设备获取目标的多模态信息，并在传统的检测算法上进行了很大的改进，将检测算法与目标跟踪算法有效结合，对于移动目标可以进行实时跟踪检测，减少误检错检的概率。
探测机器人搭载的RealSense D435i RGBD相机的深度分辨率为1280×720，RGB分辨率为1920×1080，深度探测范围0.2m-10m。将RGB图像送入目标检测网络中进行目标检测。该网络的训练使用自行采集的针对不同场景的大量多类样本数据，将带有目标信息的样本图片作为训练的正样本，标注信息使用数据集中目标类别的标注信息。将所有的训练样本分批次输入网络进行训练。训练时采用分类损失、定位损失、置信度损失加权求和作为整个训练过程的损失函数，公式定义如式(1)所示：
/>
式中：表示真实的类别标签的独热编码，/>表示预测的类别标签的概率输出，x,y,w,h分别表示预测的检测框的中心坐标以及宽度与高度，tx,ty,tw,th表示实际标注框的中心坐标以及宽度与高度，/>为实际的存在标签，表示目标是否真实存在与检测框中，为预测的存在标签。λcls,λloc,λconf分别为分类权重、定位损失权重、置信度损失的权重，用于控制各个损失的重要性。
通过训练好的网络，对机器人采集到的RGB图像进行订阅并输入到网络中，可以得到一帧图像中目标的位置并进行框选，此外，为确定被检测目标的准确位置，需要从检测的边框信息中提取一个像素点用来定义并代表该物体，用于作为一副图像中检测目标的位置点。这里将像素点定义为Object(x,y)。设标记框图的输入的数据为：采集到的RGB图像和标配后的深度图像。根据YOLOv5模型检测后所标出的框图位置可以得到该框图的左上点坐标Object(Xs,Ys)，右下点坐标Object(Xe,Ye)，根据这两点由公式(2)可以得到代表某一检测目标的像素位置点。
Object(x,y)＝[(Object(Xe,Ye)-Object(Xs,Ys))/2] (2)
在获得到目标的检测边框后，就进行目标追踪过程，其整体流程如图3所示，将框内的特征进行提取，这个特征包括表观特征和运动特征。然后使用卡尔曼滤波公式进行先验状态预测，其中卡尔曼滤波预测公式如式(3)和式(4)所示。利用上一帧k-1的后验估计值，通过状态转移矩阵F变换，得到当前帧k的先验估计状态，其中状态转移矩阵F如下：
式中：(u,v)表示预测框的中心坐标，γ表示宽高比，h表示高度，表示前四个变量的速度信息；/>表示第k步的状态预测值，Fk表示第k步的状态转移矩阵，/>表示第k-1步的状态预测值，Bk表示输入矩阵，uk表示第k步的控制输入，P表示某一步的估计误差，Q表示系统噪声协方差矩阵。
然后使用上一帧k-1的后验估计协方差来计算当前帧k的先验估计协方差，从而预测对应边框的轨迹。将上一帧检测出的目标框与预测的框进行交并比匹配，交并比公式如式(5)所示，再通过交并比匹配的结果计算其代价矩阵。
式中：x1,y1,x2,y2表示第一个矩形框的左上角和右下角的坐标，x3,y3,x4,y4表示第二个矩形框的左上角和右下角的坐标，xinter1,yinter1,xinter2,yinter2表示两个矩形框的交集区域的左上角和右下角的坐标。
将得到的代价矩阵作为匈牙利算法的输入，得到线性匹配的结果，通过匈牙利算法，得到三种结果：1、轨迹失配，此时直接将失配的轨迹删除；2、检测失配，此时将这样的检测结果初始化为一个新的轨迹；3、检测框与预测框配对成功，将其对应的检测结果通过卡尔曼滤波更新对应的轨迹变量。不断重复，实现目标的持续跟踪。
同时检测完成后，使用检测到的目标信息进行定位坐标估计。
定位流程如图3所示，定位模块能够实现有GNSS/RTK信号、有已知地图、无GNSS/RTK信号和已知地图情况下的定位。定位模块的输入包括激光雷达点云数据和相机图像数据，在没有已知地图时需要GNSS/RTK设备提供定位信息，而在没有已知地图和GNSS/RTK定位信息时则使用激光雷达/IMU里程计定位的方式。
目标的定位分为两步，首先进行载体坐标系定位，即确定目标相对于搭载激光雷达和相机等传感器的载体的位置坐标，该坐标可以用一个三维点pv表示，然后是确定载体相对于地图坐标系的坐标，若求得载体坐标系到地图坐标系的变换矩阵Tvm，该变换矩阵由旋转矩阵Rvm和平移向量tvm组成，此时目标在地图坐标系下的坐标即可以表示为：
pm＝Tvmpv (6)
式中：pv载体坐标系定位坐标，pm表示目标在地图坐标系下的坐标。
载体坐标系定位坐标pv由激光雷达点云数据和相机图像数据计算得到，相机图像经由目标检测网络得到目标的2D框，通过公式2激光点投影到图像平面：
其中x、y、z为某一个激光点的坐标，fx、fy、cx、cy为相机的内参，Tlc为激光雷达到相机坐标系的变换矩阵，如图4所示。
由此可以得到一个四棱锥的点云块，通过基于欧式距离的聚类算法可以后获得多个目标点云簇，计算点云簇的重心作为目标物体的坐标。
从多个目标位置中筛选唯一的目标坐标，首先按目标的距离对目标进行排序，由近及远选取目标中包含的点数量大于τ倍四棱锥中包含点的数量的目标作为唯一的目标，其重心即为pv。
激光雷达/IMU里程计定位、3D激光配准定位模块、GNSS/RTK定位模块分别用于求取没有GNSS/RTK信号和已知地图、有已知地图但没有GNSS/RTK、有GNSS/RTK情况下的载体坐标系到地图坐标系的变换矩阵Tvm。
激光雷达/IMU里程计定位可以采用LIO-SAM等激光雷达/IMU里程计输出的载体到里程计起点的变换矩阵信息，同时激光雷达/IMU里程计还能够在运行时构建前端所需展示的地图信息。
3D激光配准定位模块通过当前获得的点云数据与已经构建好的地图进行配准获得定位结果，本例中采用NDT配准算法实现。
GNSS/RTK定位模块获取模块的经纬高和朝向信息，根据与初始经纬高的经纬高差获取位置变换量即平移向量tvm，由当前朝向与正东朝向的夹角获奖旋转矩阵Rvm，并且根据初始经纬高截取地图作为前端展示所用的地图。
定位解析模块负责将地图坐标系的坐标转换为前端模块中地图图像上的像素坐标值，若地图中单位像素代表的距离为d米，地图图像像素为m×n，地图坐标原点在地图图像上的坐标值为(uo，vo)，则地图坐标系的物体位置(x，y)的像素坐标值为：
最后通过无线电台，采用UDP通讯，将图片、定位等多模态信息打包发向救援人员端与指挥端。
步骤2：在探测机器人构建初步态势图的同时，前线救援人员也可以携带终端设备进入救援场景进行同步检查构建地图。如果在搜救过程中，救援人员发现探测机器人检测到了两个类似的目标，但却出现在地图的不同位置上；或者对于不同的目标将其认定为同一个目标的情况时，可以通过点击地图目标，将信息发送至指挥端进行目标匹配算法，得到匹配结果后由人进行矫正，选择是否去除重复目标或者合并目标。通过人机结合交互修正，进一步提升态势地图的可靠性与正确性。
增加了人与机之间的交互性，从而能够实现人机协同工作，人可以对机展现出的结果进行判断。机的优势是安全、高效，可以以更少的人力代价进行更多的探测工作。但缺点是算法固有的缺陷以及准确度会导致探测结果的不稳定。而人的优势是准确。本方法中探测过程以机探测为主，人探测为辅；确定目标过程由人决定为主，机的探测结果为辅，结合二者优势。实现机帮助人进行探测，人对机的结果进行修正判断。最终人机协同对AI增智得到最准确的结果，取得最终正确的态势地图。
具体实现时，救援人员携带摄像设备、终端设备进入救援场景，可以选择与机器人同时、不同方向进行探测或是由机器人先行探测，救援人员后续探测。救援人员通过终端设备上的显示程序可以实时观察态势图的变化，并且救援人员身上的摄像设备同样通过无线电台与指挥端进行传输。如果指挥端或者救援人员发现机器人的探测结果有误，比如同一目标在地图上的位置不同、不同目标被算法认定为同一目标或是将目标类别判断错误，则可以在终端设备进行修改矫正。如果通过肉眼无法判断目标是否相同，则指挥端可以采用匹配算法进行目标相似度匹配。
本申请实施例提供的匹配算法采用无监督的ReID方法进行训练，通过利用这种方法本申请实施例可以在没有任何标记数据的情况下学习人的重识别任务，解决了数据无标记的问题，此无监督ReID方法是基于集群级别的内存字典进行训练的，本申请实施例为了均衡精度和速度使用的骨干网络为ResNet50网络进行特征提取，训练阶段首先通过骨干网络和全局池化提取所有训练图像的图像特征，通过基于密度的聚类方法DBSCAN，对所有的训练图像进行聚类过滤噪声，然后得到伪标签，并且分配伪标签给各个训练图像，并且计算各个聚类的聚类中心作为集群字典并保存，在每一个阶段的训练，上述都必须被重新执行一次，并且不需要计算梯度。由于上一步已经得到了标签，将噪声图像进行过滤，组成一个新的训练数据集，新数据集的标签就是上述生成的伪标签，新数据集通过ResNet50骨干网络提取特征，通过和上述集群字典计算infoNCE损失，以式(9)表示：
其中，c是各个聚类中心，c+标签此训练样本所属的聚类中心，τ表示温度系数，本申请实施例将其设置为了一个较小的数，让本申请实施例的模型更加有自信力，同时本申请实施例也可以通过动量更新法去更新集群字典，因为初始的集群中心不够准确，无法区别开各个样本，具体做法为：本申请实施例将生成的特征*0.5加新特征所属的聚类中心特征*0.5得到新的聚类中心特征，然后进行迭代更新，模型结构如图5所示。
用于匹配使用时，输入两张图片，使用ResNet骨干网络和全局池化分别提取两张图片特征，根据特征计算两张图片的余弦相似度，根据经验将图像匹配阈值设置为0.65，大于0.65即判定匹配成功，小于0.65判定为这个是失败的。
步骤3：在通过探测机器人与前线救援人员同步构建态势地图后，指挥端可以查看统计地图的信息，根据多模态信息研判救援态势，判定救援优先级，设计最终的救援方案，提升救援效率。
通过无线电台，救援人员端、探测机器端、指挥端能够保持实时的信息传输，救援前线通过救援人员与探测机器人不断的获取目标信息，指挥端也能够同步获取到这些信息，通过在终端设备上查看操作态势图上的目标信息，根据多模态信息能够方便研判救援的优先级并设定救援方案高效开展救援任务，有效避免二次伤亡。
指挥端在后方通过无线电台实时获取救援人员与探测机器人发送来的数据，在终端设备上进行查看，指挥端也可以对地图上的目标进行判断，包括合并目标、修改目标、添加、删除目标等。在探测完成后，指挥端可以根据这个结合了人机协同构建的态势图判断救援的难度以及进行救援方案的设计。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加上必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例或者实施例的某些部分所述的方法。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模型来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (10)

1.一种人机协同救援态势地图构建方法，其特征在于，包括：
通过进入救援场景中的探测机器人端搭载的多种感知设备获取所述救援场景的多视角、多模态数据，以使所述探测机器人端根据所述多视角、多模态数据通过目标检测模块进行目标检测获得目标检测结果，并在确定检测到目标后结合所述多视角、多模态数据以及目标定位模型进行目标定位获得目标定位结果，并利用所述目标检测结果以及所述目标定位结果构建初步态势地图；
通过救援端设备以及指挥端设备分别接收所述探测机器人端发送的所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图并展示，以便用户根据所述多视角、多模态数据、所述目标检测结果、所述目标定位结果以及所述初步态势地图对探测结果是否有误进行判断；
通过所述救援端设备和/或所述指挥端设备接收用户输入的修正信息并将所述修正信息发送至所述探测机器人端，以使所述探测机器人端根据所述修正信息对所述初步态势地图进行修正获得目标态势地图；所述修正信息为用户确定所述探测结果有误后根据所述探测结果有误的类型所确定；
其中，多种所述感知设备至少包括相机、GNSS/RTK设备以及激光雷达；所述目标检测模块包括改进的YOLOv5目标检测网络，所述改进的YOLOv5目标检测网络在训练过程中采用分类损失、定位损失、置信度损失加权求和作为整个训练过程的损失函数；所述目标定位模型用于实现有GNSS/RTK信号、有已知地图、无GNSS/RTK信号和已知地图情况下的目标定位。
2.根据权利要求1所述的人机协同救援态势地图构建方法，其特征在于，所述损失函数由下式表示：
式中：表示真实的类别标签的独热编码，/>表示预测的类别标签的概率输出，x,y,w,h分别表示预测的检测框的中心坐标以及宽度与高度，tx,ty,tw,th表示实际标注框的中心坐标以及宽度与高度，/>为实际的存在标签，表示目标是否真实存在与检测框中，/>为预测的存在标签，λcls,λloc,λconf分别为分类权重、定位损失权重、置信度损失的权重，用于控制各个损失的重要性。
3.根据权利要求1所述的人机协同救援态势地图构建方法，其特征在于，还包括探测机器人端在确定检测到目标后结合所述多视角、多模态数据以及跟踪算法进行实时跟踪，然后将所述多视角、多模态数据送入所述目标定位模型进行位置计算，将坐标映射到世界坐标系上获得所述目标定位结果。
4.根据权利要求3所述的人机协同救援态势地图构建方法，其特征在于，所述相机包括RealSense D435i RGBD相机；所述改进的YOLOv5目标检测网络用于根据输入的所述RealSense D435i RGBD相机采集到的RGB图像输出目标的位置并进行框选生成检测框；所述跟踪算法用于执行以下操作：
提取所述检测框内的特征，所述特征包括表观特征以及运动特征；
使用卡尔曼滤波公式进行先验状态预测；
利用上一帧的后验估计值，通过状态转移矩阵变换，得到当前帧的先验估计状态；
使用上一帧的后验估计协方差来计算当前帧的先验估计协方差，预测对应边框的轨迹；
将上一帧检测出的目标框与预测的框进行交并比匹配，再通过交并比匹配的结果计算其代价矩阵；
将得到的代价矩阵作为匈牙利算法的输入，得到线性匹配的若干检测结果，将对应的检测结果通过卡尔曼滤波更新对应的轨迹变量；不断重复，实现目标的持续跟踪。
5.根据权利要求4所述的人机协同救援态势地图构建方法，其特征在于，
所述状态转移矩阵F如下：
式中：(u,v)表示预测框的中心坐标，γ表示宽高比，h表示高度，表示前四个变量的速度信息；
所述卡尔曼滤波预测公式如下式所示：
式中：表示第k步的状态预测值，Fk表示第k步的状态转移矩阵，/>表示第k-1步的状态预测值，Bk表示输入矩阵，uk表示第k步的控制输入，P表示某一步的估计误差，Q表示系统噪声协方差矩阵；
所述交并比公式如下式所示：
式中：x1,y1,x2,y2表示第一个矩形框的左上角和右下角的坐标，x3,y3,x4,y4表示第二个矩形框的左上角和右下角的坐标，xinter1,yinter1,xinter2,yinter2表示两个矩形框的交集区域的左上角和右下角的坐标。
6.根据权利要求1所述的人机协同救援态势地图构建方法，其特征在于，所述目标定位模型用于执行以下操作：
进行载体坐标系定位以便确定目标相对于所述多种感知设备的位置坐标，确定载体相对于地图坐标系的坐标，确定求得载体坐标系到地图坐标系的变换矩阵Tvm，变换矩阵Tvm由旋转矩阵Rvm和平移向量tvm组成，此时目标在地图坐标系下的坐标即可以表示为：
pm＝Tvmpv
式中：pv载体坐标系定位坐标，pm表示目标在地图坐标系下的坐标；
载体坐标系定位坐标通过激光雷达点云数据和相机图像数据计算得到，相机图像经由目标检测网络得到目标的2D框，通过下式将激光点投影到图像平面：
式中：x、y、z为某一个激光点的坐标，fx、fy、cx、cy为相机的内参，Tlc为激光雷达到相机坐标系的变换矩阵；
通过基于欧式距离的聚类算法获得多个目标点云簇，计算点云簇的重心作为目标物体的坐标。
7.根据权利要求6所述的人机协同救援态势地图构建方法，其特征在于，所述目标定位模型包括激光雷达/IMU里程计定位模块、3D激光配准定位模块、GNSS/RTK定位模块以及定位解析模块；
所述激光雷达/IMU里程计定位模块用于采用LIO-SAM激光雷达/IMU里程计输出的载体到里程计起点的变换矩阵信息，并在运行时构建前端所需展示的地图信息；
所述3D激光配准定位模块用于通过当前获得的点云数据与已经构建好的初步态势地图进行配准获得定位结果；
所述GNSS/RTK定位模块用于获取模块的经纬高和朝向信息，根据与初始经纬高的经纬高差获取位置变换量即平移向量tvm，由当前朝向与正东朝向的夹角获奖旋转矩阵Rvm，并且根据初始经纬高截取地图作为前端展示所用的地图；
所述定位解析模块用于将地图坐标系的坐标转换为前端模块中地图图像上的像素坐标值，若地图中单位像素代表的距离为d米，地图图像像素为m×n，地图坐标原点在地图图像上的坐标值为(uo，vo)，则地图坐标系的物体位置(x，y)的像素坐标值为：
8.根据权利要求1所述的人机协同救援态势地图构建方法，其特征在于，所述探测结果有误包括同一目标在地图上的位置不同、不同目标被算法认定为同一目标以及将目标类别判断错误。
9.根据权利要求8所述的人机协同救援态势地图构建方法，其特征在于，对探测结果是否有误进行判断包括：
所述指挥端设备接收所述救援端设备获取的匹配图像，并利用目标匹配算法对所述匹配图像以及所述初步态势地图中的目标的相似度进行匹配；以便用户根据匹配结果对探测结果是否有误进行判断；
所述目标匹配算法采用无监督的ReID方法进行训练。
10.根据权利要求9所述的人机协同救援态势地图构建方法，其特征在于，所述目标匹配算法采用骨干网络为ResNet50网络进行特征提取，训练阶段首先通过骨干网络和全局池化提取所有训练图像的图像特征，通过基于密度的聚类方法DBSCAN，对所有的训练图像进行聚类过滤噪声得到伪标签；
分配伪标签给各个训练图像，计算各个聚类的聚类中心作为集群字典并保存；
组成新的训练数据集，所述新的训练数据集的标签为所述伪标签，所述新的训练数据集通过ResNet50骨干网络提取特征，通过和所述集群字典计算infoNCE损失，由下式表示：
式中：c是各个聚类中心，c+标签此训练样本所属的聚类中心，τ表示温度系数；
将生成的特征*0.5加新特征所属的聚类中心特征*0.5得到新的聚类中心特征，然后进行迭代更新；
用于匹配使用时，输入两张图片，使用ResNet骨干网络和全局池化分别提取两张图片特征，根据特征计算两张图片的余弦相似度。
CN202311494443.6A 2023-11-10 2023-11-10 一种人机协同救援态势地图构建方法 Pending CN117570959A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311494443.6A CN117570959A (zh) 2023-11-10 2023-11-10 一种人机协同救援态势地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311494443.6A CN117570959A (zh) 2023-11-10 2023-11-10 一种人机协同救援态势地图构建方法

Publications (1)

Publication Number Publication Date
CN117570959A true CN117570959A (zh) 2024-02-20

Family

ID=89885407

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311494443.6A Pending CN117570959A (zh) 2023-11-10 2023-11-10 一种人机协同救援态势地图构建方法

Country Status (1)

Country Link
CN (1) CN117570959A (zh)

Similar Documents

Publication Publication Date Title
CN110956651B (zh) 一种基于视觉和振动触觉融合的地形语义感知方法
CN110827415B (zh) 一种全天候未知环境无人自主工作平台
EP4318397A2 (en) Method of computer vision based localisation and navigation and system for performing the same
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN108801268A (zh) 目标对象的定位方法、装置及机器人
CN112508985B (zh) 一种基于语义分割的slam回环检测改进方法
Mseddi et al. YOLOv5 based visual localization for autonomous vehicles
Le Saux et al. Rapid semantic mapping: Learn environment classifiers on the fly
CN115909096A (zh) 无人机巡航管道隐患分析方法、装置及系统
CN113448340B (zh) 一种无人机的路径规划方法、装置、无人机及存储介质
CN112907625A (zh) 应用于四足仿生机器人的目标跟随方法及系统
Nandkumar et al. Simulation of Indoor Localization and Navigation of Turtlebot 3 using Real Time Object Detection
CN117570959A (zh) 一种人机协同救援态势地图构建方法
CN116109047A (zh) 一种基于三维智能检测的智能调度方法
CN113313824A (zh) 一种三维语义地图构建方法
Liu et al. Vision-IMU multi-sensor fusion semantic topological map based on RatSLAM
Serrano et al. YOLO-Based Terrain Classification for UAV Safe Landing Zone Detection
CN113160191A (zh) 一种基于激光雷达的环境构图完整性判别方法和装置
Le Barz et al. Absolute geo-localization thanks to Hidden Markov Model and exemplar-based metric learning
Grelsson Vision-based localization and attitude estimation methods in natural environments
Huang et al. Multi-object Detection, Tracking and Prediction in Rugged Dynamic Environments
Hroob et al. Learned Long-Term Stability Scan Filtering for Robust Robot Localisation in Continuously Changing Environments
Nouduri et al. Deep learning based landmark matching for aerial geolocalization
Belyaev et al. Increase of the Navigation Landmark Detection Accuracy Based on the Computer Vision Methods in a Lack of Visibility
Putranto et al. Identification of safe landing areas with semantic segmentation and contour detection for delivery uav

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