优美网站源码,河北网站建设报价,佳木斯建设网站,项目管理系统开发文章目录声明1.四旋翼飞行控制简介2.飞行控制算法2.1.接收机PWM生成2.2.PID算法位置PID速度PID3.摄像头算法3.1.图像处理3.2.霍夫曼变换3.3.巡线算法3.3.寻找目标点降落算法声明 \qquad本文的算法在openMV IDE例程的基础上进行原创#xff0c;在比赛结束后予以发表#xff1b…
文章目录声明1.四旋翼飞行控制简介2.飞行控制算法2.1.接收机PWM生成2.2.PID算法位置PID速度PID3.摄像头算法3.1.图像处理3.2.霍夫曼变换3.3.巡线算法3.3.寻找目标点降落算法声明
\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_ref7.08 # 死区中点零点处的PWM占空比
death_zone 0.2 # 死区上限-死区中点死区大小一半
prop850 # 占空比与控制量大小的换算比例可随飞控系统灵敏度不同作调整
class Flight_Ctrl():def __init__(self):tim Timer(4, freq47.19) # 定时器4频率47.19Hzself.ch1 tim.channel(1, Timer.PWM, pinPin(P7), pulse_width_percentPWM_ref) # YAWself.ch2 tim.channel(2, Timer.PWM, pinPin(P8), pulse_width_percentPWM_ref) # PITself.ch3 tim.channel(3, Timer.PWM, pinPin(P9), pulse_width_percentPWM_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算法
\qquadPID算法的参数的整定本文不作详细讨论就重点对PID算法在openMV中的书写做说明。以下是位置PID算法和速度PID算法的代码
位置PID
位置PID的输出直接代表了期望控制量的大小数字位置PID的时域表达式如下 u(k)kpe(k)kiT0∑k1ne(k)kd⋅e(k)−e(k−1)T0u(k)k_p e(k)k_iT_0\sum_{k1}^n e(k)k_d\cdot\frac{e(k)-e(k-1)}{T_0} u(k)kpe(k)kiT0k1∑ne(k)kd⋅T0e(k)−e(k−1) T0\qquad T_0T0程序中对应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}ud(k)kd⋅T0e(k)−e(k−1)低通滤波器传递函数为F(s)1Tfs1F(s)\frac{1}{T_fs1}F(s)Tfs11低通滤波器的截止频率ωf1/2πTf\omega_f1/2\pi T_fωf1/2πTf一般略大于控制周期我们巡线的控制频率为42Hz我们选用50Hz的截止频率此时滤波器时间常数Tf0.02sT_f0.02sTf0.02s程序中对应_RC变量。选好滤波常数之后微分部分被改造如下 ud(k)TfT0Tfud(k−1)kdT0⋅T0T0Tf[e(k)−e(k−1)]u_d(k)\frac{T_f}{T_0T_f}u_d(k-1)\frac{k_d}{T_0}\cdot\frac{T_0}{T_0T_f}[e(k)-e(k-1)]ud(k)T0TfTfud(k−1)T0kd⋅T0TfT0[e(k)−e(k−1)]书写程序时可以令αTfT0Tf\alpha\frac{T_f}{T_0T_f}αT0TfTf则上式可以改写为 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)]ud(k)αud(k−1)T0kd(1−α)[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∑k1ne(k)ud(k)]u(k)K*[k_p e(k)k_iT_0\sum_{k1}^n e(k)u_d(k)]u(k)K∗[kpe(k)kiT0k1∑ne(k)ud(k)] 其中ud(k)TfT0Tfud(k−1)kdT0⋅T0T0Tf[e(k)−e(k−1)]u_d(k)\frac{T_f}{T_0T_f}u_d(k-1)\frac{k_d}{T_0}\cdot\frac{T_0}{T_0T_f}[e(k)-e(k-1)]ud(k)T0TfTfud(k−1)T0kd⋅T0TfT0[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, p0.4, i0.08, d0.1, imax20, out_max50, separationTrue):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, scaler6):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)kp[e(k)−e(k−1)]kiT0e(k)ud(k)u(k)k_p[e(k)-e(k-1)]k_iT_0e(k)u_d(k)u(k)kp[e(k)−e(k−1)]kiT0e(k)ud(k) 其中 ud(k)TfT0Tf[ud(k−1)−ud(k−2)]kdT0Tf[e(k)−2e(k−1)e(k−2)]u_d(k)\frac{T_f}{T_0T_f}[u_d(k-1)-u_d(k-2)]\frac{k_d}{T_0T_f}[e(k)-2e(k-1)e(k-2)]ud(k)T0TfTf[ud(k−1)−ud(k−2)]T0Tfkd[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, p0.4, i0.08, d0.1, imax20, out_max50, separationFalse):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, scaler800):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_errorself._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_errorself._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:160x120sensor.QQVGA2128x160sensor.HQVGA240x160sensor.QVGA320x240sensor.VGA640x480sensor.QQCIF88x72sensor.QCIF176x144sensor.CIF352x288
在openMV中通过sensor.set_framesize()设置大小在我们算法中普遍采用灰色的QQVGA格式图像。选择图像尺寸的原则是在保证信息不丢失的情况下让占用的内存最小。 \qquad常用的滤波算法有中值滤波、均值滤波、核滤波、卡通滤波、众数滤波等等其中核滤波对于去除高斯噪声保留有用信息效果最好。在核滤波之前我们需要对图像取颜色梯度然后使用核滤波矩阵进行滤波最后进行“洪水腐蚀”根据图像的信噪比剔除椒盐噪声。
信息损失处理完好
一般情况下需要关注以下几个参数
镜头畸变矫正强度、缩放img.lens_corr(strenth0.8,zoom1)核滤波矩阵大小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θ\rhox_0cos\thetay_0sin\thetaρx0cosθy0sinθ其中ρ\rhoρ为直线距离坐标原点的距离注意图像学中一般以左上角为原点θ\thetaθ则是直线和y的正半轴的夹角函数里面返回的是0~180°我们在程序中将其整定为-90°—90°。简单地来说ρ\rhoρ参数返回的是直线偏离画面中心距离实际上并不完全是我们用了余弦函数结合θ\thetaθ做了矫正我们采用横滚通道(roll)的PIDθ\thetaθ参数是直线沿前进方向旋转的角度我们采用(yaw)方向的PID。结合二者的控制延时我们再整定出一个前进速度偏移角度过大或者偏移中心过大会减慢前进速度为调节航向角和横向偏差留出控制时间就形成了巡线PID控制了。巡线的具体函数代码如下 follow_line()
ANO Flight_Ctrl()
flag_takeoff 0
isdebugfalse #调试变量为假时不显示调试内容
list_rho_err list()
list_theta_err list()
rho_pid PID(p0.7,i0.14,d0.13,imax100,out_max100)
theta_pid PID(p0.7,i0.14,d0.13,imax120,out_max120)
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(strenth0.8,zoom1)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_output0.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(THEROSHOLD6000) # 寻找目标点的函数返回的是目标点的坐标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) # 没有寻找到目标降落区域前进寻找希望本文对您有帮助谢谢阅读。
