文章目錄

• 聲明
• 1.四旋翼飛行控制簡介
• 2.飛行控制算法
• • 2.1.接收機PWM生成
  • 2.2.PID算法
  • • 位置PID
    • 速度PID
• 3.攝像頭算法
• • 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_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算法

\qquadPID算法的參數的整定本文不作詳細討論，就重點對PID算法在openMV中的書寫做說明。以下是位置PID算法和速度PID算法的代碼：

位置PID

位置PID的輸出直接代表了期望控制量的大小，數字位置PID的時域表達式如下：
$$u(k)=k_{p}e(k)+k_i{T}_0\sum _{k=1}^{n}e(k)+k_d?\frac{e(k)?e(k?1)}{T_0}$$
$T_0$（程序中對應delta_time）是PID控制器的采樣頻率，同時也是控制周期，需要在程序里面測出（這里我們使用的是pyb模塊的millis()函數，返回的是開機以來的毫秒數，二者相減即可得到控制周期，在我們的算法中，控制周期是會隨著算法改變的，因此需要實時測量。）
\qquad由于微分控制會引入高頻干擾，我們將微分的部分單獨提出來作低通濾波處理，構成不完全微分，克服高頻干擾，同時讓微分的作用時間變長。設微分部分$$u_d(k)=k_d?\frac{e(k)?e(k?1)}{T_0}$$低通濾波器傳遞函數為$$F(s)=\frac{1}{T_{f}s+1}$$低通濾波器的截止頻率${\omega }_f=1/2\pi T_f$一般略大于控制周期，我們巡線的控制頻率為42Hz，我們選用50Hz的截止頻率，此時濾波器時間常數$T_f=0.02s$（程序中對應_RC變量）。選好濾波常數之后，微分部分被改造如下：
$$u_d(k)=\frac{T_f}{T_0+T_f}{u}_d(k?1)+\frac{k_d}{T_0}?\frac{T_0}{T_0+T_f}[e(k)?e(k?1)]$$書寫程序時，可以令$\alpha =\frac{T_f}{T_0+T_f}$，則上式可以改寫為：
$$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?[k_{p}e(k)+k_i{T}_0\sum _{k=1}^{n}e(k)+u_d(k)]$$
其中$$u_d(k)=\frac{T_f}{T_0+T_f}{u}_d(k?1)+\frac{k_d}{T_0}?\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_i{T}_{0}e(k)+u_d(k)$$
其中
$$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霍夫曼變換或者最小二乘法返回的是直線的極坐標方程為$$\rho =x_{0}cos\theta +y_{0}sin\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) # 沒有尋找到目標降落區域，前進尋找

希望本文對您有幫助，謝謝閱讀。

總結

以上是生活随笔為你收集整理的【openMV与机器视觉】四旋翼飞行控制背景下的PID控制与摄像头算法简介的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。