王智敏，王歡歡，崔敬魁，李東達

（西安理工大學自動化與信息工程學院，陜西 西安 710048）

四旋翼飛行器

王智敏，王歡歡，崔敬魁，李東達

（西安理工大學自動化與信息工程學院，陜西 西安 710048）

四旋翼飛行器屬于多旋翼直升機，能垂直起降，無需滑翔跑道，具有良好的機動性，在許多領域都有重要的應用價值。因此，我們設計并搭建四旋翼飛行器軟硬件平臺，主要包括了嵌入式四旋翼飛行器飛行控制板，四旋翼飛行器地面站和上位機三部分。通過分析飛行動力系統(tǒng)數學模型，慣性導航技術和進行四元數梯度下降濾波器的姿態(tài)解算，設計四旋翼飛行器串級PID姿態(tài)控制器，并對高度進行頻域矯正控制，主要實現了四旋翼飛行器的手控模式飛行和一鍵起飛與降落的自主定高模式飛行；同時具有低電壓報警、并通過上位機實時監(jiān)測飛行器狀態(tài)等功能。

四旋翼飛行器；自主起降；姿態(tài)解算；姿態(tài)控制

編者按：2015年9月20日，首屆“薩馳杯”智能科技創(chuàng)新大賽決賽在蘇州大學敬賢堂成功舉辦。大賽由薩馳華辰機械（蘇州）有限公司主辦，中國石油和化工勘察設計協(xié)會橡膠塑料設計專業(yè)委員會、中國橡膠工業(yè)協(xié)會機械模具分會、《橡塑技術與裝備》雜志社、上海交通大學機械與動力工程學院、哈爾濱工業(yè)大學機電工程學院、東南大學機電工程學院、蘇州大學機電工程學院、青島科技大學、西門子工廠自動化工程有限公司及羅克韋爾自動化（中國）有限公司共同協(xié)辦。

為共同促進我國科技教育多元化，實現中國制造2025的中長期目標提供有力的支持，促進企業(yè)文化活躍橡塑行業(yè)科技文化事業(yè)發(fā)展。本刊將分期連續(xù)選登參賽作品，以供業(yè)內人士全方位、寬領域了解科技發(fā)展動態(tài)。

四旋翼飛行器屬多輸入多輸出的非線性、欠驅動模型，其輸入為四個電機的轉速，輸出為六自由度飛行器運動狀態(tài)（三個自由度姿態(tài)，三個自由度位置），故各控制量直接必然存在耦合，給控制帶了一定的困難，對于四旋翼飛行器常用的較為穩(wěn)定的控制算法有滑?？刂啤⒎床椒?、PID控制等。

小型無人機在軍事、醫(yī)療、電力，農業(yè)、航拍等領域都有巨大的應用價值，對四旋翼飛行器的深入研究可推動小型無人機的推廣。

1 飛行器動力學模型

1.1 四旋翼飛行器簡介

1.1.1 四旋翼飛行器結構

四旋翼飛行器具有四個呈對稱分布的螺旋槳，其中1，3電機和2，4電機旋轉方向相反，如圖1，螺旋槳下方電機固定在四個機架軸上，四個機架軸在同一水平面上，各相鄰軸間夾角為90°，軸的中心連接在一起。

1.1.2 四旋翼飛行器運動狀態(tài)

（1）垂直運動

當四旋翼螺旋槳產生的拉力在大于或小于飛行器重力時，飛行器做升降運動，若拉力正好等于飛行器重力時，飛行器可實現懸停，如圖2所示。

圖1 四旋翼飛行器螺旋漿電機驅動示意圖

圖2 飛行器懸停時各螺旋漿受力示意圖

（2）俯仰運動和翻滾運動

如圖，當四旋翼M1，M2電機和M3，M4電機拉力保持平衡時，四旋翼在俯仰方向保持平衡，若M1，M2電機拉力大于M3，M4電機時，四旋翼做機頭向上傾斜動作，反之向下。同理，四旋翼M1，M4電機拉力大于M2，M3電機時，四旋翼向左傾斜，反之向右，如圖3所示。

圖3 飛行器俯仰、翻滾各螺旋漿受力示意圖

（3）偏航運動

設四旋翼M1，M3電機順時針旋轉，M2，M4電機逆時針旋轉，則在螺旋槳旋轉過程中，M1，M3電機的螺旋槳對電機軸產生了逆時針方向的反扭力，M2，M4電機對電機軸產生了順時針反方向的反扭力，由于電機固連在飛行器上，故反扭力直接作用在了四旋翼機身上，當M1，M3電機反扭力和M2，M4電機反扭力平衡時，四旋翼航向不發(fā)生變化，當M1，M3電機反扭力大于M2，M4電機時，飛行器逆時針旋轉偏航，反之順時針旋轉，如圖4所示。

圖4 飛行器旋轉偏航各螺旋槳受力示意圖

1.2 坐標變換矩陣

1.2.1 導航坐標系n

導航坐標系是指地球地理坐標系，通常我們以東北天坐標系作為地理地坐標系（g）方向，規(guī)定xg，yg，zg依次指向東、北、天方向。

1.2.2 機體坐標系b

機體坐標系是指與飛行器固連的坐標系，原點為飛行器重心位置，xb軸對應于飛行器右側方向 ，yb軸對應于飛行器前方，z軸垂直于飛行器平面向上，與xb，yb軸形成右手系。

1.2.3 旋轉矩陣

在飛行動力學中，通常以歐拉角來描述機體坐標系與導航坐標系之間的關系，如圖5所示。

歐拉角定義如下：

圖5 歐拉角描述機體坐標系與導航坐標系之間的關系

航向角Ψ——機體坐標系xb軸在水平面上投影與地面坐標系xg軸（在水平面上，指向目標為正）之間的夾角。

俯仰角θ——機體坐標系X軸與水平面的夾角。當X軸的正半軸位于過坐標原點的水平面之上（抬頭）時，俯仰角為正，否則為負。

滾轉角Φ——機體坐標系zb軸與通過機體xb軸的鉛垂面間的夾角，機體向右滾為正，反之為負。

從機體坐標系到導航坐標系的姿態(tài)矩陣為：

1.3 四旋翼飛行器動力學模型

為了建立數學模型更加方便，做出如下假設：

（a）飛行器是剛體，飛行過程中其質量保持不變；

（b）將導航坐標系視為慣性坐標系，即忽略地球的自傳，公轉及曲率影響；

（c）導航坐標系原點與機體坐標系原點和飛行器質心重合；

（d）四旋翼飛行器受到的螺旋槳拉力和螺旋槳轉速平方成正比；

（e）四旋翼飛行器在飛行過程中受到的重力和阻力保持不變，忽略空氣摩擦等其他因素影響。

根據牛頓第二定律和動量矩定理有：

設Fx，Fy，Fz； p，q，r分別為、在機體坐標系下三個坐標軸x，y，z，上的分量。

1.3.1 線運動方程

機體受到的外力有重力、旋翼拉力和阻力。

重力為：G=mg

式中：

g——重力加速度；

ρ——空氣密度；

Cd——旋翼阻力系數；

Ct——旋翼升力系數；

Ωi——第i個旋翼的角速度。

式（2）在機體坐標系下，有：

1.3.2 角運動方程

歐拉角的角速度與機體角速度有如下關系：

解得：

由前述假設，四旋翼重心與機體坐標系原點重合，故飛行器慣性矩陣I為對角陣：

由歐拉運動方程，可得飛行器坐標系下三個軸分量Mx、My、Mz的角運動方程為

1.3.3 模型簡化

為了便于分析，在此定義四個獨立通道的輸入量U1、U2、U3、U4，則有如下公式：

其中U1表示油門通道輸入，U2表示滾轉通道輸入，U3表示俯仰通道輸入，U4表示航向通道輸入。

忽略空氣阻力，將式（10）帶入式（5）得到線運動方程為：

角運動方程為：

其中I為旋翼中心到機體坐標系原點的距離。

2 梯度下降法姿態(tài)濾波器

2.1 飛行器姿態(tài)的四元數表示

四元數是有四個元組成的數

中，q0，q1，q2，q3是實數，i，j，k既是互相正交的單位向量，又是虛單位

描述剛體旋轉的四元數是規(guī)范化四元數，有

歐拉角旋轉矩陣和四元數旋轉矩陣對應關系得：

2.2 梯度下降濾波器

常用的濾波方法有互補濾波，卡爾曼濾波，擴展卡爾曼濾波等。本設計采用梯度下降法和互補濾波結合的姿態(tài)濾波器法。

四元數微分方程有

2.2.1 建立誤差函數

首先根據機載慣性測量單元（三軸加速度計和三軸磁力計）的測量值與導航坐標系固連參考做差作為誤差估計函數，認為在導航坐標系下加速度僅為重力加速度，通過導航坐標系到機體坐標系的變換:

對于磁力計，因為地磁場可分解到水平面和重力方向，與定義的東北天坐標系對應，認為水平分量正好與x軸方向對準，導航坐標系下磁力計只在x，z方向上存在分量。

由于加速度計是基于導航坐標系重力方向校準的，無法判斷航向，而磁力計可以進行航向校準，故需要兩者結合作姿態(tài)校準，有：

然后計算梯度，如果僅使用加速度計，則選擇下式中的第一個梯度，選擇同時加速計和磁力計時選擇第二個作為梯度。

2.2.2 梯度下降法迭代

梯度下降法迭代公式為：

μt為梯度下降法步長， Δt為陀螺儀采樣周期，為陀螺儀測量的姿態(tài)改變速率，而α是μ對加速度計和磁力計的放大比例系數。

3 飛行控制與軟件設計

上文引入的四個控制量，油門控制量U1，滾轉控制量U2，俯仰控制量U3，偏航控制量U4，而其對應輸出分別為螺旋槳基礎轉速，滾轉角Φ，俯仰角θ，偏航角Ψ。這樣引入控制量可以實現對欠驅動系統(tǒng)的解耦，實際上，此處只控制了四旋翼發(fā)姿態(tài)角，然而四旋翼飛行器的線運動受角運動的影響，如需進行線運動控制做位置閉環(huán)，可采用線運動控制作為外環(huán)，角運動控制作為內環(huán)來控制，因為角運動較線運動而已具有頻帶快，運動速度快的特點。

3.1 姿態(tài)控制

姿態(tài)控制采用串級PID控制，內環(huán)為角速度環(huán)，外環(huán)為角度環(huán)，從而很大程度的削弱了模型內部的非線性，取得了良好的控制效果。

為避免在特定條件下微小誤差長時間累積導致積分項過大，導致系統(tǒng)由積分項主導的不穩(wěn)定現象，本設計在PID控制器內對積分項進行了限幅，從而有效的避免了積分項過大導致系統(tǒng)不穩(wěn)定的情況出現，如圖6所示。

圖 6 PID控制器內對積分項限幅框圖

3.2 高度控制

高度控制采用普通頻域矯正PID控制，如圖7所示。

圖7 普通頻域矯正PID控制框圖

在高度控制方面，為防止輸入突變后由于微分作用引起輸出的巨大變化，本設計采用微分分離的方法，有效的避免了輸入突變后由于微分引起的系統(tǒng)震蕩。同時在也在積分上做了積分限幅。

4 測試與結果分析

4.1 姿態(tài)解算效果測試

通過上位機實時觀察姿態(tài)角和實際飛行器姿態(tài)角對比，可看出姿態(tài)解算精度和快速性均可滿足控制要求，如圖8所示。

同時通過上位機可進行PID在線調參和傳感器校準，如圖9所示。

4.2 飛行姿態(tài)控制效果測試

飛行過程中姿態(tài)角波形如圖10所示，其縱坐標單位為角度，其中灰色線為橫滾角、藍色線為俯仰角，圖示角度均是四旋翼飛行器在飛行過程中實測得到。由此可見飛行過程中姿態(tài)波動小于0.5°。

飛行效果如下圖11所示。

4.3 高度閉環(huán)測試

飛行過程中高度變化如圖12所示，其縱坐標單位為米，可見高度控制效果良好。

圖8 由上位機實時觀察對比

圖9 PID在線調參和傳感器校準

圖10 飛行過程中姿態(tài)角波形圖

5 結論

本設計對四旋翼飛行器姿態(tài)控制，高度控制和飛行原理進行了分析，并對四旋翼飛行器進行動力學建模，同時采用梯度下降法姿態(tài)濾波器進行姿態(tài)解算，并根據四旋翼飛行器簡化模型設計了串級PID控制器。并通過制作四旋翼飛行機身結構和硬件電路，設計方向控制PCB板和四旋翼飛行器地面站，在四旋翼實體上進行了控制策略和算法的驗證，從而證明了梯度下降法姿態(tài)濾波器和串級PID姿態(tài)控制器的可靠性。

圖11 飛行照片圖

圖12 飛行過程中高度變化波形圖

［1]秦永元. 慣性導航［M］. 北京：科學出版社，2014：244～259.

［2]Sebastian O.H. Madgwick. Estimation of IMU and MARG orientation using a gradient descent algorithm［C］. IEEE International Conference on Rehabilitation Robotics，Switzerland，2011:1～5.

［3]STMicroelectronics.Using LSM303DLH for a tilt compensated electronic compass［Z］.2010:20～24.

［4]Memsense. Bias and Sensitivity Corrections of Magnetometers Revision 1［Z］.1～4.

（R-03）

Four-rotor aircraft

Four-rotor aircraft

Wang Zhimin, Wang Huanhuan, Cui Jingkui, Li Dongda
(School of Automation and Information Engineering, Xi'an University of Technology, Xi'an 710048, Shaanxi)

The four-rotor aircraft is a multi-rotor helicopter that can take off and land vertically without arunway glide. It has good fl exibility and important applications in many areas. Therefore, we designed and built hardware and software platforms for the four-rotor aircraft, including embedded four-rotor aircraft fl ight control panels, ground station and host computer. By analyzing the flight dynamic mathematical model, inertial navigation technology and attitude algorithm of quaternion gradient descent fi lter, we designed a cascade PID attitude controller for the four-rotor aircraft, and run frequency domain correction control for the height. They mainly achieved manual mode fl ight and autonomous height mode fl ight with one-key take-off and landing; the platform also had functions such as low voltage alarm and real-time monitoring of aircraft status through the host computer.

four-rotor aircraft; autonomous takeoff and landing; attitude algorithm; attitude control

V212.4

1009-797X（2016）02-0100-07

B

10.13520/j.cnki.rpte.2016.02.039

王智敏（1989-），女，就讀于西安理工大學碩士研究生，研究方向智能機器人。

2015-10-26