智能预警消防车定位研究

赵建峰 庄乾辉 蔡依霖 李翔宇 赵 婧

南京工程学院自动化学院 江苏南京 211167

近年来，随着经济和科技的快速发展，各种大型石油化工企业密集，隧道、地铁公共交通设施等不断增多，油品燃气、毒气泄漏爆炸事故时有发生。灭火机器人进入易燃易爆、有毒、缺氧、浓烟等危险灾害事故现场进行救援灭火，有效地解决了消防人员面临的人身安全问题，并能对火灾情况进行科学判断，高效灭火。本文设计了一种基于STM32处理器并带有多传感器的智能消防小车，可实现火源的精准定位及灭火。

1 定位系统

1.1 计算偏航角

计算姿态角，要用到欧拉角，如图1所示。

图1 欧拉角

欧拉角是用来确定定点转动刚体位置的一组独立角参量，由章动角β、旋进角(即进动角)α和自转角γ组成。任何方向都可以通过3个基本旋转角来实现，即围绕坐标系的轴旋转。欧拉角可以通过这3个旋转角来定义，它们也可以通过元素定义的几何形状和几何定义来表明。3个角的基本旋转可以是外在的，或者固有的。不同的需求或场合可以使用不同的旋转轴组来定义欧拉角。因此，任何使用欧拉角分析的模型都应该在定义之后进行研究。在此项目里，欧拉角只针对姿态角中的偏航角进行分析计算[1,2]。

根据捷联惯性导航原理，预警车的姿态角就是载体坐标系b相对于导航坐标系n的方位关系。本文定义载体坐标系OXbYbZb，该坐标系与车体固联，原点选为车体质心Xb轴沿车体横轴指向右，Yb轴沿车体纵轴指向前，Zb轴垂直指向车体上方，且构成右手坐标系。定义地理坐标系OXnYnZn作为导航坐标系，该坐标系的原点在车体的质心Xn，Yn和Zn轴分别从原点出发指向东、北和天向，简称导航坐标系ENU。

设载体坐标系b是由导航坐标系n按照Z→Y→X顺序，分别旋转ψ→δ→θ角度得到。其中ψ为滚转角；δ为偏航角；θ为俯仰角。本系统只需要用到偏航角δ，直接套用欧拉角微分方程来解算欧拉角姿态：

1.2 追踪导向

智能预警消防车通过自身旋转360°，并利用超声波和火焰传感器的扫描来躲避障碍并寻找火源。每当消防车启动，其身可视为一条线段，水泵视为箭头，指向方向即为0°。每旋转1°，停止车体并扫描前方，记录周围环境。

超声波通过计算发出信号与接收到返回信号时间t来测距离D，其公式如下：

火焰传感器通过红外探测火源，其输出信号转换值V(模数转换)与离火源距离D成反比关系。受环境影响，其反比关系成非线性。此处忽略误差，将其近似看成线性方程，公式如下：

通过传感器获得的障碍物距离和火源距离，结合姿态传感器的偏航角，即可描绘以车为原点时360°范围内的所有物体。将火源位置一一标出，从近到远依次进行灭火。

1.3 PID调节

由于机械结构和地形环境等因素，车体在自转时会与设定角度产生偏差。前往火源时会与设定路线产生偏差，不能停止在火焰前方最佳距离处，因此需要采用闭环自动控制技术。

目前，闭环自动控制技术主要是基于反馈的概念以减少不确定性。反馈理论的要素由3个部分组成：测量、比较和执行。测量被控变量的实际值，并与期望值相比较，用这个偏差来纠正系统的响应，执行调节控制。由于单级PID稳定性不高[3]，本项目决定采用串级PID方式准确控制车的自转角度和直线行驶。车前端能够在给定角度值的小范围内波动(±0.4°以内)时，车能近似直线行驶。

PID[4]基本算法：

比例项Kpe(t)：调整稳态误差，值越大则稳态误差越小，但系统稳定性会降低。

本文中定义系统偏差：

外环中，u(t)是角度期望值，y(t)为当前角度值。通过第一次外环PID先积分后微分的运算，得出角速度环期望值u(t)'和角速度值y(t)'。然后再经过第二次内环PID先积分后微分的运算得到需要的输出值。

2 系统仿真

为了验证理论分析的真实性和准确性，利用MatLab[5]数字仿真软件，对算法效果进行精准度和响应速率的仿真。

对两级PID进行仿真，预设外环常量：

内环常量：

经过MatLab多次仿真实验，得出最优外环常量：

内环常量：

当设置期望角度为1°时，算法响应速率如图2所示。

图2 模拟响应

预估采样时间为0.15 s，但如图2测试结果显示，在开始时会有0.3°的超调，但0.2 s后会达到平稳，因此0.2 s是最好的采样时间。这样可以在系统开始响应之前，获得更多时间来采样。当设置期望角度为1°时，消防车实际行驶角度偏移变化曲线如图3所示。

图3 消防车角度偏移变化曲线

3 实验验证

此实验目的在于测试预警车追踪火源、停车灭火时车和与火源的距离。受车身长度和灭火装置的推动力影响，最佳灭火位置为距火源点15 cm处。

通过5组实验对比，第一次关闭预警车的PID运算功能，观察其停止位置距离火源距离与设定值(15cm)之间的偏差ΔD。第二次实验开启预警车PID运算功能，与第一次实验进行对比，观察算法精准度。5组实验中，第一次无PID算法时最大偏差值为-5 cm，最小偏差值为±2 cm。而有PID算法时，平均误差只有±1 cm，大大提高了定位精准(见表1)。

表1 算法优化结果

4 结语

生活中存在各种安全隐患，层出不穷的安全事故不仅给人们带来了财产上的损失，更造成了心灵上的伤害。近年来，国家大力研发新型智能防火机器人来降低损失，造福人类。本文介绍的多功能智能预警消防车正符合以上需求，具有结构简单、成本低廉、开放性好、功能性强、效率高等优点，具有重要的研发价值。