講課PPT_修改后

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

2、行器飛行器用途：用途：飛行器簡(jiǎn)介飛行器簡(jiǎn)介 20122012年年2 2月，賓夕法月，賓夕法尼亞大學(xué)尼亞大學(xué)的的VijayKumarVijayKumar教授在教授在TEDTED上做出了四旋翼上做出了四旋翼飛行器發(fā)展歷史上飛行器發(fā)展歷史上里程碑式的演講。里程碑式的演講。這一場(chǎng)充滿數(shù)學(xué)公這一場(chǎng)充滿數(shù)學(xué)公式的演講居然大受式的演講居然大受歡迎，迄今已經(jīng)有歡迎，迄今已經(jīng)有三百多萬(wàn)次觀看，三百多萬(wàn)次觀看，是是TEDTED成百上千個(gè)演成百上千個(gè)演講中瀏覽量最高的講中瀏覽量最高的演講之一。演講之一。PIXPIX:ETH Zurich :ETH Zurich 實(shí)驗(yàn)室研發(fā)實(shí)驗(yàn)室研發(fā). ARM cortex_M4.

3、ARM cortex_M4處理器處理器. . 雙雙GPS GPS . Nuttx. Nuttx實(shí)時(shí)操作系統(tǒng)實(shí)時(shí)操作系統(tǒng). . 配備地面站配備地面站LilyLily: :. .采用防水結(jié)構(gòu)采用防水結(jié)構(gòu). .圖像圖像+GPS+GPS定位跟蹤定位跟蹤. .圖像軟件去抖圖像軟件去抖. .實(shí)時(shí)實(shí)時(shí)1080P1080P圖傳圖傳飛行器簡(jiǎn)介飛行器簡(jiǎn)介國(guó)內(nèi)研究現(xiàn)狀國(guó)內(nèi)研究現(xiàn)狀: : DJIDJI（大疆公司）（大疆公司）：S1000S1000六旋翼飛行器六旋翼飛行器- -內(nèi)置內(nèi)置GPSGPS導(dǎo)導(dǎo)航功能，可以在戶外很大的范圍內(nèi)飛行。航功能，可以在戶外很大的范圍內(nèi)飛行。S1000S1000提供了掛提供了掛載相機(jī)的連接

4、架，讓用相機(jī)的人們有了從天空向下的拍載相機(jī)的連接架，讓用相機(jī)的人們有了從天空向下的拍攝視角。特別地，與傳統(tǒng)的飛機(jī)和直升機(jī)航拍不同，多攝視角。特別地，與傳統(tǒng)的飛機(jī)和直升機(jī)航拍不同，多旋翼系統(tǒng)小巧靈活，能讓拍攝者自由地控制角度和距離旋翼系統(tǒng)小巧靈活，能讓拍攝者自由地控制角度和距離. .飛行器簡(jiǎn)介飛行器簡(jiǎn)介飛行器簡(jiǎn)介飛行器簡(jiǎn)介我們的目的是我們的目的是ContentsContents123456四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模K為旋翼系數(shù)，為電機(jī)轉(zhuǎn)速G=m*g重力包括了四個(gè)機(jī)體支架、電池、四個(gè)電機(jī)和旋翼、主控板等 C為阻力系數(shù)，S為旋翼面積， 為空氣密度， 為螺旋槳旋轉(zhuǎn)的速度x

5、V四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模四旋翼飛行器動(dòng)力學(xué)分析及建模KV90022222+2 d26412xmrmhMRMHIm22z42MRImd123xyzFKxxmFKyymFmgKzzm四旋翼飛行器建模四旋翼飛行器建模xyzFxmFymFm gzm42311324(

6、FF ) d(FF) dxyzIIMMMMI1123424233141324uFFFFuFFuFFuFFFF為Z 軸方向線運(yùn)動(dòng)的控制量為滾轉(zhuǎn)角與X軸方向線運(yùn)動(dòng)的控制量 為俯仰角與Y軸方向線運(yùn)動(dòng)的控制量為偏航角的控制量1u2u3u4u111234(sincoscossinsin )(sinsincoscossin )coscosxyzuxmuymumyzmu dIu dIuI角度傳遞函數(shù)為: 位置傳遞函數(shù)為: 30.905s9.7986sContentsContents123456四旋翼四旋翼飛行器硬件飛行器硬件系統(tǒng)系統(tǒng) 飛行器的動(dòng)力來(lái)源螺旋槳的反作用力：整套系統(tǒng)的執(zhí)行器包含 螺旋槳、無(wú)刷電機(jī)、

7、無(wú)刷驅(qū)動(dòng)器、電池。規(guī)格型號(hào)： 螺旋槳規(guī)格，一般由4位數(shù)字表示，前兩位數(shù)表示直徑，后兩位表示螺距。以1060漿為例，10表示槳的直徑是10英寸，60表示漿角（螺距，6.0英寸，也就是152.4mm)。四旋翼四旋翼飛行器硬件飛行器硬件系統(tǒng)系統(tǒng)槳的衡量參數(shù)：槳的平衡性。經(jīng)驗(yàn)：槳當(dāng)然是選擇APC和DJI。大多航拍還是建議選擇APC，是正品APC哦，仿的拿來(lái)試飛是個(gè)不錯(cuò)的選擇，炸了也不心疼。APC效率高。大載重的可以選擇碳纖維槳。載重很大的話可以選擇櫸木槳，不易變形。 四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)1 電機(jī)KV值：大KV配小槳，小KV配大槳。KV值是每1V的電壓下電機(jī)每分鐘空轉(zhuǎn)的轉(zhuǎn)速，例如KV

8、800,在1V的電壓下空轉(zhuǎn)轉(zhuǎn)速是800轉(zhuǎn)每分鐘。2 電機(jī)型號(hào)：定子粗的，力氣大。電機(jī)型號(hào)，如2212，3508，4010，這些數(shù)字表示電機(jī)定子的直徑和高度（如下圖）。前面兩位是定子直徑，后面兩位是定子高度，單位是毫米。四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(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)飛行器硬件系統(tǒng)電調(diào)主流品牌好盈，中特威。 把電調(diào)當(dāng)成變頻器的：電調(diào)是在將輸入的直流電，通過(guò)電調(diào)的6路開(kāi)關(guān)管不斷的在切換電流方向，輸出三相交流電，但是換相的頻率不是由電調(diào)決定的，因?yàn)殡娬{(diào)的換

9、相頻率是由電機(jī)決定，電調(diào)換相頻率必須和電機(jī)保持同步，超前的換相頻率會(huì)使電機(jī)失步停轉(zhuǎn)，滯后的換相頻率會(huì)使電機(jī)的效率很低。所以電調(diào)并沒(méi)有主動(dòng)的在調(diào)整換相頻率，而是被動(dòng)的。 四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)1.一般電調(diào)都有5V穩(wěn)壓輸出（可以為飛控、遙控器直接提供電源）最大3A2.如果沒(méi)5V輸出的 建議使用LM2575 、 LM2596 最好不要使用78053.電調(diào)驅(qū)動(dòng)方式 PWM（周期20MS 占空比4.2%9.8%）四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)1、電池容量： 5200mAh，意味著以5.2A電流放電，可以放1小時(shí)。當(dāng)然，我們只是這么理解。實(shí)際放電時(shí)間，需要參考電池廠家提供的相關(guān)

10、技術(shù)參數(shù)。2、放電能力： 30C電池，指的是電池的放電能力。對(duì)于30C電池，最大持續(xù)放電電流為：電池容量X放電C。3、電池的分類： 2S（7.2V）6S(22.2V)四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(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、650mm22*0.8/ 25.4四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)自行設(shè)計(jì)與加工的飛行器自行設(shè)計(jì)與加工的飛行器機(jī)架

11、機(jī)架四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)自行設(shè)計(jì)與加工的飛行器機(jī)架自行設(shè)計(jì)與加工的飛行器機(jī)架四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)也可直接購(gòu)買也可直接購(gòu)買F450這種機(jī)架（工程塑料）這種機(jī)架（工程塑料）實(shí)惠！實(shí)惠！四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)碳素機(jī)架：優(yōu)點(diǎn)碳素機(jī)架：優(yōu)點(diǎn) 價(jià)格高、重量輕、強(qiáng)度高價(jià)格高、重量輕、強(qiáng)度高四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)MPU6050 、HMC5883提供三個(gè)軸度上的加速提供三個(gè)軸度上的加速度、角速度、方位數(shù)據(jù)組成九軸演算度、角速度、方位數(shù)據(jù)組成九軸演算 (輸出數(shù)據(jù)精度輸出數(shù)據(jù)精度可達(dá)可達(dá)0.1) HMC5883(電子羅盤電子羅盤)MPU60

12、50 陀螺陀螺儀儀+加速度計(jì)加速度計(jì)四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)1 1、陀螺儀陀螺儀輸出的是角速度輸出的是角速度 角速度直接積分不就是角速度直接積分不就是角度了嗎？角度了嗎？為什么需要這么多的東西那為什么需要這么多的東西那？2 2、加速度加速度用來(lái)做什么？用來(lái)做什么？3 3、羅盤羅盤又用來(lái)做什么？又用來(lái)做什么？4 4、干嘛需要用地坐標(biāo)進(jìn)行描述飛行器？、干嘛需要用地坐標(biāo)進(jìn)行描述飛行器？四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng) 陀螺儀是對(duì)陀螺儀是對(duì)震動(dòng)敏感震動(dòng)敏感，也就是是說(shuō)在，也就是是說(shuō)在短時(shí)短時(shí)間間的情況下陀螺儀積分出來(lái)是很準(zhǔn)確的。但是的情況下陀螺儀積分出來(lái)是很準(zhǔn)確的。但是由于角度

13、的變化或是時(shí)間的關(guān)系會(huì)影響陀螺儀由于角度的變化或是時(shí)間的關(guān)系會(huì)影響陀螺儀的準(zhǔn)確性此時(shí)我們需要利用的準(zhǔn)確性此時(shí)我們需要利用加速度、羅盤加速度、羅盤主要主要是用來(lái)是用來(lái)修正修正陀螺儀陀螺儀。羅盤主要是確定地理坐標(biāo)。羅盤主要是確定地理坐標(biāo)四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)BMP180BMP180：為飛行器提供高度、溫度信息。為飛行器提供高度、溫度信息。超聲波超聲波：在近地面的時(shí)候可以提供更準(zhǔn)確的信息：在近地面的時(shí)候可以提供更準(zhǔn)確的信息BMP180氣壓計(jì)氣壓計(jì)超聲波模塊超聲波模塊四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng) 飛行器控制核心最好采用飛行器控制核心最好采用ARMARM內(nèi)核的單片機(jī)、內(nèi)核的

14、單片機(jī)、DSPDSP。市面。市面上的飛控一般是上的飛控一般是STM32STM32的的 cortex-M3cortex-M3、 cortex-M4cortex-M4內(nèi)核內(nèi)核( (帶帶DSPDSP浮點(diǎn)單元浮點(diǎn)單元) ) 、飛思卡爾、飛思卡爾MK60MK60等。等。 四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng) 選用處理能力強(qiáng)的單片機(jī)的選用處理能力強(qiáng)的單片機(jī)的原因原因： 1 1、計(jì)算四元數(shù)的過(guò)程中為了保證精度需要大量的浮點(diǎn)、計(jì)算四元數(shù)的過(guò)程中為了保證精度需要大量的浮點(diǎn)運(yùn)算、三角運(yùn)算。運(yùn)算、三角運(yùn)算。 2 2、一般要保證將飛行器的控制周期確定在、一般要保證將飛行器的控制周期確定在10msz10msz左右左

15、右（常用的控制系統(tǒng)保證控制周期（常用的控制系統(tǒng)保證控制周期7-15ms7-15ms都可以的）都可以的） 建議：建議：在不添加操作系統(tǒng)的情況下最好是開(kāi)定時(shí)器去控制，在不添加操作系統(tǒng)的情況下最好是開(kāi)定時(shí)器去控制，在在while(1)while(1)里面存放數(shù)據(jù)回傳、接收的函數(shù)塊。這樣既能保里面存放數(shù)據(jù)回傳、接收的函數(shù)塊。這樣既能保證數(shù)據(jù)傳輸又不會(huì)影響控制進(jìn)程。證數(shù)據(jù)傳輸又不會(huì)影響控制進(jìn)程。四旋翼四旋翼飛行器硬件系統(tǒng)飛行器硬件系統(tǒng)飛行器硬件框圖飛行器硬件框圖ContentsContents123456飛行器姿態(tài)解算飛行器姿態(tài)解算（1）地面坐標(biāo)系（OEXEYEZE）地理坐標(biāo)系一般選用地軸系。原點(diǎn)O設(shè)在

16、地面上的某一點(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)解算飛行器姿態(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。飛行器

17、姿態(tài)解算飛行器姿態(tài)解算歐拉角是基于飛機(jī)歐拉角是基于飛機(jī)本身軸旋轉(zhuǎn)得到的，本身軸旋轉(zhuǎn)得到的，但是得到的姿態(tài)卻是但是得到的姿態(tài)卻是相對(duì)于地面的參考坐相對(duì)于地面的參考坐標(biāo)系而說(shuō)的。標(biāo)系而說(shuō)的。飛行器姿態(tài)解算飛行器姿態(tài)解算1、演示、演示姿態(tài)姿態(tài) 2、講解講解代碼代碼3、講解四元數(shù)講解四元數(shù)理論理論 飛行器姿態(tài)解算飛行器姿態(tài)解算四元素轉(zhuǎn)歐拉角四元素轉(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è)空間向

18、量，就是飛行器從姿態(tài)原點(diǎn)（物體的三個(gè)歐拉角都是0的姿態(tài)），圍繞向量（ Q1，Q2，Q3 ）旋轉(zhuǎn)一個(gè)角度f(wàn)(Q0)。 飛行器姿態(tài)解算飛行器姿態(tài)解算 向量a a向量b b得到的也是一個(gè)向量，向量方向和ab都垂直，大小等于ab組成的平行四邊形面積即：向量a a向量b b =|a a|b b|sin在這里表示兩向量之間的角夾角（0 180）。知識(shí)回顧：知識(shí)回顧：飛行器姿態(tài)解算飛行器姿態(tài)解算四元數(shù)相關(guān)定理四元數(shù)相關(guān)定理定理定理1 1：令p為三維（投影）空間內(nèi)的一個(gè)點(diǎn)，用齊次坐標(biāo)將其表示成四元數(shù)的形式即為： p = (x:y:z:w) = (x, y, z), w = v, w ； 令q為任一非零四元數(shù)。

19、那么：結(jié)論結(jié)論1)1) 表達(dá)式 qpq-1的結(jié)果將使p=v v, w變換到p=vv, w， 二者模長(zhǎng)相等。結(jié)論結(jié)論2)2) 任何非零實(shí)數(shù)與q相乘，上式仍然成立。結(jié)論結(jié)論3)3) 如果上式中的q為N(q) = 1（即q為單位四元數(shù)），那么q = v v sin , cos 表示一個(gè)旋轉(zhuǎn)動(dòng)作：將p沿著單位軸v v 旋轉(zhuǎn)2即得到p。性質(zhì)：性質(zhì)：四元素四個(gè)數(shù)的平方和等于1。 飛行器姿態(tài)解算飛行器姿態(tài)解算 根據(jù)四元素第一個(gè)結(jié)論可得：根據(jù)四元素第一個(gè)結(jié)論可得： 這里這里Q Q是四元數(shù)是四元數(shù)Q Q* *是是q q的共軛，就是把的共軛，就是把ijkijk的系數(shù)取相反數(shù)。的系數(shù)取相反數(shù)。r r是旋轉(zhuǎn)前的向量，

20、是旋轉(zhuǎn)前的向量，r r就就是旋轉(zhuǎn)以后的向量。是旋轉(zhuǎn)以后的向量。飛行器姿態(tài)解算飛行器姿態(tài)解算 上述上述黃色黃色這個(gè)這個(gè)式子式子得到得到: : 在載體系定義一個(gè)矢量 ，設(shè)其在參考系中表示為 。則有 ，其中 ， 為共軛復(fù)數(shù)。*222222222222()()()0 ()2()2() 2()()2() 2()2()() nbrqr qabicjdkxiyjzk abicjdkabcdxbcad ybdac z ibcad xabcdycsab z jbdac xcdab yabcdz kbrxiyjzk*qnr*nbrqr qqabicjdk飛行器姿態(tài)解算飛行器姿態(tài)解算 用用矩陣來(lái)表達(dá)矩陣來(lái)表達(dá)成成四

21、元素旋轉(zhuǎn)矩陣四元素旋轉(zhuǎn)矩陣：用用這個(gè)這個(gè)矩陣左乘矩陣左乘需要旋轉(zhuǎn)的向量，就需要旋轉(zhuǎn)的向量，就可以得到旋可以得到旋轉(zhuǎn)后的向量了轉(zhuǎn)后的向量了。這里。這里的的旋轉(zhuǎn)，指的是同一個(gè)坐標(biāo)旋轉(zhuǎn)，指的是同一個(gè)坐標(biāo)系下，一個(gè)向量旋轉(zhuǎn)的到另一個(gè)系下，一個(gè)向量旋轉(zhuǎn)的到另一個(gè)向量向量。可是我們可是我們時(shí)間的情況時(shí)間的情況是是同一個(gè)向量，在兩個(gè)坐標(biāo)系下的不同一個(gè)向量，在兩個(gè)坐標(biāo)系下的不同同表達(dá)表達(dá)。飛行器姿態(tài)解算飛行器姿態(tài)解算四元數(shù)四元數(shù)微分方程微分方程：四元數(shù)微分方程本來(lái)只是基于四元數(shù)微分方程本來(lái)只是基于角速度角速度的，也就是的，也就是說(shuō)，已知上個(gè)周期的姿態(tài)，和本次測(cè)量得到的角速說(shuō)，已知上個(gè)周期的姿態(tài)，和本次測(cè)量得到

22、的角速度，得到本周期的姿態(tài)，在角速度里加入誤差反饋，度，得到本周期的姿態(tài)，在角速度里加入誤差反饋，來(lái)調(diào)節(jié)姿態(tài)，起到減小誤差的來(lái)調(diào)節(jié)姿態(tài)，起到減小誤差的作用作用。飛行器姿態(tài)解算飛行器姿態(tài)解算四元數(shù)四元數(shù)微分方程微分方程：00112233001020bbbxyzbbbxzybbbyzxbbbzyxqqqqqqqq 分別分別表示載體坐標(biāo)系相對(duì)于地理坐標(biāo)系表示載體坐標(biāo)系相對(duì)于地理坐標(biāo)系沿沿各個(gè)各個(gè)軸向的角度分量。軸向的角度分量。 ,bbbxyz飛行器姿態(tài)解算飛行器姿態(tài)解算四元素與歐拉角的關(guān)系四元素與歐拉角的關(guān)系 ( ( 、 為歐拉角為歐拉角 a、b、c、d 為四元素為四元素 q0、 q1、q2、q3)

23、 當(dāng)我們利用四元數(shù)表示出飛行器修正后的當(dāng)我們利用四元數(shù)表示出飛行器修正后的“姿態(tài)姿態(tài)”時(shí)，就可以反向計(jì)算出歐拉角。時(shí)，就可以反向計(jì)算出歐拉角。飛行器姿態(tài)解算飛行器姿態(tài)解算飛行器姿態(tài)角求解過(guò)程：飛行器姿態(tài)角求解過(guò)程： 1 1 把機(jī)體上的羅盤數(shù)據(jù)通過(guò)四元數(shù)把機(jī)體上的羅盤數(shù)據(jù)通過(guò)四元數(shù)旋轉(zhuǎn)矩陣轉(zhuǎn)化到地坐標(biāo)系進(jìn)行標(biāo)定。旋轉(zhuǎn)矩陣轉(zhuǎn)化到地坐標(biāo)系進(jìn)行標(biāo)定。 2 2 將標(biāo)準(zhǔn)的重力、以及標(biāo)定后的地將標(biāo)準(zhǔn)的重力、以及標(biāo)定后的地磁矢量轉(zhuǎn)換回機(jī)體坐標(biāo)系。磁矢量轉(zhuǎn)換回機(jī)體坐標(biāo)系。 3 3將步驟將步驟2 2中的矢量與加速度矢量進(jìn)中的矢量與加速度矢量進(jìn)行比較，計(jì)算誤差。行比較，計(jì)算誤差。44利用四元數(shù)微分方程將誤差迭代帶利

24、用四元數(shù)微分方程將誤差迭代帶陀螺儀數(shù)據(jù)中并修正旋轉(zhuǎn)方程。陀螺儀數(shù)據(jù)中并修正旋轉(zhuǎn)方程。55規(guī)范四元數(shù)。規(guī)范四元數(shù)。66轉(zhuǎn)換成歐拉角表示。轉(zhuǎn)換成歐拉角表示。飛行器姿態(tài)解算飛行器姿態(tài)解算姿態(tài)修正過(guò)程：姿態(tài)修正過(guò)程： 1、將機(jī)體坐標(biāo)系的電子羅盤測(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)解算姿態(tài)修正過(guò)程：姿態(tài)修正過(guò)程： 我們知道重力在地面坐標(biāo)系下向量是（0,0,1），可是地磁我們并不清楚，我們只能假設(shè)地面坐標(biāo)系x軸方向朝北，這樣的話y方向地

25、磁是0，認(rèn)為地面坐標(biāo)系下，z方向就是該向量z方向的分量，y方向是0，x方向是該向量xy平面上的投影長(zhǎng)。接下來(lái)的事就和處理重力向量一樣了。變換回機(jī)體坐標(biāo)系，再和實(shí)際測(cè)量的地磁求誤差。飛行器姿態(tài)解算飛行器姿態(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*m

26、z*(0.5f - q1q1 - q2q2);bx = sqrt(hx*hx) + (hy*hy);by=0;/人為的標(biāo)定為0bz = hz; 飛行器姿態(tài)解算飛行器姿態(tài)解算姿態(tài)修正過(guò)程：姿態(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 -

27、 q0q3) + 2*bz*(q0q1 + q2q3); wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); 飛行器姿態(tài)解算飛行器姿態(tài)解算姿態(tài)修正過(guò)程：姿態(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)解算姿態(tài)修正過(guò)程

28、：姿態(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)解算姿態(tài)修正過(guò)程：姿態(tài)修正過(guò)程： 6、歸一化處理：norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3

29、*q3); q0 = q0 * norm; q1 = q1 * norm; q2 = q2 * norm; q3 = q3 * norm;飛行器姿態(tài)解算飛行器姿態(tài)解算姿態(tài)修正過(guò)程：姿態(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;00112233001020bbbxyzbbbxzyb

30、bbyzxbbbzyxqqqqqqqq飛行器姿態(tài)解算飛行器姿態(tài)解算姿態(tài)修正過(guò)程：姿態(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

31、 飛行器姿態(tài)解算飛行器姿態(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)解算飛行器姿態(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è)量值，

32、這個(gè)值和剛剛的理論值有差別，用這個(gè)差別來(lái)糾正旋轉(zhuǎn)矩陣。程序一遍一遍的跑，這個(gè)誤差就會(huì)一點(diǎn)點(diǎn)減小了。飛行器姿態(tài)解算飛行器姿態(tài)解算MWC_ IMU代碼解讀代碼解讀 i2c_writeReg(MPU6050_ADDRESS, 0 x1C, 0 x10); /ACCEL_CONFIG +/-8G ;i2c_writeReg(MPU6050_ADDRESS, 0 x1B, 0 x18); /GYRO_CONFIG 2000 deg/secMWC 中加速度計(jì)量程為+/-8G 陀螺儀量程為2000 deg/sec 飛行器姿態(tài)解算飛行器姿態(tài)解算陀螺儀讀取函數(shù)陀螺儀讀取函數(shù)void Gyro_getADC ()

33、 i2c_getSixRawADC(MPU6050_ADDRESS, 0 x43); GYRO_ORIENTATION( (rawADC02 , (rawADC22 , (rawADC42 ); GYRO_Common();飛行器姿態(tài)解算飛行器姿態(tài)解算加速度讀取及處理函數(shù)：加速度讀取及處理函數(shù)：void ACC_getADC () i2c_getSixRawADC(MPU6050_ADDRESS, 0 x3B); ACC_ORIENTATION( (rawADC03 , (rawADC23 , (rawADC43 ); 加速度加速度數(shù)據(jù)左移了三位數(shù)據(jù)左移了三位 陀螺儀陀螺儀數(shù)據(jù)左移了兩位數(shù)據(jù)左

34、移了兩位飛行器姿態(tài)解算飛行器姿態(tài)解算 還記得我們校準(zhǔn)加速度記的時(shí)還記得我們校準(zhǔn)加速度記的時(shí)候輸出的數(shù)據(jù)為什么是候輸出的數(shù)據(jù)為什么是0,0,512 嗎？嗎？8G的量程對(duì)應(yīng)的量程對(duì)應(yīng)16位有符號(hào)數(shù)位有符號(hào)數(shù)32768，則，則1g對(duì)應(yīng)的對(duì)應(yīng)的數(shù)值應(yīng)該是數(shù)值應(yīng)該是4096，又因?yàn)椋忠驗(yàn)镸WC在采集數(shù)據(jù)以后進(jìn)在采集數(shù)據(jù)以后進(jìn)行了右移處理（右移行了右移處理（右移3位位 相當(dāng)于除以相當(dāng)于除以8）所以最后）所以最后1g 對(duì)應(yīng)對(duì)應(yīng)512飛行器姿態(tài)解算飛行器姿態(tài)解算void rotateV(struct fp_vector *v,float* delta) fp_vector v_tmp = *v; v-Z -

35、= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y; v-X += deltaROLL * v_tmp.Z - deltaYAW * v_tmp.Y; v-Y += deltaPITCH * v_tmp.Z + deltaYAW * v_tmp.X; 飛行器姿態(tài)解算飛行器姿態(tài)解算繞繞X軸旋轉(zhuǎn)軸旋轉(zhuǎn)飛行器姿態(tài)解算飛行器姿態(tài)解算繞繞Y軸旋轉(zhuǎn)軸旋轉(zhuǎn)飛行器姿態(tài)解算飛行器姿態(tài)解算繞繞Z軸旋轉(zhuǎn)軸旋轉(zhuǎn)飛行器姿態(tài)解算飛行器姿態(tài)解算一種旋轉(zhuǎn)一種旋轉(zhuǎn) 繞繞Z軸旋轉(zhuǎn)軸旋轉(zhuǎn)=yaw-繞繞X軸旋轉(zhuǎn)軸旋轉(zhuǎn)=pitch-繞繞Y軸軸旋轉(zhuǎn)旋轉(zhuǎn)=roll余弦旋轉(zhuǎn)矩陣余弦旋轉(zhuǎn)矩陣1112132

36、12223313233000zyzxyxCCCCCCCCCC飛行器姿態(tài)解算飛行器姿態(tài)解算000bibXbibYbibXbibZbibYbibZbib角增量輸出角增量輸出飛行器姿態(tài)解算飛行器姿態(tài)解算飛行器姿態(tài)解算飛行器姿態(tài)解算 int32_t sqGZ = sq(EstG32.V.Z); int32_t sqGX = sq(EstG32.V.X); int32_t sqGY = sq(EstG32.V.Y); int32_t sqGX_sqGZ = sqGX + sqGZ; float invmagXZ = InvSqrt(sqGX_sqGZ); invG = InvSqrt(sqGX_sqGZ

37、 + sqGY); angleROLL = _atan2(EstG32.V.X , EstG32.V.Z); anglePITCH = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);飛行器姿態(tài)解算飛行器姿態(tài)解算ContentsContents123456數(shù)據(jù)濾波數(shù)據(jù)濾波傳感器輸出數(shù)據(jù)中噪聲的存在（螺旋槳切割空氣時(shí)所產(chǎn)生的高頻振動(dòng)引起的）。數(shù)據(jù)濾波數(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ù)濾波數(shù)據(jù)濾波#define N 12 /隊(duì)列長(zhǎng)度unsigned char value_bufN;unsigned char filter() unsigned char I, value; int sum=0; value_bufi+ = get_ad(); /采集到的數(shù)據(jù)放入最高位 for(i=0;i450)/限制積分幅度 Counter_KI_Out0=450;else if(Counter_KI_Out00