无GPS导航下基于光流的小型无人机导航算法

曾幼涵， 姜香菊

(兰州交通大学 自动化与电气工程学院，甘肃 兰州 730070)

0 引 言

目前飞行器的位置与速度的估计广泛采用全球定位系统(global positioning system,GPS)和惯性导航系统实现[1]，然而在建筑物内等无GPS信号情况下，很难实现无人飞行器的定位[7]。因此,进行飞行器无GPS信号的导航技术研究对于飞行器自主飞行有重要意义。

吕强等人[2]使用光流传感器辅助高精度惯性单元测得飞行器的速度，实现了飞行器悬停效果。方家豪等人[3]使用地面微基站系统实现了无人机在无GPS环境下的定位。杨天雨等人[4]通过光流传感器采集数据设计了基于无迹卡尔曼滤波(unscented Kalman filtering,UKF)的导航算法，实现了飞行器无GPS环境下的自主导航。

以上研究实现了在无GPS导航下的飞行器定位，但也存在相应不足：只能估计飞行器的速度无法得到飞行器的位置信息、需要已知飞行环境并提前在地面布置外部导航系统以及需要建立精确的系统模型，对处理器运算能力要求高，而小型飞行器由于受自身载荷有限，板载计算能力较弱，无法使用过于复杂算法。

本文引入一种基于光流与惯性测量单元(inertial measurement unit,IMU)的算法，将传感器采集的数据进行融合，在不需要已知飞行环境和外部定位系统的情况下，不仅可以获得飞行器的姿态、速度而且还可以估计位置信息。实验表明该方法可以在无GPS导航下实现对小型无人机进行有效控制，且具有精度高、运算量小、迭代简单的优点。

1 PX4FLOW光流传感器检测算法

1)采用SAD块匹配算法，并选择连续2帧图像中的最佳匹配块

(1)

假设fk(m,n)为第k幅图像中(m,n)处的像素灰度值，fk(m+i,n+j)为k+1图像中(m+1,n+1)处的像素值，m×n为搜索范围，当寻找到两块像素灰度差异最小后，即可认为t时刻(m,n)处像素点在t+Δt时刻运动(m+i,n+j)处，而(i,j)即为Δt时间像素点的偏移值。

2)旋转补偿

为去除由于摄像头的安装误差以及自旋产生的干扰，利用光流传感器测得的角速度ω和摄像头的焦距f对旋转的器件进行补偿，分别得到图像水平方向的光流分量vx，vy

(2)

式中T=[Tx,Ty,Tz]为平移变换矩阵,z为高度,x，y分别为x，y方向的像素偏移量。

3)尺度缩放

根据投影公式计算得到飞行器的水平移动速度vfx，vfy

(3)

式中z为飞行器的飞行高度。

2 融合策略

加速度计、陀螺仪、光流传感器有不同的频率响应特性，从频域对传感器数据进行融合处理，可以提升测量精度、减小估计误差。加速度计动态特性较差，在高频段易受干扰，产生高频噪声，陀螺仪和光流传感器在低频段动态响应特性较差，而高频段动态响应特性好，即短时间内的数据稳定可靠，但长时间数据易受感染，虽然陀螺仪加速度计和光流传感器都存干扰和误差，但在频域上的响应特性互补，通过3只传感器的在不同频域下动态响应的互相补充，可以得到整个频域范围内优秀的动态响应。

2.1 加速度计与陀螺仪的互补滤波器设计

通过加速度计与陀螺仪互补滤波，初步得到加速度与陀螺仪读数。具体流程如图1。

图1 互补滤波器的算法流程

加速度计信号首先经过低通滤波器消除高频噪声，然后，对得到的结果进行归一化，由重力向量与加速度计向量垂直分量叉乘，得到姿态误差值。

误差值经过比例—积分(proportional integral,PI)控制器，得到角速度修正值，与陀螺仪测量值经过高通滤波器后的预测值进行叠加，得到修正后的角速度值，根据一阶龙格库塔方法求解一阶微分方程，可以求出四元数的值，从而得到飞行器姿态。

2.2 光流传感器对加速度校正

加速度通过积分获得的速度可以用光流测得的速度来校正[5,6]。校正后的加速度通过积分可以得到飞行器的位移。光流校正结构如图2所示。

图2 光流校正结构

该算法的数学表达式可以表达为

(4)

综上，设计的滤波器可以得到飞行器的姿态、速度、位移，通过这些信息可以对飞行器进行完全控制。

3 实验结果分析

本文采用自主搭建的四旋翼飞行器硬件平台进行测试，传感器安装位置如图3所示，用于对比光流和惯性单元融合的数据算法的精确度。

图3 光流传感器安装位置

在室外环境光线良好的条件下进行定点悬停实验，实验中首先用遥控器控制飞行器起飞并抵达指定位置，然后发送指令，飞行器进入自主悬停程序，悬停高度1 m,时间2 min。测试和对比惯性导航算法和光流融合算法的速度估计数据和位置估计信息。

使用SD卡记录传感器数据，将光流融合算法结果与惯性导航算法结果进行对比。

读取惯性导航算法的速度信息与光流融合算法得到的速度信息，如图4所示。可以看出，悬停实验条件下，随着时间的推移通过惯性导航算法得到的速度估计曲线已经出现严重的发散现象，而通过光流融合算法得出的速度估计曲线依然可以得到高精确度的飞行器速度数据。

图4 惯性导航与光流融合的速度估计

因为惯性导航算法速度估计产生严重的误差，导致位置估计误差发散的更快，因此,未给出惯性导航的位置估计图形。融合算法得到的飞行器位置与目标位置如图5所示，可以看出当飞行器进入悬停状态，飞行器一直稳定在期望位置附近，可以实现0.5 m以内的精确的定点飞行效果。

图5 水平位置估计值与期望值对比

4 结束语

本文提出一种四旋翼飞行器水平方向上速度与位置信息的测量方法，该算法可以在无GPS条件下，不需要外部辅助定位系统，不需要已知飞行环境，不需要添加激光雷达等昂贵的传感器，仅需要光流传感器和IMU模块的数据就可以实现飞行器速度和位置的精确估计，该算法迭代简单，很适合搭载微型处理器，自身载荷有限的小型无人机平台。实验结果表明：光流与IMU的数据算法,能够得到精确的位置和速度信息，为小型飞行器的自主控制提供精确的导航参数，克服了传统微惯性导航系统对GPS的严重依赖。