基于贝叶斯估计与区域划分遍历的四轴飞行器避障路径规划算法

王家亮，李树华，张海涛

（中国民航大学计算机科学与技术学院，天津 300300）

（*通信作者电子邮箱lsh3438526746@163.com）

0 引言

随着科技的发展，小型无人机与人们的生活联系得越来越紧密，且具有造价低、体积小、使用方便、稳定、灵活等特点。小型无人机中最为常用的是四轴飞行器，具有机动灵活且能垂直起降的特点，可用来执行比较危险的任务［1-3］。在抗击新冠病毒肺炎疫情期间，新浪网2020 年2 月27 日报道：上海临港新片区无人机出发战“疫”，执行任务120 余次、飞行总里程达1 000 km、提醒佩戴口罩28 人次、发现人群聚集现象9 次、乱设摊7 次——这是2 月以来上海自贸区临港新片区7 架无人机完成的“战绩”［4］。小型无人机由于其独特的优势，应用非常广泛，如：物流、农业植保、巡检、安防、救援、测绘、直播、编队飞行、未来云端人工智能（Artificial Intelligent，AI）自主飞行等［5］。

国内外许多高校以及研究机构均已经开展了四轴飞行器的研究工作，如路径规划［6-7］、稳态飞行控制［8］、安全［9］、飞行姿态调整［10］、定位［11］、避障［12］、导航［13］、多机协同飞行控制［14］等，并取得了显著的技术成果。四轴飞行器避障算法的研究主要有两个方面［15-16］：一是基于A*算法、蚁群算法、人工势场法等路径规划算法进行避障；二是依据四轴飞行器与障碍物之间的位置关系，并与飞行速度、姿态角度（俯仰角、偏航角、滚动角）等信息进行融合计算从而实现避障。四轴飞行器的避障直接决定了它的自主飞行的能力，对完成飞行任务至关重要。区别于目前现有的利用多传感器（激光测距仪、双目摄像头、雷达等）进行研究，本文只对单一传感器（单目摄像头）采集的视频图像信息进行研究，基于贝叶斯估计与区域划分遍历，旨在实现四轴飞行器高实时性避障路径规划算法。

1 动力学模型

四轴飞行器是一种能够垂直起降的非共轴的多旋翼飞行器，同一对角线上的一组旋翼采用逆时针旋转，另一组旋翼采用顺时针旋转，相互抵消了旋翼旋转带来的反扭矩力。通过调节对称分布的四个旋翼转速，可以改变四轴飞行器的位置，从而实现对飞行姿态的控制，比如悬停、俯仰、偏航和滚动等。根据四轴运动方式的不同，可将四轴飞行器分为“+”型（如图1（a）所示）和“×”型（如图1（b）所示），本文采用的“×”型结构。

图1 四轴飞行器动力学模型Fig.1 Dynamic model of quad-rotor helicopter

四轴飞行器的三轴角度反映了它相对于地面的运动状态，其中：绕X轴旋转的Ф角为俯仰角；绕Y轴旋转的θ角为偏航角；绕Z轴旋转的Ψ角为滚动角。对障碍物的避开路径规划，主要是通过改变四轴飞行器的俯仰角、偏航角、滚动角，使四轴飞行器产生不同的飞行姿态从而实现自主避障飞行。

2 基于贝叶斯估计的视频图像分析

对四轴飞行器所处三维环境进行物理建模如图2 所示，以四轴飞行器所在的坐标系（xoy）为基准将空间分为无限个离散点。按照图中ABCD方向进行图片帧的划分，并依据后续的图像分析将这些离散点区分为障碍点和非障碍点。

根据四轴飞行器摄像头沿着飞行方向的扫描区域可以看出acdb所在区域的内部均为障碍点，而扫描区域内的其他点则为非障碍点；同时，非扫描区域的障碍物eghf区域内部也是障碍点。

图2 四轴飞行器飞行环境模型Fig.2 Flight environment model of quad-rotor helicopter

很多情况下，在进行障碍物检测时，需要对每一帧图像均进行障碍物的检测，从而使四轴飞行器可以更加精准地进行避障，因此四轴飞行器在飞行过程中处理数据的时效性是一个很重要的指标。贝叶斯估计提供了一种计算假设概率的方法，是基于假设的先验概率（将待估计的参数看作符合某种先验概率分布的随机变量）、给定假设下观察到不同数据的概率以及观察到的数据本身，因此考虑将贝叶斯估计运用到障碍物的检测中。

贝叶斯估计的过程如图3 所示：首先将待估计参数的概率默认为一个假设概率，即先验概率p(θ)；再求出该待估计参数依据样本集D的分布函数；之后利用贝叶斯公式计算出待估计参数的后验分布，进而计算出贝叶斯估计值。

图3 贝叶斯估计过程Fig.3 Bayesian estimation process

贝叶斯估计的本质是通过贝叶斯决策得到参数θ的最优估计，并使总期望风险最小。定义在样本X下的条件风险为：

利用四轴飞行器传输的视频流进行障碍检测，对于障碍物的分析采用背景差法，需要对四轴飞行器传输的图像进行逐帧分析做差分，进而识别出障碍物。由于视频流传输速度较快，每秒传输30 个图像帧，若逐帧对所有图片都进行分析会耗费大量的时间进而降低四轴飞行器避障的实时性。根据以上公式中对贝叶斯估计得出的最优估计，可有效进行四轴飞行器采集图像后的视频预处理。

应用贝叶斯估计的思想，首先对一帧图像进行障碍物分析。得出分析结果后，由贝叶斯估计可以得知，在后续的图片帧中，有一定数目的图片帧的处理结果和该图片大概率是一致的，所以跳过这些图片，减少识别图片的数目，进而减少识别时间。虽然在四轴飞行器运行过程中有小概率会有障碍物突然出现，而出现的时间恰好处在检测障碍物图片帧中间，导致该障碍物未被识别出，但是根据实际情况，突然出现的障碍物基本会存在两种可能：一是障碍物一闪即逝，并不会影响到避障进行；另一种则是障碍物出现时间超过1 s，则在后续的识别中也能被检测出来，并不会影响避障进程。

如图4 所示，基于贝叶斯估计的原理，当得到第一帧图片的分析结果时，则默认后续的i-1帧（49帧）图像的分析结果与第一帧一致，直到图片帧处理至第i+1 帧时，再次进行障碍分析。

图4 贝叶斯估计应用于图像帧分析Fig.4 Bayesian estimation used in image frame analysis

3 基于区域划分的像素点遍历算法

障碍物识别流程如图5 所示：在接收四轴飞行器传输的视频流后，对每一帧图像进行灰度化以及高斯滤波的处理；然后利用背景差法检测出障碍物，并将障碍物用填充色标记出来，得到识别出障碍物的图像信息。

图5 障碍物识别流程Fig.5 Obstacle identification process

充分利用四轴飞行器传输的视频流数据，将四轴飞行器起飞后的第一帧图像作为背景，下一帧图像与此背景进行背景差分来识别障碍物。当四轴飞行器运动到新的坐标点后，将新坐标点处的第一帧图像作为背景，下一帧图像与此背景进行背景差分。通过不断选取新坐标点处的背景，来提高障碍物识别的准确性，进而提高避障的精确度。

将分析出障碍物的图像（像素点个数：480×640）划分为50 个区域（5×10），如图6 所示。四轴飞行器当前的起飞位置位于图中的Φ区域，对该区域的像素点进行遍历，当发现存在标记的像素点时（在障碍物检测阶段，对检测出障碍物的像素点进行标记），默认该区域四轴飞行器不可通过。之后按照图中1→49 的区域编号顺序依次进行像素点的遍历，直到发现某一区域无标记的像素点，表明该区域可通过，四轴飞行器下一次运动的目标点坐标即为该区域的中心点。例如，四轴飞行器检测到当前区域（Ф）存在障碍点无法通过时，会扫描1号区域，若1 号区域仍旧存在障碍点，继续扫描2 号区域。若2号区域只存在非障碍点，则将该区域的中心点坐标转换后发送给四轴飞行器，使其进行下一步动作，若2 号区域仍旧不可通过，则继续向下扫描直至发现可通过区域。当扫描到49 区域仍为不可通过时，提示“NoWays！”，四轴飞行器降落。算法的执行过程如图7所示。

图6 图像帧划分为50个区域Fig.6 Image frame divided into 50 regions

图7 基于区域划分的像素点遍历算法流程Fig.7 Flowchart of pixel traversal algorithm based on region division

由于四轴飞行器所处坐标系和图像像素点处理的坐标系不同，所以在获得可通过区域的中心点坐标后，需要将该坐标转化为四轴飞行器所处坐标系的坐标。四轴飞行器根据转化后的坐标进行相应的移动，图像所处坐标系（xoy）以及四轴飞行器所处坐标系（XYZ）的关系如图8所示。其中：图片像素个数为480 × 640；height为所选区域上边界距离图片上边界的距离；width为所选区域左边界距离图片左边界的距离。

将可通过区域的中心坐标转化为四轴飞行器所在坐标系的坐标，转化后的坐标为：

其中：

若遍历所有区域后均未发现可通过区域，即“无路可飞”，提示“Noways！”，四轴飞行器自动降落。

图8 图像坐标系以及四轴飞行器坐标系Fig.8 Coordinate system of quad-rotor helicopter and coordinate system of image

4 实验结果与分析

4.1 软硬件平台

软件平台为python2.7开发环境，硬件平台为小型四轴飞行器Tello，如图9 所示。Tello 飞行器是睿炽科技自主研发的第一款无人机产品，其飞行控制系统由深圳市大疆创新公司授权使用，同时还搭载了英特尔高性能处理器。Tello 支持抛飞、全向翻滚等功能，具备红外定高、气压计定高、下视视觉等功能，携带500万像素摄像头，并可以进行720P高清图传。

图9 小型四轴飞行器TelloFig.9 Small quad-rotor helicopter Tello

4.2 结果分析

为了验证所提出的基于贝叶斯估计的图像预处理，以及基于区域划分的像素点遍历算法的有效性，在室内环境布置如图10所示的测试环境。

图10 飞行实验场景Fig.10 Flight experimental scene

四轴飞行器起飞后进入避障阶段，其飞行场景以及图像处理结果如图11 所示。其中：图11（a）为四轴飞行器起飞时所在的实际场景；图11（b）为四轴飞行器在执行飞行实验时其摄像头采集到的视频帧；图11（c）为系统终端显示的障碍物的识别结果，其中多个实心矩形框覆盖区域为识别出的障碍物区域。

当四轴飞行器避开第一组障碍物时，实验结果如图12 所示。其中：图12（a）为四轴飞行器避开第一组障碍物时的实景；图12（b）为四轴飞行器在执行飞行实验时其摄像头采集到的视频帧；图12（c）为系统终端显示的障碍物检测结果，其中所有的实心矩形框覆盖区域为识别出的障碍物区域。

图11 四轴飞行器实时飞行场景以及图像处理结果Fig.11 Real-time flight scene and image processing result of quad-rotor helicopter

图12 避障实验结果Fig.12 Experimental result of obstacle avoidance

四轴飞行器识别出障碍物之后，依据区域划分的像素点遍历算法将图片帧划分为50 个区域，按照图6 所示的顺序对每一个区域进行像素点的扫描。当遇到某一区域无填充像素时，终止扫描，将该区域的中心坐标点作为四轴飞行器下次运动的目的坐标；当四轴飞行器接收到降落命令或者“NoWays！”指令时，将四轴飞行器运行过程中的坐标点绘制在三维坐标图中，如图13所示。其中图13（a）和（b）为四轴飞行器在不同起飞位置进行实验，所实现的四轴飞行器避障过程的轨迹呈现。

图13 飞行轨迹Fig.13 Flight trajectories

图14 中实线路径为四轴飞行器实际飞行轨迹，虚线路径为理想的飞行轨迹，理想轨迹为综合了当时环境中存在的所有障碍情况及四轴飞行器的所在位置、相关距离等信息而生成的轨迹。两条轨迹中存在两处偏移：第一处偏移为理想轨迹判定该位置距离第二组障碍物的距离较近，应先向左上方避开，再直行，而实际运行轨迹中，四轴飞行器的摄像头未探测机体右侧信息，选择直行；第二处偏移为理想轨迹认为四轴飞行器正前方的床架可以直接通过，而实际运行轨迹中将床架检测为障碍物，选择向右上方偏移以避开此处。实际轨迹与理想轨迹产生偏差，一方面的原因是障碍物的识别精度问题，另一方面是由于四轴飞行器只依靠摄像头采集到的图像进行避障，无法有效采集到环境的深度信息，所以会将远方障碍物（无须避让）仍然视为避障目标，从而导致轨迹发生偏移。

分析四轴飞行器实验中的两组数据（理想轨迹与实际飞行轨迹坐标点间的距离，以像素点为单位），理想轨迹与实际飞行轨迹这两条轨迹的最大距离为25.6 cm，最小距离为0.2 cm。为了计算两条轨迹的拟合度，计算得到的方差值为194.61个像素点。由于图像中的点每偏移10个像素点，实际飞行器会偏移1 cm，所以实际飞行轨迹与理想轨迹之间的距离方差对应为19.461 cm。实验结果表明，本文提出的算法具有较好的效果。

图14 飞行轨迹与理想轨迹对比图Fig.14 Comparison of flight trajectory and ideal trajectory

在实验过程中，四轴飞行器飞行了24 s，每秒钟传输30帧图片，则总共传输图片720 帧，利用提出的基于贝叶斯估计的四轴飞行器避障算法进行取样，该实验过程中，总计处理图片14帧，处理时间为0.229 3 s。若未使用贝叶斯估计原理，则处理图片720 帧，需要处理时间11.464 3 s。处理不同数目的图片帧，图像预处理的时间差别非常大。图15 为处理每一帧图片所需时间；图16 为基于贝叶斯估计的图像预处理以及原算法（对所有视频帧进行处理）在处理不同数目的图片帧时的图片预处理时间对比。

由图15、16 可以看出，基于贝叶斯估计的图像预处理可有效节省图像帧处理时间，从而提升四轴飞行器的避障实时性，为四轴飞行器的实时自主避障飞行提供了保障。

图15 图片帧预处理执行时间测试Fig.15 Preprocessing time test of image frame

图16 两种算法处理不同数目的图片帧的效率对比Fig.16 Efficiency comparison of two algorithms for processing different numbers of image frames

5 结语

本文充分利用四轴飞行器所携带的摄像头，将其采集到的图像信息作为避障的依据，将视频流分解为一帧帧的图片后，对图片帧进行灰度化以及高斯滤波的处理，进而利用背景差分的方法来识别障碍物。背景差分中背景图的选取方式为四轴飞行器运动到新坐标点后的第一帧图像作为新的背景图，从而提高四轴飞行器识别障碍物的准确性。其间充分利用贝叶斯估计的原理简化图像预处理的过程，从而提高四轴飞行器避障的实时性。同时，将识别出障碍物的图片进行区域划分，利用像素点遍历的方式来判断某一区域是否存在障碍物：若不存在障碍物则将该区域的中心点坐标转化后发送给四轴飞行器；若存在障碍物则继续扫描下一个区域。扫描完50个区域仍未发现可通过区域时，则提示“NoWays！”，四轴飞行器降落，此次避障结束。四轴飞行器完成避障过程后，将四轴飞行器运动过程中的坐标点绘制在三维坐标系中，进行四轴飞行器避障轨迹的三维呈现。下一步考虑综合利用激光测距仪、双目摄像头、雷达等额外的传感器进行四轴飞行器的避障及路径规划。