0 引言

    随着科技的发展，作为智能制造与通用航空融合发展的四旋翼无人机受到了人们关注，其在民用与军事领域应用越来越广泛，如高空航拍、电力系统巡查、抵近侦查等^[1]。

    本文视觉定位系统是指四旋翼无人机在一定高度上机载图传设备，通过摄像头获取光流数据计算四旋翼无人机相对地面移动的位移，与高度、惯性数据融合后作出决策，自动调整自身的姿态与位置，使得四旋翼无人机可以在一定区域内悬停。

1 系统硬件组成

    本文四旋翼无人机控制器核心芯片采用意法半导体公司的STM32F407ZET6，该芯片内部有1 MB大容量Flash，具有低功耗、多功能的外设接口，其最大的优点在于具有浮点运算单元^[2]，提高了数据处理能力。

    四旋翼无人机用于姿态控制的惯性传感器采用MPU6050,其内部集成了三轴MEMS加速度计和三轴MEMS陀螺仪，三轴磁场计AK8975经过椭圆拟合后补偿磁场数据用于定向，两个传感器使用IIC与主控制器通信，将得到九轴数据进行姿态解算后得到横滚角、俯仰角、偏航角^[3]。

    为了能够实现稳定视觉定位，实验平台增加定高模式，选用HC-SR04超声波传感器模块，通过互补滤波融合惯性数据完成定高功能。

    由于需要用到许多复杂计算机视觉处理算法，而普通单片机处理数据能力有限，需要借助上位机来处理，所以选用了700线FPV摄像头采集图像，图像信息经5.8 G无线发射器传输到接收器，接收器通过数据采集卡传输到上位机。上位机图像处理后将数据用无线数传模块传输到四旋翼无人机控制器上进行视觉区域定位任务。四旋翼无人机视觉区域定位系统结构图如图1所示。

2 视觉区域定位算法

    本文采用了金字塔Lucas-Kanade稀疏光流的跟踪方法，该算法最早起源于昆虫运动引起视网膜变化^[4]。Lucas-Kanade稀疏光流基于以下3个假设：(1)亮度恒定；(2)运动是“小运动”；(3)空间一致^[5]。

    光流跟踪角点采用Shi和Tomasi方法，通过光流得到角点移动偏差量d存放在动态数组中，然后除以检测到角点数量得到一个平均值，动态数组使用完后将其内存释放，提高运行效率。

    在实际用光流跟踪点中，有一些点在跟踪过程中会发生很大变化或突然消失，这种情况会造成点的跟踪失败，前向与后向轨迹误差能够很好地去除误匹配点的情况^[7]。

    将得到前向后向特征点轨迹误差从小到大排列取中位值。当特征点轨迹误差大于中位值时，则把该特征点丢弃；当特征点轨迹误差小于等于中位值时，则保留该特征点。

    当四旋翼无人机在某一水平方向发生漂移时，可以发现该方向的两个电机转速降低，使得产生一个倾角，倾角越大导致该方向惯性器件的加速度值就越大。将滤波后惯性器件的加速度值积分成速度，作为PID速度内环反馈输入：

其中，a_x是惯性器件x方向加速度值，v_x是加速度积分后得到的速度值，t表示运行周期，为2 ms。y方向速度值同理得到。

    本文视觉定位算法结构框架如图2所示。期望水平偏差值d_{exp_x(y)}和期望水平速度值v_{exp_x(y)}都为0， PID控制算法全部采用双闭环位置式PID，内环与外环采用不同频率控制，位置式PID控制方法如下：

式中k_p、T_i、T_d分别为控制器比例、积分和微分参数。e(k)为差值，u(k)为PID控制器输出值。

    定位控制器融合多种数据，通过PWM(Pulse Width Modulation，PWM)输出至电机，融合方式以式(5)形式给出：

式中，M_i是电机输出PWM值，u_ultra是超声波定高PWM值，u_yaw是偏航角姿态控制PWM值，u_{x_out}是x方向位姿控制器PWM值，u_{y_out}是y方向位姿控制器PWM值。

3 实验与分析

    根据摄像机成像原理，可以推导出相似三角形公式为：当焦距f为定值，四旋翼无人机处在一定高度值h，则图像坐标系上角点移动距离x(y)与四旋翼无人机运动水平距离X(Y)是成正比关系的。

    图3为四旋翼无人机速度和高度值曲线图。将加速度积分后速度与超声波速度互补滤波后得到融合速度，作为PID内环反馈输入，2 ms控制一次，如图3(a)所示；超声波高度值作为PID外环反馈输入，50 ms控制一次，如图3(b)所示，四旋翼无人机高度误差在±5 cm范围内。

    为了减少定位过程中的误差，使用前向后向误差减少误匹配点，将四旋翼以一个y方向上下移动仿真飞行状态，如图4所示，使用前向后向误差校准后去除一些误匹配点。

    对使用前向后向误差的图4进行分析，发现图中有一些零点，表现为目标跟踪点丢失。使用卡尔曼滤波对光流数据进行状态估计，估计后数据变得非常平滑，如图5所示。

    四旋翼无人机飞到0.6 m左右高度切换到定高模式，然后调飞至目标物上方，如图6所示，飞稳后切换到视觉定位模式。

    图7为前后方向速度和光流偏差值曲线图。将机体坐标系上的x方向加速度积分后得到的速度值取负，作为PID内环反馈输入，2 ms控制1次，如图7(a)所示；图像坐标系上y方向光流偏移量经过卡尔曼滤波后，作为PID内环反馈输入，80 ms控制1次，如图7(b)所示。两者变化关系相反且共同控制前后方向。

    图8为左右方向速度和光流偏差值曲线图。将机体坐标系上y方向加速度积分后得到速度值取负，作为PID内环反馈输入，2 ms控制1次，如图8(a)所示；图像坐标系上x方向光流偏移量经过卡尔曼滤波后，作为PID外环反馈输入，80 ms控制1次，如图8(b)所示。两者变化关系相反且共同控制左右方向。

    由于用机载摄像头测出的是一种局部变化量，实验定位结果误差不能以本身实验测量值给出，所以本文选择在一块宽1 m、长1.5 m黑白棋布上飞行定位，且视觉定位区域在1 m2左右。

4 结论

    针对四旋翼无人机在无人控制时出现漂移情况，本文使用了金字塔Lucas-Kanade稀疏光流得到水平偏移量，将高度值和惯性数据融合进行视觉定位控制。实验取得了一定效果。但由于惯性单元存在零点漂移造成累积误差很难全部去除，图传和数传无线传输过程产生干扰等原因导致本实验平台很难在非常小范围内视觉定位，后续将优化状态估计等算法以进一步减小定位误差。

参考文献

[1] 段渭超.面向小型无人机的视觉高度测量和目标识别技术研究与FPGA优化[D].长沙：国防科学技术大学，2013.

[2] 江斌.小型无人低空四旋翼飞行器综合设计[D].杭州：浙江大学，2013.

[3] 吕印新，肖前贵，胡寿松.基于四元数互补滤波的无人机姿态解算[J].燕山大学学报，2014，38(2)；175-180.

[4] Fumiya Iida．Biologically inspired visual odometer for navigation of a flying robot[J]．Robotics and Autonomous Systems，2003，44(3)：201-208．

[5] 江志军，易华蓉.一种基于图像金字塔光流的特征跟踪方法[J].武汉大学学报(信息科学版)，2007，32(8)：680-683.

[6] 鲜斌，刘洋，张旭，等.基于视觉的小型四旋翼无人机自主飞行控制[J].机械工程学报，2015，51(9)：58-63.

[7] KALAL Z，MIKOLAJCZYK K，MATAS J.Forward-backward error：Automatic detection of tracking failures[C].Pattern Recognition(ICPR)，2010 20th International Conference on.IEEE，2010：2756-2759.

作者信息:

张  鹏，沈  捷，王  莉，任重庚

（南京工业大学 电气工程与控制科学学院，江苏 南京211816）