1、ISSN 1006 7167CN 31 1707/TESEACH AND EXPLOATION IN LABOATOY第 42 卷 第 1 期Vol 42 No12023 年 1 月Jan 2023DOI:10 19927/j cnki syyt 2023 01 018基于改进粒子 BPF 算法的室内自主导航灭火机器人李韬，郑恩让(陕西科技大学 电气与控制工程学院，西安 710021)摘要:采用搭载 OS 机器人操作系统的灭火机器人，利用改进粒子滤波器(BPF)算法优化采样分布和采样次数，减小采样粒子的个数和粒子的退化率，实现灭火机器人对周围环境的二维栅格地图构建。同时在检测到火焰位置时，利用
2、A*算法和 DWA 算法完成路径规划，使灭火机器人自发性地赶往火焰发生位置并进行灭火动作。实验表明:该灭火机器人在检测到火焰发生位置时，能较为良好地完成建图、定位、导航和灭火任务，满足实际需求，为智能灭火机器人提供了一种可行方案。关键词:灭火机器人;粒子滤波器算法;地图构建;路径规划;机器人操作系统中图分类号:TP 242.6文献标志码:A文章编号:1006 7167(2023)01 0089 05Indoor Autonomous Navigation Fire Extinguishing obotBased on Improved BPF AlgorithmLI Tao，ZHENG Enr
3、ang(School of Electrical and Control Engineering，Shaanxi University of Science and Technology，Xi an 710021，China)Abstract:A fire-fighting robot equipped with the OS robot operating system is used，and the improved BPF algorithmis used to optimize the sampling distribution and sampling times，and reduc
4、e the number of sampling particles and thedegradation of particles It realizes the construction of a 2-dimensional grid map of the surrounding environment by thefire-fighting robot At the same time，when the position of the flame is detected，the A*algorithm and the DWAalgorithm are used to complete t
5、he path planning，so that the fire-fighting robot rushes to the position of the firespontaneously to carry out the fire-fighting action Experiments show that the fire-extinguishing robot can performmapping，positioning，navigation and fire-extinguishing tasks well when detecting the location of the fir
6、e，which meetsthe actual needs and provides a feasible solution for the intelligent fire-extinguishing robotKey words:fire fighting robot;rao-blackwellized particle filter(BPF)algorithm;map construction;path planning;robot operating system收稿日期:2022-06-08作者简介:李韬(1997 )，男，山西太原人，硕士生，主要研究方向为智能机器人的建图和导航算法
7、研究。Tel:15234159919;E-mail:200612067 sust edu cn通信作者:郑恩让(1962 )，男，陕西凤翔人，教授，主要研究方向为自动化过程控制和智能信息处理。Tel:13992097522;E-mail:zhenger sust edu cn0引言发生火灾时，救援人员在面对高温和浓烟的复杂环境中，如若贸然进入现场，往往面临着巨大的危险，灭火机器人1 可替代救援人员进入现场进行探测和救援，并在抢险救援中发挥巨大的作用。国外对灭火机器人相关技术的研究起步较早，美国弗吉尼亚理工大学研制的消防机器人 THO，以及德国马格德堡-施滕达尔大学研制的消防机器人 OLE，均可
8、在复杂的火灾现场实施探测和救援，并成功应用于救灾现场，同时他们正在加速推进高级智能机器人的研究。我国对灭火机器人的研究起步晚2-4，研究重点侧重于机械结第 42 卷构和消防设备，对智能控制的方向研究较少。目前国内绝大部分灭火机器人依赖操作人员干预并进行指定动作的控制，无法根据火灾现场情况进行自发性的导航和避障，对火灾的处理不够及时。本文设计了一种可用于室内灭火的智能机人，在检测到火焰位置时能自发性地导航到火焰发生位置，进行灭火动作。所设计灭火机器人使用树莓派 4B 搭载 OS 机器人操作系统5，使用改进粒子滤波器算法(ao-Blackwellized Particle Filter，BPF)激
9、光同步定位与 建 图(Simultaneous Localization and Mapping，SLAM)6 算法进行实时的环境地图构建。使用 WiFi的多传感器局域网通信技术，获取分布式火焰传感器的火焰位置坐标，使用 A*算法和 DWA 算法实现灭火机器人的全局路径规划和局部路径规划7，驱动灭火机器人到达火灾发生位置，打开灭火装置将火焰扑灭，为室内自主灭火机器人提供一种可行的方案。1系统整体框架1.1灭火机器人的硬件结构本文设计的灭火机器人硬件结构如图 1 所示。灭火机器人的硬件控制核心分为底层控制系统与上位机控制系统两部分。底层控制系统选用 2 块 BTN7971芯片组成的全桥电路去驱动
10、 12 V 直流电动机工作，使用欧姆龙 2000 线型号的编码器 E6B2-CWZC 测出左右轮电动机转速度，使用 IMU 惯性单元 MPU9250 测出灭火机器人的位姿，使用 3 块 Z7886 全桥驱动芯片驱动 3 个电缸执行伸缩动作(拔消防栓，挤压喷射口，调整喷射方向)。图 1灭火机器人的硬件结构框图上位机控制系统使用树莓派 4B 为核心控制器，运行 OS 操作系统，搭载思岚 A1 型号的激光雷达，用于对算力要求较高的激光雷达数据采集和处理，构建环境地图和导航等任务。在机械结构方面使用 T 型角铝自行搭建机器人的底板和支架，并根据干粉灭火器的消防栓位置、按压口位置和喷射口位置安装电缸，组
11、装方式灵活，便于调整干粉灭火器安装位置和喷射高度。1.2灭火机器人的软件框架灭火机器人的软件结构如图 2 所示。底层控制系统以 STM32 为核心控制器，完成灭火机器人的火焰位置读取、运动控制、通信、灭火动作等基本任务。上位机控制系统，完成灭火机器人对周围环境的建图、定位和自主导航等任务。图 2灭火机器人的软件结构底层控制器通过串口将编码器采集的速度数据和IMU 角度数据发送给上位机控制器。上位机根据灭火机器人的双轮差速控制模型8，建立融合数据后的里程计模型，并将里程计数据和激光雷达探测的深度数据利用改进 BPF9 算法，完成静态 2D 栅格地图的构建。在火焰传感器采集到火焰位置坐标后，使用A
12、MCL10 算法完成机器人的自主定位，并利用 A*算法和 DWA 算法完成机器人的路径规划，控制灭火机器人运动到火焰发生位置，打开干粉灭火器扑灭火焰，形成一套完善的自主定位、建图和导航的控制系统。2灭火机器人的模型建立2.1运动模型和里程计模型灭火机器人采用双轮差分机器人运动模型，模型的输入为编码器采集到的左右轮电动机速度 vL和 v，输出为机器人的旋转角速度 和线速度 v，其计算表达式为:=(v vL)/(2d)(1)v=r=(v+vL)/2(2)式中:r 为机器人的旋转半径;d 为机器人底盘中心分别到左右轮的距离。将得到的线速度和角速度进行时间 t 的积分，得到机器人在全局地图 x 和 y
13、 坐标系下的增量位移 dx、dy 和旋转角度增量 d，将所得增量通过下式xy=xy+cos sin 0sin cos 0001dxdyd(3)进行累加，可得到机器人里程计模型表达式。由于机器人在运动过程中的打滑以及编码器本身的测量误差，采用 EKF11 融合算法，将差速机器人模型中的线09第 1 期李韬，等:基于改进粒子 BPF 算法的室内自主导航灭火机器人速度 v1和角速度 1，融合 IMU 测得的角速度 2以及角速度积分后的线速度 v2，提升灭火机器人里程计的可靠性。同理，在逆向运动学求解过程中，通过指定的线速度和角速度值，机器人将朝着目标点进行移动。2.2激光雷达畸变去除由于机器人在运动
14、过程中，激光雷达的测量数据并不在同一时刻测得，1 帧激光雷达的数据测量值并不在同一个坐标系，导致运动畸变的产生，激光雷达的运动畸变示意图如图 3 所示。图 3激光雷达畸变示意图传统的去除畸变的方法为使用 ICP 或 VICP 算法进行修正。ICP 算法是使用纯估计的方式计算激光点云之间的旋转矩阵和平移向量，但并未考虑机器人的运动和可能出现的错误激光数据;VICP 算法假设机器人做匀速运动，但在低速率的激光雷达中，机器人做匀速运动的假设并不成立。考虑到上述情况，使用里程计数据对激光雷达的畸变进行修正12。在 1 帧激光雷达数据的时间间隔中，里程计的更新频率远高于激光雷达的发布频率，把 1 帧雷达
15、数据采集的 n 个激光点数据按照时间间隔等分为 n 个区间，并假设机器人在这 n 个区间内做匀加速直线运动。在每个时间间隔内对里程计数据进行 2 次线性插值，使得里程计数据与 1 帧激光数据对应的测量数据匹配。通过里程计数据将 1 帧数据的 n 个激光点数据转换到同一坐标系下再发布，实现 1 帧激光数据的畸变校正。3灭火机器人的 SLAM 地图创建3.1BPF 算法原理BPF 算 法 是 在 Fastslam 的 基 础 上 发 展 而 来的13，是一种基于粒子滤波的 SLAM 算法，该算法将同时求解机器人运动轨迹 x1 t和地图 m 的联合概率问题，转换为先求解机器人 1 t 时刻的运动轨迹
16、 x1 t，再根据机器人的运动轨迹 x1 t来进行地图 m 的构建，如下式所示:p(x1 t，m|z1 t，u1t 1)=p(m|x1 t，z1 t)p(x1 t|z1 t，u1 t 1)(4)式中:p(x1 t|z1 t，u1 t 1)为利用机器人 1 t 时刻的激光雷达测量数据 z1 t和1 t 时刻的里程计的控制数据u1 t，对机器人的运动位姿轨迹 x1 t进行估计;p(m|x1 t，z1 t)为在已知机器人的轨迹 x1 t和激光雷达的观测数据 z1:t的情况下，进行地图 m 的构建和计算。由于机器人的轨迹模型是非线性的，不便于进行直接求解，故BPF 采用粒子滤波算法，对机器人位姿轨迹 xi1 t的 i个采样粒子进行采样估计，用粒子的采样结果近似后验概率的估计值，其中每 1 个粒子都代表机器人的 1个轨迹估计和地图，用估计结果来近似机器人位姿轨迹 x1 t和地图 m 的后验概率 p(x1 t，m|z1 t，u1 t 1)。3.2BPF 算法改进3.2 1改进采样法在使用 BPF 粒子滤波算法对机器人位姿轨迹进行采样时，由于里程计的概率分布比较广，要使用较多的粒子去采样，这加大了内
