用于农场自主管理的四旋翼飞行器控制系统及控制方法
【专利摘要】本发明涉及一种用于农场自主管理的四旋翼飞行器控制系统及控制方法，智能移动终端通过GPRS网络将作业农场的范围和设定入侵者信息送四旋翼飞行器，四旋翼飞行器通过搭载的GPS模块获取搜索区域位置执行飞行任务，利用搭载的语音模块播放刺激声波对设定入侵者进行恐吓。任务过程中四旋翼飞行器采集图像数据并无线传送至地面控制中心，地面控制中心通过图像处理获得四旋翼飞行器的实时位置及与入侵者的相对姿态数据，通过GPRS网络将飞行引导数据送至四旋翼飞行器使之接近入侵者并播放刺激声波，直至入侵者驱离作业农场范围。用于实现非干预情况下识别、跟踪和驱逐隐藏在作物下的鸟类。可广泛用于大型农场、机场等需要自动驱赶鸟类等入侵物的场景。
【专利说明】
用于农场自主管理的四旋翼飞行器控制系统及控制方法
技术领域
[0001] 本发明涉及一种控制系统，特别涉及一种用于农场自主管理的四旋翼飞行器控制 系统及控制方法。
【背景技术】
[0002] 随着科技和社会发展，传统农业正加速转型，将来会形成具有一定规模的农场。大 型农场存在鸟类、家畜等动物"入侵破坏"的现象，雇佣工人每天进行巡查存在着成本高、巡 视盲点多、巡视周期长等缺点。因此，采用四旋翼飞行器代替人工进行监测、巡逻、干预和驱 逐，实现农场非干预管理是现代农业发展的必然趋势。目前，国内尚无此类专利公开。
【发明内容】
[0003] 本发明是针对农场中存在的鸟类破坏作物的问题，提出了一种用于农场自主管理 的四旋翼飞行器控制系统及控制方法，用于实现非干预情况下识别、跟踪和驱逐隐藏在作 物下的鸟类。
[0004] 本发明的技术方案为：一种用于农场自主管理的四旋翼飞行器控制系统，包括四 旋翼飞行器和远程控制子系统，飞行控制子系统、侦测和驱逐子系统装在四旋翼飞行器上， 飞行控制子系统包括嵌入式微控制器、2对螺旋桨电机、GPRS网络、GPS模块和惯性测量单 元，侦测和驱逐子系统包括摄像头、语音模块和5.8GHz图像传输模块，远程控制子系统包括 图像接收模块、地面控制中心和智能移动终端；
[0005] 智能移动终端通过GPRS网络将作业农场的范围和设定入侵者信息送四旋翼飞行 器中嵌入式微控制器，摄像头采集的图像数据通过5.8GHz图像传输模块采用无线传输方式 送出，由图像接收模块接收后送到地面控制中心，地面控制中心通过图像处理获得四旋翼 飞行器的实时位置及设定入侵者的相对姿态数据，通过GPRS网络将飞行引导数据送至四旋 翼飞行器，四旋翼飞行器中嵌入式微控制器控制2对螺旋桨电机进行飞行引导，四旋翼飞行 器通过GPS模块获取搜索区域位置送嵌入式微控制器，嵌入式微控制器送信号到侦测和驱 逐子系统，控制摄像头进行采集图像，控制语音模块播放刺激声波对设定入侵者进行恐吓；
[0006] 惯性测量单元测量加速度及角速度送嵌入式微控制器，嵌入式微控制器根据滤波 和姿态解算处理后进行姿态融合并输出PWM波到电机，以此来控制四个电机的转速的方式 改变四旋翼的姿态，超声波信号送嵌入式微控制器，嵌入式微控制器根据超声波测距原理 控制飞行器在搜索目标时的飞行高度。
[0007] 所述嵌入式微控制器采用两层双串级PID控制进行目标跟踪，位置PID作为外环， 姿态PID为内环，控制对象为电机转速，期望位置是地面控制中心根据接收到的视频数据计 算并发送的飞行引导数据，包含世界坐标系下被跟踪目标与四旋翼飞行器的水平位移，期 望位置与实际位置反馈信号差值送外环PID，外环PID输出与当前姿态比较后送姿态PID，姿 态PID输出为跟踪信号，进而通过控制四个电机的转速来改变四旋翼自身的姿态，实现对目 标的连续跟踪。
[0008] 所述嵌入式微控制器采用两层双串级PID控制进行作业域搜索，位置PID作为外 环，高度PID作为内环，搜索域中心点坐标作为期望位置，期望位置与实际位置反馈信号动 差送外环PID，外环PID输出与当前高度比较后送高度PID，高度PID输出为搜索跟踪信号，不 断修正四旋翼飞行器姿态，逐步接近期望位置。
[0009] 所述地面控制中心对设定入侵者进行识别和跟踪方法包含以下步骤：
[0010]①：四旋翼飞行器机载摄像头采集图像并通过无线方式发送；
[0011] ②:远程控制中心的地面站PC端软件获取步骤①的视频图像；
[0012] ③:进行图像处理，实现对图像的特征点提取及特征匹配；
[0013] ④：目标识别，利用帧差法提取运动目标并进行二值化及膨胀腐蚀处理；
[0014] ⑤：目标跟踪，使用粒子滤波算法与强跟踪Kalman滤波算法对目标进行跟踪；
[0015]⑥:通过GPRS网络把步骤⑤的跟踪坐标发送给嵌入式微控制器；
[0016] ⑦:嵌入式微控制器接收步骤⑥的数据并控制电机的转速来改变姿态，从而对运 动的目标连续跟踪。
[0017] 本发明的有益效果在于:本发明用于农场自主管理的四旋翼飞行器控制系统及控 制方法，能自动识别、跟踪和驱逐农场鸟类等"入侵者"包括隐藏在作物下的鸟类，整个工作 过程无需人工操作干预。很大程度上节约了人力成本和时间，具有较好的实用性和便捷性。 可广泛用于大型农场、机场等需要自动驱赶鸟类等"入侵物"的场景。
【附图说明】
[0018] 图1为本发明用于农场自主管理的四旋翼飞行器控制系统结构示意图；
[0019] 图2为本发明飞行器在跟踪目标时使用的目标跟踪PID控制器原理图；
[0020]图3为本发明四旋翼飞行器姿态控制PID控制器原理图；
[0021 ]图4为本发明摄像头透视成像模型图；
[0022]图5为本发明飞行器在作业域搜索目标时使用的作业域搜索PID控制器原理图；
[0023] 图6为本发明地面站PC端对目标进行识别和跟踪的流程图。
【具体实施方式】
[0024] 如图1所示用于农场自主管理的四旋翼飞行器控制系统结构示意图，包括四旋翼 飞行器和远程控制子系统，飞行控制子系统、侦测和驱逐子系统装在四旋翼飞行器上，飞行 控制子系统包括嵌入式微控制器(飞行控制中心）、2对螺旋桨、GPRS网络、GPS模块和惯性测 量单元，侦测和驱逐子系统包括摄像头、语音模块和5.8GHz图像传输模块，远程控制子系统 包括图像接收模块、地面控制中心和智能移动终端，其中，所述飞行控制子系统完成四旋翼 飞行器的空中姿态控制、飞行引导和搜索位置控制，所述侦测和驱逐子系统用于"入侵者" 发现、追踪和驱逐，所述远程控制子系统是用户端控制设备，用于用户确定作业农场的范围 及远程控制四旋翼飞行器作业过程。惯性测量单元测量加速度及角速度送嵌入式微控制 器，嵌入式微控制器根据滤波和姿态解算处理后进行姿态融合并输出PWM波到电机，以此来 控制2对螺旋桨四个电机的转速的方式改变四旋翼的姿态，超声波信号送嵌入式微控制器， 嵌入式微控制器根据超声波测距原理控制飞行器在搜索目标时的飞行高度。
[0025] 在上述方案中，所述的飞行控制子系统利用采用两层串级闭环控制方法实现对所 述2对螺旋桨电机的控制，实现飞行姿态的稳定；所述摄像头采集的图像数据通过所述 5.8GHz图像传输模块采用无线传输方式把图像送到所述地面控制中心，所述地面控制中心 通过图像处理获得所述四旋翼飞行器的实时位置及其与"入侵者"的相对姿态数据，并通过 GPRS网络将飞行引导数据送至所述四旋翼飞行器进行飞行引导；所述四旋翼飞行器利用 GPS模块获取当前搜索位置，并利用确定农场下一个搜索区域。
[0026]在上述方案中，所述的侦测和驱逐子系统利用所述地面控制站对所述摄像头采集 的图像数据进行处理，对麻雀等鸟类"入侵者"进行识别，并，经GPRS向所属飞行控制系统发 送并利用所述语音模块播放刺激声波对"入侵者"进行恐吓，达到驱逐效果。
[0027]在上述方案中，智能移动终端通过GPRS网络将作业农场的范围和设定入侵者信息 送四旋翼飞行器中四旋翼飞行器，所述的远程控制子系统利用所述智能手机终端在地图上 确定作业农场的范围，并启动作业任务，所述四旋翼飞行器在接收任务后启动并飞往指定 农场，作业过程中，所述智能手机终端可利用GPRS网络发送命令远程中断所述四旋翼飞行 器的任务并返航。
[0028] 在上述方案中，所述的四旋翼飞行器，作业域搜索、"入侵"识别及跟踪和驱逐过程 自动进行，不需要人为干预和操控。
[0029] 在上述方案中，所述的作业域搜索方法，包括以下几个步骤：
[0030] 1、所述四旋翼飞行器到达待作业农场后，根据储存的任务数据，包括待作业农场 的形状和面积数据，进行最优计算，确定搜索域的个数和搜索域中心点的地理坐标位置；
[0031] 2、使用所述飞行器上的超声波传感器，设定其飞行高度为距作物50公分；
[0032] 3、所述飞行器准备飞行，并到达第一个搜索域中心点；
[0033] 4、获取所述摄像头图像信息并实时发送到地面控制站，启动所述语音模块放音功 能，发出刺激声波实现预驱逐效果；
[0034] 5、若有麻雀等运动物体飞出，由地面控制站识别，采用"入侵"目标跟踪方法进行 跟踪，整个放音过程持续到被跟踪目标超过农场边界；
[0035] 6、重复步骤2~5直到搜索过程覆盖整个作业域。
[0036]在上述方案中，所述的"入侵"目标跟踪方法，包括以下几个步骤：
[0037] 1、地面控制站接收图像信息；
[0038] 2、基于粒子滤波和卡尔曼滤波的目标跟踪算法对目标进行跟踪，并预测目标下一 次可能出现的位置；
[0039] 3、通过GPRS网络发送飞行引导数据，包含被跟踪目标期望俯仰角和横滚角；
[0040] 4、飞行器接收步骤4发送的数据，通过目标跟踪PID控制器及姿态PID控制器构成 的两层串级控制器不断修正飞行器的姿态；
[0041 ] 5、重复以上步骤，直到目标离开农场边界。
[0042]图2为飞行器在跟踪目标时使用的目标跟踪PID控制器原理图，目标跟踪PID控制 器由两层双串级控制器组成，位置PID作为外环，姿态PID为内环共同构成了目标跟踪控制 器，期望位置是地面控制中心根据接收到的视频数据计算并发送的飞行引导数据(包含了 世界坐标系下被跟踪目标与四旋翼的水平位移），期望位置与实际位置反馈信号动差经过 外环PID后送内环姿态PID，与当前姿态比较后送姿态PID，姿态PID输出为跟踪信号，姿态控 制上应用了角速度作为内环，角度作为外环的串级PID控制。
[0043] 图3为四旋翼飞行器姿态控制PID控制器原理图，该PID控制器由两层双串级控制 器组成，角度PID作为外环，角速度PID为内环。期望姿态角根据公式(4)、（5)求得。
[0044] 图4为摄像头透视成像模型，根据世界坐标系与摄像机坐标系之间的转换关系式 和摄影几何学知识，目标在图像平面的像素位置与目标和飞行器之间水平相对位移坐标变 换公式：
[0047]其中，x，y为水平位移(在二维平面上目标到飞行器的距离），h为由超声波测量出 的飞行器当前高度，β为摄像头的俯仰角，Θ为物像连线与镜头光轴之间的夹角，（m，Vl)为像 素点在图像平面坐标系中的位置，（u Q，VQ)为图像像素坐标系中的坐标原点。设每一个像素 在X轴与y轴上的物理尺寸分别为dx与dy，位置PID期望输入为u= [ux Uy uz]T，则经过位置 PID计算有：
[0049] 其中，0d，Yd，Wd分别为姿态PID期望输入姿态角(俯仰角、横滚角、偏航角），ho为u 到原点的距离。
[0050] 则得到姿态PID期望姿态角Θ4Ρ γ d:
[0051] yd = sin_1[(ux sinitd-Uy cositd)/ho] (4)
[0052] 0d = sin_1[(uxcositd-uysinitd)(cos γ d)0.5/h0] (5)
[0053] 图5为飞行器在作业域搜索目标时使用的作业域搜索PID控制器原理图，作业域搜 索PID控制器由串级控制器组成，位置PID作为外环，高度PID作为内环共同构成了作业域搜 索控制器。搜索域中心点坐标作为期望位置，经过PID控制器后，不断修正飞行器姿态，使其 逐步接近期望位置。
[0054] 对隐藏在作物下的鸟类等进行搜索时使用作业域搜索方法，该方法主要包括以下 几个步骤：
[0055]步骤①:使用超声波模块测距(本实施例中超声波模块型号为HC-SR04)，利用公式 s = (Eh· c)/2,式中，s为测量距离(m)，Eh为高电平持续时间（高电平的持续时间就是超声波 从发射到返回的时间），c为声速(340m/s)，控制飞行器在搜索目标时的飞行高度；
[0056]步骤②:首先假定农场为规则的方形s，其边长为m，均勾分成η个搜索域(mi，ni2， ni3，…），以A矩阵表不：
[0058] 式中，n = N2。
[0059] 步骤③:该区域由一系列方形组成，其边长为m/n。由于逐一在每个区域进行检测 会浪费大量的时间而且四旋翼携带的电池能量有限，考虑到鸟类等对周边的环境变化比较 敏感，因此只要在一个中间区域进行干扰，则会影响到两边的区域。具体步骤为：
[0060] (1)剔除边界点，实际搜索矩阵A°:
[0062]式中，u为不确定点，mj(i = 2,3,4......，j = 2,4,6......)为划分的区域块；
[0063] (2)简化搜索矩阵sA:
[0065] (3)从左到右从上到下蛇形排列，分别提取矩阵SA的元素。
[0066] 以n22这点为例详细说明搜索步骤。
[0068] 从矩阵Si可以看出：Π 22影响了周边的四个点，用符号 > 表不。如果在这四个点中任 意一处发现目标，则进行驱逐。由于鸟类具有群体活动行为，某个区域有活动会影响到其它 区域鸟类的活动，因此排除点如^^咖心^以此方法进行搜索目标洇此实际需要检测的 区域可以简化为N2/9。假定区域1112有驱逐目标(丹），则四旋翼到达区域n 12有效高度处，此时 四旋翼悬停，近似为不动点。然后鸣笛干扰，利用帧差法对区域m2进行检测。如果有驱逐目 标(Pi)飞出则会检测到移动物体，对其进行追踪直到该目标离开S区域，以防止(Pi)逃离n 12 再次到r^(i#l，j辛2);若此时没有检测到(P〇,则继续对n44进行搜索，重复上面的步骤，最 后完成整个S域的搜索。
[0069] 图6为地面中心PC端对目标进行识别和跟踪的流程图，该"入侵"目标跟踪方法包 含了以下步骤：
[0070] 步骤①:飞行器机载摄像头采集图像并通过无线方式发送；
[0071 ]步骤②:远程控制中心的地面站PC端软件获取①的视频图像；
[0072]步骤③:进行图像处理，实现对图像的特征点提取及特征匹配；
[0073]步骤④：目标识别，利用帧差法提取运动目标并进行二值化及膨胀腐蚀处理；
[0074]步骤⑤：目标跟踪，使用粒子滤波算法与强跟踪Kalman滤波算法对目标进行跟踪；
[0075] 步骤⑥:通过GPRS把⑤的跟踪坐标发送给嵌入式微控制器；
[0076] 步骤⑦:嵌入式微控制器接收⑥的数据并控制电机的转速来改变姿态，从而对运 动的目标连续跟踪。
【主权项】
1. 一种用于农场自主管理的四旋翼飞行器控制系统，其特征在于，包括四旋翼飞行器 和远程控制子系统，飞行控制子系统、侦测和驱逐子系统装在四旋翼飞行器上，飞行控制子 系统包括嵌入式微控制器、2对螺旋桨电机、GPRS网络、GPS模块和惯性测量单元，侦测和驱 逐子系统包括摄像头、语音模块和5.8GHz图像传输模块，远程控制子系统包括图像接收模 块、地面控制中心和智能移动终端； 智能移动终端通过GPRS网络将作业农场的范围和设定入侵者信息送四旋翼飞行器中 嵌入式微控制器，摄像头采集的图像数据通过5.8GHz图像传输模块采用无线传输方式送 出，由图像接收模块接收后送到地面控制中心，地面控制中心通过图像处理获得四旋翼飞 行器的实时位置及设定入侵者的相对姿态数据，通过GPRS网络将飞行引导数据送至四旋翼 飞行器，四旋翼飞行器中嵌入式微控制器控制2对螺旋桨电机进行飞行引导，四旋翼飞行器 通过GPS模块获取搜索区域位置送嵌入式微控制器，嵌入式微控制器送信号到侦测和驱逐 子系统，控制摄像头进行采集图像，控制语音模块播放刺激声波对设定入侵者进行恐吓； 惯性测量单元测量加速度及角速度送嵌入式微控制器，嵌入式微控制器根据滤波和姿 态解算处理后进行姿态融合并输出PWM波到电机，以此来控制四个电机的转速的方式改变 四旋翼的姿态，超声波信号送嵌入式微控制器，嵌入式微控制器根据超声波测距原理控制 飞行器在搜索目标时的飞行高度。2. 根据权利要求1所述用于农场自主管理的四旋翼飞行器控制系统的控制方法，其特 征在于，所述嵌入式微控制器采用两层双串级PID控制进行目标跟踪，位置PID作为外环，姿 态PID为内环，控制对象为电机转速，期望位置是地面控制中心根据接收到的视频数据计算 并发送的飞行引导数据，包含世界坐标系下被跟踪目标与四旋翼飞行器的水平位移，期望 位置与实际位置反馈信号差值送外环PID，外环PID输出与当前姿态比较后送姿态PID，姿态 PID输出为跟踪信号，进而通过控制四个电机的转速来改变四旋翼自身的姿态，实现对目标 的连续跟踪。3. 根据权利要求1所述述用于农场自主管理的四旋翼飞行器控制系统的控制方法，其 特征在于，所述嵌入式微控制器采用两层双串级PID控制进行作业域搜索，位置PID作为外 环，高度PID作为内环，搜索域中心点坐标作为期望位置，期望位置与实际位置反馈信号动 差送外环PID，外环PID输出与当前高度比较后送高度PID，高度PID输出为搜索跟踪信号，不 断修正四旋翼飞行器姿态，逐步接近期望位置。4. 根据权利要求1所述述用于农场自主管理的四旋翼飞行器控制系统的控制方法，其 特征在于，所述地面控制中心对设定入侵者进行识别和跟踪方法包含以下步骤： ① :四旋翼飞行器机载摄像头采集图像并通过无线方式发送； ② :远程控制中心的地面站PC端软件获取步骤①的视频图像； ③ :进行图像处理，实现对图像的特征点提取及特征匹配； ④ ：目标识别，利用帧差法提取运动目标并进行二值化及膨胀腐蚀处理； ⑤ ：目标跟踪，使用粒子滤波算法与强跟踪Kalman滤波算法对目标进行跟踪； ⑥ :通过GPRS网络把步骤⑤的跟踪坐标发送给嵌入式微控制器； ⑦ :嵌入式微控制器接收步骤⑥的数据并控制电机的转速来改变姿态，从而对运动的 目标连续跟踪。
【文档编号】G05D1/08GK106094849SQ201610436351
【公开日】2016年11月9日
【申请日】2016年6月17日
【发明人】盛存宝, 乐燕芬, 施伟斌, 罗红玉, 莫礼东, 倪驾宇
【申请人】上海理工大学