第一篇：新手四旋翼算法總結

新手四旋翼算法總結 一．姿態結算（匿名版程序）

首先，程序中一般用了兩種求解姿態的方法，一種為歐拉角法，一種為四元數法

（1）歐拉角法靜止狀態，或者總加速度只是稍微大于g時，由加計算出的值比較準確。

使用歐拉角表示姿態，令Φ,θ和Φ代表ZYX 歐拉角，分別稱為偏航角、俯仰角和橫滾角。載體坐標系下的 加 速 度(axB,ayB,azB)和參考坐標系下的加速度(axN, ayN, azN)之間的關系可表示為(1)。其中 c 和 s 分別代表 cos 和 sin。axB,ayB,azB就是mpu讀出來的三個值。

這個矩陣就是三個旋轉矩陣相乘得到的，因為矩陣的乘法可以表示旋轉。

c?c??axB???ayB????c?s??s?s?c? ?????azB????s?s??c?s?c?c?s?c?c??s?s?s??s?c??c?s?s??s???axN??ayN?(1)s?c?????c?c?????azN???axN??0??????ayN???0?（2）?a????zN??g?飛行器處于靜止狀態，此時參考系下的加速度等于重力加速度，即

把（2）代入（1）可以解的

??arctg(axBa?a2yB2zB)（3）

??arctg??ayB??（4）?azB?即為初始俯仰角和橫滾角，通過加速度計得到載體坐標系下的加速度即可將其解出，偏航角可以通過電子羅盤求出。

（2）四元數法（通過處理單位采樣時間內的角增量(mpu的陀螺儀得到的就是角增量)，為了避免噪聲的微分放大，應該直接用角增量-------抄的書）

上匿名的程序

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){

float norm;// float hx, hy, hz, bx, bz;

float vx, vy, vz;// wx, wy, wz;

float ex, ey, ez;

// 先把這些用得到的值算好

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;// float q0q3 = q0*q3;

float q1q1 = q1*q1;// float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

if(ax*ay*az==0)

return;

norm = sqrt(ax*ax + ay*ay + az*az);

//acc數據歸一化

ax = ax /norm;

ay = ay / norm;

az = az / norm;

// estimated direction of gravity and flux(v and w)

vx = 2*(q1q3q1q1az*vy);

//向量外積在相減得到差分就是誤差

ey =(az*vxay*vx);

exInt = exInt + ex * Ki;

//對誤差進行積分

eyInt = eyInt + ey * Ki;

ezInt = ezInt + ez * Ki;

// adjusted gyroscope measurements

gx = gx + Kp*ex + exInt;

//將誤差PI后補償到陀螺儀，即補償零點漂移

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

//這里的gz由于沒有觀測者進行矯正會產生漂移，表現出來的就是積分自增或自減

// integrate quaternion rate and normalise

//四元素的微分方程

q0 = q0 +(-q1*gxq3*gz)*halfT;

q1 = q1 +(q0*gx + q2*gzq1*gz + q3*gx)*halfT;

q3 = q3 +(q0*gz + q1*gy2 * q3* q3 + 1)* 57.3;// yaw

Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch

Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1q0q2);

/

vy = 2*(q0q1 + q2q3);

vz = q0q0q2q2 + q3q3;可以看到vx，vy，vz為CRb的最后一列的三項，四元數矩陣帶入（1）式得vx，vy，vz分別是axB，ayB，azB每一項g前的系數。且靜止情況下vx，vy，vz組成向量模長基本可以認為為1.3.norm = sqrt(ax*ax + ay*ay + az*az);

//acc數據歸一化

ax = ax /norm;

ay = ay / norm;

az = az / norm;以上已說，由四元數倒推回去的加速度，向量模長為1，為了比較誤差進行歸1化，算的由加計得出的向量。4.ex =(ay*vzax*vz);

ez =(ax*vyq2*gyq3*gy)*halfT;

q2 = q2 +(q0*gyq2*gx)*halfT;對四元數進行跟新，這里用的是方程的數值解法，求得的解釋近似解，總之就是跟新了四元數 8．

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;對四元數進行規范化，即化為模長為1，因為只有規范化的四元數才能表示剛體旋轉。9．

Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch

Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1ACC_OFFSET.X;MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2])<< 8)| mpu6050_buffer[3])ACC_OFFSET.Z;MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8])<< 8)| mpu6050_buffer[9])GYRO_OFFSET.Y;MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12])<< 8)| mpu6050_buffer[13])-GYRO_OFFSET.Z;

這里還要說一點，這里加速計的數據用的是滑動平均值濾波法 void Prepare_Data(void){ static uint8_t filter_cnt=0;int32_t temp1=0,temp2=0,temp3=0;uint8_t i;

MPU6050_Read();MPU6050_Dataanl();

ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;for(i=0;i

temp1 += ACC_X_BUF[i];

temp2 += ACC_Y_BUF[i];

temp3 += ACC_Z_BUF[i];}

ACC_AVG.X = temp1 / FILTER_NUM;ACC_AVG.Y = temp2 / FILTER_NUM;ACC_AVG.Z = temp3 / FILTER_NUM;filter_cnt++;if(filter_cnt==FILTER_NUM)filter_cnt=0;

GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.0001;GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.0001;GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.0001;} 資料在附帶文件中。

第二篇：四旋翼產品說明書最終稿（范文）

2012年遼寧省普通高等學校大學生 機械設計大賽參賽作品說明書

參賽單位：遼東學院 設計課題：四軸碟形飛行器

參賽學生：吳家帥、李成祥、石 巖、祁 勇、楊惠麟 指導教師：劉 瀛、劉廣達

-***782這款四軸飛行器適合多種型號的飛控板配合使用。有利于批量生產，四軸飛行器又一亮點是成本遠遠低于功能相同的航模價格。此外，工人易操作，使用簡單，維護方便，用途多樣。1.2國內外相關研究現狀

在微型飛行器的研究方面,目前歐美等發達國家遠遠走在全球的前列,且成功研究了固定翼式、旋翼式和撲翼式各類微型飛行器。相比較之下，我國研究所和各大高校的研究人員雖然也開始展開了相關研究,但更多停留在理論設計階段，實踐的成功案例較為少見。產品說明書

2.1設計方案

現有的四軸飛行器存在著操作困難、安全保障低、價格昂貴等原因，受人們的喜愛程度較低，而且最主要原因是不能解決人員的安全問題。但是，性能好的四軸飛行器的價格比較昂貴，這種因素的限制之下，航模的市場很難滿足飛行操作者的欲望要求。本產品恰恰從這個角度出發，為航模愛好者量身打造具有價格低廉，操作簡單，維護方便，人身傷害比較小，有較為廣泛的應用的四軸碟形飛行器。

為了實現安全飛行的目的我們小組起初設想了以下幾種方案： 方案一：機架“十字形”方案

方案二：碟形外殼“十字鑲嵌”方案

方案三：模具制作外殼、將電機、槳、電路板嵌在殼體內方案 通過反復的實驗，我們小組決定采用第三種方案。

首先，與方案一相比較，方案三在外殼制作及組合安裝上更容易，可以完成批量生產。從具體實施以及可行性角度分析，方案三更加具體完整。由于螺旋槳、電池、電路板等都是內鑲的，根據空氣動力學可知，碟形外殼更加有利于飛行穩定性，大大提高安全系數。

其次，與方案二相比較，方案三殼體較輕，負載比較小，容易固定，協調性也有了很大的改善。最后，與前兩個方案相比較，方案三飛行效果及安全性更好，動作靈敏度更高，飛行動力更加持久，抗風抗干擾性能力更強，能夠有效的完成各種動作。

綜上所述，我們改進方案

一、方案

二、決定采用方案三為最終的作品方案。理論設計計算

3.1作品內部重要零部件介紹

四軸碟形飛行器中各參數說明表

標準配置 總體 螺旋槳 電子調速器

電機 遙控器 攝像頭 四軸飛行器

描述 700*700

正反2對90*50槳；尺寸在200mm至300mm之間；總體載重量在600g至1000g之間

好贏2212 10A 新西達2212 KV980 天地飛6通道2.4GHz

300萬像素

總體質量在1500g至2500g之間

表1

圖1電機

圖2螺旋槳

圖3 電池

圖5 飛行控制主板

圖7 接收設備

圖4 電調

圖6 電壓測試器

圖8 拍攝設備

動力源提供裝置：

采用直流電源3S鋰電池，該電池的容量大，有充放電的保護，電池的質量輕，其質量為191.9克。電子調速器：

由于我們需要四軸碟形飛行器在空中完成各種動作，所以需要一個調速的裝置進行調速，當操作者想要改變方向時，電子調速器會將從接收器得來的信號進行處理將直流電源轉換成交流帶信號，這樣電機就會接收到信號，自動將速度進行改變，進而改變飛行器的姿態及位置。

拍攝及接收裝置：

拍攝的攝像頭采用可以發射出模擬信號，將拍攝到的流暢彩色視頻及聲音進行無線傳輸，調制模式采用AM調制，有效傳輸距離120m，工作時間可達2小時。其質量為32克。

接收裝置采用支持帶UHF波段模擬電視接收功能的多種電子設備終端,包括手機,電視,筆記本電腦,MP4,數碼相框等電子設備，我們的作品采用的是手機。

遙控器及接收器

遙控器采用天地飛六通道2.4GHz，有效傳輸范圍在200m左右。接收器是天地飛遙控器配套的接收器，其質量為8克。

工作流程圖 飛行流程圖：

飛行前的準備→遙控器及飛行器解鎖→起飛→操作遙控器慢慢退下油門使飛行器慢慢減速降落 左右轉彎流程圖：

起飛→向左（右）旋轉→遙控器方向舵向左（右）打→飛行器實現向左（右）飛行 前后流程圖：

起飛→向前（后）飛行→遙控器方向舵向前（后）打→飛行器實現向前（后）飛行 其他的動作（翻滾、俯仰、橫滾）參考以上的流程圖即可實現。創新點及應用

1）操作和控制簡便。集合了原有四軸飛行器的優點，碟形的設計更加美觀，在平衡方面更加有優勢。可以進行危險作業的同時還具有很強的趣味性，自主研發和獨到的設計理念給航模愛好者耳目一新的感覺，與此同時在娛樂方面也增長了知識。

2）安全指數高。相對簡單的機械構造，使安全指數大幅度提高，無論是作為航空模型還是作為遙控平臺，安全永遠是第一位的。

3）成本低廉。飛行器外殼采用的是模具成型技術，為工業開發其商業用途奠定了必要的基礎。4）適用于進行危險性系數比較大的作業。在發生震后塌方和泥石流等自然災害的時候，監控室的監控人員可以實現對飛行器無線的遙控，當飛行器飛行到災后現場上空時，通過所攜帶的攝像頭將圖像資料無線傳輸給監控室,給現場決策提供幫助，減少傷害的發生。

5）適用于航測，軍事應用等。足夠的穩定性和抗風性，可以滿足很多要求懸停能力的測量及軍事應用。

目前在國內，四軸碟形飛行器的發展尚未成熟，很多不足都需要改進，因此我們獨到的設計可以引領四軸碟形飛行器進入一個創新領域。

參考文獻

[1] JOEL M.,MATTHEW T.Development of the Black Widow Micro Air Vehicle[R].AIAA-01-0127,2001.[2] WILSON J.R.MicroSAR Meets MAV[J].Aerospace American,1999,10(2):32-35.[3] PORNSIN-SISIRAK T.N.,LEE S.W.,NASSEF H.,GRASMEYER J.,etc.MEMS Wing Technology for A Battery-Powered Ornithopter.Micro Electro Mechanical Systems,2000,MEMS 2000,IEEE[C].2000,1:799-804.[4]劉曉杰.基于視覺的微小型四旋翼飛行器位姿估計研究與實現[D].長春:吉林大學,2009.[5]范承志,王字峰,林小娥等.一種位置無傳感器無刷直流電動機驅動電路[J].微電機,2001(3):19-20,24.[6]曹杰,史金飛,戴敏.基于MEGA8單片機的無傳感器無刷直流電動機控制系統設計[J].自動化儀表,2005,26(12):13-16.[7]劉煥曄.小型四旋翼飛行器飛行控制系統研究與設計[D].上海:上海交通大學,2009.

第三篇：關于四旋翼飛行器的心得

關于四旋翼飛行器的心得

對于飛行器或者航模之類的映像，是在高中時期，學校有航模小組，經常可以看到拿著航模的學生在進行試飛，當時心中感覺“航模”是非常有意思并且“高科技”。如今已經歷高考進入大學，在學校的為我們安排的導師制計劃中，非常幸運的加入無人機航拍飛行器小組，關于四旋翼飛行器，在查閱了相關資料后，有了一定的了解。

四旋翼飛行器也稱為四旋翼直升機，是一種有4個螺旋槳且螺旋槳呈十字形交叉的飛行器。Seraphi 是一款可用于空中拍攝的一體化多旋翼飛行器，它外觀時尚精美，做工精湛，還擁有集成了自身研發的飛行動力系統，并配置專業的無線電遙控系統。Seraphi集成易作、易維護的穩定設計，在出廠前已經設置并調試所有的飛行參數及功能，具有免安裝、免調試的快速飛行模式。Seraphi 攜帶方便，可以搭配GoPro或者其它微型相機錄制空中視頻。

記得在TED的講座中，有一期叫做“TED-紅遍全球的的炫酷飛行器”，這個講座說明了四旋翼飛行器的一些特點。1.時尚精美、做工精湛。Seraphi外觀時尚精美，做工精湛，還擁集成了自身研發的飛行動力系統，并配置專業的無線電遙控系統。2.集成易作、易維護的穩定設計。Seraphi集成易作、易維護的穩定設計。Seraphi 攜帶方便，可以搭配GoPro或者其它微型相機錄制空中視頻。3.自由切換多種飛行模式。Seraphi內置自身研發的飛行控制系統，具備多種飛行模式，可以根據不同的飛行需要以及不同的飛行環境進行實時的智能切換以達到不一樣的飛行體驗。4.方向控制靈活。Seraphi具備自身研發飛控系統，方向控制靈活。在通常飛行過程中，可以根據需要，進行靈活縱。

制作航拍飛行器能夠讓培養我們的團隊合作意識，拓寬我們的知識領域，同時讓我們動手實踐的能力得到提升，相信這次經歷肯定能成為我的大學生活中最值得回憶的事情之一。

第四篇：動態系統建模仿真實驗報告四旋翼仿真

動態系統建模仿真

實驗報告（2）

四旋翼飛行器仿真

2012

1實驗內容

基于Simulink建立四旋翼飛行器的懸停控制回路，實現飛行器的懸停控制；

建立UI界面，能夠輸入參數并繪制運動軌跡；

基于VR

Toolbox建立3D動畫場景，能夠模擬飛行器的運動軌跡。

2實驗目的通過在Matlab

環境中對四旋翼飛行器進行系統建模，使掌握以下內容：

四旋翼飛行器的建模和控制方法

在Matlab下快速建立虛擬可視化環境的方法。

3實驗器材

硬件：PC機。

工具軟件：操作系統：Windows系列；軟件工具：MATLAB及simulink。

4實驗原理

4.1四旋翼飛行器

四旋翼飛行器通過四個螺旋槳產生的升力實現飛行，原理與直升機類似。

四個旋翼位于一個幾何對稱的十字支架前，后，左，右四端，如圖

所示。旋翼由電機控制；整個飛行器依靠改變每個電機的轉速來實現飛行姿態控制。

圖1四旋翼飛行器旋轉方向示意圖

在圖

中，前端旋翼

和后端旋翼

逆時針旋轉，而左端旋翼

和右端的旋翼

順時針旋轉，以平衡旋翼旋轉所產生的反扭轉矩。

由此可知，懸停時，四只旋翼的轉速應該相等，以相互抵消反扭力矩；同時等量地增大或減小四只旋翼的轉速，會引起上升或下降運動；增大某一只旋翼的轉速，同時等量地減小同組另一只旋翼的轉速，則產生俯仰、橫滾運動；增大某一組旋翼的轉速，同時等量減小另一組旋翼的轉速，將產生偏航運動。

4.2建模分析

四旋翼飛行器受力分析,如圖

所示

圖2四旋翼飛行器受力分析示意圖

旋翼機體所受外力和力矩為：

重力mg,機體受到重力沿方向；

四個旋翼旋轉所產生的升力

(i=

1,2,3,4)，旋翼升力沿方向；

旋翼旋轉會產生扭轉力矩

(i=

1,2,3,4)。垂直于葉片的旋翼平面，與旋轉矢量相反。

力模型為：，旋翼通過螺旋槳產生升力。是電機轉動力系數，可取，為電機轉速。旋翼旋轉產生旋轉力矩Mi(i=1,2,3,4)，力矩Mi的旋向依據右手定則確定。力矩模型為，其中是電機轉動力系數，可取為電機轉速。當給定期望轉速后，電機的實際轉速需要經過一段時間才能達到。實際轉速與期望轉速之間的關系為一階延遲：響應延遲時間可取0.05s(即)。期望轉速則需要限制在電機的最小轉速和最大轉速之間，范圍可分取[1200rpm，7800rpm]。

飛行器受到外界力和力矩的作用，形成線運動和角運動。線運動由合外力引起，符合牛頓第二定律：

r為飛機的位置矢量。

角運動由合力矩引起。四旋翼飛行器所受力矩來源于兩個方面：1）旋翼升力作用于質心產生的力矩；2）旋翼旋轉產生的扭轉力矩。角運動方程如下式所示。其中，L

為旋翼中心建立飛行器質心的距離，I

為慣量矩陣。

4.3控制回路設計

控制回路包括內外兩層。外回路由Position

Control

模塊實現。輸入為位置誤差，輸出為期望的滾轉、俯仰和偏航角。內回路由Attitude

Control

模塊實現，輸入為期望姿態角，輸出為期望轉速。Motor

Dynamics

模塊模擬電機特性，輸入為期望轉速，輸出為力和力矩。Rigid

Body

Dynamics

是被控對象，模擬四旋翼飛行器的運動特性。

圖3包含內外兩個控制回路的控制結構

（1）內回路：姿態控制回路

對四旋翼飛行器，我們唯一可用的控制手段就是四個旋翼的轉速。因此，這里首先對轉速產生的作用進行分析。假設我們希望旋翼1的轉速達到，那么它的效果可分解成以下幾個分量：

：使飛行器保持懸停的轉速分量；

：除懸停所需之外，產生沿ZB軸的凈力；

：使飛行器負向偏轉的轉速分量；

：使飛行器正向偏航的轉速分量；

因此，可以將期望轉速寫成幾個分量的線性組合：

其它幾個旋翼也可進行類似分析，最終得到：

在懸浮狀態下，四個旋翼共同的升力應抵消重力，因此：

此時，可以把旋翼角速度分成幾個部分分別控制，通過“比例-微分”控制律建立如下公式：

綜合以上三式可得到期望姿態角-期望轉速之間的關系，即內回路。

外回路：位置控制回路

外回路采用以下控制方式：通過位置偏差計算控制信號（加速度）；建立控制信號與姿態角之間的幾何關系；得到期望姿態角，作為內回路的輸入。期望位置記為。可通過PID

控制器計算控制信號：

是目標懸停位置是我們的目標懸停位置(i=1,2,3)，是期望加速度，即控制信號。注意：懸停狀態下線速度和加速度均為0，即。

通過俯仰角和滾轉角控制飛行器在XW和YW平面上的運動，通過控制偏航角，通過控制飛行器在ZB軸上的運動。可得：

根據上式可按照以下原則進行線性化：

（1）將俯仰角、滾轉角的變化作為小擾動分量，有，，（2）偏航角不變，有，其中初始偏航角，為期望偏航角（3）在懸停的穩態附近，有

根據以上原則線性化后，可得到控制信號（期望加速度）與期望姿態角之間的關系：

則內回路的輸入為：

5實驗步驟與結果

（1）

根據控制回路的結構建立simulink模型；

（2）

為了便于對控制回路進行參數調整，利用Matlab軟件為四旋翼飛行器創建GUI參數界面；

（3）

利用Matlab的VR

Toolbox建立四旋翼飛行器的動畫場景

（4）

根據系統的結構框圖，搭建Simulink模塊以實現模擬飛行器在指定位置的懸停。使用默認數據，此時xdes=3，ydes=4，zdes=5，開始仿真，可以得到運動軌跡x、y、z的響應函數，同時可以得到在xyz坐標中的空間運動軌跡。然后點擊GUI中的VR按鈕使simulink的工作空間中載入系統仿真所需的參數，把x、y、z的運動軌跡和Roll，Pitch，Yaw輸入至VR中的模擬飛行器中，觀察飛行器的運動軌跡和運動姿態，然后再使用一組新的參數xdes=-8，ydes=3，zdes=6進行四旋翼飛行器運動進行仿真模擬，可以看出仿真結果和動畫場景相吻合。

6實驗總結與心得

此次MATLAB實驗綜合了SIMULINK、GUI和VR場景等多個部分，對四旋翼飛行器運動進行了仿真模擬。由仿真結果可以看出，四旋翼飛行器最終位置達到了期望給定的位置，三個方向的響應曲線最終平穩，對應飛行器懸停在期望位置，達到了控制要求。

本次試驗收獲很多，學習到了很多知識，首先是熟悉了SIMULINK由簡至繁搭建系統的過程，學習了利用VR建立虛擬模型，并在SIMULINK中連接。其次是熟悉了MATLAB

GUI界面的編寫和搭建過程。Matlab提供了強大的用戶圖形界面，以幫助用戶不必編寫底層程序而直接在軟件包基礎上進行自行開發，這點在諸多軟件中都有所體現。另外通過實驗，對四旋翼飛行器的受力分析、模型建立、控制回路設計等有了較為細致的了解。

第五篇：動態系統建模（四旋翼飛行器仿真）實驗報告

動態系統建模（四旋翼飛行器仿真）

實驗報告

院（系）名稱

大飛機班

學號

學生姓名

任課教師

2011年

X月

四旋翼飛行器的建模與仿真

一、實驗原理

I．四旋翼飛行器簡介

四旋翼飛行器通過四個螺旋槳產生的升力實現飛行，原理與直升機類似。四個旋翼位于一個幾何對稱的十字支架前、后、左、右四端，如圖1-1所示。旋翼由電機控制；整個飛行器依靠改變每個電機的轉速來實現飛行姿態控制。

在圖1-1中，前端旋翼1

和后端旋翼3

逆時針旋轉，而左端旋翼2

和右端的旋翼4

順時針旋轉，以平衡旋翼旋轉所產生的反扭轉矩。由此可知，懸停時，四只旋翼的轉速應該相等，以相互抵消反扭力矩；同時等量地增大或減小四只旋翼的轉速，會引起上升或下降運動；增大某一只旋翼的轉速，同時等量地減小同組另一只旋翼的轉速，則產生俯仰、橫滾運動；增大某一組旋翼的轉速，同時等量減小另一組旋翼的轉速，將產生偏航運動。

圖1-1

四旋翼飛行器旋翼旋轉方向示意圖

從動力學角度分析，四旋翼飛行器系統本身是不穩定的，因此，使系統穩定的控制算法的設計顯得尤為關鍵。由于四旋翼飛行器為六自由度的系統（三個角位移量，三個線位移量），而其控制量只有四個（4

個旋翼的轉速），這就意味著被控量之間存在耦合關系。因此，控制算法應能夠對這種欠驅動（under-actuated）系統足夠有效，用四個控制量對三個角位移量和三個線位移量進行穩態控制。本實驗針對四旋翼飛行器的懸浮飛行狀態進行建模。

II．飛行器受力分析及運動模型

（1）整體分析

如圖1-2所示，四旋翼飛行器所受外力和力矩為：

?

重力mg，機體受到重力沿-Zw方向

?

四個旋翼旋轉所產生的升力Fi(i=1,2,3,4)，旋翼升力沿ZB方向

?

旋翼旋轉會產生扭轉力矩Mi

(i=1,2,3,4)，Mi垂直于葉片的旋翼平面，與旋轉矢量相反。

圖1-2

四旋翼飛行器受力分析

（2）電機模型

?

力模型

（1.1）

旋翼通過螺旋槳產生升力。是電機轉動力系數，可取，為電機轉速。

?

力矩模型

旋翼旋轉產生旋轉力矩Mi(i=1,2,3,4)，力矩Mi的旋向依據右手定則確定。

（1.2）

是電機轉動力系數，可取為電機轉速。

?

轉速模型

當給定期望轉速后，電機的實際轉速需要經過一段時間才能達到。實際轉速與期望轉速之間的關系為一階延遲：

（1.3）

響應延遲時間可取0.05s(即)。期望轉速則需要限制在電機的最小轉速和最大轉速之間，范圍可分取[1200rpm，7800rpm]。

（3）運動方程

飛行器受到外界力和力矩的作用，形成線運動和角運動。線運動由合外力引起，符合牛頓第二定律，如公式(1.4)所示：

（1.4）

r為飛機的位置矢量。注意：公式（1.4）是在地平面坐標系中進行描述的。

角運動由合力矩引起。四旋翼飛行器所受力矩來源于兩個方面：1）旋翼升力作用于質心產生的力矩；2）旋翼旋轉產生的扭轉力矩。角運動方程如公式（1.5）所示。其中，L

為旋翼中心建立飛行器質心的距離，I

為慣量矩陣。

（1.5）

III．控制回路設計

控制回路包括內外兩層。外回路由Position

Control

模塊實現。輸入為位置誤差，輸出為期望的滾轉、俯仰和偏航角。內回路由Attitude

Control

模塊實現，輸入為期望姿態角，輸出為期望轉速。Motor

Dynamics

模塊模擬電機特性，輸入為期望轉速，輸出為力和力矩。Rigid

Body

Dynamics

是被控對象，模擬四旋翼飛行器的運動特性。如圖1-3

圖1-3

包含內外兩個控制回路的控制結構

（1）內回路：姿態控制回路

對四旋翼飛行器，我們唯一可用的控制手段就是四個旋翼的轉速。因此，這里首先對轉速產生的作用進行分析。假設我們希望旋翼1的轉速達到，那么它的效果可分解成以下幾個分量：

：使飛行器保持懸停的轉速分量；

：除懸停所需之外，產生沿ZB軸的凈力；

：使飛行器負向偏轉的轉速分量；

：使飛行器正向偏航的轉速分量；

因此，可以將期望轉速寫成幾個分量的線性組合：

（1.6）

其它幾個旋翼也可進行類似分析，最終得到：

（1.7）

在懸浮狀態下，四個旋翼共同的升力應抵消重力，因此：

（1.8）

此時，可以把旋翼角速度分成幾個部分分別控制，通過“比例-微分”控制律建立如下公式：

（1.9）

綜合式（1.7）、（1.8）、（1.9）可得到期望姿態角-期望轉速之間的關系，即內回路。

（2）外回路：位置控制回路

外回路采用以下控制方式：

?

通過位置偏差計算控制信號（加速度）；

?

建立控制信號與姿態角之間的幾何關系；

?

得到期望姿態角，作為內回路的輸入。

期望位置記為。可通過PID

控制器計算控制信號：

（1.10）

是目標懸停位置是我們的目標懸停位置(i=1,2,3)，是期望加速度，即控制信號。注意：懸停狀態下線速度和加速度均為0，即。

通過俯仰角和滾轉角控制飛行器在XW和YW平面上的運動，通過控制偏航角，通過控制飛行器在ZB軸上的運動。對（1.4）進行展開，可得到：

（1.11）

根據上式可按照以下原則進行線性化：

（1）將俯仰角、滾轉角的變化作為小擾動分量，有，，（2）偏航角不變，有，其中初始偏航角，為期望偏航角（3）在懸停的穩態附近，有

根據以上原則線性化后，可得到控制信號（期望加速度）與期望姿態角之間的關系：

（1.12）

根據式(1.10)已經通過PID

控制器得到了作為控制信號的期望加速度，因此，將（1.12）式反轉，由期望加速度計算期望姿態角，作為內回路的輸入：

（1.13）

二、實驗步驟

I．搭建Simulink仿真控制回路

根據實驗原理中運動方程及控制回路設計，搭建Simulink控制回路，如圖2-1所示。主要分為五個部分：Position

Control（由期望的位置誤差通過控制律設計計算出期望的姿態角），Attitude

Control（由姿態角信息和各軸角速度信息通過控制律計算出給電機的控制信號），Motor

Dynamics（通過給電機的控制信號由電機模型計算出每個電機的輸出力和力矩），Rigid

Body

Dynamics為四旋翼飛行器的仿真模型，由產生的力和力矩計算出仿真模型的姿態和位置信息，VR

Sink為四旋翼飛行器的虛擬顯示模型。

圖2-1

仿真Simulink模型

下面給出每個子系統的仿真結構圖及控制律設計部分。

圖2-2

Position

Control子系統

圖2-3

位置PID控制器結構

圖2-4

Attitude

Control子系統

圖2-5

姿態角和三軸角速度之間的轉換關系

圖2-6

Motor

Dynamics子系統輸出力及力矩模型

圖2-7

Rigid

Body

Dynamics子系統

II．利用V-Realm

Builder建立四旋翼飛行器的虛擬模型

利用V-Realm

Builder建立四旋翼飛行器的大致虛擬模型，并建立四個父類分別為Simulink輸入提供質心位移信息和機體姿態信息，如圖2-8所示。

圖2-8

四旋翼飛行器虛擬模型

III．利用MATLAB

GUI建立四旋翼飛行器仿真的控制界面

利用MATLAB

GUI建立仿真控制界面，所建立的控制界面如圖2-9所示。

圖2-9

MATLAB

GUI仿真控制界面

界面主要分為四個部分，Struct

Parameters

Panel設置飛行器的結構參數和外部變量，Desired

Position

Panel設置期望控制飛行器所到達的位置，Control

Parameters

Panel設置PID控制律所需的增益參數和仿真時間，Plot

Panel顯示仿真結果圖形并對圖形效果進行簡單的控制。

三、仿真結果

運行GUI，輸入所需參數或者采用默認參數，點擊load

data按鈕分別將三組參數載入，點擊Start按鈕，仿真開始運行。跳出VR顯示，并在仿真結束后繪制飛行器三方向的坐標信息曲線和飛行器位置曲線。VR顯示過程中某一時刻如圖3-1所示，仿真結束后控制界面顯示的曲線如圖3-2所示。期望達到的目標點設置為（10,15,20）。

圖3-1

VR顯示四旋翼飛行器運動狀態

圖3-2

四旋翼飛行器控制平臺

四、總結與體會

由仿真結果可以看出，四旋翼飛行器最終位置達到了期望給定的位置，三個方向的響應曲線最終平穩，對應飛行器懸停在期望位置，達到了控制要求。本次試驗收獲很多，學習到了很多知識，熟悉了SIMULINK由簡至繁搭建系統的過程，學習了利用V-Realm

Builder建立虛擬模型，并在SIMULINK中連接，也熟悉了MATLAB

GUI界面的編寫和搭建過程。