基于ROS的多传感器融合巡检机器人系统研究

Research on the Design of ROS Inspection Robot Based on Multi-Sensor Fusion

摘要: 商场、写字楼等室内场所经常存在异物堆放和发生火灾烟雾的情况，这些室内场所作为重要的活动区域，为了消除存在的安全隐患。需要经常对商场、写字楼等室内场所进行巡检，然而人工巡检效率低下，且不能全天候巡检。智能巡检机器人能够高效、全天候对巡检区域进行巡检。设计研究基于多传感器融合的ROS巡检机器人，该巡检机器人包括对激光雷达数据的预处理，基于粒子滤波的Gmapping算法实现建图，AMCL用于室内巡检机器人的重定位。结合A^*全局路径规划算法和DWA局部路径规划算法共同实现室内巡检机器人的导航避障。深度相机结合改进YOLOv7检测算法对室内是否存在火灾烟雾与行人依靠电梯等安全隐患进行巡检。实验表明：该室内巡检机器人具有良好的建图、导航避障、视觉检测的功能。可见研究成果对室内巡检机器人的研究具有一定参考价值。

Abstract: Shopping malls, office buildings and other indoor places often have foreign objects piled up and fire smoke occur, these indoor places as an important activity area, in order to eliminate the potential safety hazards. It is necessary to frequently inspect indoor places such as shopping malls and office buildings, but manual inspection is inefficient and cannot be inspected around the clock. The intelligent inspection robot can efficiently and round-the-clock inspect the inspection area. The ROS inspection robot based on multi-sensor fusion is designed and studied, which includes the preprocessing of lidar data, the mapping algorithm based on particle filtering, and the AMCL is used for the relocation of the indoor inspection robot. Combined with the global path planning algorithm and the DWA local path planning algorithm, the navigation and obstacle avoidance of indoor inspection robots are realized. The depth camera combined with the improved YOLOv7 detection algorithm inspects whether there are potential safety hazards such as fire, smoke and pedestrians relying on elevators in the room. Experiments show that the indoor inspection robot has good functions of mapping, navigation and obstacle avoidance, and visual detection. The research results have a certain reference value for the research of indoor inspection robots.

1. 引言

国外对移动巡检机器人的相关研究起步较早，斯坦福大学的Stanford Cart是一款在上世纪70年代开始设计的移动机器人，它拥有自主导航和避障能力，被认为是早期移动机器人研究的重要里程碑。斯坦福大学的TurtleBot项目是一个基于ROS的移动机器人平台，用于室内巡检和导航任务的研究和实验，推动了机器人在家庭和办公等室内环境中的应用。国内的移动巡检机器人产业起步虽然起步较晚，但是在这方面的进步也十分地迅速，中国科学院沈阳自动化研究所的研究团队在基于ROS系统的室内移动巡检机器人研究中取得了一定成果，探索了地图建模、自主导航、视觉监测等方面的技术。

针对厂房、银行和大型超市等需要人工巡逻的场所，胡兵等[1]一款基于ROS的巡逻机器人自主导航系统。黄鼎键等[2]开展对机器人路径规划方面研究，使用融合A^*算法和TEB算法的路径规划方法进行导航避障研究。这样既实现了激光SLAM精准建图，又能够有效躲避动态障碍物。蔡明珠等[3]结合基于图优化的cartographer算法、A^*算法和基于g2o优化的TEB算法构建了智能机器人自主导航系统，并开展相关实验研究。

因此，一套成熟、可移植性强、稳定的移动机器人是实现室内安全巡检的前提。基于上述内容设计研究一款移植性强、远程控制、可视化显示、实时监控、稳定性强的多传感器融合的ROS巡检机器人。该机器人可实现室内的地图建模、自主导航、实时监控等功能[4]。能够对室内环境进行有效巡检，做到及时发现室内隐患、消除室内隐患。通过实地测试，检验了该巡检机器人的各项功能以及稳定性。

2. 系统总体设计

设计一款多传感器融合ROS巡检机器人，主要分为硬件和软件两大部分[5]。该机器人可以主要用于各类办公大楼、车站候车区、商场等场所的无人自主导航巡检。该巡检机器人基于ROS开发平台，采取分层化和功能模块化设计的思路，建立起多传感器融合的ROS巡检机器人的系统框架，其总体架构如图1所示。架构总共分为5个层次：主要是控制层、ROS系统层、驱动层、PL层、机器人硬件层。

如图1所示，控制层是运行于系统服务器端的，通过WIFI局域网建立PC端与ROS巡检机器人进行通讯。在PC端编写相关的算法程序，利用机器人中的Jetson Nano开发板进行数据采集与处理与算法实现，并且具有远程可视化界面，实现人机交互，便于远程监控与运动控制。驱动层主要是涉及到电机等外设驱动与深度相机、激光雷达驱动，相关的传感器主要是通过I/O扩展与Type-c进行连接，并进行相关数据传输。

Figure 1. Overall system design framework

图1. 系统总体设计框架图

2.1. 硬件结构部分

该巡检机器人的底盘结构如图2所示，并且使用四个麦克纳姆轮作为驱动轮，可以在各种复杂的环境进行纵横向自由移动，相较于传统的机器人的机动性更高。同时四驱轮平台架构效率更高，编写控制运动代码较为简单，相关计算量较小，增加了机器人定位和自主导航的准确性。

Figure 2. Overall chassis mechanical structure

图2. 整体底盘机械结构

巡检机器人硬件部分如图3所示，由麦克纳姆轮、Asttra pro Plus摄像头、思岚S2激光雷达、直流电机等组成。开发板采用STM32和Jetson Nano。

Figure 3. ROS inspection robot

图3. ROS巡检机器人

激光雷达、IMU、里程计以及监测的图像的数据在内置PC系统中进行处理，对巡检机器人的控制命令通过PC系统控制并且监测界面可视化。

2.2. 软件算法部分

软件部分主要包括ROS巡检机器人的上位机界面的设计与机器人建图定位与自主导航、路径规划算法以及检测等相关算法的设计[6]。在Ubuntu16.04系统中进行实现。主要是负责对算法的处理以及多传感器融合计算，其中包括深度相机、激光雷达驱动控制、同各个开发板保持实时话题通讯等。设计好上位机界面，在PyQt设计的监控界面上可以实时观察巡检的周围环境状况。且ROS系统下的相应组件极大方便了ROS机器人的相应开发进程。由于Gmappinng与AMCL只适用于2D雷达数据[7]，在此，使用3D点云转为2D的激光雷达数据。从而让Gmapping建图与AMCL重定位过程更加的精准。AMCL与Gmapping都依赖于里程计的数据，且机器人巡检过程中也可能存在打滑与巡检地面凹凸不平的情况，这对建图与重定位有着很大影响。融合扩展卡尔曼滤波(EKF)对里程计和IMU的数据进行处理，提升里程计数据的准确度，得到的数据与激光雷达的数据用于建立栅格地图，最终栅格地图与雷达数据的信息实现AMCL重定位。大致框架如图4。

视觉检测模块使用改进的YOLOv7的RGB图像目标检测方法，其主要结构如图5所示。它的主要改进原理是结和COT (Contextual Transformer)结构，改进YOLOv7主干，指导动态注意力矩阵的学习，增强视觉表征能力[8]。其次，在主干网络的第一个池化层后面添加了MSC模块，扩大感受野，更好地理解全局特征信息，提高检测精度；最后在头部网络的特征融合部分添加了三维SimAM注意力机制，能够对不同通道和空间位置的神经元分配不同的注意权重。在不增加参数的情况下，避免结构复杂化，利用该注意力机制加强目标在提取特征中的权重并抑制背景干扰权重，从而提升模型检测性能。

Figure 4. Gmapping and AMCL positioning framework

图4. Gmapping建图与AMCL定位框架

Figure 5. Improving the YOLOv7 network architecture

图5. 改进YOLOv7网络结构

其次为了减小MSC模块的参数量，将得到的特征图沿通道方向分成4个特征图。随后，将这些特征映射回拼接操作前的大小，然后对它们进行拼接，以进一步扩展图像的感受野。接下来，通过应用1 × 1卷积来增加通道的数量，从而控制网络的通道数量，有效控制模型的复杂性。最后，引入残差结构，缓解YOLOv7网络层较深导致梯度消失的问题，提高网络识别目标任务的泛化能力。结构如图6所示。

Figure 6. MSC network structure

图6. MSC网络结构

改进YOLOv7的RGB图像目标检测算法主要用于ROS巡检车检测室内是否存在火灾烟雾以及行人倚靠电梯等危险行为的发生。室内巡检机器人检测到室内是否发生火灾烟雾以及行人依靠电梯等行为对于室内安防具有十分重要的意义，该功能的运用可以有效消除室内的大部分安全隐患。

3. SLAM与自主导航设计

SLAM即时定位与地图构建，常用的建图算法[9]有Hector算法、Cartographer算法、Karto算法、Gmapping算法。Gmapping算法是基于RBPF粒子滤波的，它将建图和定位过程分离开来，是先定位后进行建图。Gmapping算法对激光雷达的频率要求也不高，对于室内小地图构建时有较好的鲁棒性和精度。经过综合实验建图效果对比，选取Gmapping算法来实现建图[10]。ROS中的Gmapping算法功能包订阅了巡检机器人的IMU信息、里程计信息，在连接好硬件与配置好相关参数后，即可开始创建并输出二维栅格地图，具体步骤如下：

(1) 新位姿采样：对于$t$ 时刻的第$i$ 个粒子的状态$X_t^i$ 是根据$t-1$ 时刻的粒子状态$X_{i-1}^i$ 和控制量$U_i$ 得到的，根据公式对当前位姿进行估计。

$X_t^i~P\left(X_{t-1}^i|X_i^{t-1},U_t\right)$ (1)

(2) 地图更新：根据传感器的数据与运动轨迹，计算地图概率更新底图。

(3) 粒子权重计算：每个粒子的权重是$\left\{W_{\text{t}}^i\right\}$ ：

$W_t^i=W_{t-1}^i\frac{P\left(Z_{\text{t}}|X_t^i,Z^{t-1}\right)P\left(X_t^i|X_{t-1}^i,U_t\right)}{\pi \left(X_t^i|X_{t-1}^i,Z^t,U_t\right)}\left(i=1,2,\cdots ,N\right)$ (2)

(4) 自适应重采样：如果频繁地进行重采样的话，粒子会过度退化。Gmapping算法有自适应重采样，当有效粒子的数量低于设定合适的阈值$\left(N_{eff}\right)$ ，才会进行一次重采样。其中，$N_{eff}$ 的计算过程如下式

$N_{eff}=\frac{1}{{\displaystyle \sum _{i=1}^N{\left(W^i\right)}^2}}$ (3)

自主导航功能

自主导航[11]的最重要就是机器人的路径规划和定位。前者是负责机器人的运动路线的设计，后者就是机器人的定位。ROS-Navigation是基于ROS移动机器人最常用的功能包集，里面集成了各种路径规划算法与AMCL [12]的实现。

如图7所示，该导航框架中的传感器等相关节点都是基于机器人硬件平台提供，主要用于反馈环境信息、坐标转换信息、运动信息等。MOVE_BASE框架中的全局代价地图、局部代价地图和恢复行为这三个模块主要用于辅助机器人整个自主导航过程中的路径规划以及运动控制。

Figure 7. ROS_Navigation framework

图7. ROS_Navigation框架

4. 路径规划

全局路径规划与局部路径规划

A^*算法[13]是一种广泛应用于图形路径查找和图搜索的启发式算法，它结合了最佳优先搜索和Dijkstra算法的优点，能够在给定的起点和目标点之间找到最短路径，并且在许多实际应用中表现出色，广泛应用于移动机器人的全局路径规划。其基本原理是：首先设置两个数组Openlist和Closedlist数组，分别存放待搜索节点和被搜索节点。从起始点S开始搜索，并移到Closedlist数组中，然后生成下一个当前处理节点的8领域子节点，添加到Openlist数组中，同时计算$S$ 点周围8个字节点，通过函数$F\left(n\right)$ 计算每个子节点的值，通过比较值最小的节点作为新的父节点，并存储到Closedlist数组中。重复上述操作，直到结束点$G$ 存储在Closedlist数组中。

A^*算法的函数是由当前节点到起点和终点的距离组成，公式如下：

$F\left(n\right)=G\left(n\right)+H\left(n\right)$ (4)

其中$F\left(n\right)$ 是起点到终点的最大估计里程，$G\left(n\right)$ 是从起点到当前节点的最大实际里程，$H\left(n\right)$ 是从当前节点到终点的最小估算里程，一般使用曼哈顿距离或者欧几里得距离[14]。鉴于欧几里得距离公式计算量较大，故采用曼哈顿距离公式。计算公式如式(5)下：

$H\left(n\right)=\left|n_x-G_x\right|+\left|n_y-G_y\right|$ (5)

其中$n_x$ 、$n_y$ 是当前节点$X$ ，$Y$ 的坐标，$G_x$ 、$G_y$ 为终点的$X$ 、$Y$ 的坐标。

由于室内场所的障碍物不是很密集分布，空间也不是非常大。A^*算法非常适合室内场所的全局路径规划，可以满足实际场景的运用。

在巡检机器人自主导航的过程中，可能会存在有新的障碍物甚至动态障碍物出现[15]。此时需要局部路径规划实现局部实时避障，DWA算法[16]运用于实时局部避障。基本原理是考虑机器人的动力学约束和环境障碍物的位置，实时计算出最优的速度和转向角，舍弃有障碍物存在的路径，从而使巡检机器人得以安全有效向目标点地移动。

巡检机器人位置与速度的关系表示为：

$X=X+V_x\Delta t\cdot \cos \left({\theta }_t\right)-V_y\Delta t\cdot \sin \left({\theta }_t\right)$ (6)

$Y=Y+V_x\Delta t\times \sin \left({\theta }_t\right)+V_y\Delta t\times \cos \left({\theta }_t\right)$ (7)

${\theta }_t={\theta }_t+w\Delta t$ (8)

$G\left(v,w\right)=\partial \cdot heading\left(v,w\right)+\beta \cdot dist\left(v,w\right)+\gamma \cdot velocity\left(v,w\right)$ (9)

式中$V_x$ 是$X$ 轴方向的速度，$V_y$ 是$Y$ 轴方向的速度，$\Delta t$ 是前进时间，${\theta }_t$ 是角度的变化量，$w$ 是巡检车的旋转角度，$G\left(v,w\right)$ 是评价函数，$\partial ,\beta ,\gamma$ 是相应评定函数的权重。

对评价函数归一化处理可得：

$normal\_head\left(i\right)=\frac{head\left(i\right)}{{\displaystyle {\sum }_{i=1}^{n}head\left(i\right)}}$ (10)

$normal\_dist\left(i\right)=\frac{dist\left(i\right)}{{\displaystyle {\sum }_{i=1}^{n}dist\left(i\right)}}$ (11)

$normal\_velocity\left(i\right)=\frac{velocity\left(i\right)}{{\displaystyle {\sum }_{i=1}^{n}velocity\left(i\right)}}$ (12)

式中$velocity\left(v,w\right)$ 是速度评定函数，$dist\left(v,w\right)$ 是距离评定函数，$heading\left(v,w\right)$ 是方位角评定函数[17]。但DWA算法在障碍物比较密集的情况下，规划出来的路径可能是次优化的路径。通过Matlab进行仿真，验证传统DWA算法和改进DWA算法的效果。圆圈代表障碍物，其半径为1。假设巡检车从原点(0, 0)出发，终点坐标设为(10, 10)。

(a) 传统DWA仿真 (b) 改进DWA仿真

Figure 8. DWA algorithm simulation

图8. DWA算法仿真

从图8(a)仿真结果可以看出，DWA算法存在一定的局限性。虽然可以做到很好的实时避障，但规划出的路线并不是最优路线。这主要是因为算法中相关参数的取值，其中主要包括算法运动模型中各参数和评定函数权重值的取值大小，其中影响DWA算法最大的因素是评定函数前权重的取值，评定函数权重的取值会直接影响DWA算法的效果[18]。此外，不同场景下的环境是有所不同的，运动模型中各参数、评定函数的权重值并不是不变，需要根据应用场景的不同进行实验并改变相应参数的值。据对上述算法的分析，经过大量的计算与仿真实验。在障碍物不变的前提下，设置好合适的权重值后，进行仿真实验，仿真效果如图8(b)所示。从两图对比来看，在选取好合适的权重值后，DWA算法规划的路径明显是要优于之前的路径。

5. 系统测试

5.1. SLAM地图构建

在进行系统测试时，需要使用Rviz这个可视化工具，Rviz是一款用于ROS系统的三维可视化工具，在Rviz中，可以使用XML文件编写机器人、周围物体属性的描述程序，并在界面中呈现。同时，Rviz还可以通过图形化实时显示机器人传感器的信息、运动状态和周围环境的变化等。将巡检机器人放置在需要建图巡检的区域，使用手柄或者手机远程控制巡检机器人开始建图，Jetson Nano开发板控制雷达获取周边环境的点云信息，接收惯性测量单元获取角度信息以及光电编码器生成位置信息，Jetson Nano开发板将得到的信息结合Gmapping算法得到最短时间内的地姿和地图，但因为不可避免的偏差累积问题，还需后端优化并通过增量式扫描和后优化的里程计和地图信息，以及闭环测试来降低全局地图的偏移现象，从而得到全局角度一致性地图，并在Rviz上可以实时的观测建图过程。建图如图9所示。

(a) 实地场景 (b) SLAM建图

Figure 9. Building scenario

图9. 建图场景

5.2. 导航实验

巡检机器人的导航功能模块主要是测试其路径规划和实时避障的能力。这主要是依靠全局路径规划和局部路径规划算法[19]实现。使用map_server功能保存好地图后，使用A^*算法和改进DWA算法共同作用规划出最优化路径并实时避障，通过闭环PID运动算法控制机器人以合适的速度从路径起点导航到路径终点。其实验场景和导航结果如图10所示。

Figure 10. Path navigation results

图10. 路径导航结果

5.3. 视觉检测

对于检测行人倚靠电梯与火灾烟雾情况的发生，系统采用了改进YOLOv7算法对人员倚靠电梯行为和火灾烟雾情况实验检测。在实现改进YOLOv7网络的搭建后，在D-Fair数据集上进行模型训练[20]。

(a) 训练损失曲线 (b) 验证MAP(0.5)曲线

Figure 11. D-Fire dataset training line

图11. D-Fire数据集训练线

训练周期为200，使用余弦退火的学习率防止模型过拟合，模型的训练损失和验证MAP、如图11所示。可以看出，在训练200个周期后，损失值稳定在0.5左右，模型达到收敛，验证MAP(0.5)稳定在92%左右。

其实验结果如表1所示。改进网络模型在D-Fire数据集上，在火灾和烟雾的检测准确率达到88.7%，其中对烟雾数据集的检测准确率为95.4%，对火灾数据集的检测准确91.9%。

对于行人检测数据集训练，将改进网络模型在LLVIP数据集中进行模型训练，包含样本包含30,976张图像即15,488对，训练周期为100。分别对使用余弦退火[21]的学习率防止模型过拟合，模型的训练损失和验证MAP、如图12所示，可以看出，在训练100个周期后，损失值稳定在0.03左右，模型达到收敛，验证MAP (0.5)稳定在95%左右。

Table 1. Fire smoke experiment results

表1. 火灾烟雾实验结果

类别	实例/张	P	R	MAP50	MAP50-95
全部	5261	0.89	0.817	0.887	0.562
火灾	2346	0.951	0.889	0.954	0.624
烟雾	2915	0.828	0.745	0.919	0.484

Figure 12. Pedestrian detection model training curve

图12. 行人检测模型训练曲线

Table 2. Comparative experimental results of various types of algorithms

表2. 各类算法对比实验结果

模型	mAP_0.5%	Parameters/M	FLOPs/G	epoch	batch size	数据集
YOLOv5-n	94.4	6.765	8.2	200	16	LLVIP
YOLOv5	95.6	7.02	15.9	150	8	LLVIP
YOLOv5-s	95.7	6.02	14.9	200	16	LLVIP
YOLOv7	92.4	6.01	13.1	150	8	LLVIP
YOLOv9-geln	95.4	31.4	117.8	200	16	LLVIP
0ur	96.3	4.14	8.6	200	16	LLVIP

在开源数据集LLVIP中，分别对YOLO系列算法进行对比实验，实验数据见表2。使用改进的行人检测网络训练出的模型准确率高达96.3%，在参数量和计算量都相较于各系列YOLO算法平均减少了25.1%。

在目标区域导航的过程中，对该区域进行巡检，检测是否有火灾或者其他危险状况的发生。本次实验巡检区域选择在电梯等候区，将检验机器人是否能检测到火灾与人员依靠电梯行为。并且系统通过局域网络，将目标检测结果在设计的界面上实时显示。实验表明，准确检测到火灾与行人依靠电梯。检测结果如图13所示。

Figure 13. Real-time detection results

图13. 实时检测结果

6. 结语

当前室内巡检机器人系统技术研究已成为国内外研究焦点，针对室内的复杂环境下机器人安全巡检中存在着运动控制和路径规划的困难。本文设计一款基于ROS系统的巡检机器人，采用STM32F4与Jetson Nano作为控制芯片，通过优化算法实现系统多传感器融合与运动控制。同时设计可视化界面实现人机交互，并将所有功能集成并显示在界面上，方便用户操作与监测。结果表明，该机器人能够完成建图与自主导航，并且能够准确检测到行人依靠电梯危险行为和火灾烟雾等突发情况。实现对商场等室内场所的部分重点区域进行全天候无人智能巡检，该研究为巡检机器人行业提供了一种可靠的方案。

基金项目

自然科学基金资助项目(62063009)；国家重点研发计划(2023YFB4302100)；江西省重大科技研发专项(20232ACE01011)；中国科学院赣江创新研究院自主部署项目(E255J001)的支持；国家重点研发计划：低成本小运量磁浮客货运输系统(2023YFB4302100)；江西省重大科技研发专项：小运量永磁悬浮轨道交通系统关键技术研究(20232ACE01011)。

NOTES

^*通讯作者。