摘要：为实现灭火机器人利用热成像仪对火灾热源快速跟踪，对二维云台进行了水平回转和垂向俯仰方向的动力学建模，采用自抗扰控制器进行了跟踪控制仿真实验并对控制器进行了参数整定。在仿真实验基础上，对云台追踪系统进行了传统 PID 和ADRC 自抗扰算法的实物对比验证。实验结果表明，所研发的自动追踪抗扰算法具备超调量小、调节时间短和抗扰动性能强等优势，为提高灭火器人定点灭火功能提供依据。

中图分类号：X924.4，TN215，TP242.3    

文献标志码：A  

文章编号：1009-0029（2020）05-0675-04

近年来，我国因火灾、危险化学品泄漏等事故造成众多消防人员牺牲，给人民生命和财产安全带来巨大威胁。消防机器人在减少人员牺牲和财产损失方面发挥着举足轻重的作用。现有主流的侦察和灭火机器人通常配置有红外热成像仪，用以侦察现场火源着火点或灾情，从而为后方指挥人员提供决策依据。目前，有机构研发了利用云 台配合红外热成像设备等对灾害现场火源寻找定位的方案，再通过控制消防水炮对火源精准定点灭火，可提高灾害现场扑救效率。因此，为提高机器人对火灾等灾害的快 速应对能力，实现对火源点的快速跟踪定位已成为提高机 器人处置作业效率的关键。

笔者研制一种具备快速火源寻找和定位跟踪功能的消防灭火机器人。为提高机器人的火源追踪速度、精度和抗扰动性能，提出追踪云台的抗扰动热源快速追踪控制算 法，实验结果证明了所提出机器人和抗扰算法的有效性。

1火源追踪系统结构组成和物理建模

1.1 消防灭火机器人整机结构

消防灭火机器人整机结构，如图 1 所示。包括：1. 车体、2. 悬挂组件、3. 驱动组件、4. 轮系组件、5. 消防水炮、6. 升降组件、7. 自喷淋组件、8. 云台组件、9. 红外热成像仪、

10. 避障组件、11. 摄像机和 12. 履带等。其中，升降组件内部通过推杆实现对云台和热成像仪的高度变换，从而实 现对不同高度处图像和热源的采集。云台组件安装在升降组件的上方，可实现水平和垂向的角度调整，带动红外热成像仪完成对空间火源的搜索、定位和瞄准追踪。

1.2 火源追踪装置结构介绍

图 1    消防灭火机器人结构组成

火源追踪装置包括上述的二维云台和红外热成像仪， 二维云台带动热成像仪对机器人前方空间点进行遍历探测；在遍历过程中，当热成像仪判断单次采集热源信息并发现火源最高温度点后，控制器需控制云台组件快速调整角度，实现热成像仪中心点实时瞄准热源，然后调整消防水炮姿态，最终完成火源定点灭火。由于火灾现场热源位置实时变换，高分辨率的热成像仪一般体积重量较大，且消防机器人负载有限，云台驱动功率冗余较小，故热成像仪在姿态调整时重心的变化对云台扰动较大，对云台快速、精准和具备抗扰性能的控制将成为热成像仪对火源定 点跟踪的关键。

1.3    火源追踪装置运动学建模

在设计控制器前要分析其正逆运动学原理，了解该机构的工作空间。云台追踪组件为两个转动自由度的关节， 分别为水平和垂直关节，物理结构简图如图 2 所示。

图 2    云台追踪组件机构物理模型

将云台追踪组件等效为 3 个连杆d₁、d₂ 和d₃ ，水平转动角度q₁和垂向转动角度q₂ 。根据连杆坐标系和连杆之间的齐次变换矩阵，可得末端 p点（热成像仪）相对于系统参考坐标系0 - xyz的变换逆矩阵，可求得末端 P 点相对参考坐标系的位置，见式（1）。

将式（1）两边同时对时间求导，可得云台速度，见式（2）。

对云台追踪组件的运动学逆问题，采用反变换法求解，即已知 n^b、O^b、a^b 和 p^b 来求解 qⁱ(i = 1,2) 。

由上述变换逆矩阵式(1)可得式（3）、式（4）。

速度逆分析用于反求云台追踪组件两个关节的速度， 由式(3)和式(4)可求得速度逆分析，见式（5）。

1.4 火源追踪装置动力学分析

对云台组件进行控制时，还需要确定各自由度驱动电 机的驱动力，根据期望的姿态调整信息获取各关节电机的 驱动力、速度以及位置信息，因此，有必要对动力学方程求解，动力学求解一般使用拉格朗日方程法建立。

云台组件各关节的势能和动能，分别见式（6）、式（7）。

根据组件的总势能V 和总动能T 以及拉格朗日函数可求得云台组件的动力学方程，见式（8）。

式中：m_i 为各关节的质量；r_ci 为质心到转动幅的距离；J_i为关节的转动惯量；M_i 为驱动力矩，

且有式（9）、式（10）。

2 火源追踪抗扰动算法设计

为提高火源追踪组件搜索热源的速度、精准度以及抗扰动性能，笔者提出采用自抗扰控制器(ADRC)对火源追踪组件控制的方案。

自抗扰控制器由跟踪微分器（TD），非线性状态误差反馈（NLSEF）和扩张状态观测器（ESO）3 个部分构成，典型的自抗扰控制器结构原理图，如图 3 所示。

TD 对系统的输入信号安排过渡过程并获取微分信号，ESO 则对系统的输出和总扰动实时估计获得估计值，通过与 TD 输出信号作差产生系统误差，NLSEF 则根据系统误差及 ESO 输出总扰动的估计值生成控制量，最终达到补偿扰动的目的。

3 仿真实验和应用验证

3.1 火源追踪装置仿真环境搭建

图 3     自抗扰控制器结构图

控制模型中，期望输入信号为火源追踪组件检测到热源最高温度点时热成像仪需要调整的角度；扰动施加模块 分别使用常规的 PID 和 ADRC 控制器进行性能对比；扰动施加模型可对云台水平和垂向方向上施加作用力作为负载扰动。

在实际系统中，控制器控制云台进行水平和垂向循环扫描，当环境中高温点出现在热成像仪中时，控制器计算热成像仪中心点与最高温点角度在水平和垂向上的偏差值并将该值分别返回至水平和垂向控制器的输入端进行控制，自抗扰控制器输出控制量驱动云台的水平回转和垂 向俯仰电机转动，带动热成像仪中心点瞄准高温点，同时热成像仪水平回转电机和垂向俯仰电机转轴上安装有编码电机实现角度反馈。在施加扰动时，对云台回转和俯仰 上施加外部干扰力，检测控制器的抗扰动控制性能，云台追踪组件实际控制原理图，如图 4 所示。

图 4    云台追踪组件控制结构原理图

3.2    追踪装置抗扰动控制实验

在进行 ADRC 仿真和实物算法实验阶段，一是要进行控制器参数的整定，一般根据经验确定较为稳定的控制力，通过调整 TD 中的 r、h，使系统尽可能跟随期望值和期望微分值；二是整定 ESO 中的 β_0i 参数，从而估计系统各状态变量以及扰动；三是整定 NLSEF 中的 β_i 提高系统闭环控制性能，通过调整 b₀ 实现对系统未知扰动的补偿，实现主动抗扰。

（1）进行水平回转方向上云台角度调整实验。其中， 云台水平初始的回转角度设定在 0°  ，实验开始时热源处于热成像仪中心点左侧 40°位置，在 50 s 时热源处于热成像仪中心点右侧 40°位置，并且在 25 s 时对云台水平方向施加 5 N 作用力。仿真实验和实物实验结果，如图 5 所示。

图 5    云台追踪组件水平角度调整实验结果

从图 5  可以看到，云台组件在水平方向进行角度调整时，两控制器均存在超调现象，但超调量和调节时间不同。在云台由初始 0°调整至左侧的-40°方向时，PID 超调 量为 25% ，调节时间约为 10  s；但 ADRC 的超调量仅为 11% ，调节时间约缩短至 4 s。当云台水平方向由-40°切换至+40°时，PID 调节震荡较大，超调量达到了 25%以上，调节时间为 12 s；而 ADRC 的控制性能表现良好，超调量为 12% ，调节时间为 5 s。在第 25 s 时刻进行控制器抗扰试验时，由于在云台水平方向反向施加了作用力，导致云台发生颤动调整，对于 PID，超调量大于 20% ，调节时间超过 10 s；ADRC 的超调量则小于 10% ，调节时间也小于 6  s。可以看到，在水平回转角度调整上，ADRC 控制器的超调量更小、调节时间更短，并且扰动施加后的恢复能力更快、更强。

对 ADRC 进行仿真实验，主要是对控制器中的参数进行整定，可为机器人实验验证提供整定参考依据。可以 看出，ADRC 控制器在仿真实验和实物验证实验时，仿真结果和实际控制数据基本一致，不仅验证了对 ADRC 控制器参数整定的有效性，也验证了所建立的二维云台动力 学模型准确性。

（2）进行垂向俯仰方向上云台角度调整实验 ，包括

ADRC 的仿真实验、PID 和 ADRC 的实物实验。其中，云台垂向初始的回转角度设定在  0°，实验开始时热源处于热成像仪中心点正下方-10°位置，在 50 s 时热源处于热成像仪中心点正上方 50°位置，并且在 25 s 时对云台水平方向施加 10 N 外部作用力。仿真实验和实物实验结果，如图 6 所示。

图 6    云台追踪组件垂向角度调整实验结果

从实验结果看到，云台组件在垂向进行角度调整时， ADRC 和 PID 控制器均存在超调现象，但是 ADRC 的整体控制效果更好，主要表现如下：在 0 s 时，ADRC 的超调量比 PID 减少了 12% ，并且缩短至 8 s 以下；在 50 s 进行垂向角度大幅度调整时，PID 的超调量达到了 33% ，调节时间超过 20  s，出现了多次震荡；而 ADRC 则表现优异，超调量仅为 10% ，且调节时间不超过 8 s。受外部作用力扰动时，PID 的超调量要比 ADRC 高 15% ，调节时间长 12  s 以上。可见，ADRC 控制器在控制性能上比 PID 优势更为显著，控制更加精准且抗外部扰动性能更强。

4 结论

灭火机器人的火源追踪系统采用自抗扰算法，实现了 云台和热成像仪对热源的快速追踪，追踪过程中云台超调 量整体小于 10% ，调节时间小于 6 s，并且受外部扰动后恢复能力也更强，满足实际应用需求。下一步，将结合灭火机器人携带的消防水炮，研究实现对热源的远程定点灭火功能。

参考文献

[1] 傅智敏.  我国火灾统计数据分析[J].  安全与环境学报，2014，14(6): 341-345.

[2] 戚斑. 近10 年亡人火灾统计数据分析及防范对策[J]. 中国消防，

2017，(11):18-23.

[3] 张树生，裴文良，孙宁，等. 消防灭火侦察机器人的研制与应用[J]. 制造业自动化，2016，38(6):55-58.

[4] 陈建斌. 消防机器人在我国灭火救援中的应用现状和前景分析[J].   地球，2017，(10):98-98.

[5] 薛晓东. 灭火救援实战中消防机器人技术的研究[J]. 科技创新与应用，2016，(33):297-297.

[6] 董非，杨卫军，李丹，等. 基于红外热像仪的火源定位方法[J]. 消防科学与技术，2019，38(3):90-93.

[7] 张航，赵敏，王璐，等. 基于等效椭圆特征的红外热像仪火灾检测[J]. 消防科学与技术，2018，37(11):111-115.

[8] 王晓晓.   四旋翼无人机载云台的建模与控制研究[D].   沈阳：沈阳航空航天大学，2016.

[9] 韩京清. 自抗扰控制技术[J]. 前沿科学，2007，(1):24-31.

[10] 石晨曦. 自抗扰控制及控制器参数整定方法的研究[D]. 无锡：江南大学，2008.

本文由入驻作者撰写，未经授权禁止转载。