【openMV與機器視覺】四旋翼飛行控制背景下的PID控制與攝像頭算法簡介

聲明

\qquad本文的算法在openMV IDE例程的基礎上進行原創,在比賽結束後予以發表;本文在作者比賽經歷的基礎上書寫,內容屬實;本文提供的參數與代碼並不一定適用於所有四旋翼,但方案是可以借鑑的。
\qquad本文的算法建議創建的硬件環境:

  1. 接收機和飛控板(推薦使用匿名科創的,使用前需要校準)
  2. STM32F407(用作輔助控制)
  3. openMV Cam3(板載攝像頭及STM32F765,高性能芯片,作視覺處理)
  4. 廣角鏡頭(120°~185°,度數越高視野越廣,魚眼效應越嚴重)
  5. 光流(定高定點,使用前需要校準)和激光定高(二者配合才能平穩起飛降落、防止懸空漂移)
  6. 航模電池(應飛機重量而異,電源線不宜太長,否則電源線產生的磁場將干擾羅盤而產生航向角零漂)
  7. 航模電池充電器(切記不要買B3,2天才能充滿一節,推薦使用B6或者A6)
  8. 電調×4;電機×4(條件允許的情況下電機和電調的錢不能省)
  9. 低速槳/高速槳×4(不能買錯方向,一正一反爲一對,共一對)
  10. 保護架4個(推薦買全方位保護架,特別是新手)
  11. 起落架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.19Hz
        self.ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=PWM_ref)  # YAW
        self.ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=PWM_ref)  # PIT
        self.ch3 = tim.channel(3, Timer.PWM, pin=Pin("P9"), pulse_width_percent=PWM_ref)  # ROL
    def 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)+kiT0k=1ne(k)+kde(k)e(k1)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)=kde(k)e(k1)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的截止頻率,此時濾波器時間常數Tf=0.02sT_f=0.02s(程序中對應_RC變量)。選好濾波常數之後,微分部分被改造如下:
ud(k)=TfT0+Tfud(k1)+kdT0T0T0+Tf[e(k)e(k1)]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(k1)+kdT0(1α)[e(k)e(k1)]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)+kiT0k=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(k1)+kdT0T0T0+Tf[e(k)e(k1)]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, isnan

class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 0.02 # 不完全微分濾波時間常Tf
    def __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 = separation
    def pid_output(self, error, scaler=6):
        tnow = millis()  # 獲取當前的系統時間
        dt = tnow - self._last_t  # 系統經過的時間
        output = 0
        # 檢測是否是第一次歸位
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I() # 重置
        self._last_t = tnow
        delta_time = float(dt) / float(1000)  # 換算成秒級
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):  # 檢測上一次的微分值是否爲空值(是否爲初始復位狀態)
                derivative = 0
                self._last_derivative = 0
            else:  # 不是初始復位狀態時,按微分計算公式計算當前的微分值
                derivative = (error - self._last_error) / delta_time

            derivative = 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_d
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time  # 積分值
            # 積分限幅
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif 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._integrator
            if output < -self._max: output = -self._max
            elif output > self._max: output = self._max
        return output

    # PID重置
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

速度PID

\qquad速度PID的控制參數整定和位置PID有所差異,一般情況下,速度PID用於自身含有積分器或者大慣性環節(近似爲積分環節)的系統中。速度PID僅需要總輸出限幅而不需要積分限幅(因其控制量相對於期望值非常小,造成的積分滯後效應可以忽略不計,但在我們的算法中仍然加入了積分限幅,主要是防止傳感器出錯造成的不可預料的重大事故)。速度PID的表達式如下:
u(k)=kp[e(k)e(k1)]+kiT0e(k)+ud(k)u(k)=k_p[e(k)-e(k-1)]+k_iT_0e(k)+u_d(k)
其中
ud(k)=TfT0+Tf[ud(k1)ud(k2)]+kdT0+Tf[e(k)2e(k1)+e(k2)]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, isnan

class 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 = separation
    def pid_output(self, error, scaler=800):
        tnow = millis()  # 獲取當前的系統時間
        dt = tnow - self._last_t  # 系統經過的時間
        output = 0
        # 檢測是否是第一次歸位
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I() # 重置
        self._last_t = tnow
        delta_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 = 0
                self._last_derivative = 0
                self._last_derivative2 = 0
            else:  # 不是初始復位狀態時,按微分計算公式計算當前的微分值
                derivative = (error - 2*self._last_error+self._last_error2) / delta_time
            self._last_derivative2 = self._last_derivative
            self._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_d
        output *= 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._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
            ## 積分分離
            #if abs(error)>self._max*0.2 or (not self._separation):
                #output += self._integrator  # 輸出加積分值
            #else:
                #output += 0.3*self._integrator
        if output < -self._max: output = -self._max
        elif output > self._max: output = self._max
        return output

    # PID重置
    def reset_I(self):
        self._integrator = 0
        self._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常用的濾波算法有中值濾波、均值濾波、核濾波、卡通濾波、衆數濾波等等,其中核濾波對於去除高斯噪聲,保留有用信息效果最好。在覈濾波之前,我們需要對圖像取顏色梯度,然後使用核濾波矩陣進行濾波,最後進行“洪水腐蝕”,根據圖像的信噪比剔除椒鹽噪聲。

信息損失 處理完好
在這裏插入圖片描述 在這裏插入圖片描述

一般情況下,需要關注以下幾個參數:

  1. 鏡頭畸變矯正(強度、縮放)img.lens_corr(strenth=0.8,zoom=1)
  2. 核濾波矩陣大小img.morph(kernel_size, kernel_matrix)
  3. 二值化閾值img.binary(side_thresholds)
  4. 洪水腐蝕(大小、閾值)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,isdebug
	img = 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()-180
		else:
			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 = True
				ANO.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_clock
	position = 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)  # 沒有尋找到目標降落區域,前進尋找

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

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章