森林消防用实时三维定位建图系统*

张景利，张 鹏，吴爱枝，蔡 晔，文 明，于富才，肖文科※

（1.北京清杉科技有限公司，北京 100095；2.北京市安全生产科学技术研究院，北京 101100）

0 引言

近些年，国内外森林火灾频发，造成了严重的生命和财产损失。例如，四川凉山的森林火灾，因为山火爆燃，造成了数十人伤亡的惨痛教训，举国哀悼[1-2]。森林消防装备的现代化是减少生命和财产损失的一个重要途径；但现状与指挥系统“信息化”的目标距离尚远：具体表现在以下方面：（1）火场中缺少可靠的人员定位手段，在森林消防现场难以获知消防人员的实时位置信息；（2）缺少现场的三维地图，在抢险救灾中通常依赖传统的纸质地图，不够形象具体，缺少对灾害现场的实时航拍监测。另外，现有装备无法实现对针叶/阔叶林木分类（需要使用模式识别技术），这是因为针叶和阔叶林木的灭火方法不同。

森林消防还存在下列特点和挑战：（1）需要快速可靠的方法和设备来进行应急救援；（2）复杂三维非结构化环境、且林木山体遮挡严重，增大了救援人员定位及SLAM应用的难度；（3）现场多烟尘、能见度差（也可能在夜间行动及监测，如图1所示）、林中穿行困难。

近年来无人机技术在森林消防中的应用日益广泛。如图1所示，美国加州的消防部门采用无人机在夜间航拍，监控火灾发展态势。但现阶段无人机的应用多停留在航空摄影阶段，仍存在森林环境中的人员定位问题、复杂环境的快速航拍建图以及模式识别（如针叶、阔叶林木的识别）等难题需解决。为此，北京市安全生产科学技术研究院联合北京清杉科技有限公司围绕森林消防应急场景中林木遮挡环境下消防人员定位难，以及在复杂非结构化灾害现场三维地形和态势信息无法及时获取的难题[3-4]，研制了森林消防用实时三维定位建图系统。该系统中存在多架搭载无线定位基站的无人机。无人机载无线定位基站通过互相测距实现实现自主时间同步及快速自组网，在公里级范围的火场内为救援人员提供定位服务，解决森林消防应用中因林木或山体遮挡等难以使用卫星定位的难题。森林消防用实时三维定位建图系统中还采用了基于无人机航拍的多数据融合三维重建[5]及模式识别技术[6-7]。搭载多种传感器的无人机通过在空中巡航对灾害现场进行快速建模及实时灾情监测（火势蔓延监测等），为指挥中心提供火场地理、状态信息，为指挥人员判断火情提供依据。本文将重点介绍无人机实时三维SLAM定位建图系统，它是机器人领域的研究难题[8-10]。

图1 美国加州的消防部门应用无人机的情况

1 系统构成

图2所示为森林消防用SLAM定位建图系统原型机，该系统采用模块化设计，可适配于多种型号的无人机，包括图中的大疆M600pro无人机。该SLAM系统包括下列单元模块。

图2 基于无人机和无线信标的森林消防用SLAM定位建图系统样机

（1）无线定位信标模块：即机载GNSS卫星导航定位模块，主要获取航拍无人机的位置与航向信息，为SLAM建图提供地理位置信息。需要指出，本系统采用自主算法，在GNSS拒止或信号微弱环境中仍能很好地完成实时三维建图任务。

（2）激光雷达：采用Velodyne多线扫描激光雷达，周围环境可以通过激光测距的方式来感知，并形成激光点云信息。

（3）红外/彩色相机模块：包括彩色和红外通道。其中彩色相机为三维建图算法提供前端视觉特征点信息，实现视觉里程计功能以及点云染色/渲染功能。红外相机获取现场红外和温度信息（态势感知及发展预判），并将其回传以便后续的调度指挥。

（4）高精度IMU惯导：采用Xsens MTi300系列惯导，高精度、低漂移，为快速建图算法提供惯导信息，包括线加速度以及角速度等。

（5）机载计算单元：基于Intel NUC电脑设计，可执行快速SLAM定位建图算法，并实时生成三维点云地图。

（6）无线数传链路模块：将航拍图像和其他传感器等获得的信息传输至远程控制端。

（7）电源及配套模块：为SLAM系统机载端供电，配套结构采用轻量化设计，同时配有减震单元，用来降低无人机震动对航拍监测建模效果的影响。

2 软件及算法原理

该无人机SLAM定位建图系统的软件部分基于Ubuntu18.04系统开发，其中，其中利用ROS（机器人操作系统）进行消息映射和管理，传感器信息读取，以及地图构建等。该软件能实现在机载端的三维地图快速构建，同时将传感信息回传给后方调度指挥部分的高性能服务器，进行进一步的回环检测及地图优化等操作。如图3所示，本系统中的SLAM算法采用鲁棒可拓展的冗余设计结构，通过合理可靠的处理IMU惯导模块、无线信标（GNSS卫星导航定位模块）、激光雷达和相机模块的数据，以有效实现项目指标。该算法具体包括：传感器的预处理，前端的运动里程计，后端的数据优化处理，检测度高的回环检测以及比较直观可视化地图构建等多个模块。详细描述如下。

图3 无人机SLAM定位建图系统的软件及算法

（1）传感器预处理

不同传感器的特性不同，使用方法及适用场景等都可能随着测试环境等的改变发生变化；并且其相对位置关系及初始化配置等可能与设计图等存在较大的相对误差[5,10]。为保证相对精确的数据获取，必须进行一系列初始化数据处理，提升可靠性，同时进一步保证后续算法进行处理过程中的数据稳定性和准确性。通过比对及实验分析，通过传感器数据预处理能够保证持续输出合理有用的信息。

（2）前端里程计

通过对处理后的数据进行融合，结合不同传感器的特性，得到实时可靠的自身位姿变化信息。惯导能获取相对可靠的空间角速度和线加速度信息，但其存在长时间数据漂移误差过大的问题，不足以持续输出可靠的自身运动信息。GNSS卫星导航定位信号能够提供较为准确的位置航向信息，但在山林等复杂环境区域，可靠性及精度无法保证。激光点云定位能够保证相对精确的信息，但其输出频率过低，无法在运行速度非常快的时候输出实时信息。而相机受视野、光照、运动等因素影响，无法保障可靠的视觉定位[11]。该模块考虑传感器的优缺点，采用融合定位方法进行多传感器间的数据处理和融合。通过惯导和无线信标进行短时间位姿信息获取，将其作为高频率预测信息，进一步融合频率较低但精度较高的激光和视觉信息，进而能够输出6自由度的位姿信息，且能保证更新频率和精度特性。

（3）回环检测

由于传感器必然会受到噪声干扰，并且里程计（Odometry，包括激光和视觉里程计）计算过程中也存在相应数据误差，这些都不可避免地产生累计误差，最终造成较大的输出误差，这对后续处理造成影响，导致较难建立一致鲁棒的大场景地图[12]。为此可通过回环检测来解决位置估计随时间漂移的问题，该部分算法可运行在后端服务器端。

（4）后端优化

通过检测到的回环数据，可对之前输出的轨迹信息进行调整，以消除累计误差和运动过程中的各类噪声，输出全局一致的位姿优化信息。通过对信息的分析和误差进行估计优化，进一步进行重定位，其能够解决状态信息的不确定程度，即对自身运动信息和周围环境信息进行不确定的表示，之后就能够通过优化的方法将这种误差状态进行估计，进而进行相应的坐标转换并将这一信息发布处理。

（5）地图模块

通过上述一系列模块的处理，将自身状态估计信息（Ego Motion Estimation）和对周围环境的描述信息进行结合，并能够建立全局一致的地图。同时这种对环境信息的描述也不是一成不变的，其能够根据不同的SLAM场景应用而进行获取，最后能够处理对应的特征地图，包括度量地图中的稀疏和稠密地图，视具体情况而定。

3 实验研究

图4所示为本文中所开发的无人机实时三维建图系统的现场实验研究。图4（a）～（b）所示为无人机远程控制界面，以及两次测试的飞行轨迹。图4（c）为现场实验录像中的两帧图像，方框中为搭载机载三维建图系统的无人机。图4（d）为三维建图的效果，图中同时给出了无人机的飞行轨迹（由SLAM算法给出），它与机载GPS反馈的无人机飞行轨迹相同。

需要指出：在整个测试过程中（约5 min），有意关闭SLAM系统中的无线信标（即GNSS定位信号），也就是SLAM算法在融合过程中没有使用任何外界辅助定位信息（例如，传统建图方法中十分依赖的GNSS/RTK卫星定位信息），来验证算法的鲁棒性和可靠性。实验表明：本系统采用独有SLAM建图算法，不使用GNSS定位仍能达到良好的建图效果。也就是说本系统在GNSS信号微弱/拒止环境中仍能正常工作。

本文所叙述的SLAM定位建图系统采用模块化设计，可以根据实际需求来对软硬件系统进行定制。同时，该系统也可采用无人车/机器人搭载或者人工背负的形式，来实现实时三维建图。图5所示为北京清杉科技有限公司研发的背包式建图系统样机，在建图过程中，用户可通过手持显示交互端实时监控建图效果。需要指出，该系统在GNSS卫星导航信号微弱或拒止环境中仍可正常工作，可实现大场景定位及三维建图。

表1所示为本SLAM建模定位系统在不同距离轨迹下的精度分析。其中，Max为误差的最大值，Mean为误差的平均值，Median为误差的中位数，而STD为误差的标准差。可以看出，建模设备的最大建模误差可以稳定维持在1 m以内。

4 结束语

图4 无人机实时三维建图实验研究及效果演示

图5 背包式实时三维建图及效果演示

表1 误差分析

针对森林消防和应急救援中对复杂非结构化灾害现场的快速建模问题，研发了一种基于无人机和无线信标（GNSS导航）的森林消防用实时三维SLAM定位建图系统。文中详细介绍了系统组成和算法原理等，并对该系统进行了实地测试。实测表明：该SLAM系统能够实现对森林环境的实时三维建图，特别是在GNSS卫星导航信号拒止/微弱环境中仍能达到良好的实时三维建图效果。该系统将会是森林消防中的有力工具，其对于应急管理的信息化建设也有十分重要的意义。

未来，还可将背包式建图（或采用地面无人车/机器人搭载）与无人机建图系统结合起来，实现功能互补。在使用过程中，无人机可精确测高，能看到树顶和树冠，但不能完全看到树干；而地面端的背包式系统在森林中可看到树干，采用RANSAC和ICP迭代匹配算法，可将无人机与地面端的三维地图及地理信息匹配起来，进一步提高系统表现。