CN103365297B - 基于光流的四旋翼无人机飞行控制方法 - Google Patents

Abstract

一种基于光流的四旋翼无人机飞行控制方法，包括如下步骤：利用基于图像金字塔的卢卡斯·卡纳德方法进行光流信息的计算；采用卡尔曼滤波方法处理光流信息；进行光流和姿态角两者数据融合，以及无人机水平位移的计算；设计比例-微分控制器，包括：确定四旋翼无人机动力学模型和进行控制算法设计。本发明的基于光流的四旋翼无人机飞行控制方法，利用机载摄像头获得的图像信息与姿态角信息进行融合，计算出无人机水平位置信息，并以此位置信息作为外环PD(比例-微分，Proportional-Differential)控制器的反馈信息对小型无人机进行位置控制。

Description

基于光流的四旋翼无人机飞行控制方法

技术领域

本发明涉及一种四旋翼无人机飞行控制方法。特别是涉及一种对室内室外环境均有很强适应能力、在微小型至小型四旋翼无人机上均可应用的基于光流的四旋翼无人机飞行控制方法。

背景技术

随着传感器技术、新材料技术和微处理器技术的发展，四旋翼无人机逐渐成为无人机研究领域中的一个热点。它具有成本低、安全性强、质量轻、体积小、灵活机动等特点，在军事领域和民用领域都有着广泛的应用。其在室内环境中可用于矿井或大型建筑物内的侦查，危险密闭环境下的搜索和营救等，在室外环境中可用于监视巡查交通状况和桥梁、堤坝、输电线等基础设施状况，或军事侦察等。

受昆虫视觉***启发，基于光流的四旋翼无人机飞行控制方法逐渐受到研究人员的关注。研究表明，飞行昆虫可以利用光流在复杂的自然环境中敏捷地运动，例如躲避障碍物，沿走廊飞行，巡航，着落。光流是图像亮度模式的一种表观运动。当摄像机与场景目标之间存在相对运动时，所观察到的亮度模式运动称为光流，或者将物体带有光学特征部位的移动投影到视网膜平面（图像平面）上就形成光流（出版社：人民邮电出版社；著者：章毓晋；出版年月：2011年；书籍名称：计算机视觉教程）。光流表达了图像的变化，包含了运动的信息，而且携带了有关场景三维结构的大量信息。

无人机能否实时感知自身的位置信息和速度信息对其实现自主飞行非常重要。相较于其他四旋翼无人机传感器方法——GPS、激光雷达、声纳、单目视觉和双目视觉，光流法对于四旋翼无人机的控制有其独特的优势。其一，在近地复杂环境中GPS信号几乎不可获取，特别是在室内环境中几乎没有GPS信号，而光流法可以同时适用于室内环境与室外环境；其二，激光雷达和声纳在原理上都是通过主动发射脉冲和接收反射脉冲来进行测距，多架无人机一起工作时相互之间会产生干扰，而光流法为被动的视觉定位方法，多机工作时信号无干扰；其三，一般单目视觉需要地面标识，适用于已知环境，而光流法不受标识限制，在未知环境中也可应用；其四，四旋翼无人机体积较小，限制了两个光学传感器之间的距离，立体视觉能力因此受限，而受生物启发基于光流的定位计算方法，由于仅使用一个摄像头或光流传感器，因而具有体积小、重量轻等优势，非常适合应用于四旋翼无人机***；其五，光流法所需设备价格较低。

最近几年，一些国内外高校和研究机构开始尝试利用光流法对四旋翼无人机进行飞行控制，并取得了一些初步成果。这些研究大体上可以分为两类：利用光流传感器获取光流信息进行控制（会议：IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems；著者：H.Lim,H.LeeandH.JinKim；出版年月：2012年；文章题目：OnboardFlightControlofaMicroQuadrotorUsingSingleStrapdownOpticalFlowSensor；页码：495-500）（会议：the31stChineseControlConference；著者：Y.Bai,H.Liu,Z.Shi,andY.Zhong；出版年月：2012年；文章题目：RobustControlofQuadrotorUnmannedAirVehicles；页码：4462-4467）和利用摄像头获取图像计算光流信息进行控制（期刊：RoboticsandAutonomousSystems；著者：F.Kendoula,I.Fantoni,andK.Nonami；出版年月：2009年；文章题目：Opticalflow-basedvisionsystemforautonomous3Dlocalizationandcontrolofsmallaerialvehicles；页码：591-602）（会议：IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems；著者：B.Herisse,T.Hamel,F-X.Russotto,andR.Mahony；出版年月：2008年；文章题目：HoveringFlightandVerticalLandingControlofaVTOLUnmannedAerialVehicleUsingOpticalFlow；页码：801-806）。前者相对于后者的优势在于其更为出色的机载图像处理能力，但是从应用场合和应用扩展能力来看后者具有更大的优势：其一，利用光流传感器的控制方法由于传感器本身的限制，只适用于低空飞行（4米以下），并且对光照条件有较为严格的要求（荧光灯的频闪效应对传感器正常工作有较强影响）；其二，利用光流传感器的控制方法不可对其光流计算方法进行修改或扩展，极大地限制了应用的灵活性。

发明内容

本发明所要解决的技术问题是，提供一种可显著提高四旋翼无人机的悬停控制精度和轨迹跟踪精度的基于光流的四旋翼无人机飞行控制方法。

本发明所采用的技术方案是：一种基于光流的四旋翼无人机飞行控制方法，包括如下步骤：

1）利用基于图像金字塔的卢卡斯·卡纳德方法进行光流信息的计算；

2）采用卡尔曼滤波方法处理光流信息；

3）进行光流和姿态角两者数据融合，以及无人机水平位移的计算；

4）设计比例-微分控制器，包括：

（1）确定四旋翼无人机动力学模型，和（2）进行控制算法设计。

步骤1）所述的基于图像金字塔的卢卡斯·卡纳德方法来完成光流的计算是：

金字塔的每一层都会得到一个从上一层传递过来的运动估计假设，用于本层光流的计算，用L_m表示金字塔的最高层层数，g^*表示图像金字塔第*层的运动估计假设，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向上的运动估计假设，I^*(x,y)表示图像金字塔第*层图像平面坐标为x和y处的亮度，d^*表示图像金字塔第*层的光流量，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向的光流量，d表示最终光流计算结果；

在第L_m层，初始运动估计假设设为零，即根据基于亮度恒定假设：

$I^{\mathrm{Lm}}(x,y)=I^{\mathrm{Lm}}(x+d_x^{\mathrm{Lm}},y+d_y^{\mathrm{Lm}})$

并利用卢卡斯·卡纳德方法算出本层的光流量传递到第L_m-1层的运动估计假设为 $g^{\mathrm{Lm}-1}={[g_x^{\mathrm{Lm}-1},g_y^{\mathrm{Lm}-1}]}^T=2(g^{\mathrm{Lm}}+d^{\mathrm{Lm}}),$ 然后同样根据亮度恒定假设：

$I^{\mathrm{Lm}-1}(x,y)=I^{\mathrm{Lm}-1}(x+g_x^{\mathrm{Lm}-1}+d_x^{\mathrm{Lm}-1},y+g_y^{\mathrm{Lm}-1}+d_y^{\mathrm{Lm}-1})$

并利用卢卡斯·卡纳德方法计算本层光流d^Lm-1，其他层的计算方法以此类推。在计算出图像金字塔最底层的光流d⁰后，最终光流计算结果：

$d=g^0+d^0=\underset{L=0}{\overset{\mathrm{Lm}}{\mathrm{\Σ}}}{2}^L{d}^L.$

步骤2）所述的采用卡尔曼滤波方法处理光流信息是采用卡尔曼滤波器的离散时间过程方程和测量方程：

X_k=AX_k-1+ω_k-1

Z_k=d=HX_k+υ_k

其中，是k时刻***状态向量，即为光流估计向量，d_x和d_y分别表示在x方向和y方向上光流的估计值；是k-1时刻***状态向量，Z_k∈R²是k时刻***观测向量，d是通过步骤1）中光流信息计算得到的数据结果，即光流的原始数据，随机信号ω_k-1和υ_k分别表示k-1时刻过程激励噪声和k时刻观测噪声，它们是相互独立且为正态分布的，A∈R^2×2和H∈R^2×2是单位矩阵。

步骤3）所述的光流和姿态角两者数据融合采用如下公式，

$d_{\mathrm{xp}}=d_x-d_{\mathrm{roll}}=d_x-\mathrm{\Δ\φ}\frac{R_x}{\mathrm{\α}}$

$d_{\mathrm{yp}}=d_y-d_{\mathrm{pitch}}=d_y-\mathrm{\Δ\θ}\frac{R_y}{\mathrm{\β}}$

其中，d_xp和d_yp分别是x和y方向的光流水平分量，d_x和d_y分别是在x和y方向经过步骤2）中卡尔曼滤波处理的总测量光流，d_roll和d_pitch分别是x和y方向的光流旋转分量，Δφ是两帧图像之间滚转角变化量，R_x是x方向的摄像头分辨率，α是x方向的视野，Δθ，R_y和β分别是在y方向的俯仰角变化量、摄像头分辨率和视野。

步骤3）所述的无人机水平位移的计算根据针孔相机模型采用如下公式，

$\mathrm{\ΔX}=d_{\mathrm{xp}}s\frac{f}{h}$

$\mathrm{\ΔY}=d_{\mathrm{yp}}s\frac{f}{h}$

其中，ΔX和ΔY分别表示在两帧图像之间其x方向和y方向飞机实际水平位移增量，d_xp和d_yp分别表示由光流和姿态角两者数据融合计算得到光流水平分量，s表示关于摄像头的图像比例常数，f表示摄像头的焦距，h表示摄像头光心到地面的距离。

步骤4）所述的确定四旋翼无人机动力学模型如下：

设定F={x_I,y_I,z_I}为右手惯性坐标系，其中z_I为垂直于地面的向量，体坐标性设定为B={x_B,y_B,z_B}，并将Ρ(t)=[x(t)y(t)z(t)]^T∈R³定义为惯性坐标系下的位置向量，Θ(t)=[θ(t)φ(t)ψ(t)]^T∈R³定义为惯性坐标系下的欧拉角向量，四旋翼无人机四个电机产生的升力分别定义为f_i(t)，i=1,2,3,4；

四旋翼无人机动力学模型化简为如下形式：

$\stackrel{\·\·}{x}=-\frac{1}{m}{K}_1\stackrel{\·}{x}+(\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ})u_1$

$\stackrel{\·\·}{y}=-\frac{1}{m}{K}_2\stackrel{\text{\·}}{y}+(\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ})u_1$

$\stackrel{\·\·}{z}=-\frac{1}{m}{K}_3\stackrel{\·}{z}-g+\cos \mathrm{\φ}\cos \mathrm{\θ}{u}_1$

$\stackrel{\·\·}{\mathrm{\φ}}=-\frac{1}{J_1}{K}_{4}l\stackrel{\·}{\mathrm{\φ}}+\frac{1}{J_1}l{u}_2$

$\stackrel{\·\·}{\mathrm{\θ}}=-\frac{1}{J_2}{K}_{5}l\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_2}l{u}_3$

$\stackrel{\·\·}{\mathrm{\ψ}}=-\frac{1}{J_3}{K}_6\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_3}c{u}_4$

其中，m∈R代表飞行器重量，J₁,J₂,J₃∈R为对应轴的转动惯量，K_i∈R，i=1,...,6代表气动阻尼系数，l为螺旋桨到四旋翼无人机重心的距离，c∈R代表一个升力-转矩常量系数，g为重力加速度，式中的虚拟控制输入信号u₁(t),u₂(t),u₃(t)和u₄(t)按如下定义，

$u_1=\frac{1}{m}(f_1+f_2+f_3+f_4)$

u₂=-f₁-f₂+f₃+f₄

u₃=-f₁+f₂+f₃-f₄

u₄=f₁-f₂+f₃-f₄

步骤4）所述的进行控制算法设计是：采用非线性内外环控制结构；其中外环根据位置误差计算得出内环姿态的期望角度，内环跟踪期望角度计算出最终的控制输出。水平位置信息通过步骤1）、步骤2）和步骤3）计算得到，高度信息通过机载声呐传感器获得。

在控制算法的计算过程中，虚拟辅助控制变量μ=[μ_x,μ_y,μ_z]通过外环控制器计算得到，表达式如下，

μ_x=u₁(cosφ_dsinθ_dcosψ_d+sinφ_dsinψ_d)

μ_y=u₁(cosφ_dsinθ_dsinψ_d-sinφ_dcosψ_d)

μ_z=u₁(cosφ_dcosθ_d)

由上式可将内环期望的姿态角φ_d、θ_d，以及螺旋桨总推力u₁解算出来，表达式如下，

u₁=(μ_x ²+μ_y ²+μ_z ²)^1/2

${\mathrm{\φ}}_d={\sin }^{-1}(\frac{1}{u_1}{u}_x\sin {\mathrm{\ψ}}_d-\frac{1}{u_1}{u}_y\cos {\mathrm{\ψ}}_d)$

${\mathrm{\θ}}_d={\sin }^{-1}\left[(\frac{1}{u_1}{u}_x\cos {\mathrm{\ψ}}_d-\frac{1}{u_1}{u}_y\sin {\mathrm{\ψ}}_d){\left(\cos {\mathrm{\φ}}_d\right)}^{1/2}\right]$

外环比例-微分控制器设计如下,

${\mathrm{\μ}}_x=-k_{\mathrm{xp}}\mathrm{\Δ}{E}_X-k_{\mathrm{xd}}\frac{\mathrm{\Δ}{E}_X}{\mathrm{\Δt}}$

${\mathrm{\μ}}_y=-k_{\mathrm{yp}}\mathrm{\Δ}{E}_Y-k_{\mathrm{yd}}\frac{\mathrm{\Δ}{E}_Y}{\mathrm{\Δt}}$

${\mathrm{\μ}}_z=-k_{\mathrm{zp}}\mathrm{\Δ}{E}_Z-k_{\mathrm{zd}}\frac{\mathrm{\Δ}{E}_Z}{\mathrm{\Δt}}$

其中，μ_x、μ_y和μ_z分别代表x、y、z方向的外环虚拟控制输入，k_*p和k_*d分别为*方向可调的比例系数与微分系数,ΔE_X、ΔE_Y和ΔE_Z分别代表x、y、z方向的位置误差。

本发明的基于光流的四旋翼无人机飞行控制方法，利用机载摄像头获得的图像信息与姿态角信息进行融合，计算出无人机水平位置信息，并以此位置信息作为外环PD(比例-微分，Proportional-Differential)控制器的反馈信息对四旋翼无人机进行位置控制。相较于现有的其他无人机飞行控制方法，本发明具有显著进步：

1.适用范围广。发明所应用的摄像头具有体积小、重量轻等优点，这使得该方法可应用于小型甚至微四旋翼无人机（重量100克以下）。本发明基于利用摄像头获取的图像信息从而计算光流的方法，相较于利用光流传感器的方法，可适用于更高的飞行高度，具有更好的算法可扩展性。本发明方法同时适用于室内环境和室外环境。

2.控制效果明显。实验结果表明，本发明控制方法简单实用，控制精度高，对图像噪声与传感器噪声具有很好的鲁棒性。

附图说明

图1是基于光流的四旋翼无人机控制方法流程图；

图2是基于图像金字塔的卢卡斯·卡纳德光流算法流程图；

图3a是无人机与地面水平飞行时X方向FOV示意图；

图3b是无人机与地面成一定角度时X方向FOV示意图；

图4是基于主机的飞行控制结构；

1：惯性导航单元2：串口

3：飞控板4：数传电台发送端

5：摄像头6：无线发射模块

7：数传电台接收端8：无线接收模块

9：遥控信号发生器10：遥控器

图5a是悬停飞行实验中俯仰角变化曲线；

图5b是悬停飞行实验中滚转角变化曲线；

图5c是悬停飞行实验中x方向线速度变化曲线；

图5d是悬停飞行实验中y方向线速度变化曲线；

图6a是悬停飞行实验中x方向位移变化曲线；

图6b是悬停飞行实验中y方向位移变化曲线；

图7a是悬停飞行实验中飞行轨迹曲线；

图7b是悬停飞行实验中x方向位移误差分布直方图；

图7c是悬停飞行实验中y方向位移误差分布直方图；

图8是正方形轨迹跟踪飞行实验平面位移信息曲线。

具体实施方式

下面结合实施例和附图对本发明的基于光流的四旋翼无人机飞行控制方法做出详细说明。

本发明的基于光流的四旋翼无人机飞行控制方法，提出了一种新颖的基于摄像头获取图像计算光流信息的四旋翼无人机控制方法，将光流信息做进一步滤波处理，继而设计内外环控制器，完成了对四旋翼无人机、特别是微四旋翼无人机的控制和轨迹跟踪飞行。本发明对环境的变化有很强的适应能力，可显著提高四旋翼无人机的悬停控制精度和轨迹跟踪精度，缩小误差范围。

如图1所示，本发明的基于光流的四旋翼无人机飞行控制方法，包括如下步骤：

1）利用基于图像金字塔的卢卡斯·卡纳德方法进行光流信息的计算；

卢卡斯·卡纳德方法是一种经典的差分光流计算方法。这种方法的提出基于三个假设：亮度恒定，时间连续或者运动是“小运动”，空间一致。但是，对于大多数25Hz的摄像头来说，大并且不连贯的运动是普遍存在的，无法满足该假设的第二个条件，影响了卢卡斯·卡纳德方法的使用。基于此，我们利用基于图像金字塔的卢卡斯·卡纳德方法（著者：Jean-YvesBouguet；发表年月：2001年；文章题目：Pyramidalimplementationoftheaffinelucaskanadefeaturetrackerdescriptionofthealgorithm；发表公司：IntelCorporation）来完成光流的计算。首先在较大空间尺度上对光流进行跟踪，然后通过图像金字塔来对初始运动估计假设进行修正，从而实现对较快和较大运动的跟踪。这种方法通过对光流的迭代计算，提高了运动估计计算的精度。

图2是基于金字塔的卢卡斯·卡纳德光流计算方法的基本流程图。光流的迭代计算从图像金字塔的最高层开始。金字塔的每一层都会得到一个从上一层传递过来的运动估计假设，用于本层光流的计算，用L_m表示金字塔的最高层层数，g^*表示图像金字塔第*层的运动估计假设，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向上的运动估计假设，I^*(x,y)表示图像金字塔第*层图像平面坐标为x和y处的亮度，d^*表示图像金字塔第*层的光流量，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向的光流量，d表示最终光流计算结果；

在第L_m层，初始运动估计假设设为零，即根据基于亮度恒定假设：

$I^{\mathrm{Lm}}(x,y)=I^{\mathrm{Lm}}(x+d_x^{\mathrm{Lm}},y+d_y^{\mathrm{Lm}})---\left(1\right)$

并利用卢卡斯·卡纳德方法算出本层的光流量传递到第L_m-1层的运动估计假设为 $g^{\mathrm{Lm}-1}={[g_x^{\mathrm{Lm}-1},g_y^{\mathrm{Lm}-1}]}^T=2(g^{\mathrm{Lm}}+d^{\mathrm{Lm}}),$ 然后同样根据亮度恒定假设：

$I^{\mathrm{Lm}-1}(x,y)=I^{\mathrm{Lm}-1}(x+g_x^{\mathrm{Lm}-1}+d_x^{\mathrm{Lm}-1},y+g_y^{\mathrm{Lm}-1}+d_y^{\mathrm{Lm}-1})---\left(2\right)$

并利用卢卡斯·卡纳德方法计算本层光流d^Lm-1，其他层的计算方法以此类推。在计算出图像金字塔最底层的光流d⁰后，最终光流计算结果：

$d=g^0+d^0=\underset{L=0}{\overset{\mathrm{Lm}}{\mathrm{\Σ}}}{2}^L{d}^L---\left(3\right)$

2）采用卡尔曼滤波方法处理光流信息；

摄像头采集到的图像信息在无线传输过程中易受到干扰并造成视频不流畅或图像条纹等现象，这将导致光流计算误差的产生。应用一个二阶离散卡尔曼滤波器可以在一定程度上解决该问题。卡尔曼滤波器的离散时间过程方程和测量方程表示如下，

X_k=AX_k-1+ω_k-1

Z_k=d=HX_k+υ_k（4）

其中，是k时刻***状态向量，即为光流估计向量，d_x和d_y分别表示在x方向和y方向上光流的估计值；是k-1时刻***状态向量，Z_k∈R²是k时刻***观测向量，d是通过步骤1）中光流信息计算得到的数据结果，即光流的原始数据，随机信号ω_k-1和υ_k分别表示k-1时刻过程激励噪声和k时刻观测噪声，它们是相互独立且为正态分布的，A∈R^2×2和H∈R^2×2是单位矩阵。

3）进行光流和姿态角两者数据融合，以及无人机水平位移的计算；

无人机的俯仰和滚转运动会产生光流测量量中的旋转分量，进而影响到通过光流对无人机水平运动的估计。将IMU（惯性测量单元，InertialMeasurementUnit）获得的姿态角数据与光流数据相融合，可以消除光流旋转分量的影响，进一步提高对无人机水平运动估计的精度。公式（5）给出了本发明中光流和姿态角两者数据融合的公式，

$d_{\mathrm{xp}}=d_x-d_{\mathrm{roll}}=d_x-\mathrm{\Δ\φ}\frac{R_x}{\mathrm{\α}}$

$d_{\mathrm{yp}}=d_y-d_{\mathrm{pitch}}=d_y-\mathrm{\Δ\θ}\frac{R_y}{\mathrm{\β}}---\left(5\right)$

其中，d_xp和d_yp分别是x和y方向的光流水平分量，d_x和d_y分别是在x和y方向经过步骤2）中卡尔曼滤波处理的总测量光流，d_roll和d_pitch分别是x和y方向的光流旋转分量，Δφ是两帧图像之间滚转角变化量，R_x是x方向的摄像头分辨率，α是x方向的视野(FOV，FieldofView)，如图2所示，Δθ，R_y和β分别是在y方向的俯仰角变化量、摄像头分辨率和FOV。

根据针孔相机模型，公式(6)给出了计算无人机水平位移的算法，

$\mathrm{\ΔX}=d_{\mathrm{xp}}s\frac{f}{h}$

$\mathrm{\ΔY}=d_{\mathrm{yp}}s\frac{f}{h}---\left(6\right)$

其中，ΔX和ΔY分别表示在两帧图像之间其x方向和y方向飞机实际水平位移增量，d_xp和d_yp分别表示由光流和姿态角两者数据融合计算得到光流水平分量，s表示关于摄像头的图像比例常数，f表示摄像头的焦距，h表示摄像头光心到地面的距离。

4）设计PD（比例-微分，Proportional–Differential）控制器，包括：（1）确定四旋翼无人机动力学模型，和（2）进行悬停控制算法设计。其中：

（1）确定四旋翼无人机动力学模型如下：

设定F={x_I,y_I,z_I}为右手惯性坐标系，其中z_I为垂直于地面的向量，体坐标性设定为B={x_B,y_B,z_B}，并将Ρ(t)=[x(t)y(t)z(t)]^T∈R³定义为惯性坐标系下的位置向量，Θ(t)=[θ(t)φ(t)ψ(t)]^T∈R³定义为惯性坐标系下的欧拉角向量，四旋翼无人机四个电机产生的升力分别定义为f_i(t)，i=1,2,3,4；

四旋翼无人机动力学模型化简为如下形式（会议：IEEEInternationalConferenceonControlApplications(CCA)；著者：ZengW,XianB,DiaoC；出版年月：2011年；文章题目：Nonlinearadaptiveregulationcontrolofaquadrotorunmannedaerialvehicle；页码：133-138）：

$\stackrel{\·\·}{x}=-\frac{1}{m}{K}_1\stackrel{\·}{x}+(\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ})u_1$

$\stackrel{\·\·}{y}=-\frac{1}{m}{K}_2\stackrel{\text{\·}}{y}+(\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ})u_1$

$\stackrel{\·\·}{z}=-\frac{1}{m}{K}_3\stackrel{\·}{z}-g+\cos \mathrm{\φ}\cos \mathrm{\θ}{u}_1$

$\stackrel{\·\·}{\mathrm{\φ}}=-\frac{1}{J_1}{K}_{4}l\stackrel{\·}{\mathrm{\φ}}+\frac{1}{J_1}l{u}_2$

$\stackrel{\·\·}{\mathrm{\θ}}=-\frac{1}{J_2}{K}_{5}l\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_2}l{u}_3$

$\stackrel{\·\·}{\mathrm{\ψ}}=-\frac{1}{J_3}{K}_6\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_3}c{u}_4---\left(7\right)$

其中，m∈R代表飞行器重量，J₁,J₂,J₃∈R为对应轴的转动惯量，K_i∈R，i=1,...,6代表气动阻尼系数，l为螺旋桨到四旋翼无人机重心的距离，c∈R代表一个升力-转矩常量系数，g为重力加速度，式（7）中的虚拟控制输入信号u₁(t),u₂(t),u₃(t)和u₄(t)按如下定义，

$u_1=\frac{1}{m}(f_1+f_2+f_3+f_4)$

u₂=-f₁-f₂+f₃+f₄（8）

u₃=-f₁+f₂+f₃-f₄

u₄=f₁-f₂+f₃-f₄

（2）所述的进行控制算法设计是：

采用非线性内外环（会议：第31届中国控制会议；著者：张垚，鲜斌，殷强，刘洋，王福；出版年月：2012年；文章题目：四旋翼无人机自主控制研究，页码：4862-4867）的PD（比例-微分，Proportional–Differential）控制结构；其中外环根据位置误差计算得出内环姿态的期望角度，内环跟踪期望角度计算出最终的控制输出。水平位置信息通过步骤1）、步骤2）和步骤3）计算得到，高度信息通过机载声呐传感器获得。

在控制算法的计算过程中，虚拟辅助控制变量μ=[μ_x,μ_y,μ_z]通过外环控制器计算得到，表达式如下，

μ_x=u₁(cosφ_dsinθ_dcosψ_d+sinφ_dsinψ_d)

μ_y=u₁(cosφ_dsinθ_dsinψ_d-sinφ_dcosψ_d)（9）

μ_z=u₁(cosφ_dcosθ_d)

由（9）式可将内环期望的姿态角φ_d、θ_d，以及螺旋桨总推力u₁解算出来，表达式如下，

u₁=(μ_x ²+μ_y ²+μ_z ²)^1/2

${\mathrm{\φ}}_d={\sin }^{-1}(\frac{1}{u_1}{u}_x\sin {\mathrm{\ψ}}_d-\frac{1}{u_1}{u}_y\cos {\mathrm{\ψ}}_d)---\left(10\right)$

${\mathrm{\θ}}_d={\sin }^{-1}\left[(\frac{1}{u_1}{u}_x\cos {\mathrm{\ψ}}_d-\frac{1}{u_1}{u}_y\sin {\mathrm{\ψ}}_d){\left(\cos {\mathrm{\φ}}_d\right)}^{1/2}\right]$

外环PD（比例-微分，Proportional–Differential）控制器设计如下,

${\mathrm{\μ}}_x=-k_{\mathrm{xp}}\mathrm{\Δ}{E}_X-k_{\mathrm{xd}}\frac{\mathrm{\Δ}{E}_X}{\mathrm{\Δt}}$

${\mathrm{\μ}}_y=-k_{\mathrm{yp}}\mathrm{\Δ}{E}_Y-k_{\mathrm{yd}}\frac{\mathrm{\Δ}{E}_Y}{\mathrm{\Δt}}---\left(11\right)$

${\mathrm{\μ}}_z=-k_{\mathrm{zp}}\mathrm{\Δ}{E}_Z-k_{\mathrm{zd}}\frac{\mathrm{\Δ}{E}_Z}{\mathrm{\Δt}}$

其中，μ_x、μ_y和μ_z分别代表x、y、z方向的外环虚拟控制输入，k_*p和k_*d分别为*方向可调的比例系数与微分系数,ΔE_X、ΔE_Y和ΔE_Z分别代表x、y、z方向的位置误差。

本发明的基于光流的四旋翼无人机飞行控制方法采用基于地面站的（host-based）飞行控制结构，如图4所示。安装在无人机底部的***头将采集到的图像信息通过2.4GHz无线图传传输到地面站。姿态角信息通过机载IMU（惯性测量单元，InertialMeasurementUnit）测量获得，并经900MHz数传电台传输到地面站。地面站将获得的图像信息与姿态角信息相融合，计算出无人机水平位置信息，进而计算出公式（8）中控制输入量。控制输入量经过PPM（脉冲调制，PulsePositionModulation）信号发生器（pulsegenerator）传输到无人机遥控器。遥控器上的教练开关可以进行手动飞行和自动飞行的切换，以确保无人机故障时安全着陆。

针对四旋翼无人机位置控制的任务，本发明提出了一种新颖的基于光流的飞行控制方法。在控制过程中，首先引入光流信息与姿态角信息进行融合来估计无人机位置信息；然后设计一种内外环结构的PD(比例-微分，Proportional-Differential)控制器，用估计到的无人机位置信息作为外环PD(比例-微分，Proportional-Differential)控制器的反馈信息。与现有方法相比，本发明所涉及传感器仅为机载摄像头和微小型惯性测量单元，具有体积小、重量轻、成本低等优势；本发明方法适用于四旋翼无人机至微四旋翼无人机（重量100克以下），同时适用于室内环境和室外环境。实验结果表明，本发明具有应用价值高、误差率低等特点，且对图像噪声与传感器噪声具有很好的鲁棒性。

下面给出具体的实例：

一、***硬件连接及配置

1.四旋翼无人飞行器本体

本实施例采用轴距200mm碳纤材料的X字型机架、无核直流电机、4寸双叶螺旋桨组装成了四旋翼无人飞行器本体，总重量为60克，最大负载50克，满载重飞行时间最长为5分钟。本发明选用Futaba品牌遥控器，机载2.4GHz遥控信号接收机。遥控器带有教练开关，可用于手动飞行模式与自动飞行模式的切换。

2.地面站***

本实施例地面站运行在笔记本计算机上。计算机配置了因特尔酷睿i5处理器，主频是2.6GHz，内存为2GB。发明相关软件程序基于C/C++编写，其中一些必要的函数应用OpenCV函数库实现。控制信号更新频率为25Hz。

3.机载姿态传感器及控制板

本实施例中无人机机载传感器有***头、无线图传、声纳、数传电台、微型惯性导航单元。其中，***头成像源为1/3CCD，分辨率为640*480像素，输出标准PAL信号。无线图传的频率为2.4GHz，数传电台的频率为900MHz。机载飞控板采用MultiWii的开源飞控板。

4.控制方法参数选定

为保证计算精度，图像金字塔层数选定为5层。摄像头图像比例常数s按摄像头参数计算为0.0075毫米/像素。实验比例系数与微分系数分别为1.10和0.80。

二、实验结果

本实施例在室内进行了定点悬停飞行控制和轨迹跟踪控制的飞行实验。两者飞行高度均为1米。图5显示了悬停飞行实验中传感器测得的滚转角、俯仰角和线速度信息。图6显示了悬停飞行实验中位移的变化曲线。图7显示了悬停飞行轨迹及位置误差信息直方图。定点悬停飞行控制实验结果显示无人机可以在直径为25cm的圆形区域内进行悬停飞行，取得了很好的控制效果。图8是对边长为100厘米正方形轨迹跟踪飞行实验平面位移信息曲线。轨迹跟踪x方向误差为±25厘米，y方向误差在±10厘米。实验中的误差主要来源于所选用的机载惯性导航单元，其精度只有±2度，这在一定程度上限制了控制的精度的提高。当配置更高精度的惯性导航单元时，该方法的控制精度可得到进一步提高。

Claims (7)

1.一种基于光流的四旋翼无人机飞行控制方法，其特征在于，包括如下步骤：

1)利用基于图像金字塔的卢卡斯·卡纳德方法进行光流信息的计算，具体是：

金字塔的每一层都会得到一个从上一层传递过来的运动估计假设，用于本层光流的计算，用L_m表示金字塔的最高层层数，g^*表示图像金字塔第*层的运动估计假设，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向上的运动估计假设，I^*(x,y)表示图像金字塔第*层图像平面坐标为x和y处的亮度，d^*表示图像金字塔第*层的光流量，包含和两个分量，和分别表示图像金字塔第*层在x方向和y方向的光流量，d表示最终光流计算结果；

在第L_m层，初始运动估计假设设为零，即根据基于亮度恒定假设：

$I^{Lm}(x,y)=I^{Lm}(x+d_x^{Lm},y+d_y^{Lm})$

并利用卢卡斯·卡纳德方法算出本层的光流量传递到第L_m-1层的运动估计假设为 $g^{Lm-1}={\[g_x^{Lm-1},g_y^{Lm-1}\]}^T=2(g^{Lm}+d^{Lm}),$ 然后同样根据亮度恒定假设：

$I^{Lm-1}(x,y)=I^{Lm-1}(x+g_x^{Lm-1}+d_x^{Lm-1},y+g_y^{Lm-1}+d_y^{Lm-1})$

并利用卢卡斯·卡纳德方法计算本层光流d^Lm-1，其他层的计算方法以此类推，在计算出图像金字塔最底层的光流d⁰后，最终光流计算结果：

$d=g^0+d^0=\underset{L=0}{\overset{Lm}{\Σ}}{2}^L{d}^L;$

2)采用卡尔曼滤波方法处理光流信息；

3)进行光流和姿态角两者数据融合，以及无人机水平位移的计算；

4)设计比例-微分控制器，包括：

(1)确定四旋翼无人机动力学模型，和(2)进行控制算法设计。

2.根据权利要求1所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，步骤2)所述的采用卡尔曼滤波方法处理光流信息是采用卡尔曼滤波器的离散时间过程方程和测量方程：

X_k＝AX_k-1+ω_k-1

Z_k＝d＝HX_k+υ_k

其中，是k时刻***状态向量，即为光流估计向量，d_x和d_y分别表示在x方向和y方向上光流的估计值；是k-1时刻***状态向量，Z_k∈R²是k时刻***观测向量，d是通过步骤1)中光流信息计算得到的数据结果，即光流的原始数据，随机信号ω_k-1和υ_k分别表示k-1时刻过程激励噪声和k时刻观测噪声，它们是相互独立且为正态分布的，A∈R^2×2和H∈R^2×2是单位矩阵。

3.根据权利要求1所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，步骤3)所述的光流和姿态角两者数据融合采用如下公式，

$d_{xp}=d_x-d_{roll}=d_x-\mathrm{\Δ}\mathrm{\φ}\frac{R_x}{\mathrm{\α}}$

$d_{yp}=d_y-d_{pitch}=d_y-\mathrm{\Δ}\mathrm{\θ}\frac{R_y}{\mathrm{\β}}$

其中，d_xp和d_yp分别是x和y方向的光流水平分量，d_x和d_y分别是在x和y方向经过步骤2)中卡尔曼滤波处理的总测量光流，d_roll和d_pitch分别是x和y方向的光流旋转分量，△φ是两帧图像之间滚转角变化量，R_x是x方向的摄像头分辨率，α是x方向的视野，△θ，R_y和β分别是在y方向的俯仰角变化量、摄像头分辨率和视野。

4.根据权利要求1所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，步骤3)所述的无人机水平位移的计算根据针孔相机模型采用如下公式，

$\mathrm{\Δ}X=d_{xp}s\frac{f}{h}$

$\mathrm{\Δ}Y=d_{yp}s\frac{f}{h}$

其中，△X和△Y分别表示在两帧图像之间其x方向和y方向飞机实际水平位移增量，d_xp和d_yp分别表示由光流和姿态角两者数据融合计算得到光流水平分量，s表示关于摄像头的图像比例常数，f表示摄像头的焦距，h表示摄像头光心到地面的距离。

5.根据权利要求1所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，步骤4)所述的确定四旋翼无人机动力学模型如下：

设定F＝{x_I,y_I,z_I}为右手惯性坐标系，其中z_I为垂直于地面的向量，体坐标系设定为B＝{x_B,y_B,z_B}，并将P(t)＝[x(t)y(t)z(t)]^T∈R³定义为惯性坐标系下的位置向量，Θ(t)＝[θ(t)φ(t)ψ(t)]^T∈R³定义为惯性坐标系下的欧拉角向量，四旋翼无人机四个电机产生的升力分别定义为f_i(t)，i＝1,2,3,4；

四旋翼无人机动力学模型化简为如下形式：

$\begin{array}{c}\stackrel{\·\·}{x}=-\frac{1}{m}{K}_1\stackrel{\·}{x}+\left(\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\right)u_1\\ \stackrel{\·\·}{y}=-\frac{1}{m}{K}_2\stackrel{\·}{y}+\left(\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\right)u_1\\ \stackrel{\·\·}{z}=-\frac{1}{m}{K}_3\stackrel{\·}{z}-g+{\mathrm{cos\φcos\θu}}_1\\ \stackrel{\·\·}{\mathrm{\φ}}=-\frac{1}{J_1}{K}_{4}l\stackrel{\·}{\mathrm{\φ}}+\frac{1}{J_1}{\mathrm{lu}}_2\\ \stackrel{\·\·}{\mathrm{\θ}}=-\frac{1}{J_2}{K}_{5}l\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_2}{\mathrm{lu}}_3\\ \stackrel{\·\·}{\mathrm{\ψ}}=-\frac{1}{J_3}{K}_6\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_3}{\mathrm{cu}}_4\end{array}$

其中，m∈R代表飞行器重量，J₁,J₂,J₃∈R为对应轴的转动惯量，K_i∈R，i＝1,…,6代表气动阻尼系数，l为螺旋桨到四旋翼无人机重心的距离，c∈R代表一个升力-转矩常量系数，g为重力加速度，式中的虚拟控制输入信号u₁(t),u₂(t),u₃(t)和u₄(t)按如下定义，

$u_1=\frac{1}{m}(f_1+f_2+f_3+f_4)$

u₂＝-f₁-f₂+f₃+f₄

u₃＝-f₁+f₂+f₃-f₄

u₄＝f₁-f₂+f₃-f₄。

6.根据权利要求1所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，步骤4)所述的进行控制算法设计是：采用非线性内外环控制结构；其中外环根据位置误差计算得出内环姿态的期望角度，内环跟踪期望角度计算出最终的控制输出，水平位置信息通过步骤1)、步骤2)和步骤3)计算得到，高度信息通过机载声纳传感器获得。

7.根据权利要求6所述的基于光流的四旋翼无人机飞行控制方法，其特征在于，在控制算法的计算过程中，虚拟辅助控制变量μ＝[μ_x,μ_y,μ_z]通过外环控制器计算得到，表达式如下，

μ_x＝u₁(cosφ_dsinθ_dcosψ_d+sinφ_dsinψ_d)

μ_y＝u₁(cosφ_dsinθ_dsinψ_d-sinφ_dcosψ_d)

μ_z＝u₁(cosφ_dcosθ_d)

由上式可将内环期望的姿态角φ_d、θ_d，以及螺旋桨总推力u₁解算出来，表达式如下，

u₁＝(μ_x ²+μ_y ²+μ_z ²)^1/2

${\mathrm{\φ}}_d={\sin }^{-1}(\frac{1}{u_1}{u}_x{\mathrm{sin\ψ}}_d-\frac{1}{u_1}{u}_y{\mathrm{cos\ψ}}_d)$

${\mathrm{\θ}}_d={\sin }^{-1}\[(\frac{1}{u_1}{u}_x{\mathrm{cos\ψ}}_d-\frac{1}{u_1}{u}_y{\mathrm{sin\ψ}}_d){\left({\mathrm{cos\φ}}_d\right)}^{1/2}\]$

外环比例-微分控制器设计如下,

$\begin{array}{c}{\mathrm{\μ}}_x=-k_{xp}{\mathrm{\ΔE}}_X-k_{xd}\frac{{\mathrm{\ΔE}}_X}{\mathrm{\Δ}t}\\ {\mathrm{\μ}}_y=-k_{yp}{\mathrm{\ΔE}}_Y-k_{yd}\frac{{\mathrm{\ΔE}}_Y}{\mathrm{\Δ}t}\\ {\mathrm{\μ}}_z=-k_{zp}{\mathrm{\ΔE}}_Z-k_{zd}\frac{{\mathrm{\ΔE}}_Z}{\mathrm{\Δ}t}\end{array}$

其中，μ_x、μ_y和μ_z分别代表x、y、z方向的外环虚拟控制输入，k_*p和k_*d分别为*方向可调的比例系数与微分系数,△E_X、△E_Y和△E_Z分别代表x、y、z方向的位置误差，ψ_d为期望的偏航角度。