Research on flight control of quadrotor UAV based on visual image

近年来，在应用市场多元激励的作用下，无人机(unmarned aerial vehide，UAV)的研发经历了前所未有的变革。伴随着各种单一功能和多用途军用无人机实践应用的日益成熟化和产品化，无人机在民事领域的应用前景也越来越广阔。与此同时，用无人机执行任务与反无人机介入的斗争也愈演愈烈。在应用市场、任务背景和应对多重挑战等因素的作用下，无人机的独立性、自主性、实时性要求越来越高。因此，如何快速准确地获取无人机和目标的飞姿信息，并将之高效精准地转化为无人机的飞控参量成为无人机迈向智能化的关键环节^[1]。

在无人机领域的研究中，基于视觉图像的位姿信息解算与参量转换研究正逐渐成为无人机自主化研究的热点，其核心是从连续的2维视觉图像及高度信息中获取无人机六自由度信息，利用牛顿力学和动量力矩等定理完成运动方程的构建和位姿信息的转换。参考文献[2]中通过对比无人机输入航迹与自身航迹的偏差，计算方向向量与加速度向量，利用比例-积分-微分(proportion-integration-differentiation, PID)控制算法完成无人机航迹和姿态进行控制。参考文献[3]中提出了一种结合学习特征的图像视觉伺服方法，利用统一投影模型, 分析了虚拟单位球体投影的解耦视觉伺服方法，研究了结合非线性模型预测控制的球体投影约束视觉伺服控制方法，并设计出结合离散化参考轨迹的无标定视觉预测控制算法。参考文献[4]中介绍了神经网络补偿结构的作用和设计方法, 基于无人机非线性姿态运动学和动力学模型设计了基于神经网络补偿的动态逆控制器，并进行了数学仿真实验。本文中主要进行无人机飞行六自由度信息的解算、运动模型转换以及动态跟踪方法的研究，并通过仿真实验对所提出的方法进行验证。

经过应用环境的选择和淘汰，无人机主要分固定翼、旋翼和直升机三大类，其中基于视觉导航跟踪及自主控制的研究又以旋翼无人机为主。旋翼无人机包括四旋翼、六旋翼和其它多旋翼无人机，四旋翼无人机又分为“十”型和“X”型两类^[5]。作者以“十”型无人机为例, 进行了相关理论研究和仿真实验。

“十”型四旋翼无人机的结构如图 1所示。从图中可以看出两条机臂成90°相交于无人机的质心，1号~4号旋翼和电机分别安装于无人机飞行空间的前后左右4个方位，其结构、大小完全一样，且关于质心对称，每个旋翼和电机距离质心的长度相同，电机控制旋翼转速产生升力。在调整无人机飞行姿态时，通过电机调整旋翼的转速，改变旋翼的总力矩。转速调整越大，总力矩变化越大，无人机飞姿调整就越快。由牛顿定理可知，旋翼在旋转的同时，会向电机施加一个反作用力(又称反扭矩)，为了消除这种反作用力，就需要在同一方向上施加另一种反方向的力^[6]。因此，四旋翼无人机的4个旋翼中相邻两个旋翼的方向是相反的。在无人机飞行时，1号、3号旋翼和2号、4号旋翼产生的顺时针和逆时针反作用力相互抵消，无人机就可以保持平稳飞行。

Figure 1.  Structure of the "十" four-rotor UAV

展开无人机控制原理的分析之前，先介绍无人机飞行的六自由度信息，分为质心的3个沿轴移动自由度和3个绕轴转动自由度，分别对应无人机的空间位置(前后量x，侧向量y，升降量z)和无人机姿态(俯仰角θ，滚转角ϕ，偏航角ψ)，即六自由度信息{x, y, z, θ, ϕ, ψ}，或六自由度速度信息{$\dot x, \dot y, \dot z, \dot \theta , \dot \phi , \dot \psi $}，或六自由度加速度信息{$\ddot{x}, \ddot{y}, \ddot{z}, \ddot{\theta}, \ddot{\phi}, \ddot{\psi}$} ^[7-8]。

无人机的飞行模式对应其六自由度信息，即前后、侧向、升降、俯仰、滚转和偏航运动。四旋翼无人机的不同飞行模式主要通过调整4个控制参量，使4个旋翼电机产生不同的旋转速度，提供4个不同大小的升力(累计为总升力F_sum)与重力mg(m为质量，g为重力加速度)矢量合成，产生指定方向和角度的外力F_ext，控制无人机按照外力的方向飞行，如图 2所示。当外力方向与重力方向同轴时，无人机进行悬停、升降飞行，当外力方向与重力方向垂直时，无人机进行横移、偏航飞行，当外力方向与重力方向成其它角度时，无人机进行俯仰、滚转飞行。

Figure 2.  Schematic diagram of the action of the four-rotor UAV

四旋翼无人机可进行6个自由度的飞行运动，但是只有4个独立的控制输入量，因此其是1种欠驱动系统。由牛顿定理可知, 升力随着转速的变化而变化，所以通过调节旋翼电机的转速就可以控制无人机的飞行姿态。无人机处于悬停状态时，4个旋翼提供的合力刚好等于无人机自身的重力，即外力F_ext=0，设定此时旋翼的转速为N₀，当改变部分或全部旋翼的转速时，外力F_ext≠0，无人机就会按照外力的方向进行飞行，如图 3所示。图中Δa, Δb, Δa′为旋翼的转速调整变量，其中Δa, Δa′为一组对应的变量，当同一轴上的旋翼分别为N₀+Δa, N₀-Δa时，无人机只存在此轴上的外力，此时无人机沿该轴做直线运动，如图 3a~图 3c所示；当同一轴上的旋翼分别为N₀+Δa, N₀-Δb时，无人机存在该轴与z轴组成平面的外力，此时无人机绕另一轴做绕轴运动，如图 3d~图 3e所示；当不同轴上的旋翼分别为N₀+Δa, N₀-Δa时，无人机存在O_ux_uy_u平面的外力，此时无人机绕z运动，如图 3f所示。

Figure 3.  Control process of four-rotor UAV

无人机飞行过程中有两个通用的坐标系，即无人机质心坐标u系O_ux_uy_uz_u和地面坐标g系O_gx_gy_gz_g^[9-10]。与不断变化的质心坐标系不同，地面坐标系是固定的，如图 4所示。为便于理解，必须先将无人机的姿态角进行明确的定义，从图中可以看出，俯仰角θ是质心坐标系x_u轴与地面坐标系O_gx_gy_g平面的夹角，以O_gx_gy_g平面上方为正；滚转角ϕ是质心坐标系y_u轴与地面坐标系O_gx_gy_g平面的夹角，以O_gx_gy_g平面上方为正；偏航角ψ是质心坐标系z_u轴在地面坐标系O_gx_gy_g平面的投影与地面坐标系x_g轴夹角，角度值以x_g轴为起点，以东偏北逆时针方向为正。

Figure 4.  Spatial model construction

四旋翼无人机运动方程主要包括物像关系式、质心运动方程和地面坐标运动方程^[11-12]，如下所示：

(1) 式为物像关系式，式中, C₀为相机已知的内参系数，其中dx, dy为像元素大小，(i_O, j_O)为质心O_u在像平面的坐标，f为相机的焦距；(2)式为无人机质心运动方程，式中, (x_{u, 0}, y_{u, o}, z_{u, o})为无人机在g系的初始坐标，(x_u, y_u, z_u)为无人机质心在无人机坐标系下实时坐标，(x_g, y_g, z_g)为无人机质心在地面坐标系的实时坐标，L_{u, g}为无人机坐标系相对于地面坐标系的转换系数，且满足:

式中, θ, ϕ, ψ为无人机的三轴姿态角；(3)式为地面坐标运动方程，其中，${F_{{\rm{sum}}}} = \sum\limits_1^4 b \mathit{\Omega }_i^2$, b为旋翼电机的升力系数，Ω_i为旋翼电机的转速，i为1号~4号电机；K={k₁  k₂  k₃}，分别为无人机在地面坐标系x轴、y轴、z轴3轴的空气阻力系数，综上可得，在地面坐标系下，无人机运动方程为：

四旋翼无人机的控制参量主要是用来控制无人机的四旋翼电机，给无人机提供特定方向的动力，因此无人机的欠输入参量与无人机的飞行动力紧密相关。由于四旋翼无人机的质心即为其中心，所以其姿态转动可以视为刚体绕质心的定点转动。根据转动力学定理可知，地面坐标系下四旋翼无人机的力学方程^[13-15]如下式所示:

式中, L为角动量，I为转动惯量，ω为角速度，M为无人机的外力矩, (dL/dt)_u为无人机质心坐标系下的角动量的变化率。

又因，，，其中, I_x, I_y, I_z为机体坐标系下xyz 3轴向地面坐标系xyz 3轴的转动惯量。所以有：

无人机转动过程中，作用力主要有旋翼产生的动力和空气阻力。由牛顿定理和欧拉转动定理可知，“十”型四旋翼的转动力矩τ如下式所示:

式中, L为旋翼中心距质心的距离，τ_a(τ_{a, x}, τ_{a, y}, τ_{a, z})为空气阻力力矩, τ_F为动力阻力力矩。

继而可得十字型无人机的角运动方程如(9)式所示，考虑到四旋翼无人机飞行速度较小，受到空气阻力作用也较小，可将空气阻力忽略，得到(10)式：

设无人机的四输入控制量的力矩分别为俯仰力矩μ₁、滚转力矩μ₂、偏航力矩μ₃、爬升力矩μ₄，可得下式:

忽略空气阻力的作用，由(5)式、(10)式可得无人机的六自由度系统方程^[16]如下式所示：

无人机实际飞行中，通常只涉及俯仰飞行和偏航飞行，特殊条件下才伴有滚转飞行，因此对滚转角暂不进行研究。本文中基于视觉图像的无人机飞姿控制研究核心内容就是：通过初始状态下无人机的定速定向飞行，识别已知运动状态的特定地面合作目标，通过图像中目标像点位移情况，推算无人机的飞姿信息和高度，并利用无人机航迹与合作目标轨迹的夹角等信息，得出无人机完成跟踪或降落过程所需要的飞控参量。从前面可知，“十”型四旋翼无人机初始高度H₀定向飞行时，质心坐标系与地面坐标系只存在一个夹角也即俯仰角θ₀，由于此时无人机没有进行滚转飞行即ϕ₀=0，因此此时无人机航迹与合作目标的航迹的像平面夹角也即偏航角ψ₀，无人机要完成跟踪或降落功能，就需要对航迹进行修正，修正量即为H₀, θ₀, ψ₀。由参考文献[17]和参考文献[18]可知，θ_n=${array}{l} \arctan \left[ {\Delta a_n^*{H_n}/(fa)} \right], {{\dot \theta }_n} = \left( {{\theta _n} - {\theta _{n - 1}}} \right)/\Delta t, {{\ddot \theta }_n} = \left( {{{\dot \theta }_n} - } \right. \left. {{{\dot \theta }_{n - 1}}} \right)/\Delta t, {\psi _n} = \arctan \left[ {\left( {a_{ni}^*\;{\rm{d}}y} \right)/\left( {a_{nj}^*\;{\rm{d}}x} \right)} \right], {{\dot \psi }_n} = \\ \left( {{\psi _n} - {\psi _{n - 1}}} \right)/\Delta t, {{\ddot \psi }_n} = \left( {{{\dot \psi }_n} - {{\dot \psi }_{n - 1}}} \right)/\Delta t {array}$，其中, a为合作目标外边长度，a_n^*为n时刻外边a的像素值，Δa_n^*为两条相对外边像的长度差，a_ni^*, a_nj^*分别为a_n^*在像素坐标的行宽和列宽。所以基于视觉图像的四旋翼无人机四输入控制量为：

设计了一种具有方向引导功能的多形状组合彩色合作目标，如图 5所示。红色、绿色引导带指示目标移动方向，黑色、白色图形提升目标与周围环境的识别率，圆形、正方形能够提供较为精确地方向和角度信息。根据合作目标独有的灰度特征、几何特征、颜色特征、以及点特征，先后经灰度变换、色相、饱和度明度(hue saturation value, HSV)色彩变换、以及Hu不变距匹配等方法，从机载相机拍摄的连续帧实时图像中准确识别出合作目标，并获取有关的位姿信息。如位移像素x_u, y_u, z_u，外边像素a_n^*，外边像素差Δa_n^*等。同时，在无人机低空和超低空飞行时，颜色导引带可以快速提供准确的目标方向信息，便于无人机快速的识别和跟踪合作目标, 如图 6所示。

Figure 5.  Cooperation target

Figure 6.  Target recognition and extraction process

根据同物不同时像平面像素变化情况，利用无人机物像关系式、质心运动方程和地面坐标方程等，计算出地面坐标系下合作目标相对于无人机的3轴位移变量，再结合第2.3节中θ_n和ψ_n的公式以及ϕ₀=0，得出无人机的3轴角度，相对时间进行求导，得出无人机三轴线速度信息{$\dot x, \dot y, \dot z$}和角速度信息{$\dot \theta , \dot \phi , \dot \psi $}，进而获得无人机的实时高度、坐标转换矩阵L_{u, g}等。

使用基于视觉图像的无人机飞行控制算法如图 7所示。主要通过提取图像中目标特征点的位移信息，获取无人机的实时位姿信息，利用前后两个固定时间段位姿信息对时间的导数，得到无人机的速度信息。以当前时刻无人机与合作目标的位姿差信息，作为下一时刻加速度的控制矢量，直至在地面坐标系下无人机与合作目标的位姿信息差为0，此时无人机完成稳定跟踪功能^[19-20]，如下式所示:

Figure 7.  UAV flight control algorithm based on visual image

式中, ΔO_c^*为合作目标当前时刻与前一时刻的位移像差。同理，当合作目标中心与无人机质心在像平面保持重合或近似重合时，无人机即可完成自主降落功能。

从(14)式可知，当Δx_g, Δy_g, Δz_g三者出现数值时，表示无人机完成搜索识别阶段，当Δx_g, Δy_g两者为零时，无人机完成稳定跟踪阶段，当Δx_g, Δy_g, Δz_g都为零时，即完成自主降落功能。

为了进一步验证基于视觉图像的无人机控制算法的科学性和可行性，利用MATLAB软件进行了仿真实验，主要研究不同高度不同速度下无人机完成识别跟踪及降落过程的时效性和精准性。

表 1中为仿真实验的部分参量。

本文中无人机控制系统分为位置控制器和姿态控制器，利用同物不同时像素位移情况，解算出无人机与合作目标的位姿关系作为当前时刻实测值，通过基于视觉图像的无人机飞行控制算法，解算无人机与合作目标位姿差作为下一时刻的期望值，根据期望值不断调整位置与姿态两个控制器，进而控制不同时刻的实测值，当实测值逐渐降为0时，即完成自主降落功能。通过研究同高异初速条件下无人机与合作目标的高度和中心位距离随时间的变换关系，验证算法的可行性和精确性，实验结果如图 8和图 9所示。

Figure 8.  Landing height curve

Figure 9.  Center and centroid distance

通过仿真实验发现，无人机初速对跟踪、靠近、着陆3个阶段的影响是不尽相同的。高度方面：跟踪阶段，初速对合作目标的识别效率有一定影响，初速为0时，识别时间越短，主要因为悬停时姿态角为0，便于无人机搜索识别。靠近阶段:初速小时，无人机z轴方向分量也就小，高度变化就慢，随着期望位姿信息的逐渐变大，z轴方向分量也随之变大，高度变换也随之加大；初速大时，无人机z轴方向分量也就大，高度变化就快，随着期望位姿信息的逐渐变小，z轴方向分量也随之变小，高度变化也随之减小，达到最大值时保持不变，降落曲线则表现为类直线。着陆阶段，由于经过前两个阶段的调整控制，无人机的状态基本一致，所以初速对着陆阶段没有影响，高度随z轴速度一致逐渐减为0。距离差方面：跟踪阶段，中心距离都是逐渐增大，初速越大，距离越大，合作目标运动方向分量大小相同后达最大值。靠近阶段，距离越小，位姿信息变化的期望值越小，距离变化就越缓；距离越大，位姿信息变化的期望值越大，距离变化就越剧烈，同理达到最大值时保持不变直至进入着陆阶段。着陆阶段，初速对距离差同样没有影响，距离随着无人机的降落逐渐减为0。

针对无人机跟踪或降落过程中信源易被干扰、环境感知较差问题，利用视觉图像的无源性、实时性特性，在同物不同时物像像素变化关系的基础上，根据牛顿力学定理与刚体转动定理，通过无人机位姿信息的提取与控制参量的转换，确立了无人机六自由度信息与四输入参量之间的关系，提出了基于视觉图像的无人机飞行控制算法，并进行了仿真实验。实验结果表明，所提出的控制系统算法在识别跟踪目标后，可以独立完成无人机自主降落功能，克服了无线电信源易干扰有延时的不足，且动态性能良好。该算法的成熟与应用将有助于真正实现无人机自主化和智能化。