CN110101996B - 复杂环境下消防机器人协同定位与自主作业方法 - Google Patents

Abstract

本发明涉及一种复杂环境下消防机器人协同定位与自主作业方法，包括灭火和侦察机器人群协同定位步骤以及灭火和侦察机器人群协同侦察和灭火步骤，侦察机器人包括第一侦察机器人和第二侦察机器人，机器人操控台包括主操控台和辅操控台，实现了在高低地面起伏的复杂环境中，灭火机器人与侦察机器人协同对灾害现场火源等危险源的全自动识别、定位和灭火机器人自动瞄准、扑救等功能，从而提高灾害现场处置速度，提高危险源处置效率，降低人员和财产损伤。

Description

复杂环境下消防机器人协同定位与自主作业方法

技术领域

本发明涉及消防机器人领域，具体涉及一种复杂环境下消防机器人协同定位与自主作业方法。

背景技术

火灾是人类社会最严重的灾害之一，给人类带来巨大损失和伤亡。随着科学研究和科学技术的发展，当前自动灭火技术有了一定发展。消防机器人作为特种机器人的一种，在灭火和抢险救援中愈加发挥举足轻重的作用。作为一种消防装备，消防机器人可替代消防人员进入高温、易燃易爆、有毒、缺氧、浓烟等危险灾害事故现场进行灭火、洗消、排烟、照明、侦查以及数据采集、处理、反馈等，可有效解决消防人员在上述场所面临的人身安全、数据信息采集不足等问题。现场指挥人员可以利用其进行先期压制，并根据其反馈结果，及时对灾情做出科学判断，从而对灾害事故现场工作做出正确、合理地决策。

目前携带消防水炮的机器人绝大多数需依赖后台消防人员远程操作，实现对水炮三维角度的调整，从而实现对着火点的灭火功能。而采用定位***的消防机器人精度无法保证而难以实现对火源精准灭火，且机器人自身的定位方法或设备应用场景非常有限。

现场环境恶劣，浓烟密布，消防人员由于自身视野或环境影响，极可能出现无法瞄准的问题；即使消防人员发现并定位着火点，需遥控消防水炮进行打水定位，这中间需要较长的调试和定位耗时，对危及的恶劣环境和抢险现场来说非常不利。故如何实现在现场高危环境下对火源的快速寻找并定位是决定能否降低灾害现场人员伤亡和财产损失的关键。

当前针对灾害现场进行灭火的机器人对火源寻找或定位的方案主要分为两类：

1)无人机高空协助检测法

该方式下，消防机器人工作前需将无人机飞至火焰上空测定高度，后将无人机自身GPS位置信息发送至消防机器人，从而确定无人机下火焰相对机器人空间位置关系，进而喷射灭火。典型的有专利号为201721688135.7的发明专利公布的“基于无人机和智能消防机器人的精确灭火***”。

2)机器人本体双目视觉定位法

通过在消防机器人上安装双目视觉设备，实现对火焰空间位置的检测，从而控制水炮进行定点灭火。典型的有专利号为201520997745.X的专利公布的智能消防机器人。还有采用热成像仪在火源不同位置检测到的夹角，间接计算火源位置，例如申请号为201610089608.5的专利公布的“火灾现场火源定位方法、定位***及消防机器人”等。

目前针对消防机器人现场对火源定位的方式中均存在弊端：

1)对于无人机高空协助检测法，存在如下主要问题：①无人机需飞至火焰上方，实际中这对设备危害性极大；②无人机的定位精度取决于GPS定位精度，传统GPS精度无法满足需求，而差分GPS价格较贵导致***成本过高；③当室内着火时，GPS定位***无法工作，该方法失效。

2)对于机器人本体上安装双目视觉设备的方案，由于消防机器人本体高度较低，当发射水柱时，对前方视线遮挡容易导致双目视觉定位失效。

3)采用移动测量方式，当地面环境复杂、起伏多变时，机器人姿态和高度等发生重大变化，这不仅影响机器人观测目标兴趣点的位置参数的精度，而且对于消防机器人上水炮发射角度影响严重。

发明内容

本发明的目的在于解决恶劣灾害现场下消防机器人自身定位和对火源等危险源的识别以及精准侦察、扑救的难题，提供一种复杂环境下消防机器人协同定位与自主作业方法。

本发明解决其技术问题所采用的技术方案是：复杂环境下消防机器人协同定位与自主作业方法，包括灭火和侦察机器人群协同定位步骤以及灭火和侦察机器人群协同侦察和灭火步骤。

侦察机器人包括第一侦察机器人和第二侦察机器人，机器人操控台包括主操控台和辅操控台，第一侦察机器人布置在左前侧、第二侦察机器人布置在右前侧，灭火机器人布局在中间前侧，主操控台和辅操控台布局在后方位置。

1)灭火和侦察机器人群协同定位步骤如下：

①灭火机器人、侦察机器人、机器人操控台布局完成，机器人操控台中的主操控台内的主通讯模块发送定位指令，灭火机器人中的灭火机器人从通讯模块、侦察机器人中的侦察机器人次通讯模块以及机器人操控台中的辅通讯模块接收到指令后进行解析并控制各自的标签天线、定位基站和辅定位基站进行参数设定并就位。

②机器人操控台中的主操控台发射通讯测量指令信号，定位标签、定位基站和辅定位基站进行回应并计算出时间信息，根据时间耗时和电磁波传输速度，获取灭火机器人相对侦察机器人、机器人操控台的距离参数。

③机器人操控台中的主操控台将获得的距离信息利用空间几何法确定灭火机器人相对主操控台、辅操控台以及侦察机器人的空间位置信息，继而通过灭火机器人姿态传感器和侦察机器人姿态传感器获得灭火机器人和侦察机器人相对原始出发位置经过一段时间后的平面(X，Y)位置信息，为后续灭火机器人实施精准灭火提供空间参数基础；

具体的，所述灭火和侦察机器人群协同定位步骤②中计算的时间信息包括：

ⅰ.信号从主操控台的主定位基站到达灭火机器人中定位标签的时间；

ⅱ.信号从辅操控台的辅定位基站到达灭火机器人中定位标签的时间；

ⅲ.信号从侦察机器人中的定位基站到达灭火机器人中定位标签的时间。

进一步的，所述灭火和侦察机器人群协同定位步骤③中空间几何法确定灭火机器人位置的方法如下：

第一侦察机器人的位置为(x₁,y₁,z₁)，第二侦察机器人的位置为(x₂,y₂,z₂)，主操控台的位置为(x₄,y₄,z₄)，辅操控台的位置为(x₃,y₃,z₃)，灭火机器人的空间位置假设为(x_i,y_i,z_i)；根据姿态传感器中加速度和角速度积分容易获取第一侦察机器人的位置为(x₁,y₁)，第二侦察机器人的位置为(x₂,y₂)；主操控台的位置(x₄,y₄，z₄)，辅操控台的位置(x₃,y₃，z₃)根据实际情况测量或设计；其中空间四点还有如下约束关系：

根据方程组求解z_i(i＝1,2)，根据冗余解可得灭火机器人的三维空间坐标(x_i,y_i,z_i)。

2)灭火和侦察机器人群协同侦察和灭火步骤如下：

机器人操控台根据上述定位算法实时测定出空间中灭火机器人、侦察机器人和机器人操控台间的三维控制坐标位置，然后发出兴趣目标点侦察指令，侦察机器人对兴趣目标点进行三维空间计算，获得兴趣目标点相对侦察机器人的空间三维位置并返回至机器人操控台；机器人操控台推算出兴趣目标点相对灭火机器人的空间位置关系，计算水炮喷射角度，控制灭火机器人定点喷射灭火。

具体的，所述灭火和侦察机器人群协同侦察和灭火步骤具体包括：

①灭火机器人、侦察机器人、机器人操控台***布置完毕后，主操控台首先通过主通讯模块发送定位指令，根据上述定位算法实时测定出空间中灭火机器人、侦察机器人和机器人操控台间的三维控制坐标位置。

②机器人操控台发出兴趣目标点侦察指令，侦察机器人通过侦察机器人次通讯模块接受指令后控制立体摄像机对目标点进行三维空间计算，从而获得兴趣目标点相对侦察机器人的空间三维位置并返回至主操控台。

进一步的，侦察机器人对目标点进行三维空间计算时，第一侦察机器人和第二侦察机器人能同时对兴趣目标点进行定位计算，然后利用冗余的数据进行滤波或修正处理，提高定位精度。

③主操控台根据兴趣目标点相对侦察机器人的空间三维位置信息以及侦察机器人、灭火机器人和机器人操控台间的相对空间位置反推计算，推算出兴趣目标点相对灭火机器人的空间位置关系。

④根据兴趣目标点相对灭火机器人的空间关系，主操控台实时计算水流压力参数，实时计算出使水流喷射至兴趣目标点上的水炮喷射抛物线轨迹，然后将计算的水炮喷射角度参数发送至灭火机器人中。

⑤灭火机器人收到指令后，通过灭火机器人控制***控制消防水炮进行角度参数调整，实现定点喷射灭火。

进一步的，所述灭火机器人的消防灭火步骤如下：主操控台中的主通讯模块通过主通讯天线将包含待喷射兴趣目标点的控制指令发送，灭火机器人的通讯模块通过从通讯天线接受指令后解析、决策，然后到达各指定位置，灭火机器人控制***自动计算自身距离兴趣目标点的空间关系，然后根据灭火机器人姿态传感器获得灭火机器人自身姿态信息，计算出水炮喷射的抛物线，从而调整控制消防水炮进行定点灭火。

进一步的，所述侦察机器人的侦察步骤如下：主操控台中的主通讯模块通过主通讯天线将包含待侦察兴趣目标点的控制指令发送，各侦察机器人的次通讯模块通过次通讯天线接受指令后解析、决策，然后到达指定位置，控制环境参数采集传感器进行定点侦察。

进一步的，所述侦察机器人的定位步骤如下：主操控台中的主通讯模块通过主通讯天线将包含待侦察兴趣目标点的控制指令发送，各侦察机器人中的侦察机器人次通讯模块通过次通讯天线接受指令后解析、决策，然后到达指定位置，侦察机器人控制***控制立体摄像机进行对兴趣目标点三维视觉定位，在此过程中，侦察机器人控制***还实时采集侦察机器人姿态传感器获取其自身姿态信息，从而补偿因立体摄像机不水平放置导致的目标兴趣点获取偏差问题；然后侦察机器人控制***计算XYZ坐标值后通过侦察机器人次通讯模块返回至主通讯模块。

本发明具有以下有益效果：

本发明复杂环境下消防机器人协同定位与自主作业方法实现了在高低地面起伏的复杂环境中，灭火机器人与侦察机器人协同对灾害现场火源等危险源的全自动识别、定位和灭火机器人自动瞄准、扑救等功能，从而提高灾害现场处置速度，提高危险源处置效率，降低人员和财产损伤。

附图说明

图1是本发明方法中各机器人***布局图。

图2是本发明方法中机器人空间几何定位原理图。

图3是本发明方法中灭火机器人的主视结构示意图。

图4是本发明方法中灭火机器人的右视结构示意图。

图5是本发明方法中灭火机器人去盖板和上装后的俯视结构示意图。

图6是本发明方法中侦察机器人的主视结构示意图。

图7是本发明方法中侦察机器人的左视结构示意图。

图8是本发明方法中第一台侦察机器人去盖板和上装后的俯视结构示意图。

图9是本发明方法中第二台侦察机器人去盖板和上装后的俯视结构示意图。

图10是本发明方法中主操控台的立体结构示意图。

图11是本发明方法中辅操控台的立体结构示意图。

具体实施方式

以下是本发明的具体实施例，对本发明的技术方案做进一步描述，但是本发明的保护范围并不限于这些实施例。凡是不背离本发明构思的改变或等同替代均包括在本发明的保护范围之内。

如图1所示，复杂环境下消防机器人协同定位与自主作业***，包括灭火机器人1、侦察机器人2、机器人操控台3，机器人操控台3包括主操控台3-A和辅操控台3-B；灭火机器人1、侦察机器人2、主操控台3-A和辅操控台3-B之间相互无线连接。灭火机器人1数量为一台，侦察机器人2数量为两台，两侦察机器人2之间通过各自的定位基站2-4进行通讯连接。

如图3-5所示，灭火机器人1包括灭火机器人本体1-1、消防水炮1-2、定位标签1-3、灭火机器人控制***1-4、标签天线1-5、灭火机器人从通讯模块1-6、从通讯天线1-7、灭火机器人姿态传感器1-8。

灭火机器人本体1-1为履带式、轮式、轮履复合等常规移动平台中的一种，驱动前进、后退和转弯等运动功能，可带动并控制其上部的消防水炮等进行三维喷射角度调整。

消防水炮1-2、标签天线1-5、从通讯天线1-7设置于灭火机器人本体1-1上部，定位标签1-3、灭火机器人控制***1-4、灭火机器人从通讯模块1-6、灭火机器人姿态传感器1-8设置于灭火机器人本体1-1内部。

消防水炮1-2实现和后方消防管路连接进行灭火，消防水炮1-2还可进行水平、垂向角度调整，可进行喷水、喷雾等喷射形式变换。实现上述功能的消防水炮1-2的详细结构可参见申请号为201811138551.9的一种全地形消防机器人及工作方法中消防水炮***200的结构。

灭火机器人本体1-1、消防水炮1-2、定位标签1-3、标签天线1-5、灭火机器人从通讯模块1-6、从通讯天线1-7、灭火机器人姿态传感器1-8均与灭火机器人控制***1-4连接，灭火机器人控制***1-4主要实现对机器人的通讯控制、驱动控制和决策等功能。

定位标签1-3为通讯模块，定位标签1-3与标签天线1-5连接实现与外部基站进行信号传输和通讯。其中定位标签1-3可通过测定其自身与第一台侦察机器人中的定位基站2-4间的通讯时间计算相距位置，计算出灭火机器人距离第一台侦察机器人的间距；相同的，定位标签1-3可通过测定其自身与第二台侦察机器人中的第二定位基站2-4-2间的通讯时间计算相距位置，计算出灭火机器人距离第二台侦察机器人的间距；且定位标签1-3还可通过测定其自身与主定位基站3-A-2、辅定位基站3-B-2的信号传输时间，测定和计算出灭火机器人距离主操控台3-A和辅操控台3-B的距离。

从通讯天线1-7与灭火机器人从通讯模块1-6连接实现信号接收和发送，灭火机器人从通讯模块1-6通过从通讯天线1-7与侦察机器人2中的侦察机器人次通讯模块2-7和机器人操控台3中的主通讯模块3-A-5进行通讯与信息传递。

灭火机器人姿态传感器1-8设置于灭火机器人本体1-1内部中央水平处，用以实时获取灭火机器人姿态和加速度参数，由此推算获取灭火机器人运动轨迹，且灭火机器人姿态传感器1-8与灭火机器人控制***1-4连接可实现姿态参数实时回传供计算、分析和决策。

如图6-8所示，侦察机器人2包括侦察机器人本体2-1、立体摄像机2-2、环境参数采集传感器2-3、定位基站2-4、侦察机器人控制***2-5、基站天线2-6、侦察机器人次通讯模块2-7、次通讯天线2-8、侦察机器人姿态传感器2-9。

类似的，对于第二台侦察机器人也包含同样的本体、立体摄像机等，还包括第二定位基站2-4-2、侦察机器人第二姿态传感器2-9-2等，如图9所示。

侦察机器人本体2-1为履带式、轮式、轮履复合等常规移动平台中的一种，驱动前进、后退和转弯等运动功能，可带动并控制其上部的侦察***等进行现场环境参数侦察。

立体摄像机2-2、环境参数采集传感器2-3、基站天线2-6、次通讯天线2-8设置于侦察机器人本体2-1上方，定位基站2-4、侦察机器人控制***2-5、侦察机器人次通讯模块2-7、侦察机器人姿态传感器2-9设置于侦察机器人本体2-1内部。

侦察机器人本体2-1、立体摄像机2-2、环境参数采集传感器2-3、定位基站2-4、基站天线2-6、侦察机器人次通讯模块2-7、次通讯天线2-8、侦察机器人姿态传感器2-9均与侦察机器人控制***2-5连接，侦察机器人控制***2-5主要实现对机器人的通讯控制、驱动控制和决策等功能。立体摄像机2-2可实现对侦察机器人本体2-1前方或周围目标兴趣点的空间三维定位。环境参数采集传感器2-3用以对灾害现场参数进行侦察并实时回传。

定位基站2-4为通讯模块，定位基站2-4与基站天线2-6连接，定位基站2-4通过基站天线2-6与灭火机器人中的标签天线1-5、第二台侦察机器人中的第二定位基站2-4-2、机器人操控台中的主定位基站3-A-2和辅定位基站3-B-2通讯。

侦察机器人次通讯模块2-7与次通讯天线2-8连接实现信号接收和发送，侦察机器人次通讯模块2-7通过次通讯天线2-8与灭火机器人中的从通讯模块1-6、机器人操控台中的主通讯模块3-A-5和辅通讯模块3-B-4通讯，实现指令接受和发送、参数传递等功能。

侦察机器人姿态传感器2-9主要用于获取侦察机器人的姿态信息，由此还可推算获取侦察机器人运动轨迹，设置于侦察机器人本体2-1内部中心水平处。侦察机器人姿态传感器2-9还与侦察机器人控制***2-5电气连接。

如图10所示，主操控台3-A包括主控制台本体3-A-1以及设置于主控制台本体3-A-1上的主定位基站3-A-2、显示模块3-A-3、主基站天线3-A-4、主通讯模块3-A-5、主通讯天线3-A-6和主电源3-A-7。

主控制台本体3-A-1为箱式或手持式操控箱，可实现对消防机器人各功能实现的控制，内部设置有控制器、电源和通讯模块等，外部工作面板上设置有摇杆等控制组件。主定位基站3-A-2、主通讯模块3-A-5、主电源3-A-7设置于主控制台本体3-A-1内部，显示模块3-A-3、主基站天线3-A-4、主通讯天线3-A-6设置于主控制台本体3-A-1壳体上。

主定位基站3-A-2为通讯模块，主定位基站3-A-2分别与灭火机器人中的定位标签1-3、侦察机器人中的定位基站2-4、第二定位基站2-4-2、辅操控台3-B中的辅定位基站3-B-2进行信号传输和通讯连接。

显示模块3-A-3与主定位基站3-A-2、主通讯模块3-A-5连接，用以对机器人各本体参数和执行机构(消防水炮、侦察***)等状态或返回的数据显示。

主基站天线3-A-4与主定位基站3-A-2连接，用以实现信号转换和数据传输通讯。

主通讯模块3-A-5与主通讯天线3-A-6连接，主通讯模块3-A-5通过主通讯天线3-A-6分别与灭火机器人1的灭火机器人从通讯模块1-6、侦察机器人2的侦察机器人次通讯模块2-7通讯连接和数据传输。

主电源3-A-7与主操控台3-A的其他部件电连接，为主操控台3-A的其他部件供电。

如图11所示，辅操控台3-B包括辅控制台本体3-B-1以及设置于辅控制台本体3-B-1上的辅定位基站3-B-2、辅基站天线3-B-3、辅通讯模块3-B-4、辅通讯天线3-B-5和辅电源3-B-6。辅定位基站3-B-2、辅通讯模块3-B-4、辅电源3-B-6设置于辅控制台本体3-B-1内部，辅基站天线3-B-3、辅通讯天线3-B-5设置于辅控制台本体3-B-1壳体上。

辅控制台本体3-B-1为箱式或手持式操控箱，可实现对消防机器人各功能实现的控制。

辅定位基站3-B-2与辅基站天线3-B-3连接，用以实现信号转换和数据传输通讯。辅定位基站3-B-2通过辅基站天线3-B-3分别与灭火机器人中的定位标签1-3、侦察机器人中的定位基站2-4、第二定位基站2-4-2以及主操控台3-A中的主定位基站3-A-2通讯连接。

辅通讯模块3-B-4与辅通讯天线3-B-5连接，辅通讯模块3-B-4通过辅通讯天线3-B-5分别与灭火机器人中的灭火机器人从通讯模块1-6、侦察机器人中的侦察机器人次通讯模块2-7通讯连接。

辅电源3-B-6与辅操控台3-B的其他部件电连接，为辅操控台3-B的其他部件供电。

1、上述灭火机器人1的消防灭火工作方法如下：

主通讯模块3-A-5通过主通讯天线3-A-6将包含待喷射兴趣目标点的控制指令发送，各灭火机器人1的通讯模块1-6通过从通讯天线1-7接受指令后解析、决策，然后到达各指定位置，灭火机器人控制***1-4自动计算自身距离火源等兴趣目标点的空间关系，然后根据灭火机器人姿态传感器1-8获得灭火机器人自身姿态信息，计算出水炮喷射的抛物线，从而调整控制消防水炮1-2进行定点灭火。

2、上述任意一台侦察机器人2的侦察和双目定位工作方法如下：

1)侦察工作方法：主通讯模块3-A-5通过主通讯天线3-A-6将包含待侦察兴趣目标点的控制指令发送，各侦察机器人的次通讯模块2-7通过次通讯天线2-8接受指令后解析、决策，然后到达指定位置，控制环境参数采集传感器2-3进行定点侦察。

2)定位工作方法：主通讯模块3-A-5通过主通讯天线3-A-6将包含待侦察兴趣目标点的控制指令发送，各侦察机器人中的侦察机器人次通讯模块2-7通过次通讯天线2-8接受指令后解析、决策，然后到达指定位置，侦察机器人控制***2-5控制立体摄像机2-2进行对目标兴趣点三维视觉定位，在此过程中，侦察机器人控制***2-5还实时采集侦察机器人姿态传感器2-9获取其自身姿态信息，从而补偿因立体摄像机2-2不水平放置导致的目标兴趣点获取偏差问题。而后，侦察机器人控制***2-5计算XYZ坐标值后通过侦察机器人次通讯模块2-7返回至主通讯模块3-A-5。

3、灭火和侦察机器人群进行协同定位方法如下：

机器人布局图可以如图1所示：

1)***布置：第一侦察机器人布置在左前侧、第二侦察机器人布置在右前侧，第一灭火机器人1-1布局在中间前侧，主操控台3-A和辅操控台3-B布局在后方位置。

2)机器人定位过程：

①机器人操控台3中的主操控台3-A内的主通讯模块3-A-5发送定位指令，灭火机器人从通讯模块1-6、各侦察机器人次通讯模块2-7以及辅通讯模块3-B-4接收到指令后进行解析并控制各自的标签天线1-5、各定位基站2-4和辅定位基站3-B-2进行参数设定并就位；

②机器人操控台3中的主操控台3-A发射通讯测量指令信号，各定位标签1-3、各定位基站2-4和辅定位基站3-B-2进行回应并计算出如下时间信息：

ⅰ.信号从主定位基站3-A-2到达灭火机器人中定位标签1-3的时间；

ⅱ.信号从辅定位基站3-B-2到达灭火机器人中定位标签1-3的时间；

ⅲ.信号从各侦察机器人中的定位基站2-4到达灭火机器人中定位标签1-3的时间；

根据时间耗时和电磁波传输速度，从而获取灭火机器人相对各侦察机器人、机器人操控台的距离参数。

③机器人操控台3中的主操控台3-A将获得的距离信息利用空间几何法确定灭火机器人相对主操控台3-A、辅操控台3-B以及各侦察机器人的空间位置信息，继而通过灭火机器人姿态传感器1-8和侦察机器人姿态传感器2-9获得灭火机器人和侦察机器人相对原始出发位置经过一段时间后的平面(X，Y)位置信息，从而为后续灭火机器人实施精准灭火提供空间参数基础。

其中，空间几何法确定灭火机器人位置的方法如下：

结合图2，第一侦察机器人的位置为(x1,y1,z1)，第二台侦察机器人的位置为(x2,y2,z2)，主操控台的位置为(x4,y4,z4)，辅操控台的位置为(x3,y3,z3)，灭火机器人的空间位置假设为(xi,yi,zi)，根据姿态传感器中加速度和角速度积分容易获取第一侦察机器人的位置为(x1,y1)，第二台侦察机器人的位置为(x2,y2)；主操控台的位置为(x4,y4，z4)，辅操控台的位置为(x3,y3，z3)根据实际情况容易测量或设计。其中空间四点还有如下约束关系：

故容易根据方程组求解zi(i＝1,2)，根据冗余解可得灭火机器人的三维空间坐标(xi,yi,zi)。

4、灭火和侦察机器人群进行协同侦察和灭火工作方法如下：

①***布置完毕后，主操控台3-A首先通过主通讯模块3-A-5发送定位指令，根据上述定位算法可实时测定出空间中灭火机器人、侦察机器人和操控台间的三维控制坐标位置。

②机器人操控台3发出火源等兴趣目标点侦察指令，侦察机器人通过侦察机器人次通讯模块2-7接受指令后控制立体摄像机2-2对目标点进行三维空间计算，从而获得火源等兴趣点相对侦察机器人的空间三维位置并返回至主操控台3-A。

进一步的，此时第一侦察机器人和第二侦察机器人可同时对火源等兴趣目标点进行定位计算，然后利用冗余的数据进行滤波或修正处理，提高定位精度。

进一步的，在定位过程中，侦察机器人可实时通过侦察机器人姿态传感器2-9获得自身姿态参数，从而对因自身双目摄像机角度偏离而导致的识别目标点的偏差进行修正。

③主操控台3-A根据火源等兴趣点相对侦察机器人的空间三维位置信息，侦察机器人、灭火机器人和机器人操控台间的相对空间位置反推计算，推算出火源等兴趣点相对灭火机器人的空间位置关系。

④根据火源等兴趣点相对灭火机器人的空间关系，主操控台3-A实时计算水流压力等参数实时计算出水炮喷射的抛物线轨迹，使水流喷射至火源等兴趣点上；然后将计算的水炮喷射角度等参数发送至灭火机器人中。

在此过程中，侦察机器人还可通过侦察机器人姿态传感器2-9实时获取自身的姿态参数，对计算的水炮喷射抛物线进行修正，防止因机身倾斜导致的水炮喷射角度偏移而无法精准定点喷射的问题。

⑤灭火机器人收到指令后，通过灭火机器人控制***1-4控制消防水炮1-2进行角度等参数调整，实现定点喷射灭火。

更进一步的，在上述***协同侦察和灭火时，灭火机器人和侦察机器人的相关任务执行可在运动状态下进行，上述过程变为通讯、控制、决策等的实时工作。

本发明中未详细公开的结构，如灭火机器人1、侦察机器人2的必要的其他结构，均为现有技术，可参照申请人在本发明的申请日之前提出的专利申请中的机器人结构。

本发明不局限于上述实施方式，任何人应得知在本发明的启示下作出的结构变化，凡是与本发明具有相同或相近的技术方案，均落入本发明的保护范围之内。

本发明未详细描述的技术、形状、构造部分均为公知技术。

Claims (9)

1.复杂环境下消防机器人协同定位与自主作业方法，其特征在于，包括灭火和侦察机器人群协同定位步骤以及灭火和侦察机器人群协同侦察和灭火步骤；

1)灭火和侦察机器人群协同定位步骤如下：

①灭火机器人、侦察机器人、机器人操控台布局完成，机器人操控台中的主操控台内的主通讯模块发送定位指令，灭火机器人中的灭火机器人从通讯模块、侦察机器人中的侦察机器人次通讯模块以及机器人操控台中辅操控台内的辅通讯模块接收到指令后进行解析，并分别控制灭火机器人中的标签天线、侦察机器人中的定位基站和机器人操控台中辅操控台内的辅定位基站进行参数设定并就位；

②机器人操控台中的主操控台发射通讯测量指令信号，灭火机器人中的定位标签、侦察机器人中的定位基站和机器人操控台中的辅定位基站进行回应并计算出时间信息，根据时间耗时和电磁波传输速度，获取灭火机器人相对侦察机器人、机器人操控台的距离参数；

③机器人操控台中的主操控台将获得的距离信息利用空间几何法确定灭火机器人相对主操控台、辅操控台以及侦察机器人的空间位置信息，继而通过灭火机器人中的灭火机器人姿态传感器和侦察机器人中的侦察机器人姿态传感器获得灭火机器人和侦察机器人相对原始出发位置经过一段时间后的平面(X，Y)位置信息，为后续灭火机器人实施精准灭火提供空间参数基础；

2)灭火和侦察机器人群协同侦察和灭火步骤如下：机器人操控台根据灭火和侦察机器人群协同定位步骤实时测定出空间中灭火机器人、侦察机器人和机器人操控台间的三维控制坐标位置，然后发出兴趣目标点侦察指令，侦察机器人对兴趣目标点进行三维空间计算，获得兴趣目标点相对侦察机器人的空间三维位置并返回至机器人操控台；机器人操控台推算出兴趣目标点相对灭火机器人的空间位置关系，计算水炮喷射角度，控制灭火机器人定点喷射灭火。

2.如权利要求1所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述侦察机器人包括第一侦察机器人和第二侦察机器人，机器人操控台包括主操控台和辅操控台，第一侦察机器人布置在左前侧、第二侦察机器人布置在右前侧，灭火机器人布局在中间前侧，主操控台和辅操控台布局在后方位置。

3.如权利要求1所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述灭火和侦察机器人群协同定位步骤②中计算的时间信息包括：

ⅰ.信号从主操控台的主定位基站到达灭火机器人中定位标签的时间；

ⅱ.信号从辅操控台的辅定位基站到达灭火机器人中定位标签的时间；

ⅲ.信号从侦察机器人中的定位基站到达灭火机器人中定位标签的时间。

4.如权利要求2所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述灭火和侦察机器人群协同定位步骤③中空间几何法确定灭火机器人位置的方法如下：

第一侦察机器人的位置为(x₁,y₁,z₁)，第二侦察机器人的位置为(x₂,y₂,z₂)，主操控台的位置为(x₄,y₄,z₄)，辅操控台的位置为(x₃,y₃,z₃)，灭火机器人的空间位置假设为(x_i,y_i,z_i)；根据姿态传感器中加速度和角速度积分容易获取第一侦察机器人的位置为(x₁,y₁)，第二侦察机器人的位置为(x₂,y₂)；主操控台的位置(x₄,y₄，z₄)，辅操控台的位置(x₃,y₃，z₃)根据实际情况测量或设计；其中空间四点还有如下约束关系：

根据方程组求解z_i(i＝1,2)，根据冗余解可得灭火机器人的三维空间坐标(x_i,y_i,z_i)。

5.如权利要求4所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述灭火和侦察机器人群协同侦察和灭火步骤包括：

①灭火机器人、侦察机器人、机器人操控台***布置完毕后，主操控台首先通过主通讯模块发送定位指令，根据灭火和侦察机器人群协同定位步骤实时测定出空间中灭火机器人、侦察机器人和机器人操控台间的三维控制坐标位置；

②机器人操控台发出兴趣目标点侦察指令，侦察机器人通过其内部的侦察机器人次通讯模块接受指令后控制侦察机器人上的立体摄像机对目标点进行三维空间计算，从而获得兴趣目标点相对侦察机器人的空间三维位置并返回至主操控台；

③主操控台根据兴趣目标点相对侦察机器人的空间三维位置信息以及侦察机器人、灭火机器人和机器人操控台间的相对空间位置反推计算，推算出兴趣目标点相对灭火机器人的空间位置关系；

④根据兴趣目标点相对灭火机器人的空间关系，主操控台实时计算水流压力参数，实时计算出使水流喷射至兴趣目标点上的水炮喷射抛物线轨迹，然后将计算的水炮喷射角度参数发送至灭火机器人中；

⑤灭火机器人收到指令后，通过其内部的灭火机器人控制***控制灭火机器人上的消防水炮进行角度参数调整，实现定点喷射灭火。

6.如权利要求5所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述灭火和侦察机器人群协同侦察和灭火步骤②中，侦察机器人对目标点进行三维空间计算时，第一侦察机器人和第二侦察机器人能同时对兴趣目标点进行定位计算，然后利用冗余的数据进行滤波或修正处理，提高定位精度。

7.如权利要求1-6任一所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述灭火机器人的消防灭火步骤如下：

主操控台中的主通讯模块通过主操控台上的主通讯天线将包含待喷射兴趣目标点的控制指令发送，灭火机器人的通讯模块通过灭火机器人上的从通讯天线接受指令后解析、决策，然后到达各指定位置，灭火机器人内的灭火机器人控制***自动计算自身距离兴趣目标点的空间关系，然后根据灭火机器人姿态传感器获得灭火机器人自身姿态信息，计算出水炮喷射的抛物线，从而调整控制灭火机器人上的消防水炮进行定点灭火。

8.如权利要求1-6任一所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述侦察机器人的侦察步骤如下：主操控台中的主通讯模块通过主操控台上的主通讯天线将包含待侦察兴趣目标点的控制指令发送，各侦察机器人的侦察机器人次通讯模块通过次通讯天线接受指令后解析、决策，然后到达指定位置，控制侦察机器人的环境参数采集传感器进行定点侦察。

9.如权利要求1-6任一所述的复杂环境下消防机器人协同定位与自主作业方法，其特征在于，所述侦察机器人的定位步骤如下：主操控台中的主通讯模块通过主操控台上的主通讯天线将包含待侦察兴趣目标点的控制指令发送，各侦察机器人中的侦察机器人次通讯模块通过次通讯天线接受指令后解析、决策，然后到达指定位置，侦察机器人中的侦察机器人控制***控制侦察机器人上的立体摄像机对兴趣目标点进行三维视觉定位，在此过程中，侦察机器人控制***还实时采集侦察机器人姿态传感器获取其自身姿态信息，从而补偿因立体摄像机不水平放置导致的目标兴趣点获取偏差问题；然后侦察机器人控制***计算XYZ坐标值后通过侦察机器人次通讯模块返回至主通讯模块。

Images