四旋翼飛行器

Name:岑汝平Email

：997576280@Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理微型飛行器：固定翼、旋翼式及直升機三種。

四旋翼飛行器在布局形式上屬于旋翼的一種，相對于別的旋翼式飛行器來說四旋翼飛行器結構緊湊，能產生更大的升力，而且不需要專門的反扭矩槳保持飛行器扭矩平衡.飛行器簡介飛行器簡介貝斯索貨運無人機航拍、偵查無人機飛行器用途：飛行器簡介

2012年2月，賓夕法尼亞大學的VijayKumar教授在TED上做出了四旋翼飛行器發展歷史上里程碑式的演講。這一場充滿數學公式的演講居然大受歡迎，迄今已經有三百多萬次觀看，是TED成百上千個演講中瀏覽量最高的演講之一。PIX:ETHZurich實驗室研發.ARMcortex_M4處理器.雙GPS.Nuttx實時操作系統.配備地面站Lily:.采用防水結構.圖像+GPS定位跟蹤.圖像軟件去抖.實時1080P圖傳飛行器簡介國內研究現狀:

DJI（大疆公司）：S1000六旋翼飛行器-內置GPS導航功能，可以在戶外很大的范圍內飛行。S1000提供了掛載相機的連接架，讓用相機的人們有了從天空向下的拍攝視角。特別地，與傳統的飛機和直升機航拍不同，多旋翼系統小巧靈活，能讓拍攝者自由地控制角度和距離.飛行器簡介飛行器簡介我們的目的是Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理四旋翼飛行器動力學分析及建模K為旋翼系數，ω為電機轉速(3)重力G=m*g重力包括了四個機體支架、電池、四個電機和旋翼、主控板等受力分析(1)螺旋槳產生的升力(2)空氣阻力C為阻力系數，S為旋翼面積，

為空氣密度，

為螺旋槳旋轉的速度四旋翼飛行器動力學分析及建模飛行器垂直爬升、下降（十字模型）四旋翼飛行器動力學分析及建模飛行器橫滾狀態（十字模型）四旋翼飛行器動力學分析及建模飛行器俯仰狀態（十字模型）四旋翼飛行器動力學分析及建模

飛行偏航狀態（十字模型）四旋翼飛行器動力學分析及建模

飛行橫滾狀態（X字模型）四旋翼飛行器動力學分析及建模

飛行俯仰狀態（X字模型）四旋翼飛行器動力學分析及建模

飛行偏航狀態（X字模型）四旋翼飛行器建模

建模分析KV900四旋翼飛行器建模

運動建模釆用牛頓-歐拉模型來進行飛行器運動的建模由牛頓第二定律,在地面坐標系下X,Y,Z三個方向上的直線運動方程為：四旋翼飛行器建模

忽略空氣阻力,運動方程簡化為:歐拉方程可知，在地面坐標系下的旋轉運動方程為：假設以下控制量方程：為Z軸方向線運動的控制量為滾轉角與X軸方向線運動的控制量為俯仰角與Y軸方向線運動的控制量為偏航角的控制量四旋翼飛行器建模

角度傳遞函數為:位置傳遞函數為:四旋翼飛行器的非線性模型為:線性化Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理四旋翼飛行器硬件系統

飛行器的動力來源螺旋槳的反作用力：整套系統的執行器包含螺旋槳、無刷電機、無刷驅動器、電池。規格型號：

螺旋槳規格，一般由4位數字表示，前兩位數表示直徑，后兩位表示螺距。以1060漿為例，10表示槳的直徑是10英寸，60表示漿角（螺距，6.0英寸，也就是152.4mm)。四旋翼飛行器硬件系統槳的衡量參數：槳的平衡性。

經驗：槳當然是選擇APC和DJI。大多航拍還是建議選擇APC，是正品APC哦，仿的拿來試飛是個不錯的選擇，炸了也不心疼。APC效率高。大載重的可以選擇碳纖維槳。載重很大的話可以選擇櫸木槳，不易變形。

四旋翼飛行器硬件系統1電機KV值：——大KV配小槳，小KV配大槳。KV值是每1V的電壓下電機每分鐘空轉的轉速，例如KV800,在1V的電壓下空轉轉速是800轉每分鐘。2電機型號：——定子粗的，力氣大。

電機型號，如2212，3508，4010，這些數字表示電機定子的直徑和高度（如下圖）。前面兩位是定子直徑，后面兩位是定子高度，單位是毫米。

四旋翼飛行器硬件系統

經驗一：整機重量，應該小于電機最大動力的2/5。經驗二：選電機一定要看參數表，如果沒有參數表，請不要選擇。盡量選擇大品牌的電機。四旋翼飛行器硬件系統電調主流品牌好盈，中特威。把電調當成變頻器的：電調是在將輸入的直流電，通過電調的6路開關管不斷的在切換電流方向，輸出三相交流電，但是換相的頻率不是由電調決定的，因為電調的換相頻率是由電機決定，電調換相頻率必須和電機保持同步，超前的換相頻率會使電機失步停轉，滯后的換相頻率會使電機的效率很低。所以電調并沒有主動的在調整換相頻率，而是被動的。

四旋翼飛行器硬件系統一般電調都有5V穩壓輸出（可以為飛控、遙控器直接提供電源）最大3A如果沒5V輸出的建議使用LM2575、LM2596最好不要使用7805電調驅動方式PWM（周期20MS占空比4.2%~9.8%）四旋翼飛行器硬件系統1、電池容量：

5200mAh，意味著以5.2A電流放電，可以放1小時。當然，我們只是這么理解。實際放電時間，需要參考電池廠家提供的相關技術參數。

2、放電能力：

30C電池，指的是電池的放電能力。對于30C電池，最大持續放電電流為：電池容量X放電C。

3、電池的分類：

2S（7.2V）~6S(22.2V)四旋翼飛行器硬件系統機架：

1、2kg以下的機子可以選玻纖機架、工程塑料；2kg以上的果斷上3K碳纖維（價格有點貴）。2、自制機架（3MM鋁板+碳管管夾）。

3、機架軸距=（槳的英寸*25.4/0.8/）*2。槳的尺寸（英寸）=（機架軸距/2）*4、一般機架尺寸：450、550、650mm四旋翼飛行器硬件系統自行設計與加工的飛行器機架四旋翼飛行器硬件系統自行設計與加工的飛行器機架四旋翼飛行器硬件系統也可直接購買F450這種機架（工程塑料）實惠！！！四旋翼飛行器硬件系統碳素機架：優點價格高、重量輕、強度高四旋翼飛行器硬件系統 MPU6050

、HMC5883提供三個軸度上的加速度、角速度、方位數據組成九軸演算

(輸出數據精度可達0.1°)HMC5883(電子羅盤)MPU6050陀螺儀+加速度計四旋翼飛行器硬件系統1、陀螺儀輸出的是角速度角速度直接積分不就是角度了嗎？為什么需要這么多的東西那？2、加速度用來做什么？3、羅盤又用來做什么？4、干嘛需要用地坐標進行描述飛行器？四旋翼飛行器硬件系統

陀螺儀是對震動敏感，也就是是說在短時間的情況下陀螺儀積分出來是很準確的。但是由于角度的變化或是時間的關系會影響陀螺儀的準確性此時我們需要利用加速度、羅盤主要是用來修正陀螺儀。羅盤主要是確定地理坐標四旋翼飛行器硬件系統BMP180：為飛行器提供高度、溫度信息。超聲波：在近地面的時候可以提供更準確的信息BMP180氣壓計超聲波模塊四旋翼飛行器硬件系統飛行器控制核心最好采用ARM內核的單片機、DSP。市面上的飛控一般是STM32的cortex-M3、

cortex-M4內核(帶DSP浮點單元)、飛思卡爾MK60等。

四旋翼飛行器硬件系統

選用處理能力強的單片機的原因：

1、計算四元數的過程中為了保證精度需要大量的浮點運算、三角運算。2、一般要保證將飛行器的控制周期確定在10msz左右（常用的控制系統保證控制周期7-15ms都可以的）

建議：在不添加操作系統的情況下最好是開定時器去控制，在while(1)里面存放數據回傳、接收的函數塊。這樣既能保證數據傳輸又不會影響控制進程。四旋翼飛行器硬件系統飛行器硬件框圖Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理飛行器姿態解算（1）地面坐標系（OEXEYEZE）地理坐標系一般選用地軸系。原點O設在地面上的某一點，OEXE軸指向正北方向，OEYE軸指向正東方向，，OEZE軸與XEOEYE平面垂直，方向為垂直向上。（2）機體坐標系（OXYZ）機體坐標系是固定在飛行器身上并與之一起移動動態坐標系。原點O為飛行器重心，OX軸與機身軸線平行并規定指向機頭方向為正方向，相對OX軸順時針旋轉90°為OY軸，OZ軸垂直XOY平面向上。（MWC是使用的機體坐標系）飛行器姿態解算地面坐標系：當我們的飛機頭朝北水平放置時載體坐標系和參考坐標系是重合的，那么接下來我們繞飛機的Z軸旋轉30°，這個旋轉的歐拉角就是我們所說的Yaw，繞飛機的Y軸旋轉30°我們得到Pitch，繞飛機X軸旋轉得到Roll。飛行器姿態解算歐拉角是基于飛機本身軸旋轉得到的，但是得到的姿態卻是相對于地面的參考坐標系而說的。飛行器姿態解算1、演示姿態2、講解代碼3、講解四元數理論

飛行器姿態解算四元素轉歐拉角

歐拉角使用roll，pitch，yaw三個值來分別表示繞(世界的)x軸、y軸、z軸旋轉的角度量值。其取值是在[0,360]間。

飛行器姿態用四元數表示為Q0，Q1，Q2，Q3，其中Q0表示一個旋轉角度（后面有描述），Q1，Q2，Q3表示的是一個空間向量，就是飛行器從姿態原點（物體的三個歐拉角都是0的姿態），圍繞向量（Q1，Q2，Q3）旋轉一個角度f(Q0)。

飛行器姿態解算

向量a×向量b得到的也是一個向量，向量方向和ab都垂直，大小等于ab組成的平行四邊形面積即：向量a×向量b=|a||b|sinθ在這里θ表示兩向量之間的角夾角（0°≤θ≤180°）。知識回顧：飛行器姿態解算四元數相關定理定理1：令p為三維（投影）空間內的一個點，用齊次坐標將其表示成四元數的形式即為：

p=(x:y:z:w)=[(x,y,z),w]=[v,w]；

令q為任一非零四元數。那么：

結論1)

表達式

qpq-1的結果將使p=[v,w]變換到p`=[v`,w]，二者模長相等。

結論2)

任何非零實數與q相乘，上式仍然成立。

結論3)

如果上式中的q為N(q)=1（即q為單位四元數），那么q=[

v

sinΩ,cosΩ]表示一個旋轉動作：將p沿著單位軸v

旋轉2Ω即得到p'。

性質：四元素四個數的平方和等于1。飛行器姿態解算

根據四元素第一個結論可得：

這里Q是四元數Q*是q的共軛，就是把ijk的系數取相反數。r是旋轉前的向量，r‘就是旋轉以后的向量。飛行器姿態解算

上述黃色這個式子得到:

在載體系定義一個矢量

，設其在參考系中表示為

。則有

，其中

，

為共軛復數。飛行器姿態解算

用矩陣來表達成四元素旋轉矩陣：用這個矩陣左乘需要旋轉的向量，就可以得到旋轉后的向量了。這里的旋轉，指的是同一個坐標系下，一個向量旋轉的到另一個向量。可是我們時間的情況是同一個向量，在兩個坐標系下的不同表達。飛行器姿態解算四元數微分方程：四元數微分方程本來只是基于角速度的，也就是說，已知上個周期的姿態，和本次測量得到的角速度，得到本周期的姿態，在角速度里加入誤差反饋，來調節姿態，起到減小誤差的作用。飛行器姿態解算四元數微分方程：分別表示載體坐標系相對于地理坐標系沿各個軸向的角度分量。飛行器姿態解算四元素與歐拉角的關系

(、θ、ψ為歐拉角a、b、c、d為四元素q0、q1、q2、q3)

當我們利用四元數表示出飛行器修正后的“姿態”時，就可以反向計算出歐拉角。飛行器姿態解算飛行器姿態角求解過程：

1>把機體上的羅盤數據通過四元數旋轉矩陣轉化到地坐標系進行標定。

2>將標準的重力、以及標定后的地磁矢量轉換回機體坐標系。3>將步驟2中的矢量與加速度矢量進行比較，計算誤差。 4>利用四元數微分方程將誤差迭代帶陀螺儀數據中并修正旋轉方程。 5>規范四元數。 6>轉換成歐拉角表示。飛行器姿態解算姿態修正過程：

1、將機體坐標系的電子羅盤測到的矢量轉成地理坐標系下的磁場矢量hxyz（測量值）然后令其在Y方向為0、X方向大小等于磁場矢量在hxy平面上的投影長。得到標定后的矢量bxyz。

思考：為什么要把X、Y軸這樣標定？飛行器姿態解算姿態修正過程：

我們知道重力在地面坐標系下向量是（0,0,1），可是地磁我們并不清楚，我們只能假設地面坐標系x軸方向朝北，這樣的話y方向地磁是0，認為地面坐標系下，z方向就是該向量z方向的分量，y方向是0，x方向是該向量xy平面上的投影長。接下來的事就和處理重力向量一樣了。變換回機體坐標系，再和實際測量的地磁求誤差。飛行器姿態解算hx=2*mx*(0.5f-q2q2-q3q3)+2*my*(q1q2-q0q3)+ 2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5f-q1q1-q3q3)+ 2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+ 2*mz*(0.5f-q1q1-q2q2);bx=sqrt((hx*hx)+(hy*hy));by=0;//人為的標定為0bz=hz;

飛行器姿態解算姿態修正過程：

2、將地理坐標系的標準重力向量(0,0,g)及標定以后的磁場矢量分別左乘上四元素旋轉矩陣，轉到機體坐標系。

vx=2*(q1q3-q0q2);

vy=2*(q0q1+q2q3);

vz=q0q0-q1q1-q2q2+q3q3;wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);

飛行器姿態解算姿態修正過程：

3、將加速度計測出來的重力向量和參考矢量做叉積，磁場的測量矢量和參考矢量也做叉積。都用來修正陀螺。

ex=(ay*vz-az*vy)+(my*wz-mz*wy);

ey=(az*vx-ax*vz)+(mz*wx-mx*wz);

ez=(ax*vy-ay*vx)+(mx*wy-my*wx);

飛行器姿態解算姿態修正過程：

4、由于叉積向量大小與陀螺積分誤差成正比，因此在誤差上乘上一個系數疊加到陀螺儀數據上。 exInt=exInt+ex*Ki*halfT; eyInt=eyInt+ey*Ki*halfT; ezInt=ezInt+ez*Ki*halfT; gx=gx+Kp*ex+exInt; gy=gy+Kp*ey+eyInt; gz=gz+Kp*ez+ezInt;

飛行器姿態解算姿態修正過程：

6、歸一化處理： norm=invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0=q0*norm;

q1=q1*norm;

q2=q2*norm;

q3=q3*norm;飛行器姿態解算姿態修正過程：

5、計算四元數的微分方程、修正旋轉矩陣： q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT; q1=q1+(q0*gx+q2*gz-q3*gy)*halfT; q2=q2+(q0*gy-q1*gz+q3*gx)*halfT; q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;飛行器姿態解算姿態修正過程：

7、最后將四元數轉化成歐拉角進行表示Q_ANGLE.Z=-atan2(2*q1*q2+2*q0*q3,-2*q2*q2- 2*q3*q3+1)*57.3;//yawQ_ANGLE.Y=-asin(-2*q1*q3+2*q0*q2)*57.3;//pitchQ_ANGLE.X=atan2(2*q2*q3+2*q0*q1,-2*q1*q1 -2*q2*q2+1)*57.3;//roll

飛行器姿態解算

我們剛開始隨便假設的四元數，已經表征了兩個坐標系之間的旋轉關系了，只不過這個旋轉矩陣跟真實的矩陣相比誤差很大，我們假設這個矩陣是正確的，然后得到兩個坐標下同一個向量的差別，用這個差別來糾正這個矩陣。這是這個算法的根本所在。

飛行器姿態解算說具體一點，我們知道重力在地理坐標系下向量是（0,0，g），如果我們的矩陣是準確的，那么用這個矩陣將（0,0，g）轉換到機體坐標，那我們應該得到在機體坐標系下的重力的向量，如果矩陣很準確，那我們從加速度計讀取的數值應該也是這個向量，可是哪能那么巧，所以呢我們讀取加速度計得到重力向量在機體坐標系下的向量，這是測量值，這個值和剛剛的理論值有差別，用這個差別來糾正旋轉矩陣。程序一遍一遍的跑，這個誤差就會一點點減小了。飛行器姿態解算MWC_IMU代碼解讀

i2c_writeReg(MPU6050_ADDRESS,0x1C,0x10);//ACCEL_CONFIG+/-8G;i2c_writeReg(MPU6050_ADDRESS,0x1B,0x18);//GYRO_CONFIG2000deg/secMWC中加速度計量程為+/-8G陀螺儀量程為2000deg/sec飛行器姿態解算陀螺儀讀取函數voidGyro_getADC(){

i2c_getSixRawADC(MPU6050_ADDRESS,0x43);

GYRO_ORIENTATION(((rawADC[0]<<8)|rawADC[1])>>2, ((rawADC[2]<<8)|rawADC[3])>>2,

((rawADC[4]<<8)|rawADC[5])>>2);

GYRO_Common();}飛行器姿態解算加速度讀取及處理函數：voidACC_getADC(){

i2c_getSixRawADC(MPU6050_ADDRESS,0x3B);

ACC_ORIENTATION(((rawADC[0]<<8)|rawADC[1])>>3,

((rawADC[2]<<8)|rawADC[3])>>3,

((rawADC[4]<<8)|rawADC[5])>>3);}

加速度數據左移了三位陀螺儀數據左移了兩位飛行器姿態解算還記得我們校準加速度記的時候輸出的數據為什么是0,0,512嗎？？？8G的量程對應16位有符號數32768，則1g對應的數值應該是4096，又因為MWC在采集數據以后進行了右移處理（右移3位相當于除以8）所以最后1g對應512飛行器姿態解算voidrotateV(structfp_vector*v,float*delta){fp_vectorv_tmp=*v;v->Z-=delta[ROLL]*v_tmp.X+delta[PITCH]*v_tmp.Y;v->X+=delta[ROLL]*v_tmp.Z-delta[YAW]*v_tmp.Y;v->Y+=delta[PITCH]*v_tmp.Z+delta[YAW]*v_tmp.X;}飛行器姿態解算繞X軸旋轉θ飛行器姿態解算繞Y軸旋轉γ飛行器姿態解算繞Z軸旋轉Ψ飛行器姿態解算一種旋轉繞Z軸旋轉Ψ=yaw->繞X軸旋轉θ=pitch->繞Y軸旋轉γ=roll余弦旋轉矩陣飛行器姿態解算角增量輸出飛行器姿態解算飛行器姿態解算

int32_tsqGZ=sq(EstG32.V.Z); int32_tsqGX=sq(EstG32.V.X); int32_tsqGY=sq(EstG32.V.Y); int32_tsqGX_sqGZ=sqGX+sqGZ; floatinvmagXZ=InvSqrt(sqGX_sqGZ); invG=InvSqrt(sqGX_sqGZ+sqGY);

angle[ROLL]=_atan2(EstG32.V.X,EstG32.V.Z); angle[PITCH]=_atan2(EstG32.V.Y,invmagXZ*sqGX_sqGZ);飛行器姿態解算Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理數據濾波傳感器輸出數據中噪聲的存在（螺旋槳切割空氣時所產生的高頻振動引起的）。數據濾波常用的濾波算法：

1、均值濾波

2、滑動窗口濾波（滑動平均濾波法）

3、限幅濾波法

4、中值濾波法說明：把連續N個采樣值看成一個隊列，隊列長度固定為N。每次采樣到一個新數據放入隊尾，并扔掉隊首的一次數據。把隊列中的N各數據進行平均運算，既獲得新的濾波結果。

優點：對周期性干擾有良好的抑制作用，平滑度高；試用于高頻振蕩的系統數據濾波#defineN12//隊列長度unsignedcharvalue_buf[N];unsignedcharfilter(){

unsignedcharI,value;

intsum=0;

value_buf[i++]=get_ad();

//采集到的數據放入最高位

for(i=0;i<N;i++)

{

value_buf[i]=value_buf[i+1];

//所有數據左移，低位扔掉

sum+=value_buf[i];

}

value=sum/N;

return(value);

}數據濾波MWC原型機的濾波方法：當前的測量值占3/4的比重、上一個的數據占1/4的比重。對數據進行濾波。數據濾波加速度、羅盤錯誤的校準方式造成的結果！！數據濾波加速度、羅盤校準方式數據濾波陀螺儀校準方式將陀螺儀放置與靜止狀態是采集陀螺儀的零偏值。數據濾波陀螺儀標度變換陀螺儀量程為2000dec/秒

對應16位有符號數32768數據濾波加速度、羅盤需要標度變換需要嗎？不需要！！Contents

飛行器簡介1飛行器的動力學分析和建模2飛行器姿態解算3四旋翼飛行器硬件系統456

PID&飛行器調試介紹數據處理PID

普通PID一般有兩種：位置式PID、增量式PIDPIDCounter_KI_Out[0]+=Counter_Ki*Counter_Err[0];//累計誤差if(Counter_KI_Out[0]>450)//限制積分幅度

Counter_KI_Out[0]=450;elseif(Counter_KI_Out[0]<-450)

Counter_KI_Out[0]=-450;Counter_PID_Output[0]=

Counter_Kp*Counter_Err[0]+ Counter_KI_Out[0]+ Counter_Kd*MPU6050_Data[0];PID個人經驗：

1、先調節P（D為零）注意分清是超調還是P參數弱了。

2、增加D參數使得系統收斂得到一個穩定的狀態。

3、繼續增加D使得系統“抖動”然后在繼續增加P參數這樣反復幾次，最終系統可以在外力作用下保持穩定就可以。

4、盡量使用串口助手調試串級PID1、得到軸姿態的角度差（angle

error），將這個值乘以角度系數p后限幅（限幅必須有，否則劇烈打舵時容易引發震蕩）作為角速度控制器期望值target_rate。2、target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（rate_error）乘以kp得到P。串級PID3、在I值小于限幅值（大概在5%油門）或者rate_error與i值異號時將rate_error累加到I中。4、前后兩次rate_error的差作為D項，值得注意的是加需要入20hz（也可以采用其它合適頻率）濾波，以避免震蕩。將P,I,D三者相加并限幅（50%油門）得到最終PID輸出。PID參數映射到模型上ＰＩＤ參數映射到電機PID參數映射到模型上飛行器調試方法飛行器調試方法謝謝!!!

MATLAB仿真

（1）比例環節P：控制器中的比例環節用來糾正系統偏差，比例項能夠快速的響應偏差，但是不能消除系統的穩態誤差，比例過大還會引起系統的不穩定；（2）積分環節I：積分項用于消除系