小型室內四旋翼飛行器懸停及避障系統設計

湛柏明，馮浩文，黃海波，付為偉，沈佳明

(湖北汽車工業學院電氣與信息工程學院，湖北 十堰 442002)

0 引言

近年來，無人飛行器在眾多領域得到了發展[1]。這是由于無人飛行器與有人飛行器相比具有很多優點，包括體積小、成本低以及具有低空飛行能力。此外，無人飛行器能為有人飛行器或人造衛星擴大工作區域。基于這些優點，不同類型的無人飛行器被應用于各種軍事行動和日常活動中[2]，例如監視、偵察、搜索和救援等任務。在這些任務當中，無人飛行器在多數情況下是在貼近地面的低空中飛行的，在飛行過程中有可能與樹木、建筑物以及其他障礙物發生碰撞。因此，無人飛行器的避障功能對其安全作業起到不可忽視的作用[3]。

許多學者對無人飛行器的避障問題進行了研究，提出了諸多避障方法，如計算機視覺[4]、毫米波雷達[5]、全球定位系統(global positioning system,GPS)[6]、激光測距儀[7]和隨機數[8]等方法。上述方法中，計算機視覺目前技術尚不成熟，大多處于理論設計階段；毫米波雷達和激光測距儀價格昂貴，不適用于小型民用飛行器；GPS在室內的定位精度較差；隨機數的運算過程較為復雜，會加大單片機的運算壓力、降低系統響應速度。

根據小型室內無人飛行器的特點，本文基于STM32控制器設計了一款小型室內四旋翼飛行器懸停及避障系統。首先，本文對慣性測量單元(inertial measurement unit,IMU)的數據進行處理，得到四旋翼飛行器實時姿態和位置信息。其次，本文通過三個控制器使四旋翼飛行器保持平穩懸停飛行。最后，四旋翼飛行器根據避障模塊測得周圍障礙物的距離，以進行實時躲避[9-10]。

1 系統整體設計

1.1 四旋翼飛行器的運行過程及其信號傳遞

一架多軸四旋翼飛行器運行時至少需要兩個部分，分別為具有自動駕駛儀的多軸機體和無線遙控裝置。地面的操作人員通過操作無線遙控裝置向四旋翼飛行器自動駕駛儀傳輸信號。四旋翼飛行器則會根據收到的信號做出相應的動作。但實際上，無線遙控裝置并非直接和主控制器傳輸信號，而是通過接收機中轉。無線遙控裝置通過無線信號和接收機建立通信。接收機把接收到的信號轉換為脈沖寬度調制(pulse width modulation,PWM)信號傳送給飛行器主控制器。系統總體框圖如圖1所示。

圖1 系統總體框圖

1.2 系統工作原理

系統通過IMU獲得的數據解算四旋翼飛行器當前的姿態；通過超聲波和光流傳感器獲取四旋翼飛行器當前的位置和高度；通過姿態比例積分微分(proportion integral derivative,PID)位置PID、高度PID控制器控制四旋翼飛行器懸停飛行。四旋翼飛行器在懸停狀態下，如果有人靠近，則改變四旋翼飛行器的Pitch或者Roll通道的值，使得四旋翼飛行器往安全的方向飛行。系統工作原理如圖2所示。

圖2 系統工作原理圖

2 系統硬件設計

2.1 機體設計

四旋翼飛行器的主控芯片采用STM32F103系統的小容量芯片。該芯片搭載了三軸加速度計和陀螺儀等IMU。超聲波傳感器和光流傳感器用作高度和位置測量。接收器用作接收遙控信號。四旋翼飛行器機體包含場效應管、直流電機和電子調速器等部件。

四旋翼飛行器的動力來源是四個槳葉。旋翼的安裝可分為“×”字型和“十”字型。四旋翼飛行器模式如圖3所示。由于“×”字型的結構穩定，所以本設計采用的模式為“×”字型，且機頭、機尾的兩個旋翼轉向均相反，對角線上的旋翼轉向相同。這樣既能使飛行器有升力，又能抵消反扭力。

圖3 四旋翼飛行器模式

2.2 電源電路

穩壓電路如圖4所示。

圖4 穩壓電路圖

四旋翼飛行器供電采用24 V、2 000 mA的Ni-Cd電池，一部分直接給電機供電，另一部分經過穩壓電路后供給各傳感器。穩壓電路采用AMS1117-3.3穩壓芯片，既能滿足各個傳感器的工作電壓(5 V)，又能滿足STM32的最小工作電壓(3.3 V)。

2.3 IMU傳感器

MPU6050集成了加速度計和陀螺儀。地磁傳感器三軸加速度和三軸角速度均為16位的模數轉換器(analog-to-digital converter,ADC)，能夠通過軟件進行分辨率的設置以適應各種場景。由于擁有較高的性價比，這款運動傳感器芯片十分適用于小型四旋翼飛行器系統。MPU6050傳感器外圍電路如圖5所示。

圖5 MPU6050傳感器外圍電路圖

IST8310傳感器外圍電路如圖6所示。

圖6 IST8310傳感器外圍電路圖

地磁傳感器采用IST8310，可以輸出當地的三軸磁場強度。通信方式均為內部集成電路(inter-integrated circuit,I2C)。

IST8310傳感器具備在±0.000 8 T的磁場環境中分辨0.000 000 5 T的能力。用地磁傳感器作為羅盤定位航向時，能達到1°至2°的精度。地磁傳感器最大的輸出頻率為160 Hz，滿足了四旋翼飛行器系統對數據更新頻率的要求。

2.4 觀測傳感器

①超聲波傳感器。

本設計采用US_100超聲波模塊。US_100超聲波模塊自帶溫度補償，測量范圍為0.02～4 m，能夠滿足四旋翼飛行器室內高度測量需求。需要注意的是，US_100超聲波模塊在安裝時不要被其他物體所遮擋。

②氣壓計傳感器。

一旦飛行器高度大于4 m時，超聲波傳感器的數據將不再精準，因此本設計需要切換到氣壓計來獲取四旋翼飛行器的實時高度。氣壓計采用MS5611。氣壓計傳感器的采樣周期最小為1.0 ms，最大為16.44 ms，采樣周期和誤差呈反比關系。MS5611安裝時要做到密封，不能被空氣流速所干擾。

③光流傳感器。

由于室內的GPS定位不準確，因此本設計采用優象LC302光流模塊來獲取四旋翼飛行器的實時位置信息。LC302用于檢測四旋翼飛行器在飛行過程中的水平方向移動，并將結果傳輸給飛控。LC302能對一般水面、草地、大理石地面、水泥地面、室內木地板等壞境進行準確檢測。LC302安裝前要注意鏡頭清理，確保鏡頭無污垢和保護膜遮擋，且光流與加速度計物理方向一致。

3 系統軟件設計

3.1 軟件系統概述

硬件初始化涵蓋底層寄存器、中斷優先級I2C、陀螺儀MPU6050、氣壓計MS5611、加速度計FBM320、ADC、電機、GPS、光流模塊、普通用I/O(超聲波、紅外傳感器)等。初始化完成后，通過遙控器控制四旋翼飛行器飛行。當未開啟懸停飛行時，四旋翼飛行器主控制器會根據遙控器輸入的信號控制飛行。

當通過遙控器開啟懸停時，四旋翼飛行器在空中懸停，避障模塊開始采集數據，判斷周圍物體與四旋翼飛行器的距離或者有無人員經過四旋翼飛行器。如果物體與四旋翼飛行器的距離小于預設值，則避障模塊通過串口向主控模塊傳輸對應的信息。主控模塊改變對應的Pitch或者Roll通道的值，使得四旋翼飛行器作出相應的規避動作。

軟件系統總流程如圖7所示。

圖7 軟件系統總流程圖

3.2 姿態解算

3.2.1 旋轉矩陣

由于四旋翼飛行器和地球是兩個不同的坐標系，要想得到四旋翼飛行器相對于地球的姿態，需要將四旋翼飛行器的坐標系轉換到地球坐標系中。設地球所在的三軸坐標系為N系、四旋翼飛行器的三軸坐標系為b系，則b系到N系的轉換相當于x、y、z三軸分別轉換的疊加。其中，x軸的旋轉矩陣為：

(1)

式中：θ為x軸的旋轉角度，(°)。

y軸的旋轉矩陣為：

(2)

式中：γ為y軸的旋轉角度，(°)。

z軸的旋轉矩陣為：

(3)

式中：φ為z軸的旋轉角度，(°)。

(4)

(5)

根據上述矩陣，可反解三個歐拉角。

(6)

3.2.2 羅德里格旋轉

羅德里格旋轉是計算三維空間中，一個向量繞旋轉軸旋轉給定角度以后得到的新向量的計算式。其結論為：

(7)

(8)

3.2.3 四元數解歐拉角

四元數是由四個元所構成的數。四元數的結構分成實數和虛數兩部分。根據其三角表達式，有：

(9)

(10)

根據式(10)反解歐拉角：

(11)

3.2.4 單片機解四元數

因為四元數直接相關的變量是角度，利用四元數的三角表示式建立四元數微分方程，并且求解該微分方程，即可成功解出四元數。

對式(9)進行時間上的微分，可得微分方程：

(12)

由于單片機只能進行離散計算，所以采用一階龍格庫塔法解式(12)的微分方程。

根據一階龍格庫塔法,可以得到求解的迭代式：

Q(t+Δt)=Q(t)+Δt×φ(t)×Q(t)

(13)

整理可得最終的計算結論：

(14)

三軸角度由三軸加速度計所測得的加速度正交和三軸陀螺儀測得的角速度積分所得。

3.2.5 互補濾波

三軸陀螺儀測得角速度積分后會產生積分漂移。所以，隨著時間的累計，誤差會越來越大。而三軸加速度計測得加速度正交后，會產生震動誤差。所以，當機體劇烈晃動時，加速度計的數據也會不準。因此，需要對所得的數據作進一步處理。

(15)

(16)

(17)

式中:ε為控制器輸出補償的角度，(°)。

積分項用于消除靜態誤差。比列項用于控制傳感器的可信度。當KP越大時，則更加信任加速度計的數據。如果KP過大，系統在受到擾動后會產生高頻振蕩。當KP越小時，則更加信任陀螺儀的數據。如果KP過小，系統在受到擾動后數據會產生大范圍的漂移。經多次調參后，確定KP=0.8、KI=0.001。該參數下的PI控制器能得到較為平滑的數據曲線。補償前后角速度輸出對比如圖8所示。用補償后的角速度進行求解，即可得到比較準確的姿態信息。

圖8 補償前后角速度輸出對比

3.3 懸停控制

3.3.1 定高控制

由于四旋翼飛行器在定高模式下可以進行橫滾、俯仰操作，而在進行這些操作的時候超聲波測得的距離并不是四旋翼飛行器真正與地面的垂直距離，所以實際高度應為：

H=h×cosφPitch×cosφRoll

(18)

式中：H為實際的高度，m；h為測得的高度，m；cosφPitch和cosφRoll為俯仰角的余弦值和橫滾角的余弦值。

在獲得實際高度后，計算實際高度與預設高度的誤差并將其傳輸到高度PID控制器中。如果實際高度小于預設高度，則加大油門；反之亦然。

3.3.2 定點控制

懸停控制流程如圖9所示。

圖9 懸停控制流程圖

定點控制采用光流傳感器檢測地面像素點的位移，可以輸出當前狀態下的水平移動速度。通過對水平速度作積分，可以得到水平移動的距離。把該距離作為偏差傳入位置PID控制器中，即可改變Pitch或者Roll通道的值,使得飛行器能夠定點飛行。

3.4 避障控制

在四旋翼飛行器完成懸停操作之后，避障模塊上的超聲波傳感器開始采集對應的數據。為了保證四個超聲波的采集不被系統更新所打斷，采用STM32的通用定時的四個通道進行輸入捕獲，且搶占優先級要高。紅外傳感器的采集方式與超聲波雷同。當避障模塊檢測到物體靠近四旋翼飛行器時，避障模塊會通過串口傳輸對應的信號給主控模塊。主控模塊根據對應的信息改變Pitch或者Roll通道的值，以進行躲避。

3.5 姿態PID控制

為了保證四旋翼飛行器的飛行姿態平穩，需對其姿態角進行PID調節。姿態角采用串級PID控制。四旋翼飛行器串級PID控制系統如圖10所示。

圖10 四旋翼飛行器串級PID控制系統框圖

由圖10可知，內環為角速度，而外環為角度環。外環參數和內環參數有以下對應關系。對于外環，有：

(19)

式中：e0為當前角度誤差，(°)；β為期望角度，(°)；β*為當前角度，(°)；A為外環P項；B為外環I項；KP1為外環P參數；TI1為外環I參數。

對于內環,有：

(20)

式中：ω為外環PI輸出；ω*為三軸陀螺儀輸出的角速度，rad/s；X為內環P項；Y為內環I項；Z為內環D項；KP2為內環P參數；TI2為內環I參數；TD2為內環D參數。

其中，KP2、TI2、TD2、KP1需要根據四旋翼飛行器的不同參數來進行調節。調節方式如下。

①KP2。隨著KP2值由小到大，四旋翼飛行器抗干擾能力越來越強。用手用力拉動四旋翼，可以明顯感覺到它在抵抗這個拉動。繼續增加KP2值，四旋翼飛行器會出現肉眼可見的高頻率振動。此時用力拉動四旋翼飛行器，它會快速抖動幾下，然后會恢復穩定。如再增加KP2值，則四旋翼飛行器可能會出現直接翻轉墜機的情況。

②TI2。根據以上分析可知，I只是用于消除靜態誤差的。如果TI2值過大，會降低系統的穩定性，達不到理想效果。隨著TI2值的逐漸增大，如果達到一個理想值附近，那么四旋翼飛行器會像一個釘子一樣釘在某個位置不動，不會再往下掉。如果這個時候繼續增大TI2值，那么四旋翼飛行器會從穩定變為不穩定，拉扯一下就會發散。

③TD2。內環是角速度，所以把內環微分后角速度就變為角加速度。如果四旋翼飛行器原本的振蕩比較大，此時再引入微分環節就更容易引起噪聲。隨著TD2由小到大，則四旋翼飛行器的抗干擾性能沒有多大改變，只是到最初的位置會更平穩。如繼續增大TD2，四旋翼飛行器會出現肉眼可見的高頻率振動，或者聽見電機發出吱吱的聲音。

④KP1。外環是角度環。當整定完內環PID后，四旋翼飛行器已經可以在受到干擾時也穩定在某一位置。如果此時要四旋翼飛行器能回中，只能從角度環下手。外環KP1值由小到大，四旋翼飛行器回中的速度越來越快，用手拉扯然后放手，它會快速達到平衡位置。雖然KP1值越大，四旋翼飛行器響應越快，但是如果KP1過大則四旋翼飛行器會有發散的趨勢。

由于內環的作用就是消除外環的誤差，所以外環的I項和D項無需再調節。

本設計采用的軸距為330 mm的四旋翼飛行器，經上述方法調節后，得出如表1所示的各部分姿態PID參數。

4 系統調試

為了驗證本文系統實際的運行狀況，以一架330 mm軸距四旋翼飛行器為測試對象，進行相關性能的調試。

在調試過程中，主要通過上位機查看四旋翼飛行器懸停的高度是否保持不變、避障模塊傳輸給主控制器的數據是否準確，以及當人靠近四旋翼飛行器時，四旋翼飛行器能否迅速地做出避障動作。

四旋翼飛行器懸停飛行測試曲線如圖11所示。由圖11可知，四旋翼飛行器在懸停時姿態角變化很小，且誤差最大不超過0.001 2°。該結果證明四旋翼飛行器飛行平穩。

圖11 四旋翼飛行器懸停飛行測試曲線

為了驗證避障模塊能否正常運行，通過串口將避障模塊和主控制器連接起來，并在四旋翼飛行器側方放置障礙物，以觀察四旋翼飛行器橫滾角的變化。避障時橫滾角的變化如圖12所示。

圖12 避障時橫滾角的變化

由圖12可知，當四旋翼飛行器在懸停過程中，一旦有障礙物靠近，四旋翼飛行器會在短時間內立馬做出規避的動作，并在規避結束后繼續保持懸停。如果人體一直靠近，四旋翼飛行器會一直移動，并且在躲避時其高度不變、姿態平穩，且無需人工干預。此外，根據三種姿態解算的效果可知，加速度計抗干擾能力較弱，陀螺儀隨時間的增加誤差越來越大，通過互補濾波后解出的姿態曲線更平滑。

5 結論

本文基于STM32F103控制器、IMU和觀測傳感器設計了一款小型室內四旋翼飛行器，并通過互補濾波和四元數解算出四旋翼飛行器的姿態，由PID控制器控制使其能夠懸停飛行。經實測，系統能在3 s內達到指定高度并保持懸停。懸停過程中，四旋翼飛行器姿態平穩，懸停飛行漂移小，且四旋翼飛行器在懸停狀態下能對周圍的障礙物進行檢測并躲避。本文系統具有穩定性強、響應速度快、安裝簡便、造價低等優點。后續研究將針對其在室外抗干擾能力較弱、對于較小的障礙物檢測不準確等缺點繼續進行系統改進，力爭在室內外具有更廣泛的應用場景。