基于光流传感器的旋翼无人机实时避障系统

余超凡 孙建辉

(浙江工业大学机械工程学院 浙江 杭州 310000)

0 引 言

近年来，在民用领域中，无人机发挥着越来越重要的作用。无人机因其便于使用与功能强大等特点，已从昔日的玩具升格为多用途的科技产品。但是，无人机却总会存在一系列的安全性问题。因此，无人机想要持续发展，除了其续航与功能的不断完善外，还需考虑其安全性能。"避障系统"目前是提升无人机安全性能必不可少的一个环节,也是无人机智能化的一个显著标志。目前可以使用声呐、红外、激光测距与计算机视等方法检测障碍物。

(1) 基于超声波传感器的避障策略 超声波传感器广泛的应用于许多场,如测距仪、倒车雷达等。但是，超声波为机械波，若在多旋翼飞行器上使用，极易受到干扰，从而导致测量精度不高，甚至产生大量野值点的问题。

(2) 基于激光雷达的避障策略 该方法采用激光雷达传感器获取飞行器周边的环境信息，可以获得飞行器与障碍物的相对距离、角度等信息。其优点是精度高，但是价格较高且极易受到强光干扰。

(3) 基于视觉传感器的避障策略 该方法可以通过两个光流传感器获取丰富的图像信息，且精度高，测量束角大，但是当障碍物位于两个光流传感器的正中间时，飞行器检测到的两侧的光流值便会相等，该方法便会失效。

因此，本文采用一个光流传感器实时获取飞行器当前的环境信息后，通过改进后的人工势场法来实现飞行器自主避障的实时路径规划。再与飞行器的飞行控制系统综合完成旋翼飞行器的自主避障飞行。

1 光流法

1.1 光流算法

本文在两幅连续帧之间采用SAD(sum of absolute differences)算法计算光流。将两幅图像的SAD值后与给定的SAD值进行比较，提取出最匹配的图像，从而得出两幅图像之间的光流值。在两幅最匹配的图像之间采用提出的双线性插值法I2A(interpolation algorithm)在亚像素精度上计算最终的光流值。

I2A算法在一个较小的时间差Δt内计算两幅连续的图像的位移s。比率s/Δt(pixel/s)与实际的光流(°/s)成正比。

(1)

(2)

令：

(3)

(4)

在本文中,将k设为半帧的大小，从而改善光流法的精度。

1.2 传感器数据融合

本质上,通过添加角速度信息,SFM(structure from motion)算法的性能会得到改善。在文献[3]中,提出IMU信息可以有效地提高运动物体追踪的抗干扰能力。他们同样展示了当获得IMU数据时,只要少量的特征点信息便可很好的恢复相机的运动情况。

飞行器的平动和转动都会产生光流向量，根据文献[4]可知,深度信息只与光流的平动部分有关。因此，本文将使用KF融合光流数据与IMU数据的策略来预测实际因飞行器平动而产生的光流值和飞行器的角速度信息,从而提高飞行器的避障性能和鲁棒性。

本文中卡尔曼滤波器的状态向量为X=(dx,dy)T∈R2,其动力学模型可以描述为：

Xk+1=AXk+αk

(5)

Yk=dmCXk+βk

(6)

此处,αk是状态噪声向量,预测的误差协方差矩阵为Qα。Yk∈R2是测量向量,QB是测量误差的协方差矩阵。A∈R2,C∈R2,两个都是单位矩阵。

图1 点Q=(X,Y,Z)由通过投影中心的光线投影到图像平面上,相应的图像点为q=(x,y,z)

如图1所示，点Q(X,Y)由通过投影中心的光线投影到图像平面上，相应的图像点为q(x,y)。几何转换方程为：

(7)

通过对式(7)求导操作，我们可以通过图像坐标系(xi,yi),机体速度、角速度(Vx,Vy,Vz,Ωx,Ωy,Ωz)与深度信息Zi来表达光流的大小[5]：

(8)

式(8)为卡尔曼滤波器的动力学模型，通过式(8),得出测量方程：

(9)

使用该融合方法对px4flow光流传感器进行二次开发，最后，图像传感器得到的数据通过串口发送给上位机进行显示，分别如图2-图4所示。

图2 融合处理后因旋转产生的光流值

图3 图2中去掉旋转部分

图4 融合处理后因平动产生的光流

其中，图2中的波动较大的曲线的为角速度，单位为rad/s，图2波动小的曲线、图3的曲线和图4中的曲线为光流值，单位为m/s,三幅图的横坐标都为当地时间。

由于飞行器pitch和roll方向最大转动角度限制在±35°，图2为将光流传感器来回旋转一定角度(±30°)产生的角速度与光流值信息。图3表明因旋转产生的光流值，该值较小。图4为光流传感器平动产生的光流值，该值较大。因此，图2-图4表明使用KF将IMU数据与光流值进行融合，可以有效过滤掉部分因旋转产生的光流值，从而能够预测实际因飞行器平动而产生的光流值，因此可以作为障碍物的检测方法。

2 基于光流的改进的人工势场法及控制算法

2.1 双目光流避障策略

当无人机进入避障模式后,可以将无人机从左右两侧的图像传感器获得的光流值取绝对值,再将这两个绝对值相减做差,从而决定无人机的为躲避障碍物机身需倾斜的方向：

光流绝对值差=|左侧光流|-|右侧光流|

假设当障碍物靠近飞行器左侧时为正，反之为负。因此，当无人机两侧的光流值的绝对值的差为正时,表明障碍物靠近飞行器左侧,无人机机身需向右倾斜一定角度来躲避障碍物。反之较靠近右侧,无人机机身需向左倾斜一定角度来躲避障碍物。

但是，虽然该避障策略对于飞行器避障有较好的效果，但其不能同时考虑目标的位置。这样没有目的性的导航会使飞行器在避障后的航线与初始航向产生较大的偏离。

2.2 人工势场法的原理

目标点对飞行器有引力作用，距离越远，引力越大：障碍物对飞行器有斥力作用，距离越近，斥力越大。因此，可得飞行器的引力、斥力势[6]：

(10)

(11)

其中，katt、katt——引力、斥力场比例系数；

X——飞行器当前位置；

Xtar——目标位置；

d——飞行器与障碍物之间的距离；

d0——障碍物的作用半径。

由式(10)可得引力函数：

Fatt(d)=-▽Uatt(d)=-katt|X-Xtar|

(12)

斥力函数为：

Frep(d)= -▽Urep(d)=

(13)

飞行器收到的合力F=Fatt+Frep决定了飞行器的运动方向。

2.3 改进后的人工势场法

由于传统人工势场有局部极小点问题，即在某些特殊位置的障碍物会使飞行器所受合力为0，此时飞行器将不能到达指定目的地而在中途停止；需要获取较详细的障碍物信息[7]。因此需要对传统的势场法进行改进。

飞行器人工势场法的避障是通过调整无人机的姿态角度来实现的，但四旋翼飞行器pitch、roll和yaw存在耦合性，其横滚角、俯仰角和偏航角的关系[8]：

(14)

其中，q,r为机体角速度ω在机体坐标系oy,oz上的分量。φ，θ，ψ分别为横滚角、俯仰角、偏航角。

由于飞行器在实际避障过程中只调整横滚角φ，而不调整俯仰角θ，因此可以令θ=0，则式(14)可改写为：

(15)

由光流值引起的横滚角φ的变化：

φ=αVor

(16)

因此改进后的人工引力、斥力场分别如式(17)、式(18)所示：

(17)

Urep(ψ)=krep(-qcosφ+rsinφ)=

krep(-qcos(αVor)+rsin(αVor))

(18)

其中，ψtar为飞行器的初始偏航角，Vor为光流值的大小，α为光流系数，由式(17)、式(18)可得引力、斥力函数：

Fatt(ψ)=-▽Uatt(ψ)=-katt|ψ-ψtar|

(19)

Frep(V)=-▽Urep(V)=krep(qsin(αVor)+rcos(αVor))

(20)

3 飞行控制算法设计

飞行器控制系统采用内外环的控制结构。外环为位置控制，内环为姿态控制。本文主要介绍内环(姿态)控制算法。其中，四旋翼飞行器的飞行姿态通过惯性测量组合元件(IMU)来获取, 主要包括三个MEMS陀螺仪和三轴加速度传感器[10]。由于四旋翼飞行器的斥力可以作为其姿态环的参考指令，所以可以将其输入内环进行姿态环的闭环控制。通过串级PID控制器控制飞行器的角度环和角速度环。本文中姿态环的控制器参数如表1所示，其中姿态环控制模型设计如图6所示。

表1 PID控制器参数

图6 无人机姿态环控制模型

4 飞行实验结果及分析

本文实验平台为450 cm轴距的四旋翼飞行器，最大飞行速度10 m/s，姿态测试的实验环境为室外，悬停高度2 m，通过地面站mission planer进行实时显示。姿态曲线如图7所示。

图7 飞行姿态曲线

其中各曲线分别为：pitch方向的期望姿态；pitch方向的实际姿态；roll方向的期望姿态；roll方向的实际姿态曲线。纵坐标为角度，横坐标为时间。

如图7所示，飞行器的姿态控制使用串级PID控制器，再通过整定合适的PID参数，其在室外环境下的pitch、roll方向都能获得理想的姿态响应曲线，可以作为避障的基础，并且可以用于植保作业或者航拍等需要较好姿态响应的场合。

在满足飞行器pitch和roll方向姿态的要求后，需对其避障性能进行实验。将光流传感器装于飞行器x方向的正中间，飞行器以一定速度向前直线飞行，在前方设置一个障碍物，如果与障碍物产生的光流值的绝对值≥0.5 m/s，飞行器开始执行避障任务；在将光流值与改进的人工势场法相结合时，需要确定出人工势场的引力、斥力场比例系数。经过大量的实际测试，本文中的引力、斥力场比例系数分别为1.8与4.0。飞行器执行避障任务时的测试曲线如图8、图9所示。

图8 飞行器执行避障任务时的测试曲线图

图9 图8中光流值的放大图

其中，避障测试实验中使用匿名地面站作为上位机对飞行器的期望、实际偏航角与光流值进行实时显示；图8中的曲线分别为飞行器避障过程中的期望偏航角与实际偏航角，单位都为°；图9中的为飞行器避障过程中产生的光流值，单位为m/s；横坐标为时间，单位为ms。

如图8所示，使用结合光流的人工势场法时，飞行器在避开障碍物之后可以继续以期望偏航角方向保持直线飞行，从而达到局部路径规划的目的。

5 结 语

本文采用光流传感器和改进的势场法设计了基于光流传感器的旋翼无人机实时避障系统。光流传感器获取当前飞行环境中与障碍物相对运动产生的光流值。改进的势场法则可以实时地对飞行器最优飞行路径进行规划，这样飞行器既可以躲避障碍物又不会迷失方向。该实时避障系统与旋翼飞行器的飞行控制系统结合成功地实现了旋翼飞行器的自主避障飞行。本文的避障策略在轴距450 cm的四旋翼飞行器上得到了很好的验证。结果表明在速度不太高的场合下，表现性能良好，可以有效地对移动的障碍物进行规避，并能在避障后保持初始的期望偏航角，使飞行器能够更顺利地完成路径规划的任务。

[1] 赵海,陈星池,王家亮,等.基于四轴飞行器的单目视觉避障算法[J].光学精密工程,2014,22(8):2232-2241.

[2] 蔡卓凡.基于多超声波传感器避障机器人小车的设计[J].自动化技术与应用,2014,33(5):85-89.

[3] 宋宇,翁新武,郭昕刚.基于光流和惯性导航的小型无人机定位方法[J].传感器与微系统,2015,34(1):13-16.

[4] Qian G,Chellappa R,Zheng Q.Robust structure from motion estimation using inertial data[J].Journal of the Optical Society of America A Optics Image Science & Vision,2001,18(12):2982-2997.

[5] Motai Y,Jha S K,Kruse D.Human tracking from a mobile agent: Optical flow and Kalman filter arbitration[J].Signal Processing:Image Communication,2012,27(1):83-95.

[6] 姬伟,程风仪,赵德安,等.基于改进人工势场的苹果采摘机器人机械手避障方法[J].农业机械学报,2013,44(11):253-259.

[7] 郑来芳,孙炜,欧阳明华,等.结合光流和人工势场的风管机器人避障方法[J].计算机工程与应用,2016,52(9):243-247.

[8] 李杰,齐晓慧,韩帅涛.基于自抗扰技术的四旋翼姿态解耦控制方法[J].电光与控制,2013,20(3):44-48.

[9] Konolige K,Augenbraun J,Donaldson N,et al.A low-cost laser distance sensor[C]//IEEE International Conference on Robotics and Automation.IEEE,2008:3002-3008.

[10] Souhila K,Karim A.Optical Flow based Robot Obstacle Avoidance[J].International Journal of Advanced Robotic Systems,2008,4(1):13-16.

[11] 肖雪,秦贵和,陈筠翰.基于光流的自主移动机器人避障系统[J].计算机工程,2013,39(10):305-308.

[12] 王铭,王瑞,李晓娟,等.非确定性环境中移动机器人实时避障的概率模型检测[J].小型微型计算机系统,2014,35(9):2104-2109.