一種新型無人駕駛飛行器姿態(tài)控制系統(tǒng)

柳 赟，余哲明，宗 偉

(1. 華北電力大學電氣與電子工程學院，北京102206； 2. 杭州海康威視數(shù)字技術(shù)股份有限公司，浙江杭州310051)

無人駕駛飛行器(簡稱無人機)是一種靠自身程序控制或遠距離遙控控制的飛行器。從技術(shù)角度出發(fā)，可以將無人機分為無人固定翼飛行器、無人多旋翼飛行器等。相比于其他常見的載人飛行器，無人機體積更小，飛行更智能，成本更低[1]，同時可以裝載各種傳感器、機載云端、電子設(shè)備等，在執(zhí)行各種高空遙感、測繪、航拍、電力巡檢、偵察等任務(wù)方面具有明顯優(yōu)勢。

四軸飛行器是一種常見的無人多旋翼飛行器，其機架上安裝有4個電機，通過控制4個電機轉(zhuǎn)動產(chǎn)生推力，進而帶動整個飛行器飛行。由于四軸飛行器的輸出為4個電機，在改變飛行器姿態(tài)時，需要同時改變4個電機的輸出大小[2-3]，因此存在功耗大的問題，限制了無人機的續(xù)航。

本文中設(shè)計了一種新型結(jié)構(gòu)的飛行器飛行控制系統(tǒng)，利用四元數(shù)對飛行器姿態(tài)進行估計，解決歐拉方程求解速度慢和萬向節(jié)鎖死的問題；采用四階龍格-庫塔算法近似求解飛行器姿態(tài)，使四軸飛行器具有更長的續(xù)航時間、更靈活的姿態(tài)變換和更小的功耗等優(yōu)點。

1 飛行器結(jié)構(gòu)和工作原理

目前常見的多軸飛行器的旋翼均是多軸水平安裝的，在水平飛行時，只能通過調(diào)節(jié)多個旋翼的轉(zhuǎn)速差，使飛行器機體以和地面呈現(xiàn)夾角的姿態(tài)飛行，從而獲得水平方向的動力，進而實現(xiàn)飛行器的前向、后向、左向、右向或者自轉(zhuǎn)等姿態(tài)。在飛行器飛行過程中，如果姿態(tài)過大，容易導(dǎo)致飛行控制系統(tǒng)中的三軸陀螺儀過載，使飛行器出現(xiàn)失控、墜毀等現(xiàn)象，因此需要調(diào)整旋翼布置方式，設(shè)計一種新的飛行器。

本文中設(shè)計的飛行器的動力來源為頂端機架上的上、下2個電機。新型飛行器結(jié)構(gòu)如圖1所示，其中電機M1、M2均為順時針旋轉(zhuǎn)，且電機M1配正向螺旋槳，電機M2配反向螺旋槳，這種配置保證了2個電機在旋轉(zhuǎn)過程中對機體的作用力均為升力。同時，旋轉(zhuǎn)方向不同的2個電機使得機體在飛行時可以通過調(diào)整電機的差速來防止自身發(fā)生自旋。通過改變電機M1、M2的轉(zhuǎn)速以及舵機M3、M4的動作，飛行器可以非常靈活的實現(xiàn)懸停、俯仰、橫滾等基本飛行動作。

圖1 新型飛行器結(jié)構(gòu)

以圖1為例，當舵機M3、M4保持油門中位，電機M1、M2轉(zhuǎn)速保持同步增大或減小時，飛行器可以實現(xiàn)在垂直方向上升或下降。飛行器航向角的變化由電機M1、M2的轉(zhuǎn)速差控制，即正轉(zhuǎn)速差時，飛行器順時針旋轉(zhuǎn)；逆轉(zhuǎn)速差時，飛行器逆時針旋轉(zhuǎn)。飛行器俯仰角可以通過改變舵機M3輸出來實現(xiàn)。同理，飛行器的橫滾可以通過改變舵機M4的輸出來實現(xiàn)。飛行器姿態(tài)與電機轉(zhuǎn)速、舵機輸出的關(guān)系如表1所示。

表1 飛行器姿態(tài)與電機轉(zhuǎn)速、舵機輸出的關(guān)系

2 飛行器姿態(tài)

要實現(xiàn)飛行器程序控制，最重要的是飛行器姿態(tài)的實時計算。本文中利用飛行控制板上的三軸陀螺儀、三軸加速度計傳感器實時讀取飛行器的橫滾角、俯仰角、偏航角的數(shù)據(jù)，通過姿態(tài)融合來求解飛行器的姿態(tài)[4]。

2.1 歐拉角微分方程求解飛行器姿態(tài)

飛行器機體與坐標系x、y、z軸的旋轉(zhuǎn)角度稱為歐拉角。由于需要求解飛行器機體實時的歐拉角ψ、φ、γ，因此需要建立歐拉角速度方程組[5]。地球坐標系與機體坐標系的關(guān)系如圖2所示。其中，ψ為進動角，φ為章動角，γ為自轉(zhuǎn)角，x0y0z0、x1y1z1、x′y′z′分別為地球坐標系、機體坐標系、機體當前轉(zhuǎn)動角度。

x0y0z0—地球坐標系； x1y1z1—機體坐標系；x′y′z′—機體當前轉(zhuǎn)動角度； ψ—進動角； φ—章動角； γ—自轉(zhuǎn)角； ω—機體坐標系相對于地球坐標x0y0z0的旋轉(zhuǎn)角速度，在坐標系x1y1z1中坐標軸上的分量分別為ωx1、ωy1、ωz1。圖2 地球坐標系與機體坐標系的關(guān)系

設(shè)ω為機體坐標系相對于地球坐標x0y0z0的旋轉(zhuǎn)角速度，經(jīng)過向量分解，由圖2可以看出，ω在坐標系x1y1z1中坐標軸上的分量分別為ωx1、ωy1、ωz1，因此

(1)

從而可得如下的歐拉角速度方程組：

(2)

式(2)是一個非線性變系數(shù)微分方程組，ψ、φ、γ分別為歐拉方程中的3個角。當知道某時刻的角速度ωx1、ωy1、ωz1，并在給定的起始條件下，通過此方程組就可以求解出飛行器的姿態(tài)角ψ、φ、γ。

歐拉角求解飛行器姿態(tài)雖然很直觀，但是存在如下2個問題： 1)利用數(shù)值積分法來求解非線性變系數(shù)微分方程的計算量大，在單片機上解算速度慢，難以滿足實時控制要求。2)在機體姿態(tài)變化過大時，方程是奇異的，會出現(xiàn)萬向節(jié)鎖死情況。

綜上，用歐拉法來計算飛行器的姿態(tài)存在缺陷，因此本文中采用四元數(shù)法來計算飛行器的姿態(tài)。

2.2 四元數(shù)法求解飛行器姿態(tài)

四元數(shù)法能夠描述2個坐標系或2個矢量之間的旋轉(zhuǎn)變化，可以解決飛行器機體姿態(tài)變化大時方程退化、計算量大等問題，已逐漸成為描述剛體角運動的主流方法。

四元數(shù)是由3個單位正交矢量i、j、k和一個實數(shù)單位組成的超復(fù)數(shù)[6]，通?？梢詫憺?/p>

λ=λ0+λ1i+λ2j+λ3k

，

(3)

式中λ0、λ1、λ2、λ3均為實數(shù)。

與歐拉法同理，為了表征飛行器三軸角度與四元數(shù)導(dǎo)數(shù)之間的函數(shù)關(guān)系，以下用四元數(shù)方程表示。

規(guī)范四元數(shù)q=(q0q1q2q3)T，則四元數(shù)方程組的矩陣形式為[6]

(4)

龍格-庫塔法是在實際設(shè)計中經(jīng)常用來求解四元數(shù)方程組的。

引入符號

(5)

(6)

(7)

則式(4)可以寫為

(8)

可以看出，式(8)為一階四維微分方程，因此滿足正交關(guān)系式

(9)

(10)

其中

(11)

式中：tk為時間節(jié)點；Δtk為初始設(shè)定值。

用龍格-庫塔法不僅減小了計算量，而且方程是非奇異的，滿足了單片機飛行器姿態(tài)計算的實時性要求[7]。

3 系統(tǒng)整體架構(gòu)

根據(jù)1節(jié)中對新型飛行器的結(jié)構(gòu)和工作原理的分析，結(jié)合市場上現(xiàn)有飛行控制的結(jié)構(gòu)，本文中設(shè)計的新型飛行器控制系統(tǒng)整體結(jié)構(gòu)如圖3所示?？刂葡到y(tǒng)整體采用雙回路閉環(huán)系統(tǒng)，內(nèi)回路基于龍格-庫塔法進行姿態(tài)控制，外回路進行軌跡控制。

根據(jù)控制系統(tǒng)結(jié)構(gòu)，飛行器姿態(tài)控制系統(tǒng)結(jié)構(gòu)如圖4所示。

圖3 新型飛行器控制系統(tǒng)整體結(jié)構(gòu)

GPS—全球定位系統(tǒng)； MOSFET—金屬-氧化物半導(dǎo)體場效應(yīng)晶體管; RC—飛行姿態(tài)控制器。圖4 飛行器姿態(tài)控制系統(tǒng)結(jié)構(gòu)

飛行器的姿態(tài)控制系統(tǒng)主要由STM32F407VGT6單片機、MCU6050芯片(集成陀螺儀、加速度計)、無刷直流電機、高壓舵機、電源管理模塊以及通信模塊組成。

1)STM32F407VGT6單片機。飛行器的姿態(tài)計算需要很大的計算量，其中STM32F407VGT6單片機主頻最高可以達到168 MHz，與常見的STM32F103系列單片機相比，F(xiàn)407擁有更強的計算能力，保證了各個傳感器計算達到實時，使飛行器的控制更加穩(wěn)定。單片機的主要任務(wù)是通過集成電路(I2C)總線讀取MCU6050芯片的三軸的加速度計和三軸的陀螺儀的數(shù)據(jù)，用上述提到的四元數(shù)算法將這些數(shù)據(jù)解算成飛行器實時的飛行姿態(tài)，并根據(jù)來自遙控的控制信號和飛行器的控制原理來對飛行器的上、下2個電機和左、右2個舵機進行實時控制。

2)MPU6050芯片。MPU6050芯片是一個六軸傳感器，將加速度計芯片和陀螺儀芯片集成到一起[8-9]，使主控芯片能在一次讀取周期內(nèi)得到6個軸所需的所有數(shù)據(jù)，不僅極大地保證了加速度數(shù)據(jù)與陀螺儀數(shù)據(jù)的同時性，也節(jié)省了數(shù)據(jù)讀取時間，使控制系統(tǒng)能以更短周期刷新控制信號。三軸陀螺儀能夠測量飛行器飛行時橫滾、偏航、俯仰3個方向的角速度變化量，對這3個方向的角速度進行積分，可以得到實際的角度變化。

陀螺儀對振動不敏感，輸出數(shù)據(jù)具有一定的平滑性；但是，經(jīng)過長期使用時數(shù)據(jù)產(chǎn)生漂移，因此單純使用陀螺儀時，數(shù)據(jù)會產(chǎn)生累積偏差[10]，需要用加速度計的數(shù)據(jù)來進行修正。加速度計讀取的是三軸角加速度的變化量，雖然加速度對振動很敏感，但是其長期數(shù)據(jù)是可信的，將兩者的數(shù)據(jù)進行互補，就得到實際的俯仰、橫滾、偏航3個方向的角度變化。MPU6050芯片模塊原理如圖5所示。

3)電源管理模塊。該模塊用于為整個動力設(shè)備提供動力以及實時測量剩余電量，保證飛行器飛行安全。TP4059為充電芯片，通過通用串行總線(USB)接口給鋰電池充電，XC6204芯片為穩(wěn)壓芯片，可以輸出穩(wěn)定的電壓，保證整個系統(tǒng)的安全。鋰電池組經(jīng)過電源模塊降壓后可以分別給微控制單元(MCU)、接收機、電機、高壓舵機供電。此外，該模塊包含的電量管理模塊在檢測到電池電量不足時，會自動給MCU發(fā)送信號，蜂鳴器會立刻報警，同時以安全的速度降落，最大限度地保證飛行器的安全[11]。電源管理模塊電路如圖6所示。

圖5 MPU6050芯片模塊原理

圖6 電源管理模塊電路

4 系統(tǒng)軟件設(shè)計

作為飛行器飛行控制，必須有以下嚴謹?shù)囊螅?)不采用軟件延時，保證足夠高的系統(tǒng)效率； 2)不隨意添加其他程序，保證可靠的執(zhí)行順序結(jié)構(gòu)； 3)重要任務(wù)需統(tǒng)計每2次相鄰調(diào)用任務(wù)時間間隔。滿足上述要求的最好方法就是僅開啟一個優(yōu)先級較高的定時器，通過時間片輪轉(zhuǎn)調(diào)度方法來調(diào)度不同任務(wù)，如圖7所示。

在設(shè)計時，根據(jù)任務(wù)的重要程度，不同任務(wù)被設(shè)計在不同執(zhí)行周期的任務(wù)列中，共設(shè)計1、2、5、10、20、40、50、100 ms共8個周期任務(wù)。每個任務(wù)的執(zhí)行時間均少于1 ms，以保證每次調(diào)用都能執(zhí)行完成。高級定時器定時周期為1 ms，每1 ms標記相應(yīng)標志位，當達到某個任務(wù)的執(zhí)行周期時，單片機自動執(zhí)行該周期內(nèi)預(yù)先設(shè)定的所有任務(wù)。該設(shè)計合理分配單片機的資源，既保證飛行控制系統(tǒng)所有的任務(wù)都能得到有效執(zhí)行，又使得飛行器具有較高的實時響應(yīng)性，控制效果更好[12]。

圖7 時間片輪轉(zhuǎn)調(diào)度示意圖

根據(jù)不同任務(wù)優(yōu)先級設(shè)置不同的任務(wù)周期。其中，傳感器數(shù)據(jù)讀取、龍格-庫塔姿態(tài)估算算法以及比例積分微分(PID)算法計算輸出設(shè)為最高優(yōu)先級，任務(wù)周期為1 ms，遙控接收設(shè)為第二階梯，任務(wù)周期為5 ms，電池電壓檢測及警告設(shè)為第三階梯，任務(wù)周期為20 ms。

5 實驗與結(jié)果

利用基于四階龍格-庫塔法對飛行器姿態(tài)進行實時計算，電機由PID算法控制。在對PID參數(shù)默認設(shè)定之后，在室內(nèi)對本文中設(shè)計的飛行器進行飛行測試。

飛行器初次飛行的測試條件如下： 1)鋰電池組11.1 V、5 200 mA·h； 2)PID控制參數(shù)分別為：比例系數(shù)P=1.5，積分系數(shù)I=1，微分系數(shù)D=0.5； 3)測試環(huán)境為室內(nèi)，無風。

飛行器在遙控和程序的操控下，順利完成懸停、俯仰、橫滾等基本飛行動作。測試結(jié)果表明，本文中設(shè)計的新型飛行器和其飛行控制系統(tǒng)能在人為遙控下完成一系列基本的飛行動作，在對控制器PID參數(shù)進行微調(diào)之后，可以實現(xiàn)更加平穩(wěn)的飛行。同時，利用串口助手將飛行器俯仰角響應(yīng)進行回傳，在不同設(shè)定值時測試系統(tǒng)性能，響應(yīng)曲線如圖8所示。

圖8 飛行器俯仰角響應(yīng)曲線

由圖可以看出，當系統(tǒng)設(shè)定值改變后，PID輸出迅速改變，且超調(diào)量非常小。此時，飛行器實時狀態(tài)值以較順滑的狀態(tài)趨向于設(shè)定值，當飛行器實時狀態(tài)和設(shè)定值基本吻合時，PID輸出不再改變，系統(tǒng)沒有振蕩。再次修改系統(tǒng)設(shè)定值，將系統(tǒng)設(shè)定值調(diào)小時，飛行器實時狀態(tài)值在較短時間內(nèi)改變到設(shè)定值，PID輸出穩(wěn)定，飛行器實時狀態(tài)偏差小，整套系統(tǒng)控制非常平穩(wěn)。飛行器控制系統(tǒng)包括飛行器模型、姿態(tài)控制、軌跡控制3個部分，在MATLAB平臺中搭建飛行器控制系統(tǒng)仿真模型，如圖9所示。

姿態(tài)算法采用四階龍格-庫塔法，設(shè)置姿態(tài)角的期望分別為ψ=0°、φ=7°、γ=0° ，對控制參數(shù)進行調(diào)整，仿真結(jié)果如圖10所示。由圖可以看出，無人機系統(tǒng)可以快速且無超調(diào)響應(yīng)期望，滿足性能要求。

6 結(jié)語

本文中針對現(xiàn)有飛行器姿態(tài)估計算法存在當飛行器姿態(tài)變化過大時，會導(dǎo)致萬向節(jié)鎖死的缺點，基于四階龍格-庫塔法的飛行器姿態(tài)估算算法，提出一種飛行器姿態(tài)控制系統(tǒng)，利用STM32F407VGT6單片機作為飛行器主控，利用時間切片輪法對不同要求的子任務(wù)進行運算，最終的飛行測試以及MATLAB軟件仿真結(jié)果表明，此控制系統(tǒng)可以有效控制飛行器的飛行姿態(tài)。

圖9 飛行器控制系統(tǒng)仿真模型

(a)進動角ψ

(b)章動角φ

(c)自轉(zhuǎn)角γ圖10 飛行器姿態(tài)控制仿真圖

在一些有干擾環(huán)境中，飛行器存在穩(wěn)定姿態(tài)控制和姿態(tài)零漂等問題，這將是后續(xù)需要進行重點研究的方向。