PID控制及圖像識(shí)別的自主循跡四旋翼設(shè)計(jì)*

，，，，，，，

(天津理工大學(xué) 工程訓(xùn)練中心，天津300384)

引 言

四軸飛行器(Quadrotor)是一種多旋翼飛行器又稱四旋翼飛行器、四旋翼直升機(jī)，簡(jiǎn)稱四軸、四旋翼,四軸飛行器的四個(gè)螺旋槳都是電機(jī)直連的簡(jiǎn)單機(jī)構(gòu)，十字形的布局允許飛行器通過改變電機(jī)轉(zhuǎn)速獲得旋轉(zhuǎn)機(jī)身的力，從而調(diào)整自身姿態(tài)[1]。近年來四軸飛行器得到了穩(wěn)定快速的發(fā)展，目前國際許多知名的四軸飛行器公司也應(yīng)運(yùn)而生，如大疆創(chuàng)新(DJI)、中國zero(零度)、美國3D Robotics、法國Parrot(派諾特)等公司。飛行控制系統(tǒng)簡(jiǎn)稱飛控，是實(shí)現(xiàn)四旋翼起飛、降落和姿態(tài)控制的重要核心控制部件，目前市場(chǎng)上有很多成熟的飛控，如大疆、APM、MWC、零度、PIX、匿名等。

本文采用STM32F407微控制器作為四軸飛行控制系統(tǒng)的控制核心，調(diào)制PWM信號(hào)控制無刷電機(jī)。通過學(xué)習(xí)四旋翼飛行器的飛行控制原理，構(gòu)造算法，然后編程、調(diào)試、不斷完善，最終實(shí)現(xiàn)了四旋翼飛行器平臺(tái)的平穩(wěn)飛行。本文采用增加一片STM32F407芯片的方式以提高圖像獲取和處理的速度，通過串口通信與飛行控制核心板連接，把由圖像處理獲得的控制量送到飛控板，從而實(shí)現(xiàn)循跡控制。通過調(diào)試和改進(jìn)，飛行器實(shí)現(xiàn)了穩(wěn)定可靠的自主循跡飛行。

1 系統(tǒng)硬件設(shè)計(jì)

本系統(tǒng)主要由STM32F407飛行控制模塊和STM32F407圖像處理模塊、電機(jī)驅(qū)動(dòng)模塊、US-100超聲測(cè)距模、MPU-9150九軸加速度模塊、OV7670攝像頭模塊組成。控制系統(tǒng)硬件組圖如圖1所示。

圖1 控制系統(tǒng)硬件組圖

1.1 飛行控制系統(tǒng)主控芯片

由于STM32F407擁有高達(dá)168 MHz的主頻，這使其擁有更強(qiáng)的浮點(diǎn)運(yùn)算和數(shù)據(jù)處理能力，而且模數(shù)轉(zhuǎn)換、USART 和 SPI 通信速度都具有更快的速度。STM32F407擁有1 MB FLash和192 KB SRAM的內(nèi)存空間、更低功耗的ADC/DAC 、32 位通用定時(shí)器和實(shí)時(shí)時(shí)鐘(RTC)，而且整體功耗為238 μA/MHz。

1.2 驅(qū)動(dòng)模塊

四軸飛行器是高精度多姿態(tài)控制系統(tǒng)，而無刷直流電動(dòng)機(jī)以自控式運(yùn)行，不會(huì)像變頻調(diào)速下重載啟動(dòng)的同步電機(jī)那樣在轉(zhuǎn)子上另加啟動(dòng)繞組，也不會(huì)在負(fù)載突變時(shí)產(chǎn)生振蕩和失步，可實(shí)現(xiàn)對(duì)轉(zhuǎn)速的精確控制[2]，所以優(yōu)先選擇無刷電機(jī)和電調(diào)模塊驅(qū)動(dòng)四旋翼。

1.3 超聲測(cè)距模塊

US-100超聲波測(cè)距模塊可實(shí)現(xiàn)0.2～4.5 m的非接觸測(cè)距功能，擁有2.4～5.5 V寬電壓輸入范圍，靜態(tài)功耗低于2 mA，自帶溫度傳感器對(duì)測(cè)距結(jié)果進(jìn)行檢驗(yàn)，探測(cè)精度為0.3 cm±1%，同時(shí)具有GPIO、串口等多種通信方式，內(nèi)帶看門狗，工作穩(wěn)定可靠[3]。

1.4 飛行姿態(tài)采集模塊

MPU-9150模塊是由三軸陀螺儀、三軸加速度MPU6050和AK8975三軸磁力計(jì)集合而成的九軸傳感器模塊。使用3～5 V的電源供電以及采用標(biāo)準(zhǔn) I2C總線接口，芯片內(nèi)置16位 ADC，可輸出16位高精度數(shù)據(jù)。

1.5 攝像頭模塊

本文采用OV7670圖像傳感器作為視覺模塊。OV7670擁有標(biāo)準(zhǔn)的SCCB接口，同時(shí)兼容I2C接口，支持RGB565、YUV(4:2:2)等輸出格式，通過SCCB 總線控制，既可以輸出整幀，也可以輸出子采樣部分窗口等，VGA圖像最高可以達(dá)到30 fps的速度， OmmiVision 圖像傳感器應(yīng)用獨(dú)特的傳感器技術(shù)可得到清晰、穩(wěn)定的彩色圖像[4]。

2 系統(tǒng)軟件設(shè)計(jì)

2.1 飛行姿態(tài)算法分析

四旋翼通過4個(gè)無刷電機(jī)的轉(zhuǎn)速對(duì)飛行姿態(tài)進(jìn)行控制。四旋翼控制系統(tǒng)通過各個(gè)傳感器獲得飛行姿態(tài)數(shù)據(jù)，經(jīng)過PID算法得出控制量，然后通過調(diào)制PWM信號(hào)來控制4個(gè)無刷電機(jī)，從而實(shí)現(xiàn)對(duì)其姿態(tài)的控制。姿態(tài)解算和PID控制是整個(gè)飛行姿態(tài)控制的基礎(chǔ)。

本文將采用四元數(shù)法結(jié)合卡爾曼濾波算法進(jìn)行姿態(tài)估計(jì)，四元數(shù)法計(jì)算量小且無奇點(diǎn)，而通過卡爾曼濾波將磁力計(jì)與陀螺儀融合則可以很好地修正姿態(tài)估計(jì)的誤差[5]。姿態(tài)解算算法實(shí)現(xiàn)流圖如圖2所示。

本系統(tǒng)采用角度控制PID與角速度控制PID相配合的方式如圖3所示，角度環(huán)為外環(huán)，角速度環(huán)為內(nèi)環(huán)。圖3為串級(jí)PID示意圖。

圖2 姿態(tài)解算算法實(shí)現(xiàn)流圖

2.2 四軸自主循跡算法

本文采用配置了云臺(tái)的OV7670攝像頭作為視覺掃描，通過STM32F407對(duì)航路黑線信息進(jìn)行采集和圖像的實(shí)時(shí)處理，從而得出控制飛行姿態(tài)的矯正量。為保證攝像頭對(duì)圖像的垂直采集，本文采用了3軸無人機(jī)無刷航拍穩(wěn)定器控制板云臺(tái)。

本文自主循跡算法如下所示：

① 對(duì)采集的圖像進(jìn)行二值化處理。由于OV7670的輸出格式是RGB，所以首先把采集的一幀圖像的像素進(jìn)行灰度轉(zhuǎn)換。本文采用的灰度轉(zhuǎn)化是一個(gè)著名的心理學(xué)公式：Y=R×0.299+G×0.587+B×0.114,轉(zhuǎn)換后的一幀灰度圖像像素?cái)?shù)據(jù)存儲(chǔ)在一個(gè)二維數(shù)組Image[i][j]中。然后，采用全局閾值的方法對(duì)灰度圖像中的每一個(gè)像素進(jìn)行二值化處理,可實(shí)現(xiàn)為：{IfImage[i][j]>T;Image[i][j]=1;elseImage[i][j]=0;}，值1表示圖形即黑線，值0表示背景即白色場(chǎng)地，值T為閾值。二值化關(guān)鍵是對(duì)閾值T的確定，可根據(jù)實(shí)際場(chǎng)地和環(huán)境多次測(cè)試獲取。

② 黑線邊沿的提取。對(duì)于二維數(shù)組Image[i][j]的每一行像素，從左往右，檢測(cè)0～1的正跳變，此時(shí)檢測(cè)到的像素點(diǎn)就是黑線的左側(cè)邊界，記下其坐標(biāo)值，接著繼續(xù)往右檢測(cè)，出現(xiàn)1～0的負(fù)跳變時(shí)，就檢測(cè)到了黑線的右側(cè)邊界像素點(diǎn)，記下此處的像素坐標(biāo)，以此類推得到整幀的邊界像素點(diǎn)坐標(biāo)。

然后通過同一行左右邊界處的像素點(diǎn)坐標(biāo)值計(jì)算出這一行黑線中心點(diǎn)處的像素坐標(biāo)值O(x,y)，再拿黑線中心點(diǎn)處的坐標(biāo)值與一幀圖像中本行的中心坐標(biāo)值進(jìn)行比較得出位置偏移量P，再通過偏移量P對(duì)飛行姿態(tài)進(jìn)行調(diào)整。

邊沿提取算法只有在飛行場(chǎng)地的背景黑白明顯時(shí)才能對(duì)飛行姿態(tài)有較好的控制，當(dāng)場(chǎng)地背景黑白對(duì)比度較低時(shí)，控制易受到干擾。本文通過對(duì)得到的邊界像素點(diǎn)坐標(biāo)值進(jìn)行抗干擾濾波的方法來解決上述問題，也就是進(jìn)行幀處理，采用提取連續(xù)段的方法進(jìn)行幀處理，去除邊界中的干擾信息。連續(xù)段提取算法流程如下：

① 以提取左側(cè)邊界為例,隔著幾行取一個(gè)邊界點(diǎn)，邊取邊對(duì)比相鄰兩個(gè)點(diǎn)所在兩列之間的水平距離，如果各個(gè)黑點(diǎn)之間的水平距離接近固定的值，那么就定義這些點(diǎn)是連續(xù)的像素點(diǎn)，取這些連續(xù)的點(diǎn)作為一個(gè)連續(xù)段。一幀圖像提取三個(gè)連續(xù)段。

② 在提取的三個(gè)段中，包含的點(diǎn)越多的段越可靠，如圖4中的連續(xù)段3，其所包含的連續(xù)點(diǎn)最多，則是三個(gè)段中最為可靠的段，反之則點(diǎn)越少越不可靠。

圖4 連續(xù)段幀處理算法

③ 以連續(xù)段3為依據(jù)，依然采用判斷兩點(diǎn)之間的水平距離的方法，確定可用段和無效段，如圖4所示。連續(xù)段1和連續(xù)段3是可用段，連續(xù)段2無效，再把連續(xù)段1和3連起來，行成一個(gè)大段。使用插補(bǔ)的方法把連續(xù)段2歸算到大段中。更新到左側(cè)邊界記錄二維數(shù)組中，同理處理右側(cè)邊界。

④ 通過每一行左右黑線邊界有效點(diǎn)的坐標(biāo)值計(jì)算出黑線在該行的中心點(diǎn)坐標(biāo)值，并記錄到對(duì)應(yīng)二維數(shù)組中，以此類推求出整幀黑線中心點(diǎn)的坐標(biāo)值并記錄。

⑤ 對(duì)所有的黑線中心點(diǎn)坐標(biāo)值求均值，即所有中心點(diǎn)坐標(biāo)值求和再除以整幀圖像的總行數(shù)，得到黑線中心點(diǎn)。

⑥ 根據(jù)得到的黑線中心點(diǎn)坐標(biāo)與整幀圖像的物理中心點(diǎn)坐標(biāo)值的對(duì)比，得出偏移量，可以判斷飛行器飛行軌跡是否偏離了黑線，進(jìn)而根據(jù)偏移量經(jīng)過相應(yīng)PID算法實(shí)現(xiàn)飛行姿態(tài)的調(diào)整，使其沿黑線飛行。

經(jīng)過測(cè)試，本文所述圖像處理算法能有效地完成自主尋黑線飛行。

結(jié) 語

齊壘(本科生)，主要研究方向?yàn)樽詣?dòng)控制、模式識(shí)別；王鑫(實(shí)驗(yàn)師)，主要研究方向?yàn)樽詣?dòng)控制、嵌入式開發(fā)；李想(助理工程師)，主要研究方向?yàn)閳D像處理；陳文龍、李富超、彭夢(mèng)媛、王朝輝、張浩然(本科生)，主要研究方向?yàn)樽詣?dòng)控制理論。