PID控制及圖像識別的自主循跡四旋翼設計*

，，，，，，，

(天津理工大學 工程訓練中心，天津300384)

引 言

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

本文采用STM32F407微控制器作為四軸飛行控制系統的控制核心，調制PWM信號控制無刷電機。通過學習四旋翼飛行器的飛行控制原理，構造算法，然后編程、調試、不斷完善，最終實現了四旋翼飛行器平臺的平穩飛行。本文采用增加一片STM32F407芯片的方式以提高圖像獲取和處理的速度，通過串口通信與飛行控制核心板連接，把由圖像處理獲得的控制量送到飛控板，從而實現循跡控制。通過調試和改進，飛行器實現了穩定可靠的自主循跡飛行。

1 系統硬件設計

本系統主要由STM32F407飛行控制模塊和STM32F407圖像處理模塊、電機驅動模塊、US-100超聲測距模、MPU-9150九軸加速度模塊、OV7670攝像頭模塊組成。控制系統硬件組圖如圖1所示。

圖1 控制系統硬件組圖

1.1 飛行控制系統主控芯片

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

1.2 驅動模塊

四軸飛行器是高精度多姿態控制系統，而無刷直流電動機以自控式運行，不會像變頻調速下重載啟動的同步電機那樣在轉子上另加啟動繞組，也不會在負載突變時產生振蕩和失步，可實現對轉速的精確控制[2]，所以優先選擇無刷電機和電調模塊驅動四旋翼。

1.3 超聲測距模塊

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

1.4 飛行姿態采集模塊

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

1.5 攝像頭模塊

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

2 系統軟件設計

2.1 飛行姿態算法分析

四旋翼通過4個無刷電機的轉速對飛行姿態進行控制。四旋翼控制系統通過各個傳感器獲得飛行姿態數據，經過PID算法得出控制量，然后通過調制PWM信號來控制4個無刷電機，從而實現對其姿態的控制。姿態解算和PID控制是整個飛行姿態控制的基礎。

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

本系統采用角度控制PID與角速度控制PID相配合的方式如圖3所示，角度環為外環，角速度環為內環。圖3為串級PID示意圖。

圖2 姿態解算算法實現流圖

2.2 四軸自主循跡算法

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

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

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

② 黑線邊沿的提取。對于二維數組Image[i][j]的每一行像素，從左往右，檢測0～1的正跳變，此時檢測到的像素點就是黑線的左側邊界，記下其坐標值，接著繼續往右檢測，出現1～0的負跳變時，就檢測到了黑線的右側邊界像素點，記下此處的像素坐標，以此類推得到整幀的邊界像素點坐標。

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

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

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

② 在提取的三個段中，包含的點越多的段越可靠，如圖4中的連續段3，其所包含的連續點最多，則是三個段中最為可靠的段，反之則點越少越不可靠。

圖4 連續段幀處理算法

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

④ 通過每一行左右黑線邊界有效點的坐標值計算出黑線在該行的中心點坐標值，并記錄到對應二維數組中，以此類推求出整幀黑線中心點的坐標值并記錄。

⑤ 對所有的黑線中心點坐標值求均值，即所有中心點坐標值求和再除以整幀圖像的總行數，得到黑線中心點。

⑥ 根據得到的黑線中心點坐標與整幀圖像的物理中心點坐標值的對比，得出偏移量，可以判斷飛行器飛行軌跡是否偏離了黑線，進而根據偏移量經過相應PID算法實現飛行姿態的調整，使其沿黑線飛行。

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

結 語

齊壘(本科生)，主要研究方向為自動控制、模式識別；王鑫(實驗師)，主要研究方向為自動控制、嵌入式開發；李想(助理工程師)，主要研究方向為圖像處理；陳文龍、李富超、彭夢媛、王朝輝、張浩然(本科生)，主要研究方向為自動控制理論。