炼焦化工厂复杂环境下的无人机巡检控制与规划

宫则强 郑曦星 赵伟

[摘    要]针对炼焦化工厂环境下无人机巡检问题进行了研究。针对工厂复杂的管线通道分布，提出了三维坐标系下的无人机动力学建模、基于PID控制方法的机体运动控制及以A*算法为基础的巡线规划算法。经仿真测试与现场实测都达到了良好的巡检效果，实现了炼焦化工厂复杂环境下的无人机智能巡检。

[关键词]炼焦化工厂;无人机;巡检;PID;A*算法

[中图分类号]TP311.52 [文獻标志码]A [文章编号]2095–6487（2022）01–00–03

[Abstract]This article studies the problem of drone inspection in the environment of a coking chemical plant. Aiming at the complex pipeline channel distribution in the factory， the UAV dynamic modeling in the three-dimensional coordinate system， the body motion control based on the PID control method， and the line-following planning algorithm based on the A* algorithm are proposed. Both the simulation test and the on-site test have achieved good inspection results， and the intelligent inspection of the drone in the environment of the coking chemical plant has been realized.

[Keywords]coking and chemical plant; UAV; inspection; PID; A* algorithm

1 系统说明

1.1 无人机选型

本文研究的算法使用了深圳大疆创新制造的Matrice 300RTK（以下简称M300）无人机作为飞行平台。该型号无人机是X型四旋翼布局，机臂能够折叠收缩。该机型能够很好地用于管线巡检，并安装有远程操作接口，可用于远程地面站控制与数据回传。

1.2 机载气体传感仪器

机载气体传感仪器主要基于STM32和泵吸式气体传感器，安装于M300的机腹位置，用于巡检时检测泄漏的一氧化碳气体并回传至远端的管理软件/手机APP，并在远端超过报警阈值后予以示警。

1.3 巡检管理系统架构

巡检管理系统将无人机GPS自动导航飞行能力与高精度一氧化碳检测技术进行结合，实现对管道的巡线和泄漏焦炉煤气的检测。硬件设备主要包括无人机、数据库服务器、机载一氧化碳检测装置和数据传输设备，软件系统包括CO检测系统管理软件、移动APP。

2 无人机控制算法设计

2.1 动力学建模

无人机在工厂环境中运动时，主要是在三维空间SO（3）中发生坐标与姿态变换，因此在该向量空间中首先建立坐标系。建立了两个常用的坐标系，包括大地坐标系和机体坐标系，这两个坐标系都符合右手定则，其中大地坐标系的原点定义为无人机质心的初始位置，XE指向正北方向，YB指向正西方向，ZB指向地心方向的反方向。机体坐标系与无人机固连，原点OB与无人机的质心重合，XB指向机头方向。无人机的姿态常用欧拉角表示，即机体坐标系相对于大地坐标系的姿态。地球大地坐标系与机体坐标系的原点重合，机体坐标系先后绕ZB轴、YB和XB轴旋转一定角度与大地坐标系重合。其中，绕XB轴旋转的角度为滚转角，绕YB轴旋转的角度为俯仰角，绕ZB轴旋转的角度为偏航角。这些角度的正方向，由右手螺旋定则定义。

在上述坐标系定义基础上，根据牛顿-欧拉公式，给出无人机的动力学模型。

将无人机位置和姿态都用三维向量表示。位置坐标表示为（x，y，z），姿态使用用欧拉角形式，表示为（φ，θ，ψ）。机体转动角速度在机体坐标系下表示为（p，q，r）。所以根据牛顿-欧拉方程可以建立无人机在空间中的平动与转动方程：

2.2 控制器设计

在本文研究中，主要是在炼焦化工厂中使用，环境复杂且干扰因素多，很难得到精确模型。因此，选择使用不依赖模型的控制方法。

而这其中经典比例-积分-微分（Proportional Integral Derivative，PID）控制方法[1]是不依赖模型控制方法中工程应用最广泛的，效果比较突出的一类方法。通过参考输入与实际输出之间的误差作为控制器的输入信号，输出控制量消除被控对象的误差[2]。PID方法可用如下公示表示

（3）控制器总体设计

根据实际的工厂环境需要，将姿态控制作为控制内环，位置控制作为外环。将二者按照闭环控制方法将飞行控制器设计为内外环结构，其中外环的位置控制器控制输出作为内环姿态控制器的控制输入。整体控制器设计（图1）如下。

位置控制器根据期望位置和实际位置计算出期望总拉力fd、期望滚转角φd、期望俯仰角θd;姿态控制器根据由外部控制指令发送的期望偏航角ψd、实际姿态以及位置控制器的输出计算出力矩期望τd;控制分配根据fd和τd计算出4个旋翼的期望转速ωd，i;最后，电机控制器根据ωd，i计算出每个电机输出功率σd，i。

3 无人机规划算法设计

在实际的炼焦化工厂环境中，整体的环境是一个三维空间，其中管道走向以横平竖直的方式为主，但周围的建筑物等会构成一些障碍点。当无人机进行巡检航迹规划时，需要将管线的走向与障碍点的位置坐标进行全盘规划计算。所以一般首先会选择利用全局静态规划参考相关的信息来生成多个航迹点，以此来形成一条完整的飞行轨迹，实现自主跟踪航迹飞行。在规划算法生成航迹以后，无人机按照指定的轨迹从第一个航迹点出发，根据航迹点顺序依次移动，直至飞行目标完成到达最后一个航迹点，但是无人机在执行飞行任务的过程中存在遇到突发性障碍的可能性，因此需要同时结合局部动态规划来进行实时规划。

本文采用的是A*（又称A-Star）算法[3]，该算法是在静态网络中规划最短路径的直接搜索方法[4-5]。将整个无人机规划的地图抽象为图的表示形式

G=（V，E）                              （8）

式中，V为节点集合，E为节点间边的集合，边的和就是不同节点间的距离。

A*算法的计算公式表示为

f*（n）=g*（n）+h*（n）                   （9）

式中，f*（n）为从初始状态经由节点n到目标节点的最小代价估计，g*（n）为在状态空间中从初始节点到节点n的最小代价，h*（n）为从节点n到目标节点的路径的最小估计代价。

算法主要流程是估计值基础上对实际h（n）的选取：

保证找到最短路径（最优解的）条件，关键在于估价函数f*（n）的选取（或者说h（n）的选取）。

以h（n）表达节点n到目标节点的估计距离。那么h（n）的选取分为以下三种情况进行讨论：

（1）当h（n）