基于雙目立體視覺避障的四旋翼飛行器設(shè)計(jì)

(廣東海洋大學(xué) 電子與信息工程學(xué)院， 廣東 湛江 524088)

0 引言

隨著工業(yè)4.0的到來，各種高新科技產(chǎn)業(yè)蓬勃發(fā)展，高新科技產(chǎn)品也逐漸的平民化，四旋翼飛行器憑借自身優(yōu)勢已逐漸滲透到各行各業(yè)之中，為人類的工作和生活帶來了極大的便利[1]。然而，在目前四旋翼飛行器研究領(lǐng)域，自主避障是亟待優(yōu)化的重大難題，理想的避障機(jī)制是能夠讓人們輕松的駕馭，且減少飛行器意外事故的發(fā)生和不必要的損失。雙目立體視覺的出現(xiàn)讓人們眼前一亮[2-3]，將之有效融入四旋翼飛行器的檢測與控制之中，有效規(guī)避高空電纜、樹枝等普通傳感器難以檢測的障礙物，這一強(qiáng)大的技術(shù)融合無疑是四旋翼飛行器智能化的重要體現(xiàn)。在本設(shè)計(jì)中用STM32作為四旋翼飛行器的主控，用筆記本電腦進(jìn)行障礙識別，將障礙識別的結(jié)果反饋到飛行器上，讓其能夠判斷前方障礙物的位置。在本文中，重點(diǎn)講述系統(tǒng)的軟件部分，姿態(tài)解算使用基于四元素姿態(tài)的梯度下降法，飛行器穩(wěn)定控制器使用的是串級PID控制器，在進(jìn)行障礙物識別時，筆記本電腦在用Visual Studio 2015開發(fā)環(huán)境下調(diào)用Opencv機(jī)器視覺庫來編寫雙目視覺檢測障礙物的程序并運(yùn)行，在電腦界面可以看到掛載在飛行器上的雙目攝像頭傳回來的實(shí)時圖像以便觀察飛行狀態(tài)。

1 系統(tǒng)總體方案設(shè)計(jì)

1.1 系統(tǒng)總體結(jié)構(gòu)

基于雙目立體視覺避障的四旋翼飛行器總體結(jié)構(gòu)由USB雙目攝像頭識別、2.4 G遙控器、控制指令解析、飛行器位置控制器、飛行器姿態(tài)控制器、匿名上位機(jī)、空心杯電機(jī)控制等模塊構(gòu)成，各模塊之間相互聯(lián)系，相互協(xié)調(diào)，使四旋翼飛行器系統(tǒng)能夠正常工作、快速反應(yīng)。四旋翼飛行器總體結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)總體結(jié)構(gòu)

各模塊功能說明如下：

1)USB雙目攝像頭識別：通過路由器運(yùn)行在OpenWrt環(huán)境下，采集USB攝像頭的圖像，并把圖像流放到8080網(wǎng)絡(luò)端口，電腦接入該網(wǎng)絡(luò)后，獲取圖像流并在OpenCV環(huán)境進(jìn)行圖像處理，識別飛行器前方視野障礙物的距離及坐標(biāo)信息，通過網(wǎng)絡(luò)發(fā)送到飛行器主控芯片上處理，并做出相應(yīng)的飛行動作。

2)2.4 G遙控器：通過搖桿遙控手動下發(fā)控制指令到飛行器上，可以切換手動控制模式與尋物模式運(yùn)行，在緊急情況下可以手動控制飛行器飛行，避免不必要的損失與傷害。

3)控制指令解析：飛行器在運(yùn)行過程中需要接收控制指令才能正常運(yùn)行，控制指令通過路由器的串口傳輸與2.4 G接收機(jī)的PWM信號傳輸，傳送到飛控的指令解析器，將指令數(shù)據(jù)傳送到控制器。

4)飛行器位置控制器：由GPS、氣壓計(jì)、超聲波的測量數(shù)據(jù)融合出飛行器在地球表面的近似直角坐標(biāo)，以此反饋信號傳輸?shù)絻?yōu)化的PID位置控制器進(jìn)行控制。在三維坐標(biāo)上不斷修正飛行器相對地面的位置，使其相對地面靜止，保證飛行器穩(wěn)定，為穩(wěn)定的圖像提供基礎(chǔ)。

5)飛行器姿態(tài)控制器：通過讀取飛行器上慣性測量單元IMU的數(shù)據(jù)，使用基于四元數(shù)姿態(tài)的梯度下降法進(jìn)行姿態(tài)融合，由公式將四元數(shù)表達(dá)方式轉(zhuǎn)換為歐拉角。反饋得到的歐拉角數(shù)據(jù)與陀螺儀數(shù)據(jù)傳送到串級PID控制器，進(jìn)行姿態(tài)控制，使飛行器穩(wěn)定起飛并在空間上保持在一定角度。

1.2 控制系統(tǒng)硬件設(shè)計(jì)

四旋翼飛行器控制系統(tǒng)的硬件設(shè)計(jì)分為兩部分，一是地面部分，另一部分是飛行器本體部分。地面部分也稱非飛行器部分，用來控制和輔助四旋翼飛行器實(shí)現(xiàn)相關(guān)功能，結(jié)構(gòu)相對簡單，主要包括筆記本電腦、中繼路由器和一個2.4 G遙控器。飛行器本體部分結(jié)構(gòu)則比較復(fù)雜，它是以STM32為主控芯片對各種數(shù)據(jù)進(jìn)行處理，并對整個機(jī)體進(jìn)行控制；除與地面部分相搭配的嵌入式路由器和2.4 G遙控器接收機(jī)以外，還在主控芯片上搭載了許多外圍模塊，如超聲波測距模塊、慣性測量單元(IMU)模塊和GPS模塊等，此外還在路由器上掛載了一個雙目攝像頭。控制系統(tǒng)硬件結(jié)構(gòu)如圖2所示。

圖2 控制系統(tǒng)硬件結(jié)構(gòu)圖

1.3 系統(tǒng)電源設(shè)計(jì)

在四旋翼飛行器系統(tǒng)中，性能穩(wěn)定是至關(guān)重要的，這就要求系統(tǒng)不僅在硬件設(shè)計(jì)上要合理，選取的器件性能好，保證在電磁波復(fù)雜的空中不受干擾穩(wěn)定運(yùn)行，而且還要求系統(tǒng)有良好穩(wěn)定的直流電源，使之在復(fù)雜的電源環(huán)境中能夠?yàn)V掉諧波，為各用電器件提供穩(wěn)定的直流電。

在進(jìn)行系統(tǒng)電源設(shè)計(jì)時，需要考慮直流電源品質(zhì)，如電源能夠提供的最大輸出電流、紋波抑制比大小、輸入系統(tǒng)的電源范圍等，只要其中一項(xiàng)不符合要求，都可能使系統(tǒng)供電中斷不能正常工作，嚴(yán)重的還會導(dǎo)致飛行器在飛行時墜落。本系統(tǒng)的電源設(shè)計(jì)框圖如圖3所示，在電調(diào)輸出的5 V直流電源基礎(chǔ)上，分配電壓到系統(tǒng)的各用電器件。

圖3 電源設(shè)計(jì)框圖

1.4 IMU模塊電路

IMU模塊如圖4所示，模塊集成了運(yùn)動慣性器件MPU6050、地磁計(jì)HMC5883L、氣壓計(jì)MS5611三個傳感器，分別采集對應(yīng)的數(shù)據(jù)。IMU模塊與STM32主控的接線方式如圖5所示。為了減小機(jī)體震動對IMU的影響，本文將IMU放置在帶減震球的板上，并用海綿蓋住，減小旋翼氣流對IMU的影響[4]。

圖4 IMU模塊圖5 IMU與STM32的接線

1.5 2.4 G遙控器設(shè)計(jì)

操縱人員通過2.4 G遙控器與飛行器主控進(jìn)行雙向通信。在設(shè)計(jì)遙控通訊時，需要考慮通信距離、通信速率、無線干擾、操作性等，本設(shè)計(jì)中選用大功率NRF24L01無線模塊，主控通過SPI通訊端口與其進(jìn)行通信，數(shù)據(jù)傳輸速率最高可達(dá)到2 Mbps，可以做數(shù)據(jù)雙向透傳，而且本通信模塊本身設(shè)計(jì)就是有功率芯片的，這大大地增加了通信的范圍，最遠(yuǎn)可以達(dá)到1.5千米，對于飛行器的通信要求來說十分適用。

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

2.1 系統(tǒng)軟件結(jié)構(gòu)

基于雙目立體視覺避障的四旋翼飛行器系統(tǒng)軟件設(shè)計(jì)框圖如圖6所示。整個軟件系統(tǒng)分為兩部分：飛行器穩(wěn)定系統(tǒng)和圖像處理系統(tǒng)。飛行器穩(wěn)定系統(tǒng)保證四旋翼飛行器在空中能夠穩(wěn)定飛行；圖像處理系統(tǒng)通過讀取掛載在飛行器上的雙目攝像頭采集回來的圖像，在筆記本電腦上處理圖像信息，進(jìn)行空間三維重建得出與前方障礙物的距離信息，通過路由器傳送到飛行器系統(tǒng)判斷飛行器是否需要采取避障繞飛。

圖6 系統(tǒng)軟件設(shè)計(jì)框圖

2.2 飛行器姿態(tài)控制器

在整個穩(wěn)定系統(tǒng)里，系統(tǒng)需要獲取控制飛行器的信息，讀取慣性測量單元的數(shù)據(jù)并進(jìn)行姿態(tài)估算，數(shù)據(jù)準(zhǔn)備好后通過姿態(tài)穩(wěn)定控制器運(yùn)算出調(diào)節(jié)飛行器運(yùn)動輸出的控制量，通過不斷循環(huán)這樣的操作使飛行器在空中穩(wěn)定飛行。

飛行器的不穩(wěn)定因素主要來自于在飛行過程中周邊的氣流的不平穩(wěn)。由于在飛行中每一處的空氣密度和氣壓大小不一致，盡管空心杯輸出的轉(zhuǎn)矩、轉(zhuǎn)速一致也不能夠保證飛行器能夠平穩(wěn)飛行。串級PID控制的基本理念就是將系統(tǒng)中的外部擾動納入副回路的調(diào)節(jié)中。可是擾動亦影響機(jī)體角速度變化，這個變化可以由IMU中的陀螺儀檢測出來，從而在副回路角速度PID控制器控制角速度從而增加飛行穩(wěn)定性。在串級PID控制系統(tǒng)的主回路中，我們通過IMU中陀螺儀的角度反饋值去控制飛行器的期望角度保持穩(wěn)定，從而可以平滑地改變飛行器的飛行姿態(tài)。

通過對四旋翼飛行器系統(tǒng)模型的分析可知，四旋翼飛行器是一個典型的非線性系統(tǒng)[5-6]，進(jìn)行控制時往往需要用相應(yīng)的非線性控制算法去匹配，但PID控制算法經(jīng)過優(yōu)化后可以在四旋翼飛行器上實(shí)現(xiàn)穩(wěn)定控制，并具有良好的控制品質(zhì)[7-8]，圖7所示為運(yùn)用串級PID實(shí)現(xiàn)的飛行器姿態(tài)控制。通過PID算法利用偏差去消除偏差的思想，飛行器主控通過遙控器發(fā)出的指令以及飛行器自身各個傳感器反饋的數(shù)據(jù)互相結(jié)合最后計(jì)算出當(dāng)前實(shí)際的飛行狀態(tài)和我們給定的目標(biāo)狀態(tài)的偏差，然后作為串級PID控制器的輸入，最終輸出PWM控制四個空心杯電機(jī)作動力輸出。

圖7 姿態(tài)串級PID控制

2.3 圖像處理系統(tǒng)

雙目檢測障礙物的原理：采集一對平行的單目攝像機(jī)的圖像，計(jì)算同一時刻兩幅圖像的視差(d=xl-xr)，障礙物三維坐標(biāo)中的距離Z=fT/d，在OpenCV中，視差d和焦距f的單位是像素，T的單位是根據(jù)攝像頭標(biāo)定時棋盤標(biāo)定板的實(shí)際大小和人為輸入的數(shù)值所決定的，根據(jù)檢測精度，一般設(shè)為毫米級或0.1毫米級。

如若還想計(jì)算障礙物的X，Y坐標(biāo)的話，還需要知道左平面和右平面的坐標(biāo)系和立體坐標(biāo)系中原點(diǎn)的偏移量Cx和Cy。通過雙目視覺立體標(biāo)定，可以獲取焦距f，攝像頭中心矩Tx,左右平面坐標(biāo)系和立體坐標(biāo)系的原點(diǎn)偏移量Cx和Cy的初始值，再通過校準(zhǔn)優(yōu)化，計(jì)算出視差d，最后求出空間點(diǎn)的三維坐標(biāo)[X,Y,Z],物體越遠(yuǎn)視差越小，物體越近視差越大，直接對攝像頭視野內(nèi)的距離進(jìn)行測量，無論前方的障礙物是什么類型，都可以得到動態(tài)變化的距離信息，從而可以讓飛行器根據(jù)該信息做出相應(yīng)的行為方案。

雙目避障的圖像處理流程如圖8所示。流程分為雙目圖像采集、雙目圖像矯正、雙目立體匹配、計(jì)算深度信息四大部分。本文通過運(yùn)行在開源OpenWrt系統(tǒng)的嵌入式路由器采集圖像信息，并通過WIFI網(wǎng)絡(luò)實(shí)時傳輸?shù)焦P記本電腦上。因此需要將路由器刷入OpenWrt系統(tǒng)，并在系統(tǒng)里安裝免驅(qū)動的USB軟件包并配置，同時安裝Ser2net軟件包，配置路由器串口進(jìn)行控制數(shù)據(jù)的傳輸。識別飛行器前方的障礙物的核心算法是基于開源計(jì)算機(jī)視覺庫OPENCV編寫的，通過SGBM算法獲得圖像的視差圖從而計(jì)算出深度信息，最終獲得飛行器前方的障礙物到攝像頭的距離以及坐標(biāo)信息，將結(jié)果顯示到屏幕上并發(fā)送到四旋翼飛行器，飛行器控制器收到障礙物的位置和坐標(biāo)信息后判斷是否需要繞行避開障礙物。

圖8 雙目避障的圖像處理流程

3 結(jié)果與分析

3.1 雙目測距結(jié)果

掛載在飛行器上的雙目攝像頭采集圖像信息，并流放到8080網(wǎng)絡(luò)端口，筆記本電腦接入該網(wǎng)絡(luò)后，獲取圖像流并在OpenCV環(huán)境進(jìn)行處理，識別到障礙物的距離及坐標(biāo)信息后，通過網(wǎng)絡(luò)發(fā)送到飛行器主控，實(shí)現(xiàn)避障功能。經(jīng)過處理后的圖像，可在圖9所示的可視化窗口中顯示，由圖可以看出，左右圖像嚴(yán)格保持在同一水平線上。

圖9 可視化窗口顯示圖像

圖10顯示的是雙目攝像頭到障礙物的距離以及坐標(biāo)信息，多次測量得出351.386、351.081、349.559、349.863 mm與實(shí)際測量距離350 mm相差1 mm左右，該雙目視覺測距系統(tǒng)成功地實(shí)現(xiàn)了實(shí)時測距功能，并且測距精度在1 mm左右，同時還可看到程序處理每一幀圖像數(shù)據(jù)的時間0.25 s。

圖10 雙目攝像頭到障礙物的距離以及坐標(biāo)

3.2 姿態(tài)解算結(jié)果

四旋翼飛行器在飛行過程中，受到大氣壓、空氣密度變化等因素影響，所以穩(wěn)定的姿態(tài)解算是飛行器能否正常運(yùn)作的重中之重。本文的姿態(tài)解算主要是利用慣性測量元件IMU中的加速度計(jì)、陀螺儀以及地磁計(jì)進(jìn)行信息融合。具體的實(shí)現(xiàn)過程是，先計(jì)算出陀螺儀的四元數(shù)的變化速率，同時把加速度計(jì)和地磁計(jì)獲取的數(shù)據(jù)進(jìn)行歸一化處理以增強(qiáng)適應(yīng)性，計(jì)算中間輔助變量，再通過地磁計(jì)的參考方向和梯度修正算法進(jìn)行修正，最后得到四元數(shù)的變化速率和變化的四元數(shù)范圍，進(jìn)行歸一化處理，再根據(jù)四元數(shù)和歐拉角之間的轉(zhuǎn)換關(guān)系得到歐拉角[9-10]。經(jīng)過基于四元數(shù)姿態(tài)的梯度下降法解算后，四旋翼飛行器的姿態(tài)能夠精確地估算出來。圖11所示為陀螺儀姿態(tài)角的實(shí)時波形，解算出來的姿態(tài)角能夠快速跟蹤，且在四旋翼飛行器快速晃動時波形能夠平滑地變化，證明用梯度下降法解算的結(jié)果能夠精確地估算出四旋翼飛行器的姿態(tài)。

圖11 陀螺儀姿態(tài)角的實(shí)時波形

通過使用匿名科創(chuàng)地面站上位機(jī)，可以方便直觀地觀察到飛行器飛行時各項(xiàng)參數(shù)的動態(tài)變化，以便進(jìn)行數(shù)據(jù)分析。在應(yīng)用經(jīng)驗(yàn)整定法調(diào)節(jié)PID參數(shù)時主要觀察陀螺儀姿態(tài)角的三個方面的變化，一是穩(wěn)定性，給定一個期望的平衡角度，觀察是否能夠長時間穩(wěn)定在期望角度附近；二是準(zhǔn)確性，當(dāng)通過遙控器發(fā)操縱命令時，飛行器能否準(zhǔn)確響應(yīng)，是否會出現(xiàn)過沖；第三是快速性，當(dāng)操縱命令發(fā)出時，飛行器能否快速順滑地跟隨操縱人員的期望動作，操縱與飛行器響應(yīng)是否存在滯后。

3.3 實(shí)驗(yàn)結(jié)果與分析

四旋翼飛行器的實(shí)物圖如圖12所示，控制電路板安放在機(jī)體上面，用減震板支撐，獲取圖像的嵌入式路由器板安放在機(jī)體中間，雙目攝像頭安裝在機(jī)體底部。四旋翼飛行器能夠?qū)崿F(xiàn)一鍵起飛、定高、定點(diǎn)巡航、雙目躲避障礙等功能；由于光流傳感器的作用，從地面靜止到在空中懸停，可以做到在微小范圍內(nèi)穩(wěn)定懸飛；控制飛行方向時，飛行器響應(yīng)速度快，跟隨性能好；在GPS定點(diǎn)巡航時，定位精度在cm級，在室外開闊場地能夠定位準(zhǔn)確。在進(jìn)行障礙物檢測時，輔以雙目視覺檢測技術(shù)，能夠利用返回障礙物的位置信息，在正常飛行時能夠通過飛行器的方向環(huán)調(diào)節(jié)，使機(jī)體繞開障礙物，避障效果良好。

圖12 四旋翼飛行器的實(shí)物圖

4 結(jié)束語

本次設(shè)計(jì)，對基于雙目立體視覺避障的四旋翼飛行器從理論構(gòu)思到系統(tǒng)硬軟件分析，通過一步步實(shí)踐做出實(shí)物，并通過編程驗(yàn)證理論，證實(shí)了當(dāng)初的構(gòu)思是可行的。在雙目避障的圖像處理方面，本文提及的算法還需要進(jìn)一步優(yōu)化，以達(dá)到精確的識別效果。