声明

$\qquad$ 本文的算法在openMV IDE例程的基础上进行原创，在比赛结束后予以发表；本文在作者比赛经历的基础上书写，内容属实；本文提供的参数与代码并不一定适用于所有四旋翼，但方案是可以借鉴的。
$\qquad$ 本文的算法建议创建的硬件环境：

接收机和飞控板（推荐使用匿名科创的，使用前需要校准）
STM32F407（用作辅助控制）
openMV Cam3（板载摄像头及STM32F765,高性能芯片，作视觉处理）
广角镜头(120°~185°，度数越高视野越广，鱼眼效应越严重)
光流（定高定点，使用前需要校准）和激光定高（二者配合才能平稳起飞降落、防止悬空漂移）
航模电池（应飞机重量而异，电源线不宜太长，否则电源线产生的磁场将干扰罗盘而产生航向角零漂）
航模电池充电器（切记不要买B3,2天才能充满一节，推荐使用B6或者A6）
电调×4；电机×4（条件允许的情况下电机和电调的钱不能省）
低速桨/高速桨×4（不能买错方向，一正一反为一对，共一对）
保护架4个（推荐买全方位保护架，特别是新手）
起落架1个，建议安装防震套，减震效果其实并不好但可以防止调试时飞机骤降导致起落架直接折断。

openMV参考网站：
星通科技openMV例程

1.四旋翼飞行控制简介

$\qquad$ 玩过四旋翼的人都知道，四旋翼的姿态控制普遍使用欧拉角表示，即三个参数——俯仰(pitch)，横滚(roll)，偏航（yaw）。按照大白话解释就是①前进/后退②左右平移③转头。接收机是四旋翼的飞控接收遥控器信号的装置，接收的是三路PWM信号，分别对应着欧拉角三个参数。PWM的频率是固定的(可以在手册上查到，务必用示波器测准，否则会造成控制失效),而三路PWM的占空比表示的就是三个控制量（俯仰、横滚、偏航）的大小。简单地说，俯仰为正代表的前进，为负代表后退；横滚为正代表右平移、为负代表左平移；航向角为正代表右转、为负代表左转。而控制量的正负是由PWM波的占空比决定的，占空比也需要使用示波器测。一般遥控器会有一个占空比死区（我在算法里往往会把它死区中点的占空比设置为零点，为了写控制量方便），在该死区内，通过光流和飞控内置的姿态保持算法保持欧拉角参数不变。超过死区上限的占空比为正，小于死区下限的占空比为负。我们算法的目的就是利用PID控制三个控制量达到我们的目标控制量。

2.飞行控制算法

2.1.接收机PWM生成

$\qquad$ 首先需要用示波器获取遥控器产生的PWM信号，测定遥控器三路通道中回中、死区上限、死区下限、最大值、最小值5个位置产生的PWM占空比及频率（频率应该是一致的，在外我们的遥控器中是47.2Hz左右），并记录下来，用openMV打开三个IO口生成PWM波，生成算法和注释如下：
fly_ctrl.py

import time
from pyb import Pin, Timer, delay, LED
PWM_ref=7.08  # 死区中点（零点）处的PWM占空比
death_zone = 0.2  # 死区上限-死区中点（死区大小一半）
prop=850  # 占空比与控制量大小的换算比例，可随飞控系统灵敏度不同作调整
class Flight_Ctrl():def __init__(self):tim = Timer(4, freq=47.19)  # 定时器4，频率47.19Hzself.ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=PWM_ref)  # YAWself.ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=PWM_ref)  # PITself.ch3 = tim.channel(3, Timer.PWM, pin=Pin("P9"), pulse_width_percent=PWM_ref)  # ROLdef yaw(self, value): # 航向角if value > 0:  # 右偏self.ch1.pulse_width_percent(PWM_ref + death_zone + value/prop)elif value < 0:  # 左偏self.ch1.pulse_width_percent(PWM_ref - death_zone + value/prop)else:self.ch1.pulse_width_percent(PWM_ref)def pit(self,value):  # 俯仰if value > 0: # 前进self.ch2.pulse_width_percent(PWM_ref + death_zone + value/prop)elif value < 0:  # 后退self.ch2.pulse_width_percent(PWM_ref - death_zone + value/prop)else:self.ch2.pulse_width_percent(PWM_ref)def rol(self,value):  # 横滚if value > 0: # 往右横滚self.ch3.pulse_width_percent(PWM_ref + death_zone + value/prop)elif value < 0:  # 往左横滚self.ch3.pulse_width_percent(PWM_ref - death_zone + value/prop)else:self.ch3.pulse_width_percent(PWM_ref)def reset(self):  # 控制量清零self.ch1.pulse_width_percent(PWM_ref)self.ch2.pulse_width_percent(PWM_ref)self.ch3.pulse_width_percent(PWM_ref)

2.2.PID算法

$\qquad$ PID算法的参数的整定本文不作详细讨论，就重点对PID算法在openMV中的书写做说明。以下是位置PID算法和速度PID算法的代码：

位置PID

位置PID的输出直接代表了期望控制量的大小，数字位置PID的时域表达式如下：
$u(k)=kpe(k)+kiT0∑k=1ne(k)+kd⋅e(k)−e(k−1)T0u(k)=k_p e(k)+k_iT_0\sum_{k=1}^n e(k)+k_d\cdot\frac{e(k)-e(k-1)}{T_0}$
$T0\qquad T_0$ （程序中对应delta_time）是PID控制器的采样频率，同时也是控制周期，需要在程序里面测出（这里我们使用的是pyb模块的millis()函数，返回的是开机以来的毫秒数，二者相减即可得到控制周期，在我们的算法中，控制周期是会随着算法改变的，因此需要实时测量。）
$\qquad$ 由于微分控制会引入高频干扰，我们将微分的部分单独提出来作低通滤波处理，构成不完全微分，克服高频干扰，同时让微分的作用时间变长。设微分部分 $ud(k)=kd⋅e(k)−e(k−1)T0u_d(k)=k_d\cdot\frac{e(k)-e(k-1)}{T_0}$ 低通滤波器传递函数为 $F(s)=1Tfs+1F(s)=\frac{1}{T_fs+1}$ 低通滤波器的截止频率 $ωf=1/2πTf\omega_f=1/2\pi T_f$ 一般略大于控制周期，我们巡线的控制频率为42Hz，我们选用50Hz的截止频率，此时滤波器时间常数 $T_f=0.02s$ （程序中对应_RC变量）。选好滤波常数之后，微分部分被改造如下：
$ud(k)=TfT0+Tfud(k−1)+kdT0⋅T0T0+Tf[e(k)−e(k−1)]u_d(k)=\frac{T_f}{T_0+T_f}u_d(k-1)+\frac{k_d}{T_0}\cdot\frac{T_0}{T_0+T_f}[e(k)-e(k-1)]$ 书写程序时，可以令 $α=TfT0+Tf\alpha=\frac{T_f}{T_0+T_f}$ ，则上式可以改写为：
$ud(k)=αud(k−1)+kdT0(1−α)[e(k)−e(k−1)]u_d(k)=\alpha u_d(k-1)+\frac{k_d}{T_0}(1-\alpha)[e(k)-e(k-1)]$ $\qquad$ 作为位置PID控制器，需要进行内限幅和外限幅处理，内限幅就是对积分项进行限幅（程序中对应self.imax），外限幅就是对总输出进行限幅（程序中对应self._max），还需要设置抗饱和积分分离算法，算法原理的讲解详见下面的链接，尝试看懂PID算法的朋友们可以看一下。
[PID算法详细讲解链接-请点击此处]
$\qquad$ 最后我们预留一个总的比例环节参数K(程序中对应scaler)用于整体调节PID，但是需要注意的是，这个参数并不会影响PID限幅值的变化，只能整体调快或者调慢控制量的变化，因此我们的总PID时域表达式变为
$u(k)=K∗[kpe(k)+kiT0∑k=1ne(k)+ud(k)]u(k)=K*[k_p e(k)+k_iT_0\sum_{k=1}^n e(k)+u_d(k)]$
其中 $ud(k)=TfT0+Tfud(k−1)+kdT0⋅T0T0+Tf[e(k)−e(k−1)]u_d(k)=\frac{T_f}{T_0+T_f}u_d(k-1)+\frac{k_d}{T_0}\cdot\frac{T_0}{T_0+T_f}[e(k)-e(k-1)]$
pid.py

from pyb import millis
from math import pi, isnanclass PID:_kp = _ki = _kd = _integrator = _imax = 0_last_error = _last_derivative = _last_t = 0_RC = 0.02 # 不完全微分滤波时间常Tfdef __init__(self, p=0.4, i=0.08, d=0.1, imax=20, out_max=50, separation=True):self._kp = float(p)self._ki = float(i)self._kd = float(d)self._imax = abs(imax)self._last_derivative = float('nan')self._max = abs(out_max)self._separation = separationdef pid_output(self, error, scaler=6):tnow = millis()  # 获取当前的系统时间dt = tnow - self._last_t  # 系统经过的时间output = 0# 检测是否是第一次归位if self._last_t == 0 or dt > 1000:dt = 0self.reset_I() # 重置self._last_t = tnowdelta_time = float(dt) / float(1000)  # 换算成秒级output += error * self._kpif abs(self._kd) > 0 and dt > 0:if isnan(self._last_derivative):  # 检测上一次的微分值是否为空值（是否为初始复位状态）derivative = 0self._last_derivative = 0else:  # 不是初始复位状态时，按微分计算公式计算当前的微分值derivative = (error - self._last_error) / delta_timederivative = self._last_derivative + \((delta_time / (self._RC + delta_time)) * \(derivative - self._last_derivative))self._last_error = error  # 上一次的误差值self._last_derivative = derivative  # 上一次的微分值output += self._kd * derivative  # 输出加上微分项*微分项系数k_doutput *= scalerif abs(self._ki) > 0 and dt > 0:self._integrator += (error * self._ki) * scaler * delta_time  # 积分值# 积分限幅if self._integrator < -self._imax: self._integrator = -self._imaxelif self._integrator > self._imax: self._integrator = self._imax# 抗饱和积分分离if abs(error)>self._max*0.3 or (not self._separation):output += self._integrator  # 输出加积分值else:output += 0.2*self._integratorif output < -self._max: output = -self._maxelif output > self._max: output = self._maxreturn output# PID重置def reset_I(self):self._integrator = 0self._last_derivative = float('nan')

速度PID

$\qquad$ 速度PID的控制参数整定和位置PID有所差异，一般情况下，速度PID用于自身含有积分器或者大惯性环节（近似为积分环节）的系统中。速度PID仅需要总输出限幅而不需要积分限幅（因其控制量相对于期望值非常小，造成的积分滞后效应可以忽略不计，但在我们的算法中仍然加入了积分限幅，主要是防止传感器出错造成的不可预料的重大事故）。速度PID的表达式如下：
$u(k)=k_p[e(k)-e(k-1)]+k_iT_0e(k)+u_d(k)$
其中
$ud(k)=TfT0+Tf[ud(k−1)−ud(k−2)]+kdT0+Tf[e(k)−2e(k−1)+e(k−2)]u_d(k)=\frac{T_f}{T_0+T_f}[u_d(k-1)-u_d(k-2)]+\frac{k_d}{T_0+T_f}[e(k)-2e(k-1)+e(k-2)]$

$\qquad$ 总体来说，速度PID控制适合阀门、舵机、电炉这种自带积分器或者大惯性环节的设备，我们尝试将速度PID嵌入我们的四旋翼算法，经过控制量初步测试和试飞测试，发现只要总体比例参数scaler取值合适，也可以获得较好的控制效果。相比位置PID会慢一些，但是平稳得多，几乎不会有抖动。由于控制量变化较小，发生事故的概率也会大大降低。
pid.py

from pyb import millis
from math import pi, isnanclass PID:_kp = _ki = _kd = _integrator = _imax = 0_last_error = _last_derivative = _last_t = 0 # e(k-1)_last_error2 = _last_derivative2 = 0  # e(k-2)_RC = 1/(2 * pi * 20) # 不完全微分的滤波器def __init__(self, p=0.4, i=0.08, d=0.1, imax=20, out_max=50, separation=False):self._kp = float(p)self._ki = float(i)self._kd = float(d)self._imax = abs(imax)self._last_derivative2 = float('nan')self._last_derivative = float('nan')self._max = abs(out_max)self._separation = separationdef pid_output(self, error, scaler=800):tnow = millis()  # 获取当前的系统时间dt = tnow - self._last_t  # 系统经过的时间output = 0# 检测是否是第一次归位if self._last_t == 0 or dt > 1000:dt = 0self.reset_I() # 重置self._last_t = tnowdelta_time = float(dt) / float(1000)  # 换算成秒级output += (error-self._last_error) * self._kp # P输出if abs(self._kd) > 0 and dt > 0:if isnan(self._last_derivative):  # 检测上一次的微分值是否为空值（是否为初始复位状态）derivative = 0self._last_derivative = 0self._last_derivative2 = 0else:  # 不是初始复位状态时，按微分计算公式计算当前的微分值derivative = (error - 2*self._last_error+self._last_error2) / delta_timeself._last_derivative2 = self._last_derivativeself._last_derivative = derivative  # 保存上次的和上上次的微分值（不含滤波）。alp = delta_time / (self._RC + delta_time)filter_derivative = alp*(self._last_derivative-self._last_derivative2)+self._kd/delta_time*(1-alp)*(error-2*self._last_error+self._last_error2)self._last_error2 = self._last_error  # e(k-2)self._last_error = error  # e(k-1)output += self._kd * filter_derivative  # 输出加上微分项*微分项系数k_doutput *= scaler  # scaler仅对比例和微分有作用，对积分无效if abs(self._ki) > 0 and dt > 0:self._integrator = (error * self._ki) * scaler * delta_time  # 积分值,scaler不含影响限幅print('I=%f'%self._integrator)# 积分限幅if self._integrator < -self._imax: self._integrator = -self._imaxelif self._integrator > self._imax: self._integrator = self._imaxoutput += self._integrator## 积分分离#if abs(error)>self._max*0.2 or (not self._separation):#output += self._integrator  # 输出加积分值#else:#output += 0.3*self._integratorif output < -self._max: output = -self._maxelif output > self._max: output = self._maxreturn output# PID重置def reset_I(self):self._integrator = 0self._last_derivative = float('nan')self._last_derivative2 = float('nan')

3.摄像头算法

3.1.图像处理

$\qquad$ 任何一副图像的采集都需要经过图像处理的步骤，从最简单的选择像素点格式、旋转格式、颜色格式到滤波器参数的选择，是获得图像有效信息的关键。图像的大小主要有这几种格式：

格式	大小
sensor.QQVGA:	160x120
sensor.QQVGA2	128x160
sensor.HQVGA	240x160
sensor.QVGA	320x240
sensor.VGA	640x480
sensor.QQCIF	88x72
sensor.QCIF	176x144
sensor.CIF	352x288

在openMV中通过sensor.set_framesize()设置大小，在我们算法中普遍采用灰色的QQVGA格式图像。选择图像尺寸的原则是在保证信息不丢失的情况下让占用的内存最小。
$\qquad$ 常用的滤波算法有中值滤波、均值滤波、核滤波、卡通滤波、众数滤波等等，其中核滤波对于去除高斯噪声，保留有用信息效果最好。在核滤波之前，我们需要对图像取颜色梯度，然后使用核滤波矩阵进行滤波，最后进行“洪水腐蚀”，根据图像的信噪比剔除椒盐噪声。

信息损失	处理完好

一般情况下，需要关注以下几个参数：

镜头畸变矫正（强度、缩放）img.lens_corr(strenth=0.8,zoom=1)
核滤波矩阵大小img.morph(kernel_size, kernel_matrix)
二值化阈值img.binary(side_thresholds)
洪水腐蚀（大小、阈值）img.erode(1, threshold = 2)

3.2.霍夫曼变换

$\qquad$ 霍夫曼变换用来将点坐标空间变化到参数空间的，可以识别直线（2参数）、圆（3参数）甚至是椭圆（4参数），但参数越多，信息点越少，识别效果越差。通过设定阈值的方法可以将识别不好的结果滤除，因为那往往是特殊形状导致的误识别。在识别直线的时候，如果识别是单一直线，可以使用最小二乘法。但是要注意，此算法的计算量是按图像像素点按平方项递增的，对于高像素的图片，可能会超出内存允许范围。对于低像素的图像（如160×120），识别效果较好，速度也较快。

3.3.巡线算法

$\qquad$ 霍夫曼变换或者最小二乘法返回的是直线的极坐标方程为 $ρ=x0cosθ+y0sinθ\rho=x_0cos\theta+y_0sin\theta$ ，其中 $ρ\rho$ 为直线距离坐标原点的距离（注意图像学中一般以左上角为原点）， $θ\theta$ 则是直线和y的正半轴的夹角，函数里面返回的是0~180°，我们在程序中将其整定为-90°—90°。简单地来说， $ρ\rho$ 参数返回的是直线偏离画面中心距离（实际上并不完全是，我们用了余弦函数结合 $θ\theta$ 做了矫正），我们采用横滚通道(roll)的PID， $θ\theta$ 参数是直线沿前进方向旋转的角度，我们采用(yaw)方向的PID。结合二者的控制延时，我们再整定出一个前进速度（偏移角度过大或者偏移中心过大会减慢前进速度，为调节航向角和横向偏差留出控制时间），就形成了巡线PID控制了。巡线的具体函数代码如下：
follow_line()

ANO = Flight_Ctrl()
flag_takeoff = 0
isdebug=false  #调试变量，为假时不显示调试内容
list_rho_err = list()
list_theta_err = list()
rho_pid = PID(p=0.7,i=0.14,d=0.13,imax=100,out_max=100)
theta_pid = PID(p=0.7,i=0.14,d=0.13,imax=120,out_max=120)
end_line = False
first_line = False
def follow_line():global list_theta_err,list_rho_err,end_line,first_line,clock,isdebugimg = sensor.snapshot()img.lens_corr(strenth=0.8,zoom=1)img.morph(kernel_size, kernel)img.binary(side_thresholds)img.erode(1, threshold = 2)line = img.get_regression([THRESHOLD], robust = True)if (line):LED(1).off()LED(2).off()LED(3).on()rho_err = abs(line.rho())-img.width()/2*abs(cos(line.theta()*pi/180 ))if line.theta()>90:theta_err = line.theta()-180else:theta_err = line.theta()list_theta_err.append(theta_err)list_rho_err.append(rho_err)if len(list_theta_err)>6:list_theta_err.pop(0)list_rho_err.pop(0)theta_err = median(list_theta_err)rho_err = median(list_rho_err)if isdebug:img.draw_line(line.line(), color = (200,200,200))print("rho_err=%d,theta_err=%d,mgnitude=%d"%(rho_err,theta_err,line.magnitude()))rol_output = rho_pid.pid_output(rho_err,4)theta_output = theta_pid.pid_output(theta_err,7)if isdebug:print("rol_output=%d,theta_output=%d"%(rol_output,theta_output))if line.magnitude() > 8 or sys_clock.avg()<follow_line_least_time:clock.reset()clock.tick()ANO.pit(110-0.1*abs(rho_err)-0.2*abs(theta_err))LED(1).off()LED(2).on()LED(3).off()ANO.rol(rol_output+0.3*theta_output)ANO.yaw(theta_output)else:if clock.avg() > 200 and abs(rho_err) < 20 and abs(theta_err) < 30:end_line = TrueANO.reset()ANO.pit(70)else:ANO.pit(70-abs(theta_err)*0.18)LED(1).off()LED(2).off()LED(3).on()ANO.rol(0.8*rol_output)ANO.yaw(theta_output)safe_clock.reset()safe_clock.tick()else:if safe_clock.avg()>800:ANO.rol(0)ANO.yaw(0)ANO.pit(400)else:ANO.reset()LED(1).on()LED(2).off()LED(3).off()

3.3.寻找目标点降落算法

$\qquad$ 寻找目标点降落时，需要识别出目标点的x和y，并与图像中心坐标作比较，将x方向的偏差量和y方向的偏差量作为输入，产生两个PID控制，并控制这个偏差量为0，这就是寻找目标点降落的算法。X方向上的控制就是横滚控制量（前后）的控制，Y方向的控制就是俯仰控制量（左右）的控制。
$\qquad$ 事实上，摄像头的位置不一定在四旋翼的正中心，而且飞机具有惯性。所以实际控制的时候，我们加入了目标丢失的惯性控制、目标停留安全时间等算法。目标丢失的惯性控制算法是指，丢失目标在一定毫秒数之内，保留原来的控制量，如果等待时间到了目标仍为找到，则认为目标确实丢失，此时向前寻找目标（适合巡线结束后寻找降落区，需要前进的情况）目标停留安全时间算法是指，找到目标后，通过X方向和Y方向的PID控制使得目标点在图像中的距离期望点达到了运行距离范围，但是必须保留一段时间（认为飞机已经在空中悬停稳定，而不是瞬间飘过）才允许降落。这段时间必须和误差允许范围配合好，如果时间太短了可能由于飞机的惯性，在下降时降落的位置并不是找到目标停留的位置；如果时间太长了，可能很长时间都找不到目标。那是因为光流定点的精度以及PID算法的控制精度达不到在误差范围内维持这么长的秒数，此时可以缩短安全降落时间，也看增大误差允许范围。
具体的算法如下：

def follow_circle():global flag_takeoff,clock,safe_clockposition = findc(THEROSHOLD=6000)  # 寻找目标点的函数，返回的是目标点的坐标if position: # 如果找到目标点了（目标点坐标不为空）LED(1).off()LED(2).on() # 亮绿灯LED(3).on()x_err = position[0]-50 # X方向偏移量y_err = -(position[1]-65)  # Y方向偏移量if abs(x_err)<3 and abs(y_err)<3: # 进入误差允许区域if safe_clock.avg()>166: #保持在误差允许区域一定时间才能降落LED(1).off()LED(2).on() # 亮绿灯LED(3).off()ANO.reset() # 控制量复位，防止降落时候控制flag_takeoff = 1  # 降落标志位pin1.high()  # 控制降落else: # 在误差允许范围内了，仍然使用PID控制，幅度较小ANO.rol(x_pid.pid_output(x_err,7))ANO.pit(y_pid.pid_output(y_err,7))else: # 不在误差允许范围，但是找到目标，X和Y方向的PID控制safe_clock.reset()  # 复位误差允许范围内计时时钟safe_clock.tick()ANO.rol(x_pid.pid_output(x_err,11))  # PID控制幅度较大ANO.pit(y_pid.pid_output(y_err,11))clock.reset()  # 目标寻找计时复位clock.tick()  # 目标寻找计时时钟重计时else:if clock.avg() > 900: # 900ms没有发现目标LED(1).on()LED(2).off()LED(3).off()ANO.reset()ANO.pit(80)  # 没有寻找到目标降落区域，前进寻找