CN110673603B - 一种火场自主导航侦察机器人 - Google Patents

Abstract

本发明公开了一种火场自主导航侦察机器人，解决目前侦察机器人在火场中定位可靠性较低、缺乏实时地图构建功能、机器人遥控器笨重，不便于携带的问题。本发明包括机器人本体、控制终端和设置在本体上的主控制器、底层控制器、环境地图构建与导航单元、执行单元、环境信息探测单元、语音单元和视频采集单元。所述主控制器通过无线局域网与控制终端连接，所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，消防员随身佩戴控制端又包括头戴式AR显示器和手握式遥控器。本发明机器人可帮助消防员快速发现火源和被困人员，确定其在地图中的位置，并引导消防员到达该区域，实施救援。该机器人不但增强了消防员在火场环境中的感知能力，而且保障了消防员的生命安全、提高了救援效率。

Description

一种火场自主导航侦察机器人

技术领域

本发明涉及消防机器人领域，具体涉及一种火场自主导航侦察机器人。

背景技术

近年来随着我国城市化进程的加快，大量的超级建筑涌现而来，这些建筑在给人们带来美好生活的同时，也使消防压力猛然增大。该类建筑结构复杂、功能繁多、面积广阔，在其中进行火场救援难度较大，工作效率低下，而且对消防员的人身安全具有较大威胁。针对超级建筑的灭火救援问题，研制消防侦察机器人，辅助消防员进行火场侦察，是提高消防员灭火救援效率的有效途径。

目前的侦察机器人建筑物中定位可靠性较低：现有的室内定位技术(超宽带（UWB）、无线射频识别（RFID）、无线局域网（WALAN）、蓝牙（BT）、ZigBee等)过于依赖基站等设施，而火场中需要对非消防用电设施进行断电处理，无法保证基站的有效供电，且火场中由于火焰燃烧等高温因素的存在，使得基站的可靠性也无法得到有效保证，因此目前大多数侦察机器人都是室外环境的，而建筑物内的侦察机器人较少；

机器人缺乏实时地图构建功能：火场内由于高温燃烧现象，导致建筑结构出现较大变化，而现有的火场机器人虽然部分加载有建筑地图，但地图过于陈旧，无法实现机器人的准确定位与导航；

机器人控制器笨重，不便于携带：当前火场机器人控制器仍以大型工控机为主，在操作时需放置在固定位置，缺乏灵活性；沉重的设备不仅增加了消防员的负担，且占用了消防员的双手，导致消防员无法进行其他救援操作。

发明内容

本发明解决的技术问题是目前的侦察机器人在建筑物中定位可靠性较低、缺乏实时地图构建功能、机器人遥控器笨重，不便于携带的问题，提供一种实现对火灾现场进行侦察、构建火场环境地图、探测火源位置，搜救被困人的火场自主导航侦察机器人。

为解决上述技术问题，本发明采用下述技术方案：一种火场自主导航侦察机器人，包括机器人本体、控制终端和设置在机器人本体上的机器人主控制器、机器人底层控制器、环境地图构建与导航单元、执行单元、环境信息探测单元、语音单元和视频采集单元，所述的机器人底层控制器与机器人主控制器连接，与机器人底层控制器相连设有环境地图构建与导航单元、执行单元、环境信息探测单元；与机器人主控制器相连设有语音单元和视频采集单元，所述的机器人主控制器通过无线局域网与控制终端连接，所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，消防员随身佩戴控制端又包括头戴式AR显示器和手握式遥控器。

所述的环境地图构建与导航单元包括激光雷达、IMU模块、里程计，激光雷达与机器人主控制器连接，IMU模块和里程计与机器人底层控制器连接。

所述的环境地图构建与导航单元使用SLAM算法结合激光雷达完成火场环境同步定位与地图构建，其具体包括以下步骤：①惯性测量单元的IMU模块和里程计对机器人本身进行航向角和里程测量获得机器人自身姿态信息；②激光雷达对周围环境中的障碍物进行环境信息探测和测距获得周围障碍物表面二维点云信息；③将步骤①和②获得的机器人自身姿态信息和周围障碍物表面二维点云信息，逐帧进行匹配；将当前帧数据根据初始位置投影到参考帧坐标系；④对应当前帧中的点，在参考帧中找到两个与之相近的点，并分别计算与当前帧之间的误差；⑤设定一个阈值剔除误差较大的帧；⑥获取控制终端输入的行动指令；⑦行动后，根据运动模型通过粒子滤波器对机器人当前位置进行估计，判断机器人自身处于地图中的位置；⑧判断是否观测到特征点，若没发现特征点，则返回上一步，若发现特征点，则进入下一步；⑨基于卡尔曼滤波器进行对环境特征进行估计；⑩将估计的环境特征与激光雷达扫描的当前环境特征进行比较，若差异性较大，则进行重采样，返回第一步，若差异性较小，则进行地图更新。

所述的环境地图构建与导航单元基于构建的火场环境平面地图，机器人可实现火场中的自主导航，具体步骤如下：机器人主控制器使用A*（A-Star）算法计算出到达导航点的最优全局路径，并且在导航的过程中使用D*（D-Star）算法动态计算局部最优路径，其具体步骤为：首先给定目标位置，根据环境地图构建与定位单元构建好的地图，启动全局路径规划A*（A-Star）算法，利用启发函数首先在地图上确定目标位置的一个大致方向，沿该方向在栅格地图上进行路径节点全局遍历；通过计算筛选出相邻栅格间最短距离，得到全局路径；机器人根据全局路径行进的过程中，为了避免现场环境与载入地图之间存在差异性，设定一个区域范围，在该范围中进行实时地图构建，同时与载入地图进行比较，得到一个局部代价地图，在局部代价地图中利用D*（D-Star）算法规划该区域内的局部路径，进而动态校准全局路径，并进行避障。

所述的环境信息探测单元包括温度传感器、烟气浓度传感器、可燃气体传感器；温度传感器、烟气浓度传感器、可燃气体传感器均设置在机器人本体上方。

所述的执行单元包括360度云台、驱动电机、风机、位置指示灯，驱动电机与行走机构连接，所述的行走机构包括若干个行走轮和履带，履带包裹在行走轮上，行走轮与驱动电机的输出轴连接；360度云台设置在机器人本体上方，风机设置在机器人本体内，位置指示灯设置在360度云台前方。

所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，所述消防员随身佩戴控制端包括头戴式AR显示器和手握式遥控器。

所述的摄像单元包括普通摄像机和热成像摄像机，普通摄像机和热成像摄像机均设置在360度云台上。

所述的语音单元包括麦克风和扬声器，麦克风和扬声器均设置在机器人本体上。

所述机器人主控制器为工控机，机器人底层控制器为STM32单片机。

本发明使用激光雷达和SLAM算法完成机器人火场中的自主导航和地图构建，并且机器人装备有热成像摄像机和烟气浓度传感器、温度传感器、可燃气体传感器等多种传感器进行火场侦察，并将火场画面和传感器信息实时传递到消防救援指挥中心PC机控制端和消防员随身佩戴控制端，不但消防救援指挥中心能实时查看到消防侦察机器人的侦察信息并对其进行控制，而且消防员也应能实现相同操作，达到对火场环境进行全方位侦察进而制定救援方案的目的。该机器人可帮助消防员快速发现火源和被困人员，确定其在地图中的位置，并引导消防员到达该区域，实施救援。该机器人不但增强了消防员在火场环境中的感知能力，而且保障了消防员的生命安全、提高了救援效率。

附图说明

图1是本发明控制系统图；

图2是本发明立体结构示意图；

图3是本发明环境地图构建与定位单元完成火场环境同步定位与地图构建的具体流程图；

图4是本发明消防员与机器人通信结构框图；

图5是本发明路径规划与导航结构框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1至图5所示，本发明包括机器人本体、控制终端和设置在机器人本体上的机器人主控制器、机器人底层控制器、环境地图构建与导航单元、执行单元、环境信息探测单元、语音单元和视频采集单元，所述的机器人底层控制器与机器人主控制器连接，与机器人底层控制器相连设有环境地图构建与导航单元、执行单元、环境信息探测单元；与机器人主控制器相连设有语音单元和视频采集单元，所述的机器人主控制器通过无线局域网与控制终端连接，所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，消防员随身佩戴控制端又包括头戴式AR显示器和手握式遥控器。

所述的环境地图构建与导航单元包括激光雷达、IMU模块、里程计，激光雷达与机器人主控制器连接，IMU模块和里程计与机器人底层控制器连接。

所述的环境地图构建与导航单元使用SLAM算法结合激光雷达完成火场环境同步定位与地图构建，其具体包括以下步骤：①惯性测量单元的IMU模块和里程计对机器人本身进行航向角和里程测量获得机器人自身姿态信息；②激光雷达对周围环境中的障碍物进行环境信息探测和测距获得周围障碍物表面二维点云信息；③将步骤①和②获得的机器人自身姿态信息和周围障碍物表面二维点云信息，逐帧进行匹配；将当前帧数据根据初始位置投影到参考帧坐标系；④对应当前帧中的点，在参考帧中找到两个与之相近的点，并分别计算与当前帧之间的误差；⑤设定一个阈值剔除误差较大的帧；⑥获取控制终端输入的行动指令；⑦行动后，根据运动模型通过粒子滤波器对机器人当前位置进行估计，判断机器人自身处于地图中的位置；⑧判断是否观测到特征点，若没发现特征点，则返回上一步，若发现特征点，则进入下一步；⑨基于卡尔曼滤波器进行对环境特征进行估计；⑩将估计的环境特征与激光雷达扫描的当前环境特征进行比较，若差异性较大，则进行重采样，返回第一步，若差异性较小，则进行地图更新。针对背景技术中火场环境需要搭建基站才能进行定位导航，且容易受火灾影响可靠性较低的问题：本发明使用SLAM(Simultaneous Localizationand Mapping)算法结合激光雷达，通过环境地图构建与导航单元完成火场环境同步定位与地图构建，其具体过程如图3所示。

所述的环境地图构建与导航单元基于构建的火场环境平面地图，机器人可实现火场中的自主导航，具体步骤如下：机器人主控制器使用A*（A-Star）算法计算出到达导航点的最优全局路径，并且在导航的过程中使用D*（D-Star）算法动态计算局部最优路径，其具体步骤为：首先给定目标位置，根据环境地图构建与定位单元构建好的地图，启动全局路径规划A*（A-Star）算法，利用启发函数首先在地图上确定目标位置的一个大致方向，沿该方向在栅格地图上进行路径节点全局遍历；通过计算筛选出相邻栅格间最短距离，得到全局路径；机器人根据全局路径行进的过程中，为了避免现场环境与载入地图之间存在差异性，设定一个区域范围，在该范围中进行实时地图构建，同时与载入地图进行比较，得到一个局部代价地图，在局部代价地图中利用D*（D-Star）算法规划该区域内的局部路径，进而动态校准全局路径，并进行避障。

根据导航点坐标，利用A*(A-Star)全局路径规划算法，沿导航点方向遍历代价地图中的有效栅格点，通过计算筛选对相邻栅格间的最短距离，进而计算出最优全局路径。随后，机器人沿全局路径向导航点行进，在行进过程中，IMU模块、里程计、激光雷达实时采集周围环境信息，若发现与载入地图出入较大的区域或出现未在载入地图中出现的障碍物时，启用D*(D-Star)局部路径规划算法，对全局路径进行及时修正。最后，顺利抵达导航点，完成导航。如图5所示是机器人路径规划的结构框图。

机器人主控制器使用A*(A-Star)算法计算出到达导航点的最优路径，并且在导航的过程中使用D*(D-Star)算法计算局部最优路径，可以及时避开障碍物，实现自主导航，不需要对机器人进行实时控制，只需确定机器人的目的地，它就可以在无人控制的情况下迅速到达目的地。到达导航点的最优路径的确定：电脑端使用ROS机器人操作系统中的rvzi工具实现，消防员随身控制端是通过其手握式遥控器实现的。

所述的环境信息探测单元包括温度传感器541、烟气浓度传感器542、可燃气体传感器543和；温度传感器541、烟气浓度传感器542、可燃气体传感器543均设置在机器人本体上方。机器人本体上安装有温度传感器541、烟气浓度传感器542、可燃气体传感器543等，使得本发明同时具备温度、烟气浓度、可燃气体等火场关键信息探测功能，侦察到的火场环境信息更加全面。

实现机器人的高温自我保护功能：当温度传感器541探测到环境温度过高时，机器人主控制器会及时调整并从新规划行走路径，具体方法是：通过机器人主控制器使用D*(D-Star)算法进行的路径从新规划，与避开障碍物的原理一样，比如某块地方温度过高不适合行走，就把该块地方当做存在障碍物避开，机器人主控制器计算后，将数据传给底层控制器，底层控制器控制机器人运动；机器人避开温度过高区域后，从新进行路径规划，寻找不威胁自身安全的到达救援点的最优路径，以实现自我保护；当烟气浓度过高影响激光雷达的探测精度时，将会自动开启风机吹散烟气，以免烟气浓度过高影响探测。

所述的执行单元包括360度云台531、驱动电机532、风机533、位置指示灯534，驱动电机532与行走机构连接，所述的行走机构包括若干个行走轮21和履带22，履带22包裹在行走轮21上，行走轮21与驱动电机532的输出轴连接；360度云台531设置在机器人本体1上方，风机533设置在机器人本体1内，位置指示灯534设置在360度云台531前方。履带22设置在机器人本体两侧，一个履带22里包裹两个行走轮；履带22的设置使得本发明具有一定的爬坡和跨越障碍物的能力，机器人采用履带式底盘能够越过一定的障碍物，更好的完成火场侦察。

所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，所述消防员随身佩戴控制端包括头戴式AR显示器和手握式遥控器。本发明的机器人同时拥有两种控制方式，一种是在消防救援指挥中心PC机控制端控制、一种是消防员随身佩戴控制端控制，二者可同时与机器人主控制器进行通信，既可以同时接收到机器人主控制器的视频信息、传感器信息等数据，并均可实现对机器人的控制。消防员随身佩戴控制端主要包括手握式遥控器和头戴式AR显示器两部分，控制器可实现对机器人的控制，头戴式AR显示器用于显示图像和传感器数据等信息，头戴式AR显示器在释放双手的基础上又能实现视场与人眼的一致性，AR技术使画面透明无边框，这种显示方式可以使消防员在观看到机器人侦察信息的同时，又不影响其对真实场景的观测，并且对消防员的正常活动造成的干扰较小，使其在火场中以最自然、最便捷、最快速的方式与机器人共同完成消防救援。

本发明机器人具有信号中断自我返航功能：当机器人主控制器与控制终端出现通信故障时，机器人会立即停止，并与控制终端尝试再次通信连接，当连续三次连接通信失败后会，停止一分钟，一分钟后再次连续三次进行通信连接，如此循环五次，五次都失败后，机器人会规划返回出发点的最优路径，在自主返回到出发点的途中会继续尝试与控制终端的通信连接，若通信连接成功则根据控制终端的指令进行相应的动作，若连接失败则继续返回到出发点。

本发明机器人具有对救援点进行标定功能：标定功能电脑端使用ROS（机器人操作系统）中的rvzi工具实现的，消防员随身控制端是通过手握式遥控器上的相应按键实现的。当机器人发现被困人员或火源时会在遥控器界面发出警报，此时消防员若认为警报属实则人为的在遥控器控制界面的地图上将该点标定为救援点。当确定火场中某点位救援点后，可在构建好的地图上对该地点进行标定，以帮助消防员快速到达救援点。

具有规划到救援点的路径规划功能：当确定某点为救援点后，机器人将在构建好的地图上规划出从出发点到救援点的最优路径，进一步提高救援效率；

具有在火场中导航与定位功能：导航首先确定导航点，导航点确定后使用A*(A-Star)算法规划到导航点的全局最优路径，路径规划成功后机器人按照规划路径行走，在行走过程中使用D*(D-Star)算法进行动态的局部路径规划，若环境改变则对全局最优路径进行局部路径更改。消防员根据机器人发送的语音提醒，通过观察机器人上的位置指示灯跟随机器人到达导航点。消防员根据机器人的位置，确定自己的位置所在。当开启导航功能时，机器人上的位置指示灯将快速闪动，并且在构建好的地图上可以看出导航到某点的规划路径，并且向消防员随身携带的控制端发送语音导航提醒；

所述的摄像单元包括普通摄像机571和热成像摄像机572，普通摄像机571和热成像摄像机572均设置在360度云台561上。本发明安装普通摄像机和热成像摄像机，两种摄像机之间互为补充，并配备360度云台，更加准确和全面的对火场环境进行侦察，获得更加充足的信息；本发明的普通摄像机是相对热成像摄像机而言的，热成像摄像机为特种摄像机，而普通相机可以选择红外枪型网络摄像机，具体型号为DH-IPC-HFW4426B-AS。

具有人员搜救功能：机器人能够完成火场环境中的人员搜救，人员搜救主要通过热成像摄像机采集的视频图像，并通过图像处理算法进行实现；图像处理算法采用特征提取结合机器学习的方法：提取正负样本的特征，正负样本指的是包括被困人员的图像和不包括被困人员的图像，然后训练分类器，然后用滑动窗口法遍历一整副完整的图像，再利用训练好的分类器对窗口进行被困人员和非被困人员的分类检测，达到检测被困人员的目的。

所述的语音单元包括麦克风551和扬声器552，麦克风551和扬声器552均设置在机器人本体1上。所述机器人主控制器为工控机，机器人底层控制器为STM32单片机。

本发明扬声器和麦克风结合热成像摄像机和普通摄像机可实现与被困人员的视频通话，既能安慰被困人员，消防员又能根据被困人员的需求携带救援物品。

Claims (7)

1.一种火场自主导航侦察机器人，其特征在于：包括机器人本体（1）、控制终端和设置在机器人本体（1）上的机器人主控制器、机器人底层控制器、环境地图构建与导航单元、执行单元、环境信息探测单元、语音单元和视频采集单元，所述的机器人底层控制器与机器人主控制器连接，与机器人底层控制器相连设有环境地图构建与导航单元、执行单元、环境信息探测单元；与机器人主控制器相连设有语音单元和视频采集单元，所述的机器人主控制器通过无线局域网与控制终端连接，所述控制终端包括消防救援指挥中心PC机控制端和消防员随身佩戴控制端，消防员随身佩戴控制端又包括头戴式AR显示器和手握式遥控器；

所述的环境地图构建与导航单元包括激光雷达（523）、IMU模块、里程计，激光雷达（523）与机器人主控制器连接，IMU模块和里程计与机器人底层控制器连接；

所述的环境地图构建与导航单元使用SLAM算法结合激光雷达完成火场环境同步定位与地图构建，其具体包括以下步骤：①惯性测量单元的IMU模块和里程计对机器人本身进行航向角和里程测量获得机器人自身姿态信息；②激光雷达对周围环境中的障碍物进行环境信息探测和测距获得周围障碍物表面二维点云信息；③将步骤①和②获得的机器人自身姿态信息和周围障碍物表面二维点云信息，逐帧进行匹配；将当前帧数据根据初始位置投影到参考帧坐标系；④对应当前帧中的点，在参考帧中找到两个与之相近的点，并分别计算与当前帧之间的误差；⑤设定一个阈值剔除误差较大的帧；⑥获取控制终端输入的行动指令；⑦行动后，根据运动模型通过粒子滤波器对机器人当前位置进行估计，判断机器人自身处于地图中的位置；⑧判断是否观测到特征点，若没发现特征点，则返回上一步，若发现特征点，则进入下一步；⑨基于卡尔曼滤波器进行对环境特征进行估计；⑩将估计的环境特征与激光雷达扫描的当前环境特征进行比较，若差异性较大，则进行重采样，返回第一步，若差异性较小，则进行地图更新；

当机器人主控制器与控制终端出现通信故障时，机器人会立即停止，并与控制终端尝试再次通信连接，当连续三次连接通信失败后会，停止一分钟，一分钟后再次连续三次进行通信连接，如此循环五次，五次都失败后，机器人会规划返回出发点的最优路径，在自主返回到出发点的途中会继续尝试与控制终端的通信连接，若通信连接成功则根据控制终端的指令进行相应的动作，若连接失败则继续返回到出发点；标定功能电脑端使用ROS（机器人操作系统）中的rvzi工具实现的，消防员随身控制端是通过手握式遥控器上的相应按键实现的，当机器人发现被困人员或火源时会在遥控器界面发出警报，此时消防员若认为警报属实则人为的在遥控器控制界面的地图上将被困人员或火源位置标定为救援点，当确定火场中救援点后，可在构建好的地图上对救援点进行标定，以帮助消防员快速到达救援点。

2.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述的环境地图构建与导航单元基于构建的火场环境平面地图，机器人可实现火场中的自主导航，具体步骤如下：机器人主控制器使用A*（A-Star）算法计算出到达导航点的最优全局路径，并且在导航的过程中使用D*（D-Star）算法动态计算局部最优路径，其具体步骤为：首先给定目标位置，根据环境地图构建与定位单元构建好的地图，启动全局路径规划A*（A-Star）算法，利用启发函数首先在地图上确定目标位置的一个大致方向，沿该方向在栅格地图上进行路径节点全局遍历；通过计算筛选出相邻栅格间最短距离，得到全局路径；机器人根据全局路径行进的过程中，为了避免现场环境与载入地图之间存在差异性，设定一个区域范围，在该范围中进行实时地图构建，同时与载入地图进行比较，得到一个局部代价地图，在局部代价地图中利用D*（D-Star）算法规划该区域内的局部路径，进而动态校准全局路径，并进行避障。

3.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述的环境信息探测单元包括温度传感器（541）、烟气浓度传感器（542）、可燃气体传感器（543）；温度传感器（541）、烟气浓度传感器（542）和可燃气体传感器（543）均设置在机器人本体上方。

4.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述的执行单元包括360度云台（531）、驱动电机（532）、风机（533）、位置指示灯（534），驱动电机（532）与行走机构连接，所述的行走机构包括若干个行走轮（21）和履带（22），履带（22）包裹在行走轮（21）上，行走轮（21）与驱动电机（532）的输出轴连接；360度云台（531）设置在机器人本体（1）上方，风机（533）设置在机器人本体（1）内，位置指示灯（534）设置在360度云台（531）前方。

5.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述的视频采集单元包括普通摄像机（571）和热成像摄像机（572），普通摄像机（571）和热成像摄像机（572）均设置在360度云台（561）上。

6.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述的语音单元包括麦克风（551）和扬声器（552），麦克风（551）和扬声器（552）均设置在机器人本体（1）上。

7.根据权利要求1所述的火场自主导航侦察机器人，其特征在于：所述机器人主控制器为工控机，机器人底层控制器为STM32单片机。