基于光流估计的无人机自主着陆

唐大全，唐管政，谷旭平

(海军航空大学， 山东 烟台 264001)

微电子学的飞速发展推动了旋翼无人机(Unmanned Aerial Vehicles，UAV)的发展[1]，由于其小巧性和敏捷性，UAV在狭窄和混乱的环境中执行任务具有一定潜力。但是，由于机载设备的载重和功耗受到严格限制，因此使用UAV实现自主飞行具有一定难度。

自然界中，昆虫飞行中的视觉导航主要基于光流，由于相对于环境的相对运动，昆虫可以感知到亮度模式的明显变化[2]。本质上，光流提供了有关速度与距离之比的信息，也因此无法直接获得到环境的实际度量距离。学者们发现，蜜蜂着陆时会根据从地面感知到的光流场的发散来控制其下降[3]。俯视时，光流场发散会传递与高度同比例缩放的垂直速度信息。通过保持向下运动的恒定散度，无人机可以自主着陆，同时以指数方式降低其向下速度，此策略已经在旋翼UAV的实验中得到了应用[4-7]。

尽管光流本身并不能提供运动的度量尺度，但光流场相关信息能应用于导航任务中，包括着陆。近几十年中，学者们提出了简单的受生物学启发的着陆策略，该策略利用从地面感知到的光流场中的视觉可视物，为三维结构，自我运动和相对姿势的视觉估计提供可行的方案，可以通过视觉同时定位和地图构建(Simultaneous Localization and Mapping，SLAM)[8]或视觉里程计[9]来实现，但是此类方法工作量较大，需要处理和维护大量的测量数据。近年来，在使用UAV进行的几项实验中[10-12]，研究者模仿飞行昆虫的导航策略，与地面上的水平运动有关的视觉可观测的是腹侧流量，利用腹侧流量进行地形跟随和自主着陆。文献[13]中通过保持向下运动速度和高度之间恒定的比率并同时放慢速度，可以平稳着陆，腹侧流量也可用于悬停稳定，以增强垂直着陆控制效果，但是应用场景有限，仅适用于垂直运动。针对以上问题，提出一种的基于光流估计的无人机自主着陆方法，适用于着陆过程中的多种运动模态下状态量的估计，且不需要处理大量数据。

1 基于光流估计的着陆

本小节建立着陆过程运动模型，如图1所示，3个坐标系：B、C和w，分别代表机体，摄像机和世界坐标系，在每个坐标系中，(X,Y,Z)表示位置，相对应的速度分量(U,V,W)。机体坐标系的中心位于UAV的重心。从世界坐标系w到机体坐标系B的转换关系由φ、θ、ψ表示，分别为翻滚角、俯仰角和偏航角。p、q、r表示翻滚角、俯仰角和偏航角旋转速率。

图1 旋翼无人机运动关系示意图

摄像机坐标系C以摄像机的焦点为中心，摄像机位于UAV重心正下方，且

XC=YB,YC=-XB

(1)

考虑ZC和ZB之间存在偏移ΔZ，得：

ZC=ZB+ΔZ

(2)

(3)

(4)

通过相机校正，可得内部参数k1、k2、xp、yp和f。

(5)

ZC=Z0+ZXZC+ZYYC

(6)

当表面平坦时，斜率是横滚角和俯仰角的切线，因此可以由机载惯性测量元件测得：

ZX=-tanφ,ZY=-tanθ

(7)

定义比例速度ϑx、ϑy、ϑz，如下所示：

(8)

根据文献[15]中的推导，因为光流场散度D=ϑz，垂直分量ϑz与D成比例，在着陆过程中保持垂直光流场散度恒定。将方程式(6)和方程式(8)代入方程式(5)，得平移光流场表达式:

(9)

旋转速度p、q会导致附加的平移速度分量引入到UC和VC中，并在ϑx和ϑy中体现：

(10)

2 基于光流的可视观测量估计

2.1 定向光流场参数估计

上节介绍了着陆过程中基于光流估计的状态估计，但是在某些情况下，例如出现孔径问题时，法向光流与光流当量有很大的偏差，图2中展示了一个示例性的光流当量和法向光流同时发散的光流场，当按方向分组时(图中带颜色的箭头)，法线光流矢量确实显示了原始的发散模式所以为观察沿法向方向的光流场发散，至少需要2个独立的法向光流向量，它们的位置要足够分开。定义一组m个方向{α1,α2,…,αm}，其中α1=0且αi-αi-1=π/m，为兼顾计算速度与精度，根据仿真结果取m=6。对于每个光流矢量，我们首先确定αi与方向αf的最接近匹配。每个方向αi都适应相似方向和相反方向的2种流动，即，当-π<αf<0时，将为αf+π计算一个匹配。

图2 发散光流场的示意图

沿选定方向αi，计算投影的法向光流位置S和大小V，从而获得沿αi的光流的一维表示：

(11)

通过机载惯性元件测量的旋转角速度值，减去旋转产生的光流分量来校正平移运动：

(12)

对于每个方向，维持一维光流场。联立式(5)和式(11)，得单一方向的光流场表示为

VT=-ϑxcosαi-ϑysinαi+ϑzS

(13)

解式(13)，利用所有方向的光流向量来计算加权最小二乘解，令cα=cosα和sα=sinα。待解的超定方程组组成如下：

(14)

其形式为AΘ≈y，可以从法线方程获得加权最小二乘解：

ATWAΘ=ATWy

(15)

其中W是由每个方向光流向量的权重组成的对角线矩阵:

W=diag(W1,…,W1,W2,…,W2,…,Wm)

(16)

权重Wi用于基于Si沿该方向的扩展来表示沿i方向的光流的可靠性。它的值由方差Var{Si}确定：

(17)

最小方差Var{Si}min设置为600。通过式(14)可知，可获得更多方向的法向光流量估计，使Θ的计算越精确。

2.2 光流场的递归更新

由Θ的方程式(15)可得视觉可见的光流场估计。但是，取决于估算器的采样率，有可能在单个周期性迭代期间，只有过少的光流可用于精确拟合，这会导致在Θ的测量中出现噪声峰值，尤其是在低速运动期间。为了抑制这种影响，矩阵A和y在每次迭代时都不会完全更新，并为其分配指数级降低的权重，类似于指数移动平均滤波器。对于每个方向，分别递归更新矩阵的一部分B=ATWA和C=ATWy。这些矩阵由以下元素组成：

(18)

从式(18)可以推得，B的矩阵元素为

(19)

同理可得，C的矩阵元素为

(20)

由于光流场统计信息是光流场的紧凑体现，而与流向量的实际数量无关。因此，可以将先前迭代的光流场信息有效地包含在后续迭代中，而无需增加式(14)中系统的大小。在每次迭代的开始，只需保留先前光流场统计信息的分数F，就可以包括来自先前迭代光流场的信息。因此，估计器精度较少依赖于算法的采样率。

(21)

F值计算如下:

(22)

其中时间常数kf的值为0.02 s。对于所有统计信息，此步骤都是相似的。当所有新的光流向量都经过分类和处理后，将使用式(15)重新计算光流场。

2.3 可靠性评估

在视觉传感器中，运动估计的可靠性根据环境变化而存在巨大差异。诸如可见地面纹理和场景照明之类的因素会影响估计效果。因此，利用基于光流场的几个值来计算置信度值，以便量化光流估计的可靠性。根据以下统计量，将该置信度值定义为3个独立置信度度量的乘积：光流场的正确估计率ρFmin；所有法向光流向量的最大方差Var{S}；式(19)的解的确定系数R2，在这里用作拟合无量纲度量。R2通常通过下式计算：

(23)

式中，残差平方和(RSS)和总平方和(TSS)的加权计算方法如下:

(24)

由此可得，总置信度值K是kρF,kVar{S}和kR2的乘积。因此，每个单独的置信度都需要接近1以获得高K值。例如，当ρF和R2非常大，但流量非常局限时，即Var{S}的最大值很小，光流估计仍不可靠。置信度K对于判定飞行期间视觉可见物的估计质量非常有效，而且它是Θ置信滤波器的主要组成部分。该滤波器基于常规的无时限脉冲响应低通滤波器，其中K与滤波器的更新常数相乘。通过在迭代k处的以下更新方程式确定视觉可见度的最终估计值：

(25)

其中kt是低通滤波器的时间常数，设置为0.02 s。利用饱和极限，将Θ中的各个值的更新幅度限制设置为最大值，以排除明显的异常值，Δϑmax的值设置为0.3。

3 实验与分析

为了评估所提出的着陆过程状态估计算法的准确性，通过MATLAB在代尔夫特理工大学公开的图像数据集[16]中进行仿真实验，数据集中附带的无人机速度姿态信息采集是通过手持机体运动生成的，位姿信息可用于提供ϑx、ϑy和ϑz的真实值。

由于ϑz是恒定散度控制器的关键，因此，要在多变化速度范围内研究该变量。然而，由于更复杂的基于光流的控制器需要考虑悬停稳定的水平分量，因此有必要对ϑx和ϑy进行估计。

1) 垂直运动：为了进行ϑz估计，在无人机垂直运动的数据集中进行仿真实验。

如图3所示为估计值与真实值的对比，以及高度测量值。图3中第三行分别显示了低散度运动和高散度运动的估计结果，同时在图中第四行显示了估计的置信度值。由图3可知，在低速时，估计器对正常流量中的局部离群值相对敏感。此外，由于检测率R2值较低，因此置信度K通常较低。在较高速度时，误差相对较小，此时K通常也较高。其结果是，由于置信度滤波器的缘故，在低置信度值对应的点上，ϑz的更新受到限制，这导致了相对于真实值的局部延迟。但是，当可以使用更高的置信度估计时，该估计会迅速收敛回真实值。

图3 垂直运动估计曲线

2) 水平运动:通过在主要由无人机水平运动组成的数据集中进行试验，来评估运动状态分量ϑx和ϑy的估计性能。

估计结果如图4所示，总体而言，尽管估计结果中仍然明显存在一些干扰，例如在t=23 s左右，但是整个水平运动状态估计过程效果良好，估计偏差与垂直运动数据集中的估计偏差相当。

图4 水平运动估计曲线

3) 旋转运动:为了评估使用当前设置可以进行正常旋转运动状态估计，在旋转运动的数据集中进行了测试。通过计算进行或不进行旋转运动的估计值，并与机体旋转速度p、q、r真实值，ϑx和ϑy的真实值做对比。

如图5所示，展示了p对ϑx的影响和对ϑy的影响(-p对应前文所述的ϑx)，为简洁起见，仅展示对ϑy的影响，由图可知最相关的是对ϑx的影响和对ϑy的影响，对ϑy的影响最小，在后一种情况下，和中存在一些残余运动，尽管它不能完全抵消图5所示的偏差。旋转估计过程效果良好，因旋转产生的光流的最大部分经过消旋处理得到成功校正。

图5 旋转运动估计曲线

4 结论

提出了一种基于光流的无人机着陆过程状态估计方法，提出的方法可以对垂直运动的运动状态进行较为准确的估计，通过置信度评估其输出的可靠性，提高了算法的计算精度，适用的速度范围宽；对于水平运动状态估计，整体估计效果良好；对于旋转运动，通过转速测量可以适当地校正快速旋转运动对状态估计的影响。