农用无人机移动补给平台自主降落算法与试验

祖林禄 侯加林 陈 民 王 红 苏 斐

(1.山东农业大学机械与电子工程学院，泰安 271018; 2.山东农业大学山东省园艺机械与装备重点实验室，泰安 271018；3.山东农业大学山东省农业装备智能化工程实验室，泰安 271018)

0 引言

近年来，农业航空技术快速发展，电动多旋翼农用无人机(简称农用无人机)由于具有可垂直起降、空中悬停、操作维护简便等优点，在农业作业中受到广泛关注[1]。但续航能力和喷洒物补给方法是制约农用无人机工作时间和工作效率的重要因素[2]。

农用无人机移动补给平台的关键技术之一是无人机能够自主降落到运动的补给平台上。农用无人机降落过程涉及运动目标识别、运动轨迹预估、跟踪控制器设计等多项技术[3]，对降落算法要求极高，包括目标识别算法快速、准确，轨迹预估算法实时、稳定，以及跟踪控制器响应速度快、跟踪误差小等。在应对运动速度较快的目标时，若采用单一PID控制，则系统响应速度较慢[3]。为此，本文提出一种基于模糊逻辑和PID分段控制的算法，并进行仿真试验和现场试验，以实现农用无人机对运动目标的稳定跟踪和在移动补给平台上的降落。

1 目标跟踪轨迹拟合

1.1 问题描述

农用无人机空基平台的目标检测跟踪不仅要求算法能适应运动背景、实时性高，还要求算法对环境变化、目标尺度变化、目标速度变化等方面具有很好的鲁棒性。背景减除法、帧差法、光流法[3]是静态背景下的运动目标检测算法，无法满足本文要求。MeanShift算法[4]因算法简单、收敛条件宽松而具有较好的实时性，但是无法适应目标尺度变化、运动速度较快等情况。Camshift算法[5]虽然实时性较好，但难以适应复杂背景下的目标跟踪。

农用无人机对补给平台的跟踪属于复杂运动背景下的快速跟踪，而且两者都处于非线性、非高斯类型的机动状态，此时对目标进行位置跟踪属于非线性系统状态估计问题[6]，所以上述5种算法均不能满足此类要求。本文提出一种解决策略：采用粒子滤波跟踪算法并配合轨迹拟合算法完成对运动目标的位置跟踪，将拟合出的预估轨迹作为期望路径输入农用无人机飞行控制器。

1.2 粒子滤波算法

粒子滤波是一种基于蒙特卡洛方法和递推贝叶斯估计的统计滤波方法。通过因子采样法对状态量进行更新，根据t-1时刻的所有粒子(即样本)的概率重新进行采样，高概率粒子被采样次数多，低概率粒子则可能被舍弃，得到t时刻的粒子。t时刻的所有粒子对应的测量值为t时刻的粒子赋予新的概率，用于t+1时刻粒子的生成[7]。粒子滤波算法归纳如下：

(i=1,2,…,Ns)

(1)

并归一化

(2)

则可获得时刻t位置参数x的最小均方估计为

(3)

(5)时刻t=t+1，转到步骤(2)。

粒子滤波适用于非高斯背景的非线性随机系统，对本文所研究的对非线性、非高斯类型的运动目标跟踪，能够使跟踪精度逼近最优估计，有助于提升识别跟踪算法的准确率。

1.3 轨迹预测

1.3.1轨迹拟合函数的选择

所选的拟合函数应能准确描述一段时间内的运动特性，因此选取可表征大部分目标运动轨迹的二次函数作为拟合函数[8]。

1.3.2拟合函数参数求解

(1)设二次函数表达式为

=a0+a1x+a2x2

(4)

此拟合函数共有3个未知参数，选用最小二乘法，通过最小化试验数据误差的平方和估计拟合函数的未知参数。

(2)误差平方和为

(5)

(3)由多元函数求极值条件得(式(5)中E→0)

(6)

(4)将式(6)转换成矩阵形式，并简化得

(7)

可得

EA=Y

(8)

A=(XTX)-1XTY

(9)

XTX为奇异矩阵，所以A=[a0a1a2]T存在唯一最优解。为保证跟踪效果，采用15组最新数据实时更新拟合函数的参数。

1.4 目标跟踪流程

为保证农用无人机准确快速地跟踪运动目标，需要把粒子滤波跟踪算法输出的目标位置坐标利用拟合函数进行运动轨迹拟合，以实现当目标因遮挡而暂时性消失时保证农用无人机按照拟合轨迹飞行，大大提升了农用无人机在非线性随机环境中的跟踪能力[9]。跟踪算法具体实现流程如图1所示。

图1 跟踪算法具体实现流程

2 模糊逻辑PID分段控制算法设计

2.1 PID控制器

PID控制器因原理简单、理论完善、功能强大得到广泛应用，包括比例控制器、积分控制器、微分控制器[10]。比例控制器中输出与误差成比例关系；积分控制器可加速系统趋近设定值，并消除稳态误差；微分控制器可提升整定时间及系统稳定性[11]。本文采用串级PID控制算法，以农用无人机与运动目标的位置偏差为外环输入，以农用无人机接近运动目标的速度偏差为内环输入，确保农用无人机能快速、准确、稳定跟踪运动目标。若定义evk为速度偏差，uvk为串级PID内环输出，则串级PID控制器内环可表达为

(10)

式中kPv、kIv、kDv——比例、积分、微分系数

2.2 模糊逻辑控制器

2.2.1结构设计

2.2.2模糊逻辑控制规则设计

模糊控制器设计中选择输入语言变量的词集为{正大(PB)、正中(PM)、正小(PS)、零(Z)、负小(NS)、负中(NM)、负大(NB)}，选择输出语言变量的词集为{正大(PB)、正中(PM)、正小(PS)、零(Z)、负小(NS)、负中(NM)、负大(NB)}。本文选用三角形和梯形混合型隶属度函数、7级输入变量、7级输出变量。图2a、2b为模糊逻辑控制器的输入变量隶属度函数及其分布，图2c为输出变量隶属度函数及其分布。

2.2.3去模糊化

(11)

2.3 模糊逻辑PID分段控制系统结构

经典PID控制器虽然控制精度较高，但其响应速度慢、超调量大。模糊控制具有响应速度快、超调量小的优点，却存在导致控制精度降低的问题。为满足农用无人机追踪降落过程中响应速度快、超调量小的控制目标，本文提出一种改进策略：以误差输入量e与设定阈值的比较结果为转换依据，进行PID控制器和模糊逻辑控制器的分段控制，且此分段控制系统不影响PID控制器与模糊逻辑控制器的独立性。

图2 隶属度函数及其分布

本文设计的分段控制系统结构如图3所示。在农用无人机与运动目标相距较远时，距离误差e大于设定阈值，此阶段由模糊逻辑控制器对农用无人机实施控制。因其响应速度快的优点可以使农用无人机快速接近移动目标，避免出现因跟踪速度慢而丢失跟踪目标的问题。当距离误差e小于设定阈值时，由PID控制器对农用无人机实施控制。其控制精度高的优点可满足农用无人机在降落过程中实现对移动平台的精确锁定，防止因农用无人机与移动平台间存在位置偏差而导致农用无人机难以成功降落到移动平台上。

图3 分段控制系统结构图

3 系统整体功能实现

3.1 农用无人机动力学模型构建

四旋翼农用无人机的运动包括3个轴向线运动和绕3个轴向角运动[14]。为准确描述农用无人机在三维空间内的姿态、位置、速度等信息，需要定义2个三维笛卡尔直角坐标系。如图4所示，全局坐标系为Oe，采用NED(North(北)-East(东)-Down(下))坐标表示方法[15]。机体坐标系为Ob，坐标原点取机体的质心。机体坐标系的Xb-Yb-Zb方向定义为机身的前-右-下。在机体坐标系下，农用无人机绕Zb轴运动为偏航(yaw)运动，绕Yb轴运动为俯仰(pitch)运动，绕Xb轴运动为横滚(roll)运动。

图4 坐标图

假设农用无人机在全局坐标系Oe中的速度为V，则V=[VXVYVZ]、重力加速度为g、农用无人机质量为m；理想状况下所有电机产生的总升力为fA；农用无人机的转动惯量为J；基向量e=[0 0 1]T；R为机体坐标系相对于全局坐标系的旋转矩阵[16]；农用无人机相对于全局坐标系的角速度为ω=[ωXωYωZ]T，可以用矩阵表达为

(12)

农用无人机在全局坐标系中位置可用P=[pXpYpZ]T表示。农用无人机动力学方程可表示为[16]

(13)

3.2 图像坐标系的变换

为方便求出农用无人机与所追踪运动目标的位置偏差，需要将该运动目标在摄像头中的三维坐标变换到全局坐标系中。如图4所示，全局坐标系为Oe，摄像机坐标系为Oc，成像平面坐标系为Oi，f为摄像机焦距，T为空间变换的平移矢量，表示全局坐标系原点在摄像机坐标系中的坐标。假设(xc,yc,zc,1)、(xe,ye,ze,1)分别为空间内任一点在摄像机坐标系和全局坐标系下的齐次坐标，则两坐标系间的变换关系为

(14)

其中，s为任意非零参数，三维坐标系旋转参数存储在3×3正交单位矩阵Rr中，O=(0,0,0)为零矢量，M为摄像机外部参数矩阵，决定了摄像机光轴在全局坐标系中的坐标，可将全局坐标系下的坐标映射到摄像机坐标系中。

3.3 不同坐标系下误差的计算

把农用无人机通过GPS和气压计等传感器得到的在大地坐标系下的空间位置坐标变换到全局坐标系Oe下得到机体坐标PN=(xN,yN,zN)，若摄像机获取并转换得到的运动目标在全局坐标系Oe中的坐标为x(t)=(x′t,y′t,z′t)，可知在全局坐标系Oe下农用无人机与运动目标的距离误差为ep=(xN-x′t,yN-y′t,zN-z′t)。把滤波后的陀螺仪传感器、加速度传感器和地磁罗盘传感器数据进行实时解算求出横滚角φ、俯仰角θ及偏航角φ。全局坐标系Oe变换到机体坐标系Ob变换矩阵[17]为

(15)

可得在机体坐标系Ob下用于输入模糊逻辑PID混合控制器的误差ee=(xe,ye,ze)。其中xe、ye经过模糊逻辑PID混合控制算法处理后的结果可分别用于控制农用无人机的横滚、俯仰运动，以完成对运动目标的跟踪，ze为机体与运动目标高度误差，在机体下降到移动补给平台的过程中消除[18]。

3.4 自主降落流程

以字母H为主要标志的降落平台结构垂直对称、清晰且便于识别，H的开口方向可用于校正农用无人机降落方向。但是此降落平台图像特征过于简单，农用无人机在运动过程中难以把此图像精确锁定在正下方并实施降落[3]。本文设计一种带有辅助识别区的降落平台，如图5所示，平台图案分为上下两部分：上部为降落区，以H图案为主要标志；下部为辅助识别区，以二维码图案为主要标志。下部辅助识别区的黑白色块图案颜色对比鲜明，能够增加运动平台的颜色可识别特征，有助于农用无人机对降落平台的精确锁定[19]。此外，农用无人机可利用光流算法检测黑白色块图像中光点与暗点的移动来计算运动平台相对于农用无人机的移动速度，并把此相对速度作为农用无人机降落过程中的参考变量，进一步提升降落阶段的稳定性。

图5 以字母H为主要标志的降落平台

农用无人机在移动补给平台上自主降落可分解为2个阶段：第1阶段，模糊逻辑算法主导控制下的追踪阶段；第2阶段，PID算法主导控制下的降落阶段。跟踪降落过程的流程如图6所示。其中xe为x轴方向误差，ye为y轴方向误差[20]。

图6 跟踪降落过程流程图

4 试验结果

为更好地验证模糊逻辑PID分段控制算法的优越性，设置模糊逻辑控制、PID控制为对照试验，并分别进行软件仿真试验及现场试验。

4.1 Matlab仿真试验

在Simulink中搭建仿真试验平台，包括期望位置模块(用于输入拟合函数拟合预测的期望路径)、当前位置模块(表示农用无人机当前运动状态和空间位置)、模糊逻辑PID分段控制器模块(处理输入的距离偏差并按照相应的控制规则转换为控制量输出)、电机模块(在输出控制量的控制下改变转速以完成农用无人机的横滚、俯仰、偏航运动)和机体模型模块(农用无人机动力学模型)。

3种算法对阶跃信号和正弦信号的跟踪仿真结果如图7所示。为衡量3种算法的控制效果，对阶跃信号跟踪时控制过程上升时间、超调量、调节时间和稳态误差指标进行求解，结果如表1所示。可得模糊逻辑PID分段控制算法比PID算法超调量低12%、调节时间缩短2 s；相比于模糊逻辑算法稳态误差低0.12 cm。对正弦信号跟踪时，应用皮尔逊相关系数衡量农用无人机实际运动轨迹和期望的正弦轨迹的拟合程度，模糊逻辑算法作用时相关系数为0.970，PID算法作用时相关系数为0.981，模糊逻辑PID分段控制算法作用时的相关系数最高，为0.998[21]。

图7 农用无人机对阶跃信号和正弦信号的跟踪结果

表1 3种控制算法控制效果对比

4.2 现场试验

现场试验采用的平台技术参数为：农用无人机轴距330 mm、1 400 kV直流无刷电机、8寸碳纤维桨、好盈40 A无刷电调、领航者飞控、格式11.1(3S)锂电池、Ublox-NEO-M8N GPS模块、LenaCV视觉摄像头、NVIDA Jetson TX2图像处理器。试验在山东农业大学体育场完成，如图8所示。将控制算法分别设置为模糊逻辑算法、PID算法、模糊逻辑PID分段算法各进行了10次测试，试验结果如表2所示。由表2可知，模糊逻辑PID分段控制算法与PID算法相比，平均响应时间由1.1 s减少到0.6 s左右，响应速度提升约45.8%，且降落时振荡情况较轻；与模糊逻辑算法相比，跟踪误差小10 cm左右，降低约72%；在降落成功率方面，分段控制算法比PID算法高40%，比模糊逻辑算法高70%；10次测试降落误差最大为7.2 cm。

图8 现场试验图

表2 现场试验结果

5 结论

(1)分段控制算法在补给平台突然加速、急转弯等运动状态突变情况下的响应速度比单一PID控制器提升45.8%；在农用无人机跟踪车载降落平台过程中的超调量比单一PID控制器控制低12%。

(2)分段控制算法在控制农用无人机对车载降落平台进行跟踪时的跟踪误差比单一模糊逻辑控制算法降低72%。

(3)针对单一H图案降落平台图像特征过于简单的问题，提出一种带有辅助识别区的降落平台，配合模糊逻辑PID分段控制算法可使农用无人机在垂直降落过程中的误差缩小至7.2 cm内。