煤矿井下多旋翼飞行器避障控制方法研究

郭爱军，王妙云，马宏伟，张旭辉，薛旭升，杜昱阳，张超

（1.神华神东煤炭集团技术研究院，陕西 神木 719315；2.西安科技大学 机械工程学院,陕西 西安 710054；3.中煤科工集团信息技术有限公司，陕西 西安 710054；4.陕西省矿山机电装备智能检测与控制重点实验室，陕西 西安 710054）

0 引言

煤矿井下重要工作面环境和设备的日常巡检是确保井下工作人员人身安全及设备安全运行、高效生产的重要措施[1]。传统的人工巡检方式劳动强度大、检测结果容易受到巡检人员水平差异的影响而存在很大的不确定性。随着电池技术发展，多旋翼飞行器以其机械结构简单，可以定点悬停和多方向飞行等特点在各行各业环境安全巡检方面得到了广泛应用。将多旋翼飞行器应用于煤矿巷道关键场所和装备的巡检，可以降低工人劳动强度，有效提升巡检效率。

煤矿井下没有全球定位系统（Global Positioning System，GPS）信号，加上多种电磁干扰的影响，飞行器在井下自主稳定飞行面临巨大挑战[2]。文献[3]提出了基于激光雷达和毫米波雷达的四旋翼巡检机器人井下巷道自主导航飞行的方案，利用5G网络作为传输通道，对5G传输系统进行了验证，效果较好。文献[4]利用扩展卡尔曼滤波算法对三轴陀螺仪、加速度计和磁力计3种传感器数据进行融合，实现了多旋翼飞行器姿态的检测。文献[5]基于迭代最近点对井下无人机实时位姿进行估计。要实现飞行器对井下设备的巡检，仅仅获得位姿信息是不够的，还需研究飞行器路径规划和自主避障策略[6]。机器人技术与虚拟现实结合成为交叉学科研究热点。文献[7]构建了移动机器人作业虚拟场景，利用虚拟现实技术设计可视化仿真系统，通过改进Dijkstra算法得到了移动机器人在不规则矩形障碍下的避障策略，但该策略难以用于井下复杂环境。文献[8]建立了履带式掘进机器人运动控制模型，结合虚拟现实技术研究了非结构化环境下的局部避障策略，但该策略不能建立飞行器的运动模型。针对煤矿井下巡检机器人自主定位和避障问题，文献[9]研究了基于激光雷达的同步定位与地图构建方法，提高了机器人的定位精度，但该方法难以保证多旋翼飞行器快速飞行的实时性要求。

针对上述问题，本文提出利用虚拟远程操控技术对煤矿井下多旋翼飞行器进行避障的控制方法，通过矿山建井图建立虚拟巷道模型，形成导航小地图，减少移动过程中静态障碍物数据的传输；考虑飞行器移动速度对避障结果的影响，提出了复合虚拟势场（Compound Virtual Force Field，CVFF）避障控制算法，实现了复杂环境下飞行器的自主避障和远程干预控制，为煤矿井下巡检提供了新的思路。

1 煤矿井下多旋翼飞行器虚拟远程操控系统

煤矿井下多旋翼飞行器虚拟远程操控系统主要由飞行器虚拟远程操控中心、数据交互中心和多旋翼飞行器及其所携带的传感装置组成，如图1所示。系统将自主飞行避障和人为远程干预相结合，实现了自主避障飞行为主、人为远程干预为辅的控制方式，提高了复杂环境中多旋翼飞行器的巡检效率。

图1 煤矿井下多旋翼飞行器虚拟远程操控系统组成Fig.1 Composition of virtual remote control system of underground coal mine multi-rotor aircraft

基于Unity3D开发平台，依据煤矿巷道建井图构建虚拟巷道模型；建立多旋翼飞行器控制数学模型，形成可交互的飞行器虚拟远程操控中心。系统采用MySQL数据库作为数据交互中心，用于存储物理样机上传的飞行器位姿数据、工况环境数据和控制中心下发的控制命令等。巡检过程中，多旋翼飞行器对自身所携带的多传感器数据进行处理，获得位置、姿态和避障结果，实现自主避障飞行。虚拟操控中心通过接收飞行器上传的姿态数据、工况信息及视频数据，驱动虚拟飞行器在虚拟巷道中同步运动，与井下巡检场景实时保持一致。飞行中若遇到复杂环境或未知障碍导致飞行器自主决策慢，此时避障检测单元根据巷道环境的变化情况，以当前的位置环境状况，利用CVFF避障控制算法实时修正飞行轨迹。紧急情况下，远程操控人员以虚拟仿真界面上飞行器虚拟样机与虚拟仿真环境之间的关系为控制依据，通过远程控制台对井下多旋翼飞行器进行人为远程干预。

2 煤矿井下多旋翼飞行器避障控制方法

在复杂巷道内巡检时，煤矿多旋翼飞行器不仅要为自主控制规划全局路径，还要在遇到动态障碍物时完成局部避障路径规划和人为远程干预[10]。通过数字化虚拟巷道可以提前获得封闭环境内的路径信息，生成导航地图，建立已知的静态环境模型，减少运动过程中飞行器对环境感知建模的任务量，可以提高虚拟远程操控的运行效率。备飞阶段，可以通过提前建立的虚拟巷道规划飞行器的移动路径。在移动过程中，飞行器通过自身携带的传感设备检测移动方向的动态障碍物信息。虚拟远程操控系统根据障碍物数据将动态障碍物实时重建于初始虚拟巷道模型中，对虚拟环境状态进行实时更新，为飞行器局部避障控制提供可靠的环境依据。虚拟远程操控系统通过读取障碍物与飞行器的位置数据和移动速度信息，采用CVFF避障控制算法进行避障路径规划，如果检测到前方障碍物对飞行器移动产生巨大威胁时，远程操控人员可根据规划的避障路径对飞行器进行远程干预，具体的避障控制流程如图2所示。

图2 复杂环境下多旋翼飞行器避障控制流程Fig.2 Obstacle avoidance control flow for multi-rotor aircraft in complex environment

2.1 虚拟巷道模型建立

煤矿多旋翼飞行器在复杂井下巷道飞行的全局路径规划基于矿井初始地图实现。根据煤矿巷道初始地图库信息在虚拟远程操控系统中建立初始巷道模型及全局导航地图，可提前获得飞行器移动过程中已知的静态障碍物信息，在全局路径中为飞行器规划初始导航路线。将初始巷道模型导入Unity3D平台[11]，通过坐标系变换和模型渲染，形成井下巷道虚拟仿真场景。全局虚拟巷道模型如图3所示，局部虚拟巷道模型如图4所示。

图4 局部虚拟巷道模型Fig.4 The local coal mine virtual roadway model

初始巷道模型建立后，在巷道模型正上方设置一个Minimap Camera相机实时拍摄模型的全局路径。在Minimap Camera相机的检视面板中设置Minimap Render Texture，将获得的全局路径生成一张用户界面（User Interface，UI）图片放置于监测界面的右上角，作为导航小地图，如图3右上角所示。导航小地图在X方向的缩放比例和Y方向的缩放比例fY分别为

图3 全局虚拟巷道模型Fig.3 The global coal mine virtual roadway model

式中：(r，u)为飞行器在巷道模型中的坐标；(R，U)为巷道模型尺寸。

在小地图上同步飞行器在巷道的位置：

式中：(r′，u′)为飞行器同步在导航小地图上的坐标；(R′，U′)为导航小地图尺寸。

在获得初始巷道模型和导航小地图之后，可通过Unity3D平台NavMesh导航网格寻路模块将复杂的三维环境结构划分成带有寻路导航信息的网格。调用Navigation Static模块，在初始巷道模型中将移动路线自动生成导航网格，Navigation视图的Bake命令将划分好的可移动区域导航网格以蓝色栅格形式显示出来，如图5所示。在规划好的路径中，通过编辑C#脚本完成目标位置TargetObject的定义，调用NavMesh Agent命令将多旋翼飞行器虚拟模型设定为移动对象，在Unity3D平台中获得全局地图，为飞行器自主飞行提供导航路径。

图5 全局地图内生成的导航网格Fig.5 Navigation grid on theglobal map

2.2 CVFF避障控制算法

虚拟势场（Virtual Force Field, VFF）算法是移动机器人避障导航技术中最常用的算法之一，该算法实质是使用栅格法来表示环境，在虚拟势场中引入环境对多旋翼飞行器的影响[12]。VFF算法的基本思想是考虑了障碍物栅格对飞行器的排斥力和目标点对飞行器的吸引力的共同作用，从而控制飞行器按照避障规划路径飞行[13]。

CVFF避障控制算法在VFF算法的基础上融入了速度势场，在计算过程中引入飞行器与障碍物、目标点之间的相对速度，提高了飞行器对动态障碍物的感知效率和精度。该算法采用栅格法将避障检测单元采集到的障碍物信息局部环境分成若干个栅格，根据障碍物的位置与形状，每个栅格将会对飞行器产生相应大小的斥力作用，每个矩形栅格都有一个累计值（Certainty Value，CV），表示此方格内存在障碍物的可能性大小。定义Ci,j为第 (i,j)个栅格内存在障碍物的可信度，Ci,j越大，表示此栅格存在障碍物的可能性越高，没有检测到障碍物的范围内Ci,j=0， Ci,j通过飞行器所携带的传感设备对空间中障碍物的检测结果进行刷新。

在位置势场和速度势场共同作用下，飞行器在复合虚拟势场中的受力情况如图6所示，其中，Frep为飞行器在复合势场中受到的障碍物栅格的排斥力，Fatt为飞行器在复合势场中受到的目标点的引力，飞行器在虚拟复合势场中所受合力为F。

图6 多旋翼飞行器在复合虚拟势场中的受力Fig.6 Theforce of multi-rotor aircraft in compound virtual forcefield

2.2.1 斥力势场强度

在飞行过程中，每个小栅格(i,j)对飞行器作用一个虚拟斥力Fi,j，斥力的大小与栅格的可信度 Ci,j成正比，与飞行器到障碍物之间的距离di,j的平方成反比。

假设飞行器的坐标为(x0,y0)，障碍物栅格的坐标为(xz,yz)，目标点的坐标为(xa,ya)，飞行器的飞行速度为Vrobot，障碍物的运动速度为 Vobject，则飞行器在复合势场中受到的障碍物栅格的排斥力为

式中：Frepp为飞行器在位置势场中受到的斥力； Frepv为飞行器在速度势场中受到的斥力。

式中：Mp为 位置势场的引力增益系数；分别为X方向和Y方向的单位向量。

式中：Mv为 速度势场中的斥力增益系数；Vb为飞行器相对于障碍物的运动速度；θ为飞行器与障碍物相对位置矢量与相对运动速度矢量的夹角；β 为Vb与X轴的夹角。

2.2.2 引力势场强度

飞行器在复合势场中受到目标点的引力为

式中： Fattp为飞行器在位置势场中受到的引力； Fattv为飞行器在速度势场中受到的引力。

式中：Kp为 位置势场的引力增益系数；da为飞行器与目标点之间的相对距离。

式中：Kv为 速度势场引力增益系数；Vg为飞行器相对于目标点的运动速度；α为 Vg与X轴的夹角。

因此，飞行器在虚拟复合势场中所受合力为

假设飞行器运动步长为l0，飞行器在虚拟复合势场中所受合力在X轴，Y轴上的分量分别为FX，FY， 时刻飞行器的坐标为 ，则该飞行器在 时刻的坐标为 ，其计算公式为t(xt，yt) t+1(xt+1，yt+1)

2.2.3 算法的设计与实现

CVFF避障控制算法流程如图7所示。

图7 CVFF避障控制算法流程Fig.7 Compound virtual force field obstacle avoidance control algorithm flow

CVFF避障控制算法具体实现步骤如下：

（1）初始化参数。获取飞行器、障碍物和目标点的位置坐标，设定位置势场和速度的相关参数。

（2）调用Compute_Angle模块分别计算飞行器与障碍物和目标位置之间的夹角。

（3）分别调用Compute_Attract模块和Compute_Repulsion模块，计算引力和斥力在X轴和Y轴的分量。

（4）计算飞行器在下一时刻的位置坐标，并进行记录。

（5）判断飞行器是否到达目标位置，如果到达指定位置则跳出程序，否则回到步骤（2），进入下一次循环。

3 仿真实验与分析

3.1 静态障碍物避障路径仿真分析

为了验证煤矿井下多旋翼飞行器避障控制方法的效果，在Matlab仿真平台中构建10×10的环境感知栅格，飞行器在避障过程中的相关参数设置[14]见表1。

表1 仿真环境参数Table 1 Simulation environment parameters

实验中设置了p0=0.5和p0=1，飞行器的起始坐标为（0，0），目标位置为（10，10），比较静态环境下VFF算法和CVFF避障控制算法的避障效果，其避障控制仿真结果如图8−图11所示。VFF算法与CVFF避障控制算法仿真结果性能比较见表2和表3。

图8 p0=0.5时VFF算法仿真结果Fig.8 Simulation resultsof virtual force field algorithm when p0= 0.5

从图8−图11和表2、表3可看出，当障碍物影响距离p0分别为0.5和1时，在相同的避障环境中，CVFF避障控制算法迭代次数更少，计算效率更高，且得到的避障轨迹长度更优，避障效果更高效。VFF算法在障碍物比较密集的区域，避障效果不理想，当避障影响距离较小时容易与障碍物之间发生碰撞。通过比较图9与图11的轨迹路线，表明了障碍物影响距离对避障路线的影响较大，在本文搭建的仿真环境中，障碍物影响距离为0.5个单位时，轨迹规划结果最理想。

图9 p0=0.5时CVFF避障控制算法仿真结果Fig.9 Simulation results of compound virtual forcefield obstacle avoidance control algorithm when p0=0.5

图10 p0=1时VFF算法仿真结果Fig.10 Simulation results of virtual force field algorithm when p0=1

图11 p0=1时CVFF避障控制算法仿真结果Fig.11 Simulation resultsof compound virtual force field obstacle avoidancecontrol algorithm when p0=1

表2 p0=0.5时2种算法性能比较Table 2 Performancecomparison of two algorithmswhen p0=0.5

3.2 动态障碍物避障路径仿真分析

利用CVFF避障控制算法对动态障碍物的避障效果进行仿真实验。首先设置了100×100的栅格仿真环境，飞行器起点坐标为（0，0），运动速度Vrobot=(1.5，1.5)，终点坐标为（100，100）。仿真过程设置了2个动态障碍物，第1个障碍物的起点坐标为（33，88），运动速度Vobject1=(0.50，−3.75)，第2个障碍物的起点坐标为（65，7），运动速度Vobject2=(0.2，2.0)。

随机选取了6个时刻记录飞行器与障碍物的运动情况，如图12所示，可看出飞行器成功避开了2个动态障碍物。

图12 多旋翼飞行器动态避障过程Fig.12 Obstacle avoidance processof multi-rotor aircraft

运动过程中飞行器与第1个动态障碍物之间的相对位置变换情况如图13所示。在t =0时，飞行器向着第1个障碍物方向飞行，两者之间距离不断减小，飞行器自动调整飞行方向；在t = 12 s时，两者距离为5 dm，此时刚好没有发生碰撞，随后飞行器朝远离第1个障碍物方向飞行，两者距离逐渐增大。

图13 飞行器与第1个障碍物之间的距离Fig.13 Distance between the aircraft and the first obstacle

飞行器与第2个障碍物的相对位置变换情况如图14所示，在t = 0时，飞行器与第2个障碍物的间距为65 dm，不会发生碰撞，飞行器向着第2个障碍物方向飞行，两者之间距离不断减小，在这一过程中飞行器自动调整飞行方向；在t = 31 s时，两者距离为8 dm，没有发生碰撞，随后飞行器朝远离第2个障碍物方向飞行，两者距离逐渐增大。

图14 飞行器与第2个障碍物之间的距离Fig.14 Distancebetween the aircraft and thesecond obstacle

以上分析表明，本文提出的避障控制方法不仅能使飞行器在静态环境中实现稳定避障，在动态威胁下也能实现精确避障飞行。

4 结论

（1）提出了一种利用虚拟远程操控技术对煤矿井下多旋翼飞行器进行避障的控制方法，根据煤矿巷道初始信息建立了虚拟巷道模型和全局导航地图，减少移动过程中静态障碍物数据的传输；通过飞行器携带的传感设备检测移动方向的动态障碍物信息，采用CVFF避障控制算法进行避障路径规划。

（2）提出了一种CVFF避障控制算法，在VFF的基础上引入飞行器与障碍物、目标点之间的相对速度，提高了飞行器对动态障碍物的感知效率和精度。

（3）从静态和动态障碍物避障路径2个方面对CVFF避障控制算法进行仿真验证，结果表明：静态情况下，相比VFF算法，CVFF避障控制算法在减少了迭代次数的同时，也缩短了飞行器的轨迹长度；动态情况下，飞行器成功避开了提前设定的2个动态障碍物，顺利到达设定目标点，验证了采用CVFF算法的多旋翼飞行器避障控制方法的有效性。