一、方案论证

技术路线

先通过集成机架和电机构成飞行器的主体机械结构；单片机、IMU和相关传感器解算获取飞行器的实时姿态，再通过PID控制算法控制电机，使飞机可以调整自己的姿态匹配预设的姿态。基本实现了飞机型器平稳飞行和按照预设姿态飞行的能力。在此基础之上，通过接收视觉模块和其他测量模块收集的信息，编写上层应用程序，不断调整预设的飞机姿态，从而使之符合题目要求的飞行动作。

系统结构

 1、软件结构

如图1，通过定时器每 1ms 产生一次中断，在中断函数中解算姿态、计算PID和更新PWM输出，从而控制飞机的基础飞行。即使在程序运行过程中产生死循环等阻塞式循环，也可以有效避免飞机空中停机。在非中断函数外的循环任务调度程序中添加实现题目要求的功能的流程——本流程不断根据题目要求改变对飞机姿态的预期，从而不断改变飞机的姿态和航向，完成要求的动作。

图1

  2、硬件结构

如图2，以MCU为核心，通过串口5与IMU通信，IMU通过其内置的姿态传感器传递给MCU传感器信息，由MCU内部烧写的姿态结算算法和PID算法来计算对应PWM波形，进而通过定时器1的PWM发生器发送给电调控制电机转动；MCU还通过串口2接收视觉模块（OpenMV）传递的数据，由结合烧写的任务流程完成对应泼洒农药的功能。MCU还通过计时器读取接收机数据，通过ADC接收电池电压数据，为飞机飞行提供保障。

图2

方案描述

  1、基础飞行功能的实现

采用小型四旋翼无人机通用的飞行方案——编写姿态解算算法（互补滤波算法）解算飞行器的实时姿态；并通过基础 PID 算法控制飞行器平稳飞行，当MCU提供预期的飞行姿态，飞行器就可以通过PID算法将自身姿态向预期的飞行姿态变化。在搭配必要的飞行控制逻辑优化飞行性能。

  2、泼洒农药任务的实现

通过地磁YAW角控制机头朝向大体不改变：因为我们组装的飞机为定点提供数据的光流模块并不在飞机的几何中心，所以我们的飞机在转动时会有很大误差，不方便激光笔精准发射激光。于是我们采用了控制飞机的朝向不变，通过改变飞机的各方向的运动速度控制飞机运动过绿色色块点亮激光。首先飞机飞向“A”(泼洒起始点)，在“A”上定点。定点后，以此处为坐标原点，此时根据设计的飞行路线，飞行器泼洒农药的飞行轨迹就确定了。仅需要调整飞行速度的方向，依照设计好的路线飞行即可。在飞行的途中可能会出现光流传感器发现不了地面纹理导致飞行器不能沿着直线飞行的情况，这时我们通过视觉模块拟合飞行途中遇到的黑线，在飞行器飞行途中不断提供修正，以达到稳定飞行的目的。

比较与选择

  1、飞行控制逻辑的选择

线性逻辑：起初我们尝试了直接依照线性流程来调用编写好的飞行控制程序来控制飞机的动作，但经常会由于程序设计的疏漏导致代码执行进入逻辑上的死循环，而产生错误的仿真结果。使飞行器产生空中停机的风险。

并行逻辑：通过定时器每 1ms 产生一次中断，在中断函数中解算姿态、计算PID和更新PWM输出，从而控制飞机的基础飞行。即使在程序运行过程中产生死循环等阻塞式循环，也可以有效避免飞机空中停机。在非中断函数外的循环任务调度程序中添加实现题目要求的功能的流程——本流程不断根据题目要求改变对飞机姿态的预期，从而不断改变飞机的姿态和航向，完成要求的动作。

  2、任务方案的选择

 （1）飞行路线的选择：我们参考了实际应用中的农业无人机的飞行方式。并避免设计过于复杂的路线而增加失误的风险，所以我们采用了需尽量减少飞行器转弯的路线（如图3） ：

图3

 （2）飞行器喷洒农药方式的选择：我们起初考虑在每一个绿色区域上悬停喷洒，但在实践中我们意识到我们的方法不仅比较消耗时间，加之频繁调整飞机位置会使机体晃动，激光光斑抖动明显，稳定性不高。后来我们参考了农业飞行器的应用，于是我们采用了以一个固定的速率飞过一排绿地，同时控制激光笔以一个固定的速率发射激光。当两者的速度匹配时，就能完成题目要求均匀喷洒农药。此方法的对于激光光斑的抖动问题解决效果较好。

 （3）飞行方式的选择：起初我们的思路是通过UWB进行室内定位来精准确定飞机的位置，但经过模块测试之后。发现UWB模块需要提前布置基站，并测    量出大量位置的精准坐标。我们预估在考试场地布置基站和调参难以实现，于是我们最终放弃了这个方案。我们最终参考题目的应用场景，采用了视觉模块捕获的图像信息来指导飞行器的飞行：通过下视图像模块捕获地面关键目标点（如起飞点、绿地、绿地边界）坐标来指导飞机执行相应动作的时间、地点。

二、理论分析与计算

控制方法描述

• 飞行器平稳飞行原理

       四旋翼：四旋翼对角对称，可以在飞行时相互抵消转动时产生的扭矩。受到MCU输出的PWM波控制，使得飞行器在飞行过程中保持相对稳定。

       姿态解算：通过IMU内置的加速度传感器、陀螺仪、磁力计，参考飞行器控制相关理论，将从这三种传感器采集到的数据进行代数运算，得到的四元数经过变换转化为对应的姿态角，从而得知飞行器此时的姿态。

       PID反馈控制：PID的作用是将事实测量出的姿态角和期望的姿态角求差，输出将实施姿态角趋近于期望姿态角的控制波形。将飞行姿态和预设的姿态进行取差，对差进行比例、积分、微分的处理，用以控制飞行器的电机的转速，从而令实际姿态可以反应快地、带有预测性质地、消除长期误差地逼近预设地期望姿态角。

• 视觉部分识别原理

       将测试地面的各种关键点抽象为具体图形：起飞点被抽象为有特定占空比的黑色色块、"A"点被抽象为周围有绿色的黑色色块。通过边沿检测探查绿地边沿，向飞行器发送一行喷洒完毕的信号，飞跃绿地后控制飞行器停止飞行。

• 喷洒农药控制方法

       我们采用固定频率喷洒农药的方法，所以飞行速度和激光发射光斑的频率匹配十分重要——这两个频率在飞行速度给定的情况下经反复实验、调整，最终得到合适匹配的取值。

参数计算

• PID三个参数的调整：

       调此参数需要大量的试验，我们进行了大量的尝试，并咨询了相似飞行器的用户关于相关参数的大致取值范围，最终调节到了一个即使飞行器动作有较大范围变动，飞行状态也相对平稳的PID参数。

• 泼洒时飞行速度：

       此参数需要匹配激光发射频率来实验得到数据。当激光发射周期为2s时，飞行器能够合适打点的速度大约为18cm/s。

• 泼洒速度（发射激光的频率）：

       规定为 2s。

• 正下方坐标：

       本参数标识飞行器正投影的中心点在视觉模块的中的位置，用于标明目标点到飞行器中心的相对位置。本参数的调节需要令激光笔反射到地面的位置为正中心，所以调节激光笔光斑的位置为视野最中心点后记录激光笔光斑的位置大约为(67,73)。

三、电路与程序设计

系统组成

  1、硬件部分（表1所示）

  2、软件部分

• 任务调度器（执行泼洒任务）：20ms一次，通过实时向飞机PID系统发送期望的飞机姿态，从而控制飞机向期望的姿态改变，从而执行规定的飞行任务。
• 平稳飞行控制模块：该模块由定时器以中断的形式每1ms调用不同的。 解析五个串口收到的数据：光流数据交换、数据传输模块数据交换、更新电调数据、更新飞行指示灯、储存收到的遥控信号灯，校准IMU上的磁力计等、读取电压信息。
• 初始化函数：进行中断优先级的设置、运算模式的设置、各种外设的配置。
• 遥控信号控制模块：解析遥控器信号、切换基础、提高部分模式、失控保护。
• 数据传输模块：在竞赛过程中，排查和调试遇到的程序、硬件问题需要精确得知飞行器的各种数据信息。有的问题需要飞行器在空中飞行过程中进行调试，此时读取数据不能通过串口等有线方式进行。飞行器的各种状态数据、模块的各种参数可以通过数据传输模块发送给地面站PC，进而方便分析和解决问题。

程序代码（视觉部分）：

# Untitled - By: hgf - 周四 11月 4 2021
import sensor
import image
import time
import network
import usocket
import sys
import math
from pyb import UART
from pyb import LED
from pyb import Pin
'''
Type:
    199:A
    200:green
    201:line
    202:直角
    203:起飞、降落点
'''
p_out = Pin('P7', Pin.OUT_PP)
grey_threshold=(69, 83, -15, 21, -37, 23)#灰色阈值
white_threshold=(97,103,-5,5,-5,5)
green_threshold=(56, 89, -47, -5, 22, 46)
black_threshold=(0, 39, -46, 16, -8, 56)
thresholds=[
green_threshold
]
area_tmp=[0,0]
illroi=(84,50,12,14)#正下方一小块
red_led = LED(1)
green_led = LED(2)
blue_led = LED(3)
ir_led = LED(4)
p_out.low()#激光打点置低位
t=0
i=0
look_a=0
uart=UART(1,115200)
def FindMax(blobs):
    max_size=1
    if blobs:
        max_blob = 0
        for blob in blobs:
            blob_size = blob.w()*blob.h()
            if ( (blob_size > max_size) & (blob_size > 100)   ) :#& (blob.density()<1.2*math.pi/4) & (blob.density()>0.8*math.pi/4)
                if ( math.fabs( blob.w() / blob.h() - 1 ) < 2.0 ) :
                    max_blob=blob
                    max_size = blob.w()*blob.h()
        return max_blob
def UART_Send(FormType, Location0, Location1, frame_type=0, area=65535, width=0):   #通信协议
    fHead = bytes([170,170])                                                        #帧头为AA
    Frame_End = [85, 85]
    fFormType_tmp = [FormType]
    prame = bytes(0)
    # 计算区域值
    area_tmp[0] = area & 0xFF #area也是十六位，但其实可以不是面积，啥都行
    area_tmp[1] = area >> 8
    area = bytes(area_tmp)
    fFormType = bytes(fFormType_tmp)
    fLocation0 = bytes([Location0])#十六位
    fLocation1 = bytes([Location1])
    fEnd = bytes(Frame_End)                                                         #帧尾为0x55
    fwidth = bytes([width])
    if frame_type == 0:
        FrameBuffe = fHead + fFormType + fLocation0 + fLocation1 + fEnd
    elif frame_type == 1:
        FrameBuffe = fHead + fFormType + fLocation0 + fLocation1 + area + fwidth + fEnd
        #print(FrameBuffe)
        pass
    return FrameBuffe
def jiguang():          #激光打点
    time0=time.ticks_ms()
    if(time0%1000<100):
        #打点0.1S
        p_out.high()
    else:
        p_out.low()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
while True:
    t=t+1
    clock.tick()
    time0=time.ticks_ms()
    img = sensor.snapshot().lens_corr(1.8)  #消除鱼眼畸变

    for l in img.find_lines(x_stride=1,y_stride=60,roi=(92,0,30,120),threshold=40,theta_margin=15,rho_margin=15):
        if(l.theta()<10 or l.theta()>170):  #寻找横线
            #img.draw_line(l.line(), color = (255, 0, 0))
            print(l.theta())
            Type=201
            p0=l.theta()
            p1=0
            area=0
            #uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))
    for l in img.find_lines(x_stride=50,y_stride=5,roi=(0,0,160,40),threshold=40,theta_margin=15,rho_margin=15):
        if(l.theta()>80 and l.theta()<100): #寻找竖线
            #img.draw_line(l.line(), color = (0, 255, 0))
            print(l.theta())
            Type=202
            p0=l.theta()#左上右下是大，左下右上是小
            print(p0)
            p1=0
            area=0
            #uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))

    green_blobs = img.find_blobs([green_threshold],x_stride=5,y_stride=5,pixel_threshold=25,roi=illroi)
    if green_blobs:
        #jiguang()
        look_a=look_a+1
        if(time0%2000<100):
            if(t>800 and look_a>200):
                p_out.high()
        elif(time0%2000>=100):
            p_out.low()
        for blob in green_blobs:
            Type=200
            p0=blob.cx()
            p1=blob.cy()
            area=blob.area()
            #uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))
            if blob.x()+blob.w()<150:
                img.draw_cross(blob.x()+blob.w(),blob.y())
                Type=202
                p0=blob.w()+blob.x()
                p1=blob.y()
                #uart.write(UART_Send(Type, p0, p1, area = 0, frame_type=1))
    else:
        p_out.off()
    black_blobs = img.find_blobs([black_threshold],x_stride=50,y_stride=3,pixel_threshold=50)
    #judge_right_angle()
    if black_blobs:
        for blob in black_blobs:
            if blob.w()/blob.h() > 5:
                img.draw_rectangle(blob.rect())
                img.draw_cross(blob.cx(), blob.cy())
                Type=201
                p0=blob.rotation_deg()
                p1=blob.cy()
                area=blob.area()
                uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))
            elif blob.w()/blob.h()>0.5 and blob.w()/blob.h()<1.5 and blob.pixels()>30:
                green_blobs_in_A=img.find_blobs([green_threshold],roi=blob.rect(),x_stride=5,y_stride=5,pixel_threshold=25)
                if green_blobs_in_A:
                    blue_led.on()
                    img.draw_rectangle(blob.rect(),color=(255,0,0))
                    img.draw_cross(blob.cx(), blob.cy(),color=(255,0,0))
                    Type=199
                    p0=blob.cx()
                    p1=blob.cy()
                    area=blob.area()
                    if(t>800):
                        uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))
                else:
                    blue_led.off()
                    img.draw_rectangle(blob.rect())
                    img.draw_cross(blob.cx(), blob.cy())
                    Type=203
                    p0=blob.cx()
                    p1=blob.cy()
                    area=blob.area()
                    if(t>800):
                        uart.write(UART_Send(Type, p0, p1, area = area, frame_type=1))

原理框图

飞行器各模块通信原理图（如图2）、程序设计原理图（如图1、图4）

图4

各部分电路图

飞机底板（如图5、6）、视觉模块（如图7）

图7

系统软件设计，流程图

软件结构图（如图1）、泼洒农药作业流程图（如图4）

四、测试方案与测试结果

测试方案及测试条件

  1、测试条件

我们根据题目所给的简易场地图片，打印了和场地等大的地板贴纸，在此贴纸贴在空地上完成整个测试。

  2、测试方案

• 基础部分：测试和记录飞行器在每个绿色块上发射的光斑数——如果所有绿色方块上累计光斑数在1-3之间，所有灰色地面不存在激光光斑，则完成基础部分的测试。
• 提高部分：我们将所有可能抽取的三张绿色草坪组合依次用白色胶布遮蔽，测试是否能依然满足：整个过程中，在每个绿色区域内，累计光斑数维持在1-3之间，灰色地面不存在光斑。

测试结果的完整性

经过多次测试，飞行器可以完美泼洒农药的频率维持在92.3%。 

测试结果分析

由于我们最终采用：尽量少使用坐标点来改变飞机航向的系统，这要求了我们需要高度依赖光流的反馈数据，也就是要求了地面需要能够较容易地被光流提取出纹路信息——这表明我们的方案对于采用哑光贴纸的飞行环境具有针对性，在亚光贴纸和非摩尔纹的地面性能很好。

五、参 考 文 献

[1].黄智伟.全国大学生电子设计竞赛训练教程。电子工业出版社，2010

[2].张淑清.嵌入式单片机STM32设计及应用技术。国防工业出版社，2015

[3].王广雄.控制系统设计[M]。宇航出版社，1992

[4].王彤.PC机在测量和控制中的应用[M]，哈尔滨工业大学出版社。1995

[5].刘豹.现代控制理论，机械工业出版社.2004

[6].秦永元.惯性导航[M]第一版，北京科学出版社。2005

[7].王冬来，吕强，刘峰.小型四轴飞行器动力学参数测定方法设计[J]，科技出版社。2011