湛柏明,馮浩文,黃海波,付為偉,沈佳明
(湖北汽車工業(yè)學(xué)院電氣與信息工程學(xué)院,湖北 十堰 442002)
近年來(lái),無(wú)人飛行器在眾多領(lǐng)域得到了發(fā)展[1]。這是由于無(wú)人飛行器與有人飛行器相比具有很多優(yōu)點(diǎn),包括體積小、成本低以及具有低空飛行能力。此外,無(wú)人飛行器能為有人飛行器或人造衛(wèi)星擴(kuò)大工作區(qū)域。基于這些優(yōu)點(diǎn),不同類型的無(wú)人飛行器被應(yīng)用于各種軍事行動(dòng)和日?;顒?dòng)中[2],例如監(jiān)視、偵察、搜索和救援等任務(wù)。在這些任務(wù)當(dāng)中,無(wú)人飛行器在多數(shù)情況下是在貼近地面的低空中飛行的,在飛行過(guò)程中有可能與樹木、建筑物以及其他障礙物發(fā)生碰撞。因此,無(wú)人飛行器的避障功能對(duì)其安全作業(yè)起到不可忽視的作用[3]。
許多學(xué)者對(duì)無(wú)人飛行器的避障問(wèn)題進(jìn)行了研究,提出了諸多避障方法,如計(jì)算機(jī)視覺[4]、毫米波雷達(dá)[5]、全球定位系統(tǒng)(global positioning system,GPS)[6]、激光測(cè)距儀[7]和隨機(jī)數(shù)[8]等方法。上述方法中,計(jì)算機(jī)視覺目前技術(shù)尚不成熟,大多處于理論設(shè)計(jì)階段;毫米波雷達(dá)和激光測(cè)距儀價(jià)格昂貴,不適用于小型民用飛行器;GPS在室內(nèi)的定位精度較差;隨機(jī)數(shù)的運(yùn)算過(guò)程較為復(fù)雜,會(huì)加大單片機(jī)的運(yùn)算壓力、降低系統(tǒng)響應(yīng)速度。
根據(jù)小型室內(nèi)無(wú)人飛行器的特點(diǎn),本文基于STM32控制器設(shè)計(jì)了一款小型室內(nèi)四旋翼飛行器懸停及避障系統(tǒng)。首先,本文對(duì)慣性測(cè)量單元(inertial measurement unit,IMU)的數(shù)據(jù)進(jìn)行處理,得到四旋翼飛行器實(shí)時(shí)姿態(tài)和位置信息。其次,本文通過(guò)三個(gè)控制器使四旋翼飛行器保持平穩(wěn)懸停飛行。最后,四旋翼飛行器根據(jù)避障模塊測(cè)得周圍障礙物的距離,以進(jìn)行實(shí)時(shí)躲避[9-10]。
一架多軸四旋翼飛行器運(yùn)行時(shí)至少需要兩個(gè)部分,分別為具有自動(dòng)駕駛儀的多軸機(jī)體和無(wú)線遙控裝置。地面的操作人員通過(guò)操作無(wú)線遙控裝置向四旋翼飛行器自動(dòng)駕駛儀傳輸信號(hào)。四旋翼飛行器則會(huì)根據(jù)收到的信號(hào)做出相應(yīng)的動(dòng)作。但實(shí)際上,無(wú)線遙控裝置并非直接和主控制器傳輸信號(hào),而是通過(guò)接收機(jī)中轉(zhuǎn)。無(wú)線遙控裝置通過(guò)無(wú)線信號(hào)和接收機(jī)建立通信。接收機(jī)把接收到的信號(hào)轉(zhuǎn)換為脈沖寬度調(diào)制(pulse width modulation,PWM)信號(hào)傳送給飛行器主控制器。系統(tǒng)總體框圖如圖1所示。
圖1 系統(tǒng)總體框圖
系統(tǒng)通過(guò)IMU獲得的數(shù)據(jù)解算四旋翼飛行器當(dāng)前的姿態(tài);通過(guò)超聲波和光流傳感器獲取四旋翼飛行器當(dāng)前的位置和高度;通過(guò)姿態(tài)比例積分微分(proportion integral derivative,PID)位置PID、高度PID控制器控制四旋翼飛行器懸停飛行。四旋翼飛行器在懸停狀態(tài)下,如果有人靠近,則改變四旋翼飛行器的Pitch或者Roll通道的值,使得四旋翼飛行器往安全的方向飛行。系統(tǒng)工作原理如圖2所示。
圖2 系統(tǒng)工作原理圖
四旋翼飛行器的主控芯片采用STM32F103系統(tǒng)的小容量芯片。該芯片搭載了三軸加速度計(jì)和陀螺儀等IMU。超聲波傳感器和光流傳感器用作高度和位置測(cè)量。接收器用作接收遙控信號(hào)。四旋翼飛行器機(jī)體包含場(chǎng)效應(yīng)管、直流電機(jī)和電子調(diào)速器等部件。
四旋翼飛行器的動(dòng)力來(lái)源是四個(gè)槳葉。旋翼的安裝可分為“×”字型和“十”字型。四旋翼飛行器模式如圖3所示。由于“×”字型的結(jié)構(gòu)穩(wěn)定,所以本設(shè)計(jì)采用的模式為“×”字型,且機(jī)頭、機(jī)尾的兩個(gè)旋翼轉(zhuǎn)向均相反,對(duì)角線上的旋翼轉(zhuǎn)向相同。這樣既能使飛行器有升力,又能抵消反扭力。
圖3 四旋翼飛行器模式
穩(wěn)壓電路如圖4所示。
圖4 穩(wěn)壓電路圖
四旋翼飛行器供電采用24 V、2 000 mA的Ni-Cd電池,一部分直接給電機(jī)供電,另一部分經(jīng)過(guò)穩(wěn)壓電路后供給各傳感器。穩(wěn)壓電路采用AMS1117-3.3穩(wěn)壓芯片,既能滿足各個(gè)傳感器的工作電壓(5 V),又能滿足STM32的最小工作電壓(3.3 V)。
MPU6050集成了加速度計(jì)和陀螺儀。地磁傳感器三軸加速度和三軸角速度均為16位的模數(shù)轉(zhuǎn)換器(analog-to-digital converter,ADC),能夠通過(guò)軟件進(jìn)行分辨率的設(shè)置以適應(yīng)各種場(chǎng)景。由于擁有較高的性價(jià)比,這款運(yùn)動(dòng)傳感器芯片十分適用于小型四旋翼飛行器系統(tǒng)。MPU6050傳感器外圍電路如圖5所示。
圖5 MPU6050傳感器外圍電路圖
IST8310傳感器外圍電路如圖6所示。
圖6 IST8310傳感器外圍電路圖
地磁傳感器采用IST8310,可以輸出當(dāng)?shù)氐娜S磁場(chǎng)強(qiáng)度。通信方式均為內(nèi)部集成電路(inter-integrated circuit,I2C)。
IST8310傳感器具備在±0.000 8 T的磁場(chǎng)環(huán)境中分辨0.000 000 5 T的能力。用地磁傳感器作為羅盤定位航向時(shí),能達(dá)到1°至2°的精度。地磁傳感器最大的輸出頻率為160 Hz,滿足了四旋翼飛行器系統(tǒng)對(duì)數(shù)據(jù)更新頻率的要求。
①超聲波傳感器。
本設(shè)計(jì)采用US_100超聲波模塊。US_100超聲波模塊自帶溫度補(bǔ)償,測(cè)量范圍為0.02~4 m,能夠滿足四旋翼飛行器室內(nèi)高度測(cè)量需求。需要注意的是,US_100超聲波模塊在安裝時(shí)不要被其他物體所遮擋。
②氣壓計(jì)傳感器。
一旦飛行器高度大于4 m時(shí),超聲波傳感器的數(shù)據(jù)將不再精準(zhǔn),因此本設(shè)計(jì)需要切換到氣壓計(jì)來(lái)獲取四旋翼飛行器的實(shí)時(shí)高度。氣壓計(jì)采用MS5611。氣壓計(jì)傳感器的采樣周期最小為1.0 ms,最大為16.44 ms,采樣周期和誤差呈反比關(guān)系。MS5611安裝時(shí)要做到密封,不能被空氣流速所干擾。
③光流傳感器。
由于室內(nèi)的GPS定位不準(zhǔn)確,因此本設(shè)計(jì)采用優(yōu)象LC302光流模塊來(lái)獲取四旋翼飛行器的實(shí)時(shí)位置信息。LC302用于檢測(cè)四旋翼飛行器在飛行過(guò)程中的水平方向移動(dòng),并將結(jié)果傳輸給飛控。LC302能對(duì)一般水面、草地、大理石地面、水泥地面、室內(nèi)木地板等壞境進(jìn)行準(zhǔn)確檢測(cè)。LC302安裝前要注意鏡頭清理,確保鏡頭無(wú)污垢和保護(hù)膜遮擋,且光流與加速度計(jì)物理方向一致。
硬件初始化涵蓋底層寄存器、中斷優(yōu)先級(jí)I2C、陀螺儀MPU6050、氣壓計(jì)MS5611、加速度計(jì)FBM320、ADC、電機(jī)、GPS、光流模塊、普通用I/O(超聲波、紅外傳感器)等。初始化完成后,通過(guò)遙控器控制四旋翼飛行器飛行。當(dāng)未開啟懸停飛行時(shí),四旋翼飛行器主控制器會(huì)根據(jù)遙控器輸入的信號(hào)控制飛行。
當(dāng)通過(guò)遙控器開啟懸停時(shí),四旋翼飛行器在空中懸停,避障模塊開始采集數(shù)據(jù),判斷周圍物體與四旋翼飛行器的距離或者有無(wú)人員經(jīng)過(guò)四旋翼飛行器。如果物體與四旋翼飛行器的距離小于預(yù)設(shè)值,則避障模塊通過(guò)串口向主控模塊傳輸對(duì)應(yīng)的信息。主控模塊改變對(duì)應(yīng)的Pitch或者Roll通道的值,使得四旋翼飛行器作出相應(yīng)的規(guī)避動(dòng)作。
軟件系統(tǒng)總流程如圖7所示。
圖7 軟件系統(tǒng)總流程圖
3.2.1 旋轉(zhuǎn)矩陣
由于四旋翼飛行器和地球是兩個(gè)不同的坐標(biāo)系,要想得到四旋翼飛行器相對(duì)于地球的姿態(tài),需要將四旋翼飛行器的坐標(biāo)系轉(zhuǎn)換到地球坐標(biāo)系中。設(shè)地球所在的三軸坐標(biāo)系為N系、四旋翼飛行器的三軸坐標(biāo)系為b系,則b系到N系的轉(zhuǎn)換相當(dāng)于x、y、z三軸分別轉(zhuǎn)換的疊加。其中,x軸的旋轉(zhuǎn)矩陣為:
(1)
式中:θ為x軸的旋轉(zhuǎn)角度,(°)。
y軸的旋轉(zhuǎn)矩陣為:
(2)
式中:γ為y軸的旋轉(zhuǎn)角度,(°)。
z軸的旋轉(zhuǎn)矩陣為:
(3)
式中:φ為z軸的旋轉(zhuǎn)角度,(°)。
(4)
(5)
根據(jù)上述矩陣,可反解三個(gè)歐拉角。
(6)
3.2.2 羅德里格旋轉(zhuǎn)
羅德里格旋轉(zhuǎn)是計(jì)算三維空間中,一個(gè)向量繞旋轉(zhuǎn)軸旋轉(zhuǎn)給定角度以后得到的新向量的計(jì)算式。其結(jié)論為:
(7)
(8)
3.2.3 四元數(shù)解歐拉角
四元數(shù)是由四個(gè)元所構(gòu)成的數(shù)。四元數(shù)的結(jié)構(gòu)分成實(shí)數(shù)和虛數(shù)兩部分。根據(jù)其三角表達(dá)式,有:
(9)
(10)
根據(jù)式(10)反解歐拉角:
(11)
3.2.4 單片機(jī)解四元數(shù)
因?yàn)樗脑獢?shù)直接相關(guān)的變量是角度,利用四元數(shù)的三角表示式建立四元數(shù)微分方程,并且求解該微分方程,即可成功解出四元數(shù)。
對(duì)式(9)進(jìn)行時(shí)間上的微分,可得微分方程:
(12)
由于單片機(jī)只能進(jìn)行離散計(jì)算,所以采用一階龍格庫(kù)塔法解式(12)的微分方程。
根據(jù)一階龍格庫(kù)塔法,可以得到求解的迭代式:
Q(t+Δt)=Q(t)+Δt×φ(t)×Q(t)
(13)
整理可得最終的計(jì)算結(jié)論:
(14)
三軸角度由三軸加速度計(jì)所測(cè)得的加速度正交和三軸陀螺儀測(cè)得的角速度積分所得。
3.2.5 互補(bǔ)濾波
三軸陀螺儀測(cè)得角速度積分后會(huì)產(chǎn)生積分漂移。所以,隨著時(shí)間的累計(jì),誤差會(huì)越來(lái)越大。而三軸加速度計(jì)測(cè)得加速度正交后,會(huì)產(chǎn)生震動(dòng)誤差。所以,當(dāng)機(jī)體劇烈晃動(dòng)時(shí),加速度計(jì)的數(shù)據(jù)也會(huì)不準(zhǔn)。因此,需要對(duì)所得的數(shù)據(jù)作進(jìn)一步處理。
(15)
(16)
(17)
式中:ε為控制器輸出補(bǔ)償?shù)慕嵌龋?°)。
積分項(xiàng)用于消除靜態(tài)誤差。比列項(xiàng)用于控制傳感器的可信度。當(dāng)KP越大時(shí),則更加信任加速度計(jì)的數(shù)據(jù)。如果KP過(guò)大,系統(tǒng)在受到擾動(dòng)后會(huì)產(chǎn)生高頻振蕩。當(dāng)KP越小時(shí),則更加信任陀螺儀的數(shù)據(jù)。如果KP過(guò)小,系統(tǒng)在受到擾動(dòng)后數(shù)據(jù)會(huì)產(chǎn)生大范圍的漂移。經(jīng)多次調(diào)參后,確定KP=0.8、KI=0.001。該參數(shù)下的PI控制器能得到較為平滑的數(shù)據(jù)曲線。補(bǔ)償前后角速度輸出對(duì)比如圖8所示。用補(bǔ)償后的角速度進(jìn)行求解,即可得到比較準(zhǔn)確的姿態(tài)信息。
圖8 補(bǔ)償前后角速度輸出對(duì)比
3.3.1 定高控制
由于四旋翼飛行器在定高模式下可以進(jìn)行橫滾、俯仰操作,而在進(jìn)行這些操作的時(shí)候超聲波測(cè)得的距離并不是四旋翼飛行器真正與地面的垂直距離,所以實(shí)際高度應(yīng)為:
H=h×cosφPitch×cosφRoll
(18)
式中:H為實(shí)際的高度,m;h為測(cè)得的高度,m;cosφPitch和cosφRoll為俯仰角的余弦值和橫滾角的余弦值。
在獲得實(shí)際高度后,計(jì)算實(shí)際高度與預(yù)設(shè)高度的誤差并將其傳輸?shù)礁叨萈ID控制器中。如果實(shí)際高度小于預(yù)設(shè)高度,則加大油門;反之亦然。
3.3.2 定點(diǎn)控制
懸??刂屏鞒倘鐖D9所示。
圖9 懸停控制流程圖
定點(diǎn)控制采用光流傳感器檢測(cè)地面像素點(diǎn)的位移,可以輸出當(dāng)前狀態(tài)下的水平移動(dòng)速度。通過(guò)對(duì)水平速度作積分,可以得到水平移動(dòng)的距離。把該距離作為偏差傳入位置PID控制器中,即可改變Pitch或者Roll通道的值,使得飛行器能夠定點(diǎn)飛行。
在四旋翼飛行器完成懸停操作之后,避障模塊上的超聲波傳感器開始采集對(duì)應(yīng)的數(shù)據(jù)。為了保證四個(gè)超聲波的采集不被系統(tǒng)更新所打斷,采用STM32的通用定時(shí)的四個(gè)通道進(jìn)行輸入捕獲,且搶占優(yōu)先級(jí)要高。紅外傳感器的采集方式與超聲波雷同。當(dāng)避障模塊檢測(cè)到物體靠近四旋翼飛行器時(shí),避障模塊會(huì)通過(guò)串口傳輸對(duì)應(yīng)的信號(hào)給主控模塊。主控模塊根據(jù)對(duì)應(yīng)的信息改變Pitch或者Roll通道的值,以進(jìn)行躲避。
為了保證四旋翼飛行器的飛行姿態(tài)平穩(wěn),需對(duì)其姿態(tài)角進(jìn)行PID調(diào)節(jié)。姿態(tài)角采用串級(jí)PID控制。四旋翼飛行器串級(jí)PID控制系統(tǒng)如圖10所示。
圖10 四旋翼飛行器串級(jí)PID控制系統(tǒng)框圖
由圖10可知,內(nèi)環(huán)為角速度,而外環(huán)為角度環(huán)。外環(huán)參數(shù)和內(nèi)環(huán)參數(shù)有以下對(duì)應(yīng)關(guān)系。對(duì)于外環(huán),有:
(19)
式中:e0為當(dāng)前角度誤差,(°);β為期望角度,(°);β*為當(dāng)前角度,(°);A為外環(huán)P項(xiàng);B為外環(huán)I項(xiàng);KP1為外環(huán)P參數(shù);TI1為外環(huán)I參數(shù)。
對(duì)于內(nèi)環(huán),有:
(20)
式中:ω為外環(huán)PI輸出;ω*為三軸陀螺儀輸出的角速度,rad/s;X為內(nèi)環(huán)P項(xiàng);Y為內(nèi)環(huán)I項(xiàng);Z為內(nèi)環(huán)D項(xiàng);KP2為內(nèi)環(huán)P參數(shù);TI2為內(nèi)環(huán)I參數(shù);TD2為內(nèi)環(huán)D參數(shù)。
其中,KP2、TI2、TD2、KP1需要根據(jù)四旋翼飛行器的不同參數(shù)來(lái)進(jìn)行調(diào)節(jié)。調(diào)節(jié)方式如下。
①KP2。隨著KP2值由小到大,四旋翼飛行器抗干擾能力越來(lái)越強(qiáng)。用手用力拉動(dòng)四旋翼,可以明顯感覺到它在抵抗這個(gè)拉動(dòng)。繼續(xù)增加KP2值,四旋翼飛行器會(huì)出現(xiàn)肉眼可見的高頻率振動(dòng)。此時(shí)用力拉動(dòng)四旋翼飛行器,它會(huì)快速抖動(dòng)幾下,然后會(huì)恢復(fù)穩(wěn)定。如再增加KP2值,則四旋翼飛行器可能會(huì)出現(xiàn)直接翻轉(zhuǎn)墜機(jī)的情況。
②TI2。根據(jù)以上分析可知,I只是用于消除靜態(tài)誤差的。如果TI2值過(guò)大,會(huì)降低系統(tǒng)的穩(wěn)定性,達(dá)不到理想效果。隨著TI2值的逐漸增大,如果達(dá)到一個(gè)理想值附近,那么四旋翼飛行器會(huì)像一個(gè)釘子一樣釘在某個(gè)位置不動(dòng),不會(huì)再往下掉。如果這個(gè)時(shí)候繼續(xù)增大TI2值,那么四旋翼飛行器會(huì)從穩(wěn)定變?yōu)椴环€(wěn)定,拉扯一下就會(huì)發(fā)散。
③TD2。內(nèi)環(huán)是角速度,所以把內(nèi)環(huán)微分后角速度就變?yōu)榻羌铀俣?。如果四旋翼飛行器原本的振蕩比較大,此時(shí)再引入微分環(huán)節(jié)就更容易引起噪聲。隨著TD2由小到大,則四旋翼飛行器的抗干擾性能沒有多大改變,只是到最初的位置會(huì)更平穩(wěn)。如繼續(xù)增大TD2,四旋翼飛行器會(huì)出現(xiàn)肉眼可見的高頻率振動(dòng),或者聽見電機(jī)發(fā)出吱吱的聲音。
④KP1。外環(huán)是角度環(huán)。當(dāng)整定完內(nèi)環(huán)PID后,四旋翼飛行器已經(jīng)可以在受到干擾時(shí)也穩(wěn)定在某一位置。如果此時(shí)要四旋翼飛行器能回中,只能從角度環(huán)下手。外環(huán)KP1值由小到大,四旋翼飛行器回中的速度越來(lái)越快,用手拉扯然后放手,它會(huì)快速達(dá)到平衡位置。雖然KP1值越大,四旋翼飛行器響應(yīng)越快,但是如果KP1過(guò)大則四旋翼飛行器會(huì)有發(fā)散的趨勢(shì)。
由于內(nèi)環(huán)的作用就是消除外環(huán)的誤差,所以外環(huán)的I項(xiàng)和D項(xiàng)無(wú)需再調(diào)節(jié)。
本設(shè)計(jì)采用的軸距為330 mm的四旋翼飛行器,經(jīng)上述方法調(diào)節(jié)后,得出如表1所示的各部分姿態(tài)PID參數(shù)。
為了驗(yàn)證本文系統(tǒng)實(shí)際的運(yùn)行狀況,以一架330 mm軸距四旋翼飛行器為測(cè)試對(duì)象,進(jìn)行相關(guān)性能的調(diào)試。
在調(diào)試過(guò)程中,主要通過(guò)上位機(jī)查看四旋翼飛行器懸停的高度是否保持不變、避障模塊傳輸給主控制器的數(shù)據(jù)是否準(zhǔn)確,以及當(dāng)人靠近四旋翼飛行器時(shí),四旋翼飛行器能否迅速地做出避障動(dòng)作。
四旋翼飛行器懸停飛行測(cè)試曲線如圖11所示。由圖11可知,四旋翼飛行器在懸停時(shí)姿態(tài)角變化很小,且誤差最大不超過(guò)0.001 2°。該結(jié)果證明四旋翼飛行器飛行平穩(wěn)。
圖11 四旋翼飛行器懸停飛行測(cè)試曲線
為了驗(yàn)證避障模塊能否正常運(yùn)行,通過(guò)串口將避障模塊和主控制器連接起來(lái),并在四旋翼飛行器側(cè)方放置障礙物,以觀察四旋翼飛行器橫滾角的變化。避障時(shí)橫滾角的變化如圖12所示。
圖12 避障時(shí)橫滾角的變化
由圖12可知,當(dāng)四旋翼飛行器在懸停過(guò)程中,一旦有障礙物靠近,四旋翼飛行器會(huì)在短時(shí)間內(nèi)立馬做出規(guī)避的動(dòng)作,并在規(guī)避結(jié)束后繼續(xù)保持懸停。如果人體一直靠近,四旋翼飛行器會(huì)一直移動(dòng),并且在躲避時(shí)其高度不變、姿態(tài)平穩(wěn),且無(wú)需人工干預(yù)。此外,根據(jù)三種姿態(tài)解算的效果可知,加速度計(jì)抗干擾能力較弱,陀螺儀隨時(shí)間的增加誤差越來(lái)越大,通過(guò)互補(bǔ)濾波后解出的姿態(tài)曲線更平滑。
本文基于STM32F103控制器、IMU和觀測(cè)傳感器設(shè)計(jì)了一款小型室內(nèi)四旋翼飛行器,并通過(guò)互補(bǔ)濾波和四元數(shù)解算出四旋翼飛行器的姿態(tài),由PID控制器控制使其能夠懸停飛行。經(jīng)實(shí)測(cè),系統(tǒng)能在3 s內(nèi)達(dá)到指定高度并保持懸停。懸停過(guò)程中,四旋翼飛行器姿態(tài)平穩(wěn),懸停飛行漂移小,且四旋翼飛行器在懸停狀態(tài)下能對(duì)周圍的障礙物進(jìn)行檢測(cè)并躲避。本文系統(tǒng)具有穩(wěn)定性強(qiáng)、響應(yīng)速度快、安裝簡(jiǎn)便、造價(jià)低等優(yōu)點(diǎn)。后續(xù)研究將針對(duì)其在室外抗干擾能力較弱、對(duì)于較小的障礙物檢測(cè)不準(zhǔn)確等缺點(diǎn)繼續(xù)進(jìn)行系統(tǒng)改進(jìn),力爭(zhēng)在室內(nèi)外具有更廣泛的應(yīng)用場(chǎng)景。