四旋翼飛行器

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

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

四旋翼飛行器

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

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

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

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

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

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

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

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

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

1.1 四旋翼飛行器簡介

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

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

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

（1）垂直運動

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

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

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

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

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

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

（3）偏航運動

設(shè)四旋翼M1，M3電機順時針旋轉(zhuǎn)，M2，M4電機逆時針旋轉(zhuǎn)，則在螺旋槳旋轉(zhuǎn)過程中，M1，M3電機的螺旋槳對電機軸產(chǎn)生了逆時針方向的反扭力，M2，M4電機對電機軸產(chǎn)生了順時針反方向的反扭力，由于電機固連在飛行器上，故反扭力直接作用在了四旋翼機身上，當(dāng)M1，M3電機反扭力和M2，M4電機反扭力平衡時，四旋翼航向不發(fā)生變化，當(dāng)M1，M3電機反扭力大于M2，M4電機時，飛行器逆時針旋轉(zhuǎn)偏航，反之順時針旋轉(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 機體坐標(biāo)系b

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

1.2.3 旋轉(zhuǎn)矩陣

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

歐拉角定義如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.3.1 線運動方程

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

重力為：G=mg

式中：

g——重力加速度；

ρ——空氣密度；

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

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

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

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

1.3.2 角運動方程

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

解得：

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

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

1.3.3 模型簡化

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

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

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

角運動方程為：

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

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

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

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

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

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

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

2.2 梯度下降濾波器

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

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

2.2.1 建立誤差函數(shù)

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

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

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

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

2.2.2 梯度下降法迭代

梯度下降法迭代公式為：

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

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

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

3.1 姿態(tài)控制

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

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

圖 6 PID控制器內(nèi)對積分項限幅框圖

3.2 高度控制

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

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

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

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

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

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

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

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

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

飛行效果如下圖11所示。

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

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

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

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

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

5 結(jié)論

本設(shè)計對四旋翼飛行器姿態(tài)控制，高度控制和飛行原理進(jìn)行了分析，并對四旋翼飛行器進(jìn)行動力學(xué)建模，同時采用梯度下降法姿態(tài)濾波器進(jìn)行姿態(tài)解算，并根據(jù)四旋翼飛行器簡化模型設(shè)計了串級PID控制器。并通過制作四旋翼飛行機身結(jié)構(gòu)和硬件電路，設(shè)計方向控制PCB板和四旋翼飛行器地面站，在四旋翼實體上進(jìn)行了控制策略和算法的驗證，從而證明了梯度下降法姿態(tài)濾波器和串級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é)碩士研究生，研究方向智能機器人。

2015-10-26