基于惯性光流的多旋翼飞行器地形跟踪

史添玮，崔文华，任玲

（1.辽宁科技大学国际金融与银行学院，辽宁鞍山114051；2.辽宁聚龙金融设备股份有限公司，辽宁鞍山114051）

基于惯性光流的多旋翼飞行器地形跟踪

史添玮1，崔文华1，任玲2

（1.辽宁科技大学国际金融与银行学院，辽宁鞍山114051；2.辽宁聚龙金融设备股份有限公司，辽宁鞍山114051）

为解决目前多旋翼飞行器光流技术存在的定位精度低与多相机数据处理过程繁琐等问题，本文应用金字塔层数自适应的Lucas-Kanade算法获取平移惯性光流以实现多旋翼飞行器稳定地形跟踪。下视球面相机获取多纹理的目标平面图像，同时将目标平面特征点动态映射至相机镜头表面；将比对两帧连续时刻图像后获取的平移光流作为非线性地形跟踪控制器的反馈，实现多旋翼飞行器地形跟踪与避免碰撞。10次室内实际飞行测试结果表明，不同地形变化区域的高度误差分别为±3 cm与±6 cm，验证了该控制器是稳定可行的。

多旋翼飞行器；平移惯性光流；地形跟踪；非线性控制器

基于视觉的光流技术克服了多旋翼飞行器有效载荷小的限制，增加了其自主性。光流技术适应性强，可借助于惯性导航系统提供精确的相对位置等信息及最大可能地为飞行器提供自主导航［1］。目前，国内学者应用光流技术实现了人体姿态识别［2］，移动目标跟踪［3］与悬停控制［4］等。国外学者将光流技术成功应用于异常视觉活动检测［5］与蜂鸟翅膀运动的自动分析［6］与运动目标检测［7］等。

目前，应用于多旋翼飞行器的光流技术存在定位精度低与多相机光流数据处理过程繁琐等问题，为此，本文首先利用下视球面相机获取目标平面图像，并结合捷联惯性导航系统（SINS）获取飞行器的实时速度、高度以及目标平面特征点的平移惯性光流等信息。其次，采用金字塔层数自适应的Lucas-Kanade算法估计并计算光流。然后，依据飞行器动力学方程，设计了将平移惯性光流作为反馈信息的用于地形跟踪的非线性控制器，并应用Lyapunov分析证明了控制器的稳定性。

1 目标平面特征点动态映射

假设下视球面相机放置在多旋翼飞行器的理论质心处且焦距为1。图1a与图1b分别为平面与球面相机的目标特征点动态映射过程。其中，O与S2分别表示平面与球面相机的镜头表面；P=(X，Y，Z)∈R3为目标平面中的可见特征点，其在平面与球面相机镜头表面的映射点分别为pO与pS。将两种相机的映射过程对时间求导，并结合雅克比矩阵可得

图1 平面与球面相机的目标特征点动态映射过程Fig.1 Dynamic mapping processes of planar camera and spherical camera

多旋翼飞行器的任意运动被认为是质心水平运动与环绕质心转动的合成运动［8］。由于目标平面中的特征点是稳定的，因此，其位置的移动仅取决于飞行器的移动。

目标特征点在球面相机镜头表面的映射点pS与相机镜头表面中心线间的动态半径可表示为［9］

式中：Ω为机体坐标系相对于地面坐标系的转动角速度在机体坐标系各轴向的分量；πp为点p在球面相机镜头表面S2的切线空间映射；向量V=RTv，v=ξ 表示飞行器在地面坐标系的飞行速度；ξ=[x,y,z]为飞行器在地面坐标系的位置；R为坐标系间的转换关系矩阵。

定义d≔d(t)为地面坐标系中的目标平面与机体坐标系水平面之间的正交距离，则目标平面的任意点P，满足如下关系［9］

式中：k′=RTk，k为飞行器水平飞行时惯性方向；μ为惯性方向与垂直于目标平面方向间的夹角。

2 金字塔层数自适应的Lucas-Kanade算法

本文采用金字塔层数自适应的Lucas-Kanade算法获取目标光流，但其存在层数确定问题：若层数选择不足，则图像尺度缩小程度不足，无法满足Lucas-Kanade算法假设；若层数选择过多，则顶层图像分辨率过低，大量有效信息丢失。通常，构建5层以上的金字塔就会造成信息丢失［10］。其优化过程如下：

（1）假设两帧连续时刻图像的最大位移为像素t，并将原始图像融入金字塔中。将第i帧图像向上、向下、向左或向右平移t次，并将其与第i+1帧图像比较。其结果可描述为

式中：T(k)为移动k(k=1,2,…,t)次的结果矩阵；Ii(k)为第i帧图像移动像素k的图像。

（2）对于第i帧图像的每个像素点，将移动过程中T(k)的最小值赋值给像素点，即：Mx,y=k。

（3）为金字塔各层设置阈值。若计算得到大于某预定阈值的最大移动值，则将其作为第i帧图像的最大移动矢量，并为其赋值，即：MVmax=max(k)。其中，m与n分别为图像的宽与高。

（4）依据MVmax值确定金字塔层数。

3 平移惯性光流计算

由于p为已知数据，Ω与k的估计值可以通过惯性测量单元获得［11］，因此，球面相机镜头表面W2窗口的平均光流可表示为［9］

式中：γˉ是以k为中心的球面镜头视场角度；Q为随时间变化的正定对称矩阵；i为目标平面中的特征点个数；∧为恒定对角矩阵；Rt为目标平面与地面坐标系间的方向矩阵。若目标平面与地面坐标系的水平面平行，则由式（5）可得平移惯性光流为

4 基于光流法的地形跟踪原理

图2 地形跟踪过程中坐标系之间的关系Fig.2 Relationship of coordinates during terrain-following

式中：kP与kD为正值；ωxt与ωzt分别为斜坡子坐标系（t坐标系）的光流分量。在机体坐标系中，依据k′方向的动力学方程可得

该式表明，只要d(t)＞0，则L＜L(0)。应用La-Salle原则，可得出d ≡0。结合式（9），很容易得出d-d*渐进趋于0，因此，d渐进趋于d*。在保证d(t)为正值的条件下，飞行器可完成地形跟踪，同时避免与地面碰撞。

5 实验结果及分析

5.1 实验参数

实验使用德国MK公司的Hexa系列六旋翼飞行器作为研究平台。相机观测角度为圆周70°，地形跟踪控制器的参数为：kP=3，kD=1。设定ωd=(0.5，0，0)T，前向速度为1 m/s，飞行期望高度为2 m，光流采样频率为30 Hz。实验共进行10组，多旋翼飞行器沿直线飞行的距离为40 m。

5.2 实验结果分析

在进行地形跟踪实验时，由于计算光流的方程多于未知参数个数，因此，对运动参数使用最小二乘法估计得到折中解。10组地形跟踪实验结果为：k′方向误差（2.4±1.1）°，X位置误差（2.1±0.9）cm，Z位置误差（-1.3±1.7）cm，前向光流（0.42± 0.11）lm，垂直光流（0.02±0.09）lm。其中，位置误差与光流测量值采用平均值±标准差表示。

图3为实验地形与实际跟踪实验结果。图3表明，地形相对平缓区域，高度方向的误差为±3 cm；地形变化明显区域，高度方向的误差为±6 cm。图4为地形跟踪实验过程中，机体坐标系中各轴向光流分量与k′方向的角度估计。由图4可以看出，各轴向的光流分别相对平稳并趋近于各自的期望光流；k′方向的角度估计与实验地形大致相同。证明了光流控制地形跟踪的可行性与稳定性。此外，在图3与图4的地形变化明显区域，存在飞行器高度响应及k′方向的角度估计滞后现象，滞后时间分别为（100±4）ms与（112±5）ms。其主要是由飞行器的飞行速度、惯性与光流采样频率较低等因素造成的。

图3 实验地形与实际跟踪结果Fig.3 Experimental terrain and actual terrain-following result

图4 光流与k′方向估计Fig.4 Optical flow and estimation ofk′direction

为了进一步验证用于实现光流控制方法的稳定性，在完成地形跟踪实验的同时，对飞行器的前向速度与前向光流的趋势进行测量，图5为所测量的结果。图5表明，前向速度与光流测量值均稳定且趋近于期望值。实验过程中，尽管实验环境的光线变化与算法的光流估计误差等因素导致观测值中存在噪声，但其并未对本文提出的惯性光流控制方法的稳定性产生影响。

与刘小明等［12］提出的利用光流传感器获取光流信息，实现飞行器地形跟踪控制的方法相比，本文提出的方法在地形跟踪精度性能上大约提高±2 cm；在飞行器随地形爬升与下降过程中的响应滞后时间性能上有提升，时间减少约176～190 ms。虽然本文没有使用光流传感器直接获取光流，但是前向速度与前向光流的稳定性差异并不明显。

图5 前向速度与前向光流Fig.5 Forward velocity and forward optical flow

6 结语

本文应用平移惯性光流方法实现了多旋翼飞行器的复杂地形跟踪。实验结果表明：各轴向的光流、前向速度与前向光流可以控制在期望值附近，并趋近于期望值；地形相对平缓区域，高度误差±3 cm；地形变化明显区域，高度误差±6 cm；k′方向的角度估计与测试地形大致相同。与类似方法相比，地形跟踪精度性能大约提高±2 cm；在飞行器随地形爬升与下降过程中的响应滞后时间性能提升约176～190 ms。

［1］SRINIVASAN M V，ZHANG S W，CHAHL J S，et al. How honeybees make grazing landings on flat surfaces［J］.Biological Cybernetics，2000，83（3）：171-183.

［2］郭利，曹江涛，李平，等.累积方向-数量级光流梯度直方图的人体动作识别［J］.智能系统学报，2014，9（1）：104-108.

［3］吴垠，李良福，肖樟树，等.基于尺度不变特征的光流法目标跟踪技术研究［J］.计算机工程与应用，2013，49（15）：157-161.

［4］史添玮，王宏.基于光流的六旋翼飞行器悬停控制［J］.东北大学学报：自然科学版，2015，36（10）：1378-1382.

［5］WANG T，SNOUSSI H.Detection of Abnormal Visual Events via Global Optical Flow Orientation Histogram［J］.IEEE Transactions on Information Forensics and Security，2014，9（6）：988-998.

［6］MARTÍNEZ F，MANZANERA A，ROMERO E.Automatic analysis and characterization of the hummingbird wings motion using dense optical flow features［J］.Bioinspiration and Biomimetics，2015，10（1）：016006-016006.

［7］PARRILLA E，ARMENGOT M，MATA M，et al.Primary ciliary dyskinesia assessment by means of optical flow analysis of phase-contrast microscopy images［J］.Computerized Medical Imaging and Graphics，2014，38（3）：163-170.

［8］宫勋，白越，赵常均，等.Hex-rotor无人飞行器及其飞行控制系统设计［J］.光学精密工程，2012，20（11）：2450-2458.

［9］HÉRISSÉ B，HAMEL T，MAHONY R，et al.Landing a VTOL unmanned aerial vehicle on a moving platform using optical flow［J］.IEEE Transactions on Robotics，2012，28（1）：77-89.

［10］李亚楠，赵耀，林春雨，等.基于图像分割的金字塔lucas-kanade光流法提取深度信息［J］.铁道学报，2015，37（1）：63-68.

［11］METNI N，PFLIMLIN J M，HAMEL T，et al.Attitude and gyro bias estimation for a flying uav［C］//Edmonton，Canada.proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems，2005：1114-1120.

［12］刘小明，陈万春，邢晓岚，等光流控制地形跟随与自动着陆［J］.北京航空航天大学学报，2012，38（1）：98-105.

Terrain-following for multi-rotor aircraft based on inertial optical flow

SHI Tianwei1，CUI Wenhua1，REN Ling2
（1.School of International Finance and Bank，University of Science and Technology of Liaoning，Anshan 114051，China；2.Liaoning Julong Financial Equipment Co Ltd，Anshan 114051，China）

To solve the optical flow technique problems of low positioning accuracy and cumbersome processing of multi-camera data in multi-rotor aircraft for the moment，the layer adaptive pyramid Lucas-Kanade algorithm is applied to get the translational inertial optical flow to achieve the stability terrain-following of multi rotor aircraft in this paper.The image of the textured target plane was captured by the bottom facing spherical camera.Simultaneously，the feature points in the target plane were dynamically mapped to the camera lens surface.Then the translational optical flow obtained from the comparison between the two time continuous images was used as feedback of the nonlinear terrain-following controller to achieve the terrain-following and collision avoidance for multi-rotor aircraft.The result of 10 indoor actual flight tests showed that the height accuracies in different changing terrains are±3 cm and±6 cm respectively.And they verified that the nonlinear controller proposed in this paper is stable and feasible.

multi-rotor aircraft；translational inertial optical flow；terrain-following；nonlinear controller

July 3，2016）

V249.1

A

1674-1048（2017）02-0155-06

10.13988/j.ustl.2017.02.014

2016-07-03。

国家自然科学基金（51405073，61071057）；辽宁省高等学校创新团队（LT2014006）。

史添玮（1982—），男，辽宁鞍山人，讲师。