消防侦检机器人的定位建图与路径规划

初雪娇

摘 要：通过针对搭载气体检测、视频采集、激光雷达等多传感器的消防侦检机器人的即时定位、地图构建与路径规划问题进行研究。同时还研究了激光雷达与深度相机数据融合、环境信息与地图信息的可视化、动态路径规划等关键技术，提升机器人的建图、定位与规划效率，实现机器人自动控制、自主定位、路径规划和信息采集，提高国内消防侦检机器人的智能化水平。

关键词：机器人控制 即时定位 路径规划 鲁棒优化

中图分类号：TP242

Localization， Mapping and Path Planning of Firefighting Detection Robots

CHU Xuejiao

（Shanghai Qiyao Environmental Technology Co.， Ltd.， Shanghai， 200090 China）

Abstract： This paper studies the problem of the simultaneous localization， mapping and path planning of firefighting detection robots equipped with multiple sensors such as gas detection， video capture and the lidar， and aslo studies key technologies such as the data fusion of the lidar and the depth camera， the visualization of environmental and map information， and dynamic path planning， which improves the efficiency of the mapping， localization and planning of robots， realizes the autonomous control， autonomous localization， path planning and information collection of robots， and enhances the intelligence level of domestic firefighting detection robots.

Key Words： Robot control; Simultaneous localization; Route planning; Robust optimization

当前，每年火灾及化学品等物质的泄漏、燃烧、爆炸等事故给人们的生命安全和财产造成了巨大损失。在发生火灾、爆炸等事故后，消防人员在高危事故现场的救援和勘察过程中存在很多困难。例如：在面临易燃易爆气体、有毒有害气体、高温浓烟等灾害情况时，由于缺少有效装备及设施，救援人员若贸然采取行动必将给消防人员带来很大危险。消防人员虽然可着防护设备进入火场，但需携带诸多探测设备对火场参数进行测量，给消防员造成沉重的体力负担，甚至会阻碍消防人员在爆炸等紧急情况下的逃生[1， 2]。

1. 国内外研究现状

目前国内消防侦检机器人主要适用于爆炸性危险区域，能替代消防救援人员进入易燃、易爆、有毒有害、缺氧、浓烟等危险灾害现场，进行侦察并将采集的信息（图像、语音、数据）实时处理和实时无线传输。但是在消防实际作战过程中，机器人需要进入陌生的楼宇或者室内区域中进行侦检任务，普通的消防侦检机器人不能快速构建导航地图并实现机器人的定位导航。

SLAM（Simultaneous Localization and Mapping），即同时定位与建图，作为移动机器人定位与建图领域非常重要的技术，MUR-ARTAL R和DISSANAYAKE M等人的研究中提及，自1986年由Hugh Durrant-Whyte 和 John J.Leonard在ICRA首次提出以来，经过近几十年的发展，已经有了诸多适用于特定场景的解决方案[3， 4]。

将SLAM与消防机器人相结合，将解决消防机器人在陌生环境中进行侦检任务不能快速构建导航地图并实现自主导航的问题。“激光+SLAM”是目前机器人自主定位导航所使用的主流技术。激光测距相比较于图像和超声波测距，具有良好的指向性和高度聚焦性，是目前最可靠、稳定的定位技术。激光雷达传感器获取地图信息，构建地图，实现路径规划与导航。机器人路径规划与运动执行的结合，需要多传感器的融合[5]。通过激光雷达和深度相机扫描周围的环境，判断智能体与环境物体的间距[6， 7]；运用跌落传感器辨别边缘区域，防止踏空跌落[8]；运用超声波传感器，探测深度相机与激光雷达的视觉盲区，如玻璃等，防止碰撞；运用碰撞传感器，让机器人在与其他物体相撞之时，能够及时调整自己的方向。在获取地图之后，消防机器人应能根据起点与终点的位置，规划避开障碍物的最短移动路径[9]。该任务设计优化算法，当前启发式优化方法可以获得较好的规划结果，但优化结果的鲁棒性有待进一步提高[10， 11]。目前，尚没有研究从建图和路径规划两方面对消防机器人做整体研究。

目前，现有的消防侦检机器人在陌生的爆炸性危险环境中执行任务时，不能构建导航地图并实现自主导航，存在信息采集不足、机器人智能化程度不高、不能自主避障等问题[12- 13]，智能化程度较低，实用性较弱，因此亟待优化完善。考虑到以上问题，本文将提出一种消防侦检机器人的集成方案，以下将从硬件结构和软件设计两方面对系统进行介绍。

2系统设计

2.1硬件结构设计

消防侦检机器人硬件结构拟包含7个主要模块：板载计算机模块、履带式移动底盘、电机驱动模块、无线通信模块、传感器组模块、激光雷达及视觉模块和电源模块。其中，传感器组模块包含有害气体检测传感器等，用于监测环境参数；惯性测量传感器，用于辅助机器人即时定位；视觉模块采用深度相机，用于地图构建、工作视频采集和障碍物辨识。基于以上硬件结构，拟实现机器人远程控制、环境参数采集及回传、自主路径规划及避障、即时定位与地图构建等功能。

2.2软件设计主框架

本系统旨在实现在未知环境中机器人的自主定位和环境地图构建。在系统初始化阶段，首先，建立SLAM系统的基本结构，包括地图、位姿估计、特征提取和描述符等核心组件。这个初始化过程为后续的SLAM操作奠定了基础。在主循环中，采用传感器数据预处理的方法，对原始数据进行优化和校正，以确保数据的质量和一致性。这包括去除镜头畸变、点云配准和惯性测量单元（Inertial Measurement Unit， IMU）数据融合等步骤。其次，执行特征提取和匹配，通过使用特征提取器从传感器数据中提取关键点和特征描述符，并进行特征匹配来建立当前帧与上一帧之间的关联。这是SLAM系统中的关键步骤，用于确定机器人的运动轨迹。位姿估计是SLAM系统的核心之一，使用特征匹配的结果来估计当前帧相对于上一帧的位姿。这一步骤采用不同的技术，包括运动模型、视觉里程计和数学优化方法。地图的构建是SLAM的另一个重要方面，通过将当前帧的特征点投影到地图上不断更新地图的信息，包括环境的几何结构和特征点分布。再次，系统还包括回环检测机制，定期检查机器人是否返回到先前访问过的地点。当检测到回环时，系统将执行修正操作，以提高位姿估计和地图的一致性。为了提高位姿估计的精度，系统还引入了位姿优化步骤，使用全局或局部优化方法，如图优化，来进一步细化位姿估计的结果。最后，对地图进行维护，删除不稳定或重复的特征点，以确保地图的准确性和可用性。最后，基于当前地图，动态调整行进路线，寻找到目标点距离最短最安全的路线，直到机器人抵达目标点。

1.3地图构建与环境信息融合

结合激光雷达、深度相机和惯性测量单元完成即时定位与地图构建，建立高鲁棒性的地图构建系统。同时，开发上位机软件，展示实时地图构建结果，并通过点云图、等线图等方式在地图上标注实时环境信息。

1.3自主侦检路径规划

自主路径规划主要包含两部分：自主避障和自主侦检路径规划。自主避障为局部优化策略，由板载计算机模块执行；自主侦检路径规划为全局优化策略，综合远程监控偏好区域设置与环境值分布规律对路径进行优化，从而提高侦检效率，保证运行安全，缩减危险区域（如泄漏点）的查找时耗。

融合障碍物预测的鲁棒路径规划是一种基于遗传算法的智能路径规划方法，可以提高机器人或自动驾驶系统在复杂环境中的导航鲁棒性。该算法主要分为初始化种群、主循环、子代生成、障碍物预测模型更新、适应值估计、种群排序与选择以及最终路径输出等步骤。

首先，初始化种群，即生成一组具有随机性的候选路径。这些路径将作为初始解的集合，种群的大小和初始路径的多样性对算法的性能有重要影响。其次，通过一个主循环来控制算法的迭代过程，直到满足结束条件。在每次迭代中，通过遗传算法的交叉和变异操作，将父代路径组合产生新的路径。这一步骤旨在保留高适应度路径的特征，并引入一些变异以增加搜索空间的多样性。最后，更新障碍物预测代理模型。这一模型的更新可以基于Kriging插值等方法，通过不断收集新的观测数据来提高对环境障碍物分布的预测精度。这样的预测模型更新使得路径规划能够更好地适应环境的变化。

算法根据预测模型的不确定性估计路径的适应值。这一步骤的关键在于将障碍物预测的不确定性融入适应值的计算中，使规划的路径更具有鲁棒性和可靠性。通过适应值对种群进行排序，并选择适应值较高的个体作为父代，用于下一轮的交叉和变异操作。这一步骤保留了适应性较强的路径，促进了遗传算法的收敛性。最后，保存最佳路径作为算法的输出。这条路径是在考虑障碍物预测不确定性的情况下，通过遗传算法搜索得到的机器人或车辆的导航路径。这条路径具有较好的鲁棒性，适应于在复杂、动态的环境中导航。

综合而言，融合障碍物预测的鲁棒路径规划算法通过遗传算法、障碍物预测模型更新和适应值估计等步骤，使机器人能够更好地应对环境变化，具备更高的路径规划鲁棒性。

3实验测试

根据以上设计，令机器人在仿真环境下进行测试。

4结语

本文研究了消防侦检机器人的硬件结构、即时定位、地图构建与路径规划问题。将SLAM技术引入消防侦检机器人，并设计了融合避障功能的鲁棒路径规划算法。仿真结果证明，所提出的硬件与软件结构可以较好地实现定位、建图与路径规划，为消防机器人的发展提供了技术支持。

参考文献

[1] 祁宇明，谢兵，邓三鹏，等.可变形履带式自动巡检消防机器人设计与性能分析[J].装备制造技术，2019（10）：62-66.

[2] 彭小清.消防机器人在我国灭火救援中的应用现状和前景分析[J].今日消防，2019，4（12）：18-19.

[3] MUR-ARTAL R，MONTIEL J M M，TARDOS J D. ORB-SLAM： A Versatile and Accurate Monocular SLAM System [J].IEEE Transactions on Robotics，2015，31（5）： 1147-1163.

[4] DISSANAYAKE M，NEWMAN P，CLARK S，et al. A solution to the simultaneous localization and map building （SLAM） problem[J]. IEEE Transactions on Robotics and Automation，2001，17（3）：229-241.

[5] CHIANG K-W，TSAI G-J，CHU H-J，et al. Performance Enhancement of INS/GNSS/Refreshed-SLAM Integration for Acceptable Lane-Level Navigation Accuracy[J].IEEE Transactions on Vehicular Technology，2020，69（3）：2463-2476.

[6] LIU C，ZHOU C，CAO W，et al.A Novel Design and Implementation of Autonomous Robotic Car Based on ROS in Indoor Scenario[J].Robotics，2020，9（1）：1-15.

[7] ZHANG T，NAKAMURA Y.Humanoid Robot RGB-D SLAM in the Dynamic Human Environment[J].International Journal of Humanoid Robotics，2020，17（2）：1-10.

[8] WEN S，ZHAO Y，YUAN X，et al.Path planning for active SLAM based on deep reinforcement learning under unknown environments[J].Intelligent Service Robotics，2020，13（2）：263-272.

[9] SATAKE Y，ISHII H.Path Planning Method With Constant Bending Angle Constraint for Soft Growing Robot Using Heat Welding Mechanism[J].IEEE Robotics and Automation Letters，2023，8（5）：2836-2843.

[10] SHAO X，GONG Y J，ZHAN Z H，et al. Bipartite Cooperative Coevolution for Energy-Aware Coverage Path Planning of UAVs [J].IEEE Transactions on Artificial Intelligence，2021，3（1）：29-42.

[11] DU W，SONG W，TANG Y，et al.Searching for Robustness Intervals in Evolutionary Robust Optimization[J].IEEE Transactions on Evolutionary Computation，2022，26（1）：58-72.

[12] 熊根.履带式消防机器人设计与仿真[D].南昌：南昌大学，2019.

[13] 秦松亭.基于物联网技术的消防侦察机器人系统设计[D].大连：大连海事大学，2017.