基于视觉处理的四旋翼无人机自主导航系统设计

张皓焱，刘 新

（郑州大学国际学院，河南 郑州 450000）

在室内或者偏远地区，全球定位系统（G l o b a l Positioning System，GPS）信号弱甚至搜星失败，无人机难以定位，容易造成失控进而引发安全事故。

基于视觉处理的无人机自主定位导航系统，弥补了传统定位方式的缺陷，在无GPS信号下也能稳定、高效运行。通过飞行实验来验证常用视觉算法的性能，实验中使用树莓派单独处理视觉模块的数据流，使用C2000芯片进行姿态解算、位置融合和动力分配，实现了视觉处理实验平台与飞行控制实验平台的分离。同时采用Open Vision Studio上位机软件作为地面监视站，直接观测视觉处理实验平台中的图像信息、位姿信息及传感器状态信息，可实时调整实验平台的参数。

文章将从视觉信息的获取、处理、封装交付以及飞控算法等方面叙述并验证视觉导航系统的工作原理。

1 四旋翼无人机导航系统验证平台构建

1.1 视觉验证平台设计

视觉处理平台的硬件核心是一张树莓派CM3计算模块，运行Ubuntu Linux操作系统。该计算模块具有大量接口，可以根据外设的通信协议建立通信。视觉处理平台采用基于SONY IMX9的摄像头，支持1 080 p以及最高60 fps录制。为保证图像处理的实时性和准确性，仅裁切画面中心320×240部分作为原始图像信息。同时，设计了用于搭载树莓派CM3计算模块的拓展板，包含STM32协处理器、US-100超声波测距单元以及平行于地面的相机感光平面[1]。

1.2 飞控验证平台设计

选用Ti公司的C2000 F28379D双核LaunchPad开发板作为本验证平台的飞控硬件。对于所需验证平台而言，单核性能足以满足全部实验的运算要求，双中央处理器（Central Processing Unit，CPU）则可实现系统冗余设计，保障实验安全。内部运行uC/OS-II操作系统，结合基于任务优先级的抢占式调度，便于飞控系统的构建[2]。

由于开发板外设不足以完成飞控系统所需硬件要求，文章设计了集成惯性传感器、气压高度计以及蓝牙通信等模块的飞控扩展板，同时，集成蓝牙通信功能，便于实验时实时调参，其资源包含HC-06蓝牙串口透传单元、MPU-6050惯性测量单元、MS5611气压高度测量单元以及含三轴数字罗盘仪和三轴数字加速度仪的LSM303D测量单元。

1.3 “视觉+飞控”导航系统验证平台设计

视觉定位导航算法的实现基于Python-OpenCV开发环境。树莓派CM3计算模块通过拓展板相机采集图像信息进行处理并传送至STM32协处理器，同时超声波测距单元的高度信息也将被传至STM32协处理器。STM32协处理器将两者信息整合打包上传至飞控。飞控还将获得来自飞控扩展板传输的姿态、加速度和磁偏角等数据信息以及从气压计获取的高度数据信息，通过一系列算法处理两路数据实现位置控制[1]。

2 视觉信息的获取

以索尼IMX9作为无人机的下视摄像头获取视觉信息。视觉模块启动时需要初始化串口与摄像头，设置输出图像的尺寸和帧率，同时，还需对其采集到的图像进行预处理，既要保证图像质量，又要保证图像处理的实时性。常见的预处理方法有像素变换和图像增强两种：

（1）像素变换，有两种变换方式。第一，类型变换。图像的类型不同，信息量也不同，所以对RGB图像进行灰度处理或二值处理，可以有效地压缩所图像大小，减轻树莓派工作负担。第二，降低像素，对采集到的图像进行剪裁。以图像的中心为基准向其四周各取适量的像素点数，得到一帧低分辨率的图像。这种保留图像中心并丢弃图像边缘的方法能明显减少处理器的运算量，提高图像处理的实时性。

（2）图像增强，即提高图像质量。图像易受机身抖动和噪声等因素的干扰，文章采用中值滤波法[3]降噪，将每一像素点的灰度值设置为该点某邻域窗口内所有像素点灰度值的中值，消除噪点。有时还需要运用Canny边缘检测法，根据图像灰度值的不连续性获取边缘信息。若图像中不同像素位置的明暗程度差距较大，则通过腐蚀和膨胀[4]进行优化，微调亮暗对比度，消除噪声，寻找图像中的极大值区域和极小值区域，以达到优化的目的。总之，图像增强是提取视觉信息的必要手段。

3 视觉信息的处理

3.1 无人机视觉导航算法概述

根据提取到的视觉信息，树莓派利用OpenCV所提供的强大接口对图像进行处理，识别目标物，计算当前无人机的位置和速度等状态，指定期望轨迹和期望速度，然后对无人机飞行控制模块发出相应指令，完成不同的飞行任务。常用的视觉算法有Blob检测算法、霍夫变换算法、循迹算法、稀疏光流算法等。

3.2 目标物识别算法—以Blob为例

Blob意为斑点，是图像中具有相似颜色和纹理特征的一块连通区域。连通域中各像素点具有相近的灰度值，然而和连通域之外的像素点却有明显的颜色差异和灰度差异。因为斑点代表的是一个区域，所以相对于单纯的角点来说具有更好的稳定性和抗干扰能力。

Blob检测通过几何分析对图像中的连通区域进行识别，并标记一些重要的几何特征，通常包括区域的面积、中心点坐标、质心坐标、最小外接矩形和主轴等，还可以提供图像中斑点的数量、位置、形状和方向以及斑点间的拓扑结构[5]。通过视觉模块将斑点的像素位置信息上传至飞控模块，可以引导四旋翼无人机的姿态控制，实现捕获目标、定点悬停、目标追踪以及自主降落等功能，如图1所示。Blob连通域检测算法的实现如下：

（1）设定阈值范围[Min, Max]，步长为s，通过一系列连续的阈值Min，Min+s，Min+2s，…，Max，把输入的灰度图像转换为一个二值图像的集合。（2）分别检测每一幅二值图像的边界，提取对应每幅图像的连通域，再根据预设的颜色、面积、形状等限定条件进一步过滤，得到初步的连通域集合。（3）使用findContours（）查找图像的边，并计算每一个轮廓的中心，将得到的每幅图像的中心点放在一起。定义一个最小距离，在这个距离区域内的特征中心点被归为一个group, 对应一个Blob特征，得到特征点集合。（4）确定灰度图像中的连通域位置和尺寸，估计Blob特征和相应的半径，并以keypoints返回。（5）将得到的连通域中心坐标交付给飞控芯片进行无人机位置控制。

图1 Blob检测算法实现运动小车的实时追踪

3.3 霍夫变换

无人机图像轮廓检测的主要目的是从图像中识别并提取出关键信息，最常用的基本方法是检测出直线和圆。由于大多数飞行场地的地面引导线在航拍图像中都表现为成对出现的、近似直线的边缘并横贯整幅图像，所以可以通过设定直线长度和倾斜角阈值的方法对直线进行筛选，从中获取目标物的位置信息，引导无人机巡航[6-7]。

文章使用OpenCV的HoughLinesP（）检测直线，首先，传入经阈值分割或边缘检测后的二值图，对检测参数进行配置。已知直线极坐形式的曲线方程为rθ=x0×cosθ+y0×sinθ，则需分别确定距离r和角度θ的精度，r值越大、θ值越小，考虑的线就越多。其次，确定累加数阈值，其值越小，考虑的线就越多。还可以根据飞行任务的需求灵活地配置最短长度阈值、最大间隔这两个可选参数，前者是长度的下限，比它短的线段会被剔除；后者指定两直线间的最小距离，间隔小于这一值的两条直线会被视作是一条线。被检测出的直线以首尾两个端点坐标的集合形式存储。通过对集合中各直线长度和斜率的筛选，可以得到符合要求的直线。根据检测的结果还可以进一步计算出两条霍夫线的交点，将交点坐标交付至飞控芯片，可以完成交叉点的识别和循迹直角转弯等飞行任务。

经典霍夫变换还可以用来检测圆和椭圆。霍夫圆变换与直线变换类似，无非是直线用（r，θ）表示，圆用（x，y）表示，从二维变成了三维，数据量变大了很多，所以一般使用霍夫梯度法减少计算量[8]。

OpenCV中的HoughCircles（）可以实现对圆形目标物的检测，首先，确定利用霍夫梯度法进行变换，累加器图像分辨率与原图一致；其次，设置两个不同圆圆心的最短距离，并确定累加数阈值，同霍夫直线检测，累加数阈值越小，考虑的圆就越多。该算法返回的是被检测圆的圆心坐标与半径大小，根据半径对结果集合进行筛选，取出目标圆，然后将坐标转化为飞控可以利用的数据。霍夫圆检测算法与Blob检测算法不同：霍夫圆检测算法能够检测非连通域的空心圆形，所以在圆形目标物的检测上，霍夫圆检测算法可以与Blob检测算法互补，以适应不同类型圆形目标物的识别，实现圆形目标的定点悬停和定点追踪，如图2所示。

图2 霍夫圆检测算法实现定点悬停

3.4 循迹飞行

循迹是指依照给定的赛道轨迹，使用摄像头拍摄赛道图片，并采用图像处理算法提取有用的信息，来引导无人机飞行。在实际的测试应用中，地面引导线多为黑色直线或者弧线，与背景有着明显的特征区分。文章通过OpenCV视觉模块采集分辨率为160×120的赛道图像，在捕获到的黑线图像中选取左、中、右3个感兴趣区域进行分割，分别将其标记为区域1、区域2、区域3，区域分割示意如图3所示。

图3 图像分割示意

经图像分割，原图的左、中、右3个感兴趣区域分别对应3幅掩模图像，然后对每幅掩模图进行灰度处理，再根据预设的阈值将3幅灰度图像二值化，进而利用OpenCV中的FindCounters（）函数分别提取各掩模图像中的最大矩形轮廓，得到的轮廓即是该幅掩模图像中被截断的黑线片段，再将这3幅掩模图像依照左、中、右的顺序分别拼接回原图对应的位置，即可得到如图4所示的航拍效果—原图中贯穿整张图像的黑线被划分为3个独立不相连的条状矩形。同时利用OpenCV的minAreaRect（）提取区域2中矩形的中心坐标以及区域1、区域2中矩形的长边与x轴的夹角α1和α2。

图4 循迹航拍

首先，循迹需要满足无人机在飞行过程中始终位于黑线上方并且机头方向与黑线的切线方向重合，所以区域2中矩形的中心应恰好位于图像中心；其次，还要将两矩形的长边与x轴的夹角α1和α2做差以求得目标航向角α=α1－α2。将目标航向角和区域2中矩形的质心坐标上传至飞控芯片，即可控制无人机循迹飞行，循迹算法的步骤如下：

（1）分割图像，将原图剪裁成3幅等分辨率的掩模图。

（2）进行图像预处理，利用inRange函数，依照预设的双边阈值将各掩模图黑白二值化。

（3）提取各掩模图中的黑色长条状矩形轮廓，即被切割分段的黑线。

（4）计算三幅图像中各黑线的位置信息，包括质心点坐标、矩形的长边与x轴的夹角。

（5）计算期望偏航角，将区域1和区域2中两段黑线的角度做差，差值作为期望偏航角，差值的正负分别对应航向上的左右。

（6）纠正横向位置，根据区域2黑线的质心坐标纠正无人机在横向上的左右位置。

（7）将偏航角和横向位置数据交付至飞控芯片。

3.5 光流控速

3.5.1 光流控速算法概述

四旋翼无人机采用基于图像金字塔的稀疏光流（Lucas-Kanade，LK）方法计算某些点集的光流，通过计算关联特征点在序列图像中像素的位置变化求解速度信息。首先，检测出一帧图像中的特征点并保存其坐标，进而跟踪特征点，获取它在下一帧图形中的像素位置信息，将两帧图像的特征点坐标作差，即可得知两帧图像之间对应特征点越过的像素点数；其次，根据摄像头的帧率计算出无人机的速度信息，将其交付至飞控芯片，控制无人机速度，维持稳定的飞行状态。所谓的特征点即指角点，OpenCV提供了可以获取准确角点位置的goodFeaturesToTrack（）函数，以获取图像中的角点集合。在循迹飞行时检测到的特征点如图5所示。

图5 特征点检测航拍

3.5.2 特征点处理

利用OpenCV的接口calcOpticalFlowPyrLK（）逐帧跟踪检测到的特征点。考虑到图像处理的实时性需求，LK光流算法不必对每帧图都像进行特征点检测。由于图像帧的实时更新，一部分特征点随着无人机的位置变化而丢失，另一部分特征点则仍存在于接下来的若干帧图像中。例如无人机在定点悬停时，其位姿变化的幅度小，特征点的更新速率较低，那么就无需重新进行特征点的检测，继续对尚存的特征点进行跟踪即可。

3.5.3 基于特征点检测的金字塔LK光流算法

首先，设置阈值，在特征点检测前比较剩余特征点数量与阈值的大小，若剩余特征点数目大于阈值，则继续进行光流跟踪，否则重新检测特征点[9]。其次，提取当前帧的特征点坐标，并作为初始值，通过LK光流法对其下一时刻图像上的坐标进行估计，再经误差补偿对LK光流估计的误差进行修正，从而准确估计特征点光流[1]，求得无人机的速度信息。其主要步骤如下：

（1）采集t时刻图像It，对其灰度处理得灰度图Gt。

（2）利用特征点检测算法，获取Gt得特征点集pt。

（3）采集t+1时刻图像It+1，对其灰度处理得灰度图Gt+1。

（4）将Gt，Gt+1，pt作为输入，采用金字塔LK光流算法估算Gt+1图像上特征点坐标pt+1。

（5）将Gt+1，Gt，pt+1作为输入，利用（4）中步骤计算Gt图像特征点坐标^pt。

（6）计算FB误差，并以误差的中值作为阈值，将FB误差大于阈值的特征点删除，计算特征点的位移光流。

（7）将无人机速度信息交付至飞控芯片，进行速度控制。

3.6 多核并发视觉处理

3.6.1 平台优势

出于科研及竞赛的各种需求，无人机在执行循迹算法平稳飞行的过程中，往往还需要不断地识别目标物，执行光流控速、Blob探测等任务以实现飞行控制，仅靠单一的循迹进程难以自主飞行。

视觉模块基于BCM2837四核ARM处理器，能实现多核并发运算，除去打包上传视觉数据占用的一个核，剩余的3个核都可以写入不同的视觉处理进程，各个核并发运行，充分地利用多核CPU的资源，增强图像处理的实时性，提高视觉模块的计算效率。同时Python提供了优秀的多进程模块—multiprocessing，支持子进程通信、子进程之间的数据共享、不同视觉处理进程的并发执行等，提供了Process，Queue，Pipe，Lock等组件，可以轻松完成从单进程到多进程并发执行的转换[10]。

3.6.2 多核处理算法

以光流算法与Blob算法的并发执行为例，实现主进程执行Blob检测算法，子进程执行光流算法的多核并发处理。在飞控中光流信息可用于速度环控制，同时Blob算法可用于位置环控制。多核处理的实现如下：

（1）初始化串口与摄像头。

（2）主进程创建光流进程。

（3）在主进程中创建图像数据队列frameQueue、光流速度信息队列flowX，flowY。

（4）主进程中初始化Blob相关参数。

（5）光流进程中初始化光流。

（6）在主进程中进入死循环。

（7）在死循环中获取图像，塞入图像队列。

（8）分别在主进程的循环中处理Blob，在光流进程中处理光流算法，最终在主进程中获取光流进程的结果。

其中frameQueue为图像队列，用于两进程之间的数据交换，其长度为1，方向为主进程压入，光流进程取出；flowX，flowY分别为光流算法中x轴速度与y轴速度的存储队列，长度为1，与frameQueue的存取方向相反，即从光流进程压入，从主进程取出。算法执行时，在主进程的死循环中不断获取图像帧并将其压入队列frameQueue，然后主进程继续处理的Blob算法；光流进程从frameQueue队列中获取图像帧，由于队列长度为1，每当取出一帧图像，队列变空，故无需手动清理队列中已被使用的图像帧。光流进程计算结束后，向flowX，flowY两个队列中压入光流算法的当次计算结果值xSpeed，ySpeed，再由主进程中接收。周而复始，视觉模块可以实时地将两进程处理完毕的目标物位置坐标和无人机速度信息同时上传至飞控芯片。

4 视觉信息的封装与交付

树莓派实时地将数据打包并传送至STM32，再由STM32将超声波模块的高度信息整合到数据包中，形成完整的信令进而交付至飞控。这一系列的数据传送需要统一的信令格式：将包头和包尾作为定界符，进行差错检测并丢弃错误的数据包；功能位（MODE）表示期望飞行模式，包括定点悬停模式、循迹模式、识别模式等；超声波高度数据占用尾部4个字节，其余字节在不同的飞行模式下被赋予不同的意义，如目标点坐标、偏航角度和速度等。对树莓派中运行的可执行文件的编写，可以实现由下位机间接控制上位机更改视觉处理模式。

数据交付至飞控之后，飞控通过产生串口中断的方式接收数据，由Ti芯片的接收端寄存器RX中的先入先出队列（First Input First Output，FIFO）来触发中断。凡是从STM32上传至飞控的数据全部由FIFO接收，满16个字节（3字节包头包尾+9字节视觉数据+4字节超声波数据）触发一次中断并加以检查。当FIFO缓冲区满16个字节即产生中断而后抛出信号量，触发飞控主函数中的下位机数据接收任务：取出视觉数据、清除FIFO的中断标志位等信息以及将信号量置零，从而完成数据接收的任务。

5 基于视觉处理的无人机飞行控制

5.1 视觉位置数据的解算

为满足进一步的飞行控制，需要从视觉数据中解算出期望轨迹，期望轨迹是一系列目标位置的点集，目标位置的确定则是基于相机对目标的捕捉，得到其在相机坐标系X’O’Y’中的位置，而目标实际位置是在世界坐标系XOY下。根据针孔成像原理[11]，假设空间内任何一点都成像于距离摄像机光心垂直距离一倍焦距f的平面处，并且由超声波模块已获取当前无人机对地高度h，由相似三角形法可以得出空间中一点P距无人机的水平距离L，目标定位示意如图6所示。

图6 针孔成像原理

空间中一点P在相机中成像于P’，P’ 距相机中心点O’的距离为L’，而无人机距P的实际距离L，那么L与L’的关系为：

以定点跟踪为例，如果规定无人机需要识别目标物并始终悬停在目标物的正上方，则要确保当前图像帧中目标物的质心必须与图像中心重合，即根据功能位Bytes[2]得知当前无人机执行定点模式，飞机需要迅速调整位置，移动到目标物的中心点的坐标位置处。假设当前图像分辨率是160×120，那么中心点的坐标是（80，60），所以通过计算x-80，y-60可知连通域中心偏离了相机中心的像素点数，然后将偏差像素点数与单个像素点的尺寸长度相乘，转换为对应的长度（本机摄像头1像素≈0.571 cm）。再根据针孔成像原理求得无人机实际偏离目标点的距离，最后将数据交付至飞控芯片做进一步处理。

5.2 飞行控制模型概述

飞控将获得两路信息，一路是来自STM32整合后的视觉位置信息数据，另一路是来自飞控扩展版上惯性测量单元（Inertial Measurement Unit，IMU）的姿态、加速度和磁偏角等数据。由上文所述，飞控根据针孔成像原理，从视觉模块数据中求解期望轨迹，那么期望轨迹与实际轨迹的差值即为轨迹误差，视其为位置观测量，IMU获取加速度观测量，由预测模型对当前的位置数据和加速度数据进行融合，得到修正后的位置数据，视其为预测数据。调节卡尔曼增益，融合观测数据与预测数据，进行飞行控制。飞控模型示意如图7所示，其中飞行控制大致分为4步：

（1）位置控制，将融合结果作为输入量，由位置模块进行处理。在位置控制中，数据先后经过高度环控制、速度环控制、加速度环控制，最后将加速度环输出的加速度与重力加速度叠加，得到期望加速度。依照控制模型，将加速度在轴上的分量分别转换为目标翻滚角（roll）、目标俯仰角（pitch）、目标偏航角（yaw），最后将三轴的期望姿态角叠加后输出最终期望姿态。

（2）姿态控制，将期望姿态作为输入量，输入到姿态控制模块，根据机身的期望姿态角度计算出期望力矩。

（3）控制分配，将期望力矩作为输入量输入到控制模块中进行控制分配，将期望力矩的效果分配到四旋翼无人机的4个电机上，输出期望升力F1，F2，F3，F4。

（4）电机控制，分别将各个电机的目标升力转换成对应的油门百分比[即脉宽调制（Pulse Width Modulation，PWM）占空比]。

图7 四旋翼飞行控制框架

5.3 无人机姿态控制基础

为了定量描述定位导航系统验证平台的姿态信息，必须建立以地面为参考的四旋翼无人机的坐标系。现确定以当地地理位置为参照，以东、北、天所对应方向为XYZ轴的正方向建立导航坐标系。以四旋翼无人机为参照，以前、右、上所对应方向为XYZ轴的正方形建立无人机坐标系。

四元数法具有运算量小、不会出现万向节锁等优点，可进行全姿态解算。本文研究采用四元数法求解无人机姿态。在初始状态下可以获得准确的姿态角和初始矩阵。由于实际的角速度测量中存在零偏等误差，更新四元数数据也会有漂移发生，因而使用卡尔曼滤波进行矫正，初始四元数值不必过于精准[12]。

5.4 位姿控制器设计

通过针孔模型以及姿态解算求解到期望角度后，设计位姿控制器实现目标定位导航。为了确保在定位导航中保持机头前进方向始终对准兴趣点方向，采用偏航角与俯仰角和翻滚角分离的控制器设计，确保定位兴趣点始终在画面像素中心点附近。

5.4.1 偏航角控制器

偏航角控制器采用单级闭环比例—积分—导数（Proportion Integral Derivative，PID）控制。期望偏航角Ye1与无人机当前位置Yo1做差得到PID的输入值。

通过控制对角线上两对电机的速度得到期望力矩，在PID调节基础上增加uy(0)项确保无人机向上的总拉力始终不变，无人机平面始终在同一高度平面。

5.4.2 俯仰角和翻滚角控制器

为了准确控制位姿并提高位姿控制的实时性，俯仰角和翻滚角控制器采用内外环串级PID控制。将期望角度[xe，ye，ze]与无人机当前位置[xo，yo，zo]做差，得到外环PID的输入值。外环控制中：

e(k)是指期望角度值和当前（度值的误差，dt为控制周期，为比例系数，Ki1为积分系数，Kd1为微分系数。在控制周期较短的情况下，外环输出值X(k)是角度关于时间t的微分，即角速度。对于内环PID控制，期望角速度为0。将外环PID总输出[ωout1，ωout2，ωout3]与当前角速度[ωo1，ωo2，ωo3]做差，得到内环PID的输入值：

up(0)为电机调速的基础量，推算出的各个电机期望拉力可以对电机既定的油门动力曲线通过up(0)进行修正，提升无人机控制实时性和稳定性。

以上控制器可以得到各个电机调速控制量，使兴趣点始终位于画面中心附近，机头方向始终与兴趣点移动方向一致，确保无人机能稳定、高效地追踪感兴趣点，提高了定位导航系统的响应速度和可靠性。

6 结语

文章提出了一种对图像数据进行像素变换、图像增强和计算机视觉化算法方案，自主设计了以PID为控制器基本控制原理的四旋翼无人机验证平台。在视觉处理核心硬件部分（即树莓派）附加STM32协同处理，在减轻视觉处理核心的处理器负载的同时提升了视觉处理的实时性和同步性。在俯仰角和翻滚角控制器中，外环使用角度作为受控对象，内环使用角速度作为受控对象，偏航角控制器使用偏航角偏差作为受控对象，从而实现定方向快速精准位置移动。经验证，在室内无GPS条件下，文章研究方案的定位精确度高，定点直径为20 cm，并能稳定追踪速度5 m/s以下物体，优于现有算法。