講課PPT-修改后的

四旋翼飛行器

Name:岑汝平Email

：997576280@Contents

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

PID&飛行器調(diào)試介紹數(shù)據(jù)處理微型飛行器：固定翼、旋翼式及直升機(jī)三種。

四旋翼飛行器在布局形式上屬于旋翼的一種，相對(duì)于別的旋翼式飛行器來(lái)說(shuō)四旋翼飛行器結(jié)構(gòu)緊湊，能產(chǎn)生更大的升力，而且不需要專門(mén)的反扭矩槳保持飛行器扭矩平衡.飛行器簡(jiǎn)介飛行器簡(jiǎn)介貝斯索貨運(yùn)無(wú)人機(jī)航拍、偵查無(wú)人機(jī)飛行器用途：飛行器簡(jiǎn)介

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

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

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

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

為空氣密度，

為螺旋槳旋轉(zhuǎn)的速度四旋翼飛行器動(dòng)力學(xué)分析及建模飛行器垂直爬升、下降（十字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模飛行器橫滾狀態(tài)（十字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模飛行器俯仰狀態(tài)（十字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模

飛行偏航狀態(tài)（十字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模

飛行橫滾狀態(tài)（X字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模

飛行俯仰狀態(tài)（X字模型）四旋翼飛行器動(dòng)力學(xué)分析及建模

飛行偏航狀態(tài)（X字模型）四旋翼飛行器建模

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

運(yùn)動(dòng)建模釆用牛頓-歐拉模型來(lái)進(jìn)行飛行器運(yùn)動(dòng)的建模由牛頓第二定律,在地面坐標(biāo)系下X,Y,Z三個(gè)方向上的直線運(yùn)動(dòng)方程為：四旋翼飛行器建模

忽略空氣阻力,運(yùn)動(dòng)方程簡(jiǎn)化為:歐拉方程可知，在地面坐標(biāo)系下的旋轉(zhuǎn)運(yùn)動(dòng)方程為：假設(shè)以下控制量方程：為Z軸方向線運(yùn)動(dòng)的控制量為滾轉(zhuǎn)角與X軸方向線運(yùn)動(dòng)的控制量為俯仰角與Y軸方向線運(yùn)動(dòng)的控制量為偏航角的控制量四旋翼飛行器建模

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

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

PID&飛行器調(diào)試介紹數(shù)據(jù)處理四旋翼飛行器硬件系統(tǒng)

飛行器的動(dòng)力來(lái)源螺旋槳的反作用力：整套系統(tǒng)的執(zhí)行器包含螺旋槳、無(wú)刷電機(jī)、無(wú)刷驅(qū)動(dòng)器、電池。規(guī)格型號(hào)：

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

經(jīng)驗(yàn)：槳當(dāng)然是選擇APC和DJI。大多航拍還是建議選擇APC，是正品APC哦，仿的拿來(lái)試飛是個(gè)不錯(cuò)的選擇，炸了也不心疼。APC效率高。大載重的可以選擇碳纖維槳。載重很大的話可以選擇櫸木槳，不易變形。

四旋翼飛行器硬件系統(tǒng)1電機(jī)KV值：——大KV配小槳，小KV配大槳。KV值是每1V的電壓下電機(jī)每分鐘空轉(zhuǎn)的轉(zhuǎn)速，例如KV800,在1V的電壓下空轉(zhuǎn)轉(zhuǎn)速是800轉(zhuǎn)每分鐘。2電機(jī)型號(hào)：——定子粗的，力氣大。

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

四旋翼飛行器硬件系統(tǒng)

經(jīng)驗(yàn)一：整機(jī)重量，應(yīng)該小于電機(jī)最大動(dòng)力的2/5。經(jīng)驗(yàn)二：選電機(jī)一定要看參數(shù)表，如果沒(méi)有參數(shù)表，請(qǐng)不要選擇。盡量選擇大品牌的電機(jī)。四旋翼飛行器硬件系統(tǒng)電調(diào)主流品牌好盈，中特威。把電調(diào)當(dāng)成變頻器的：電調(diào)是在將輸入的直流電，通過(guò)電調(diào)的6路開(kāi)關(guān)管不斷的在切換電流方向，輸出三相交流電，但是換相的頻率不是由電調(diào)決定的，因?yàn)殡娬{(diào)的換相頻率是由電機(jī)決定，電調(diào)換相頻率必須和電機(jī)保持同步，超前的換相頻率會(huì)使電機(jī)失步停轉(zhuǎn)，滯后的換相頻率會(huì)使電機(jī)的效率很低。所以電調(diào)并沒(méi)有主動(dòng)的在調(diào)整換相頻率，而是被動(dòng)的。

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

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

2、放電能力：

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

3、電池的分類：

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

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

3、機(jī)架軸距=（槳的英寸*25.4/0.8/）*2。槳的尺寸（英寸）=（機(jī)架軸距/2）*4、一般機(jī)架尺寸：450、550、650mm四旋翼飛行器硬件系統(tǒng)自行設(shè)計(jì)與加工的飛行器機(jī)架四旋翼飛行器硬件系統(tǒng)自行設(shè)計(jì)與加工的飛行器機(jī)架四旋翼飛行器硬件系統(tǒng)也可直接購(gòu)買(mǎi)F450這種機(jī)架（工程塑料）實(shí)惠?。?！四旋翼飛行器硬件系統(tǒng)碳素機(jī)架：優(yōu)點(diǎn)價(jià)格高、重量輕、強(qiáng)度高四旋翼飛行器硬件系統(tǒng) MPU6050

、HMC5883提供三個(gè)軸度上的加速度、角速度、方位數(shù)據(jù)組成九軸演算

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

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

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

四旋翼飛行器硬件系統(tǒng)

選用處理能力強(qiáng)的單片機(jī)的原因：

1、計(jì)算四元數(shù)的過(guò)程中為了保證精度需要大量的浮點(diǎn)運(yùn)算、三角運(yùn)算。2、一般要保證將飛行器的控制周期確定在10msz左右（常用的控制系統(tǒng)保證控制周期7-15ms都可以的）

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

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

PID&飛行器調(diào)試介紹數(shù)據(jù)處理飛行器姿態(tài)解算（1）地面坐標(biāo)系（OEXEYEZE）地理坐標(biāo)系一般選用地軸系。原點(diǎn)O設(shè)在地面上的某一點(diǎn)，OEXE軸指向正北方向，OEYE軸指向正東方向，，OEZE軸與XEOEYE平面垂直，方向?yàn)榇怪毕蛏?。?）機(jī)體坐標(biāo)系（OXYZ）機(jī)體坐標(biāo)系是固定在飛行器身上并與之一起移動(dòng)動(dòng)態(tài)坐標(biāo)系。原點(diǎn)O為飛行器重心，OX軸與機(jī)身軸線平行并規(guī)定指向機(jī)頭方向?yàn)檎较?，相?duì)OX軸順時(shí)針旋轉(zhuǎn)90°為OY軸，OZ軸垂直XOY平面向上。（MWC是使用的機(jī)體坐標(biāo)系）飛行器姿態(tài)解算地面坐標(biāo)系：當(dāng)我們的飛機(jī)頭朝北水平放置時(shí)載體坐標(biāo)系和參考坐標(biāo)系是重合的，那么接下來(lái)我們繞飛機(jī)的Z軸旋轉(zhuǎn)30°，這個(gè)旋轉(zhuǎn)的歐拉角就是我們所說(shuō)的Yaw，繞飛機(jī)的Y軸旋轉(zhuǎn)30°我們得到Pitch，繞飛機(jī)X軸旋轉(zhuǎn)得到Roll。飛行器姿態(tài)解算歐拉角是基于飛機(jī)本身軸旋轉(zhuǎn)得到的，但是得到的姿態(tài)卻是相對(duì)于地面的參考坐標(biāo)系而說(shuō)的。飛行器姿態(tài)解算1、演示姿態(tài)2、講解代碼3、講解四元數(shù)理論

飛行器姿態(tài)解算四元素轉(zhuǎn)歐拉角

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

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

飛行器姿態(tài)解算

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

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

令q為任一非零四元數(shù)。那么：

結(jié)論1)

表達(dá)式

qpq-1的結(jié)果將使p=[v,w]變換到p`=[v`,w]，二者模長(zhǎng)相等。

結(jié)論2)

任何非零實(shí)數(shù)與q相乘，上式仍然成立。

結(jié)論3)

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

v

sinΩ,cosΩ]表示一個(gè)旋轉(zhuǎn)動(dòng)作：將p沿著單位軸v

旋轉(zhuǎn)2Ω即得到p'。

性質(zhì)：四元素四個(gè)數(shù)的平方和等于1。飛行器姿態(tài)解算

根據(jù)四元素第一個(gè)結(jié)論可得：

這里Q是四元數(shù)Q*是q的共軛，就是把ijk的系數(shù)取相反數(shù)。r是旋轉(zhuǎn)前的向量，r‘就是旋轉(zhuǎn)以后的向量。飛行器姿態(tài)解算

上述黃色這個(gè)式子得到:

在載體系定義一個(gè)矢量

，設(shè)其在參考系中表示為

。則有

，其中

，

為共軛復(fù)數(shù)。飛行器姿態(tài)解算

用矩陣來(lái)表達(dá)成四元素旋轉(zhuǎn)矩陣：用這個(gè)矩陣左乘需要旋轉(zhuǎn)的向量，就可以得到旋轉(zhuǎn)后的向量了。這里的旋轉(zhuǎn)，指的是同一個(gè)坐標(biāo)系下，一個(gè)向量旋轉(zhuǎn)的到另一個(gè)向量。可是我們時(shí)間的情況是同一個(gè)向量，在兩個(gè)坐標(biāo)系下的不同表達(dá)。飛行器姿態(tài)解算四元數(shù)微分方程：四元數(shù)微分方程本來(lái)只是基于角速度的，也就是說(shuō)，已知上個(gè)周期的姿態(tài)，和本次測(cè)量得到的角速度，得到本周期的姿態(tài)，在角速度里加入誤差反饋，來(lái)調(diào)節(jié)姿態(tài)，起到減小誤差的作用。飛行器姿態(tài)解算四元數(shù)微分方程：分別表示載體坐標(biāo)系相對(duì)于地理坐標(biāo)系沿各個(gè)軸向的角度分量。飛行器姿態(tài)解算四元素與歐拉角的關(guān)系

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

當(dāng)我們利用四元數(shù)表示出飛行器修正后的“姿態(tài)”時(shí)，就可以反向計(jì)算出歐拉角。飛行器姿態(tài)解算飛行器姿態(tài)角求解過(guò)程：

1>把機(jī)體上的羅盤(pán)數(shù)據(jù)通過(guò)四元數(shù)旋轉(zhuǎn)矩陣轉(zhuǎn)化到地坐標(biāo)系進(jìn)行標(biāo)定。

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

1、將機(jī)體坐標(biāo)系的電子羅盤(pán)測(cè)到的矢量轉(zhuǎn)成地理坐標(biāo)系下的磁場(chǎng)矢量hxyz（測(cè)量值）然后令其在Y方向?yàn)?、X方向大小等于磁場(chǎng)矢量在hxy平面上的投影長(zhǎng)。得到標(biāo)定后的矢量bxyz。

思考：為什么要把X、Y軸這樣標(biāo)定？飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

我們知道重力在地面坐標(biāo)系下向量是（0,0,1），可是地磁我們并不清楚，我們只能假設(shè)地面坐標(biāo)系x軸方向朝北，這樣的話y方向地磁是0，認(rèn)為地面坐標(biāo)系下，z方向就是該向量z方向的分量，y方向是0，x方向是該向量xy平面上的投影長(zhǎng)。接下來(lái)的事就和處理重力向量一樣了。變換回機(jī)體坐標(biāo)系，再和實(shí)際測(cè)量的地磁求誤差。飛行器姿態(tài)解算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;//人為的標(biāo)定為0bz=hz;

飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

2、將地理坐標(biāo)系的標(biāo)準(zhǔn)重力向量(0,0,g)及標(biāo)定以后的磁場(chǎng)矢量分別左乘上四元素旋轉(zhuǎn)矩陣，轉(zhuǎn)到機(jī)體坐標(biāo)系。

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);

飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

3、將加速度計(jì)測(cè)出來(lái)的重力向量和參考矢量做叉積，磁場(chǎng)的測(cè)量矢量和參考矢量也做叉積。都用來(lái)修正陀螺。

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);

飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

4、由于叉積向量大小與陀螺積分誤差成正比，因此在誤差上乘上一個(gè)系數(shù)疊加到陀螺儀數(shù)據(jù)上。 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;

飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

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

q0=q0*norm;

q1=q1*norm;

q2=q2*norm;

q3=q3*norm;飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

5、計(jì)算四元數(shù)的微分方程、修正旋轉(zhuǎn)矩陣： 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;飛行器姿態(tài)解算姿態(tài)修正過(guò)程：

7、最后將四元數(shù)轉(zhuǎn)化成歐拉角進(jìn)行表示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

飛行器姿態(tài)解算

我們剛開(kāi)始隨便假設(shè)的四元數(shù)，已經(jīng)表征了兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系了，只不過(guò)這個(gè)旋轉(zhuǎn)矩陣跟真實(shí)的矩陣相比誤差很大，我們假設(shè)這個(gè)矩陣是正確的，然后得到兩個(gè)坐標(biāo)下同一個(gè)向量的差別，用這個(gè)差別來(lái)糾正這個(gè)矩陣。這是這個(gè)算法的根本所在。

飛行器姿態(tài)解算說(shuō)具體一點(diǎn)，我們知道重力在地理坐標(biāo)系下向量是（0,0，g），如果我們的矩陣是準(zhǔn)確的，那么用這個(gè)矩陣將（0,0，g）轉(zhuǎn)換到機(jī)體坐標(biāo)，那我們應(yīng)該得到在機(jī)體坐標(biāo)系下的重力的向量，如果矩陣很準(zhǔn)確，那我們從加速度計(jì)讀取的數(shù)值應(yīng)該也是這個(gè)向量，可是哪能那么巧，所以呢我們讀取加速度計(jì)得到重力向量在機(jī)體坐標(biāo)系下的向量，這是測(cè)量值，這個(gè)值和剛剛的理論值有差別，用這個(gè)差別來(lái)糾正旋轉(zhuǎn)矩陣。程序一遍一遍的跑，這個(gè)誤差就會(huì)一點(diǎn)點(diǎn)減小了。飛行器姿態(tài)解算MWC_IMU代碼解讀

i2c_writeReg(MPU6050_ADDRESS,0x1C,0x10);//ACCEL_CONFIG+/-8G;i2c_writeReg(MPU6050_ADDRESS,0x1B,0x18);//GYRO_CONFIG2000deg/secMWC中加速度計(jì)量程為+/-8G陀螺儀量程為2000deg/sec飛行器姿態(tài)解算陀螺儀讀取函數(shù)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();}飛行器姿態(tài)解算加速度讀取及處理函數(shù)：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);}

加速度數(shù)據(jù)左移了三位陀螺儀數(shù)據(jù)左移了兩位飛行器姿態(tài)解算還記得我們校準(zhǔn)加速度記的時(shí)候輸出的數(shù)據(jù)為什么是0,0,512嗎？？？8G的量程對(duì)應(yīng)16位有符號(hào)數(shù)32768，則1g對(duì)應(yīng)的數(shù)值應(yīng)該是4096，又因?yàn)镸WC在采集數(shù)據(jù)以后進(jìn)行了右移處理（右移3位相當(dāng)于除以8）所以最后1g對(duì)應(yīng)512飛行器姿態(tài)解算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;}飛行器姿態(tài)解算繞X軸旋轉(zhuǎn)θ飛行器姿態(tài)解算繞Y軸旋轉(zhuǎn)γ飛行器姿態(tài)解算繞Z軸旋轉(zhuǎn)Ψ飛行器姿態(tài)解算一種旋轉(zhuǎn)繞Z軸旋轉(zhuǎn)Ψ=yaw->繞X軸旋轉(zhuǎn)θ=pitch->繞Y軸旋轉(zhuǎn)γ=roll余弦旋轉(zhuǎn)矩陣飛行器姿態(tài)解算角增量輸出飛行器姿態(tài)解算飛行器姿態(tài)解算

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);飛行器姿態(tài)解算Contents

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

PID&飛行器調(diào)試介紹數(shù)據(jù)處理數(shù)據(jù)濾波傳感器輸出數(shù)據(jù)中噪聲的存在（螺旋槳切割空氣時(shí)所產(chǎn)生的高頻振動(dòng)引起的）。數(shù)據(jù)濾波常用的濾波算法：

1、均值濾波

2、滑動(dòng)窗口濾波（滑動(dòng)平均濾波法）

3、限幅濾波法

4、中值濾波法說(shuō)明：把連續(xù)N個(gè)采樣值看成一個(gè)隊(duì)列，隊(duì)列長(zhǎng)度固定為N。每次采樣到一個(gè)新數(shù)據(jù)放入隊(duì)尾，并扔掉隊(duì)首的一次數(shù)據(jù)。把隊(duì)列中的N各數(shù)據(jù)進(jìn)行平均運(yùn)算，既獲得新的濾波結(jié)果。

優(yōu)點(diǎn)：對(duì)周期性干擾有良好的抑制作用，平滑度高；試用于高頻振蕩的系統(tǒng)數(shù)據(jù)濾波#defineN12//隊(duì)列長(zhǎng)度unsignedcharvalue_buf[N];unsignedcharfilter(){

unsignedcharI,value;

intsum=0;

value_buf[i++]=get_ad();

//采集到的數(shù)據(jù)放入最高位

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

{

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

//所有數(shù)據(jù)左移，低位扔掉

sum+=value_buf[i];

}

value=sum/N;

return(value);

}數(shù)據(jù)濾波MWC原型機(jī)的濾波方法：當(dāng)前的測(cè)量值占3/4的比重、上一個(gè)的數(shù)據(jù)占1/4的比重。對(duì)數(shù)據(jù)進(jìn)行濾波。數(shù)據(jù)濾波加速度、羅盤(pán)錯(cuò)誤的校準(zhǔn)方式造成的結(jié)果！！數(shù)據(jù)濾波加速度、羅盤(pán)校準(zhǔn)方式數(shù)據(jù)濾波陀螺儀校準(zhǔn)方式將陀螺儀放置與靜止?fàn)顟B(tài)是采集陀螺儀的零偏值。數(shù)據(jù)濾波陀螺儀標(biāo)度變換陀螺儀量程為2000dec/秒

對(duì)應(yīng)16位有符號(hào)數(shù)32768數(shù)據(jù)濾波加速度、羅盤(pán)需要標(biāo)度變換需要嗎？不需要??！Contents

飛行器簡(jiǎn)介1飛行器的動(dòng)力學(xué)分析和建模2飛行器姿態(tài)解算3四旋翼飛行器硬件系統(tǒng)456

PID&飛行器調(diào)試介紹數(shù)據(jù)處理PID

普通PID一般有兩種：位置式PID、增量式PIDPIDCounter_KI_Out[0]+=Counter_Ki*Counter_Err[0];//累計(jì)誤差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個(gè)人經(jīng)驗(yàn)：

1、先調(diào)節(jié)P（D為零）注意分清是超調(diào)還是P參數(shù)弱了。

2、增加D參數(shù)使得系統(tǒng)收斂得到一個(gè)穩(wěn)定的狀態(tài)。

3、繼續(xù)增加D使得系統(tǒng)“抖動(dòng)”然后在繼續(xù)增加P參數(shù)這樣反復(fù)幾次，最終系統(tǒng)可以在外力作用下保持穩(wěn)定就可以。

4、盡量使用串口助手調(diào)試串級(jí)PID1、得到軸姿態(tài)的角度差（angle

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

MATLAB仿真

（1）比例環(huán)節(jié)P：控制器中的比例環(huán)節(jié)用來(lái)糾正系統(tǒng)偏差，比例項(xiàng)能夠快速的響應(yīng)偏差，但是不能消除系統(tǒng)的穩(wěn)態(tài)誤差，比例過(guò)大還會(huì)引起系統(tǒng)的不穩(wěn)定；（2）積分環(huán)節(jié)I：積分項(xiàng)用于消除系