王智敏，王歡歡，崔敬魁，李東達(dá)

（西安理工大學(xué)自動(dòng)化與信息工程學(xué)院，陜西 西安 710048）

四旋翼飛行器

王智敏，王歡歡，崔敬魁，李東達(dá)

（西安理工大學(xué)自動(dòng)化與信息工程學(xué)院，陜西 西安 710048）

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

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

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

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

四旋翼飛行器屬多輸入多輸出的非線性、欠驅(qū)動(dòng)模型，其輸入為四個(gè)電機(jī)的轉(zhuǎn)速，輸出為六自由度飛行器運(yùn)動(dòng)狀態(tài)（三個(gè)自由度姿態(tài)，三個(gè)自由度位置），故各控制量直接必然存在耦合，給控制帶了一定的困難，對(duì)于四旋翼飛行器常用的較為穩(wěn)定的控制算法有滑模控制、反步法、PID控制等。

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

1 飛行器動(dòng)力學(xué)模型

1.1 四旋翼飛行器簡(jiǎn)介

1.1.1 四旋翼飛行器結(jié)構(gòu)

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

1.1.2 四旋翼飛行器運(yùn)動(dòng)狀態(tài)

（1）垂直運(yùn)動(dòng)

當(dāng)四旋翼螺旋槳產(chǎn)生的拉力在大于或小于飛行器重力時(shí)，飛行器做升降運(yùn)動(dòng)，若拉力正好等于飛行器重力時(shí)，飛行器可實(shí)現(xiàn)懸停，如圖2所示。

圖1 四旋翼飛行器螺旋漿電機(jī)驅(qū)動(dòng)示意圖

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

（2）俯仰運(yùn)動(dòng)和翻滾運(yùn)動(dòng)

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

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

（3）偏航運(yùn)動(dòng)

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

圖4 飛行器旋轉(zhuǎn)偏航各螺旋槳受力示意圖

1.2 坐標(biāo)變換矩陣

1.2.1 導(dǎo)航坐標(biāo)系n

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

1.2.2 機(jī)體坐標(biāo)系b

機(jī)體坐標(biāo)系是指與飛行器固連的坐標(biāo)系，原點(diǎn)為飛行器重心位置，xb軸對(duì)應(yīng)于飛行器右側(cè)方向 ，yb軸對(duì)應(yīng)于飛行器前方，z軸垂直于飛行器平面向上，與xb，yb軸形成右手系。

1.2.3 旋轉(zhuǎn)矩陣

在飛行動(dòng)力學(xué)中，通常以歐拉角來描述機(jī)體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的關(guān)系，如圖5所示。

歐拉角定義如下：

圖5 歐拉角描述機(jī)體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的關(guān)系

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

俯仰角θ——機(jī)體坐標(biāo)系X軸與水平面的夾角。當(dāng)X軸的正半軸位于過坐標(biāo)原點(diǎn)的水平面之上（抬頭）時(shí)，俯仰角為正，否則為負(fù)。

滾轉(zhuǎn)角Φ——機(jī)體坐標(biāo)系z(mì)b軸與通過機(jī)體xb軸的鉛垂面間的夾角，機(jī)體向右滾為正，反之為負(fù)。

從機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的姿態(tài)矩陣為：

1.3 四旋翼飛行器動(dòng)力學(xué)模型

為了建立數(shù)學(xué)模型更加方便，做出如下假設(shè)：

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

（b）將導(dǎo)航坐標(biāo)系視為慣性坐標(biāo)系，即忽略地球的自傳，公轉(zhuǎn)及曲率影響；

（c）導(dǎo)航坐標(biāo)系原點(diǎn)與機(jī)體坐標(biāo)系原點(diǎn)和飛行器質(zhì)心重合；

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

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

根據(jù)牛頓第二定律和動(dòng)量矩定理有：

設(shè)Fx，F(xiàn)y，F(xiàn)z； p，q，r分別為、在機(jī)體坐標(biāo)系下三個(gè)坐標(biāo)軸x，y，z，上的分量。

1.3.1 線運(yùn)動(dòng)方程

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

重力為：G=mg

式中：

g——重力加速度；

ρ——空氣密度；

Cd——旋翼阻力系數(shù)；

Ct——旋翼升力系數(shù)；

Ωi——第i個(gè)旋翼的角速度。

式（2）在機(jī)體坐標(biāo)系下，有：

1.3.2 角運(yùn)動(dòng)方程

歐拉角的角速度與機(jī)體角速度有如下關(guān)系：

解得：

由前述假設(shè)，四旋翼重心與機(jī)體坐標(biāo)系原點(diǎn)重合，故飛行器慣性矩陣I為對(duì)角陣：

由歐拉運(yùn)動(dòng)方程，可得飛行器坐標(biāo)系下三個(gè)軸分量Mx、My、Mz的角運(yùn)動(dòng)方程為

1.3.3 模型簡(jiǎn)化

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

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

忽略空氣阻力，將式（10）帶入式（5）得到線運(yùn)動(dòng)方程為：

角運(yùn)動(dòng)方程為：

其中I為旋翼中心到機(jī)體坐標(biāo)系原點(diǎn)的距離。

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

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

四元數(shù)是有四個(gè)元組成的數(shù)

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

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

歐拉角旋轉(zhuǎn)矩陣和四元數(shù)旋轉(zhuǎn)矩陣對(duì)應(yīng)關(guān)系得：

2.2 梯度下降濾波器

常用的濾波方法有互補(bǔ)濾波，卡爾曼濾波，擴(kuò)展卡爾曼濾波等。本設(shè)計(jì)采用梯度下降法和互補(bǔ)濾波結(jié)合的姿態(tài)濾波器法。

四元數(shù)微分方程有

2.2.1 建立誤差函數(shù)

首先根據(jù)機(jī)載慣性測(cè)量單元（三軸加速度計(jì)和三軸磁力計(jì)）的測(cè)量值與導(dǎo)航坐標(biāo)系固連參考做差作為誤差估計(jì)函數(shù)，認(rèn)為在導(dǎo)航坐標(biāo)系下加速度僅為重力加速度，通過導(dǎo)航坐標(biāo)系到機(jī)體坐標(biāo)系的變換:

對(duì)于磁力計(jì)，因?yàn)榈卮艌?chǎng)可分解到水平面和重力方向，與定義的東北天坐標(biāo)系對(duì)應(yīng)，認(rèn)為水平分量正好與x軸方向?qū)?zhǔn)，導(dǎo)航坐標(biāo)系下磁力計(jì)只在x，z方向上存在分量。

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

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

2.2.2 梯度下降法迭代

梯度下降法迭代公式為：

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

3 飛行控制與軟件設(shè)計(jì)

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

3.1 姿態(tài)控制

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

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

圖 6 PID控制器內(nèi)對(duì)積分項(xiàng)限幅框圖

3.2 高度控制

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

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

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

4 測(cè)試與結(jié)果分析

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

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

同時(shí)通過上位機(jī)可進(jìn)行PID在線調(diào)參和傳感器校準(zhǔn)，如圖9所示。

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

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

飛行效果如下圖11所示。

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

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

圖8 由上位機(jī)實(shí)時(shí)觀察對(duì)比

圖9 PID在線調(diào)參和傳感器校準(zhǔn)

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

5 結(jié)論

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

圖11 飛行照片圖

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

［1]秦永元. 慣性導(dǎo)航［M］. 北京：科學(xué)出版社，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-），女，就讀于西安理工大學(xué)碩士研究生，研究方向智能機(jī)器人。

2015-10-26