【正文】 這個方法，陀螺計算的姿態(tài)占主要，加速度計和羅盤只是輔助糾正。 Sebastian . Madgwick提出了更加有效的方法 —— 用梯度下降法計算姿態(tài) [5]。 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 29 陀 螺 儀加 速 度 計 叉 積羅 盤 對 角 線1z?1nnAAn? r1nEA ? q nEA gy rq nEA qamp。, ( 1 )nnE E EA A a c c m a g A g y raa? ? ? ? ? ?tq t q qt （ 432） nEAq即為輸出的姿態(tài)。 α為插值系數(shù)，范圍在 [0,1]，越接近 0，第一部分的姿態(tài)占的權(quán)重就越大，一般取接近 0 的值。 2 1EA acc mag ??q q q （ 431） 把兩個部分算出的姿態(tài)進(jìn)行插值，就可以得到當(dāng)前的姿態(tài)。 記加速度計輸出為 Ama ，羅盤輸出為 Amh ，規(guī)范化的等效重力加速度為? ?0 0 1E Tc ?a ，規(guī)范化的地磁場強度為 0E Tc EEyz??? ??hhh 測量的加速度和磁場強度的平面法線向量和對角線向量為： AAA mmmAAmm??ahc ah （ 422） AAAmmmAAmm??ahd ah （ 423） 等效重力加速度敞亮和地磁場強度常量的平面法線向量和對角線為： E E E ccc??c a h （ 424） E E E ccc??d a h （ 425） 定義一個函數(shù)，用于把旋轉(zhuǎn)軸和轉(zhuǎn)角轉(zhuǎn)換成四元數(shù)： ( , )qu ate rn ion ??r ? 王賓： 微型四旋翼控制系統(tǒng) 28 co s( )2sin( )2sin( )2sin( )2rrrrwxxyyzz?????????????? （ 426） 再定義一個函數(shù)，用于獲取把一個向量旋轉(zhuǎn)到另一個向量的旋轉(zhuǎn)四元數(shù)： ( , ) ( , a r c t a n ( , ) )r o ta te f t q u a te r n io n f t f t f t? ? ? ? ?r （ 427） 第一次旋轉(zhuǎn)，把平面法線旋轉(zhuǎn)到重合： 1 ( , )AEmmro ta te?q c c （ 428） 經(jīng)過第一次旋轉(zhuǎn)，資格向量已經(jīng)共面，測量的加速度和磁場強度的對角線向量變成： 1()AAmmR? ??d q d （ 429） 再進(jìn)行第二次旋轉(zhuǎn)，把對角線重疊在一起： 2 ( , )AEmcrotate ??q d d （ 430） 把兩次旋轉(zhuǎn)組合起來，就是第二部分所求的姿態(tài)了。最接近的原則可定為：一、旋轉(zhuǎn)后四個向量共面；二、加速度和磁場成一定角度，旋轉(zhuǎn)后的角平分線與常量的角平分線重合。兩對變量為加速度和磁場強度，測量出的加速度和 磁場強度是在機體坐標(biāo)系的，而地理坐標(biāo)系的加速度和磁場是常量，存在一個旋轉(zhuǎn)，可以把機體坐標(biāo)系的加速度和磁場強度轉(zhuǎn)換到與地理坐標(biāo)系的對應(yīng)的常量重合，這個旋轉(zhuǎn)就是所求的姿態(tài)。 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 27 11 nn n nAEEA gyr A A????q q r （ 421） 然后是第二個部分。記陀螺輸出的角速度為 Tg g gx y z??? ??g ，單位為 rad/s，采樣間隔為 t? 。 首先是第一個部分 陀螺儀積分 算姿態(tài)。為了把兩個系統(tǒng)的優(yōu)點結(jié)合起來，對兩個系統(tǒng)的結(jié)果進(jìn)行插值，得到的值作為當(dāng)前的姿態(tài)。為了得到穩(wěn)定的、近實時的姿態(tài)，對各傳感器的數(shù)據(jù)取長補短，需要研究各種數(shù)據(jù)融合算法。加速度計測量對象為比力，受運動加速度的影響很大， 特別是受飛行器機架振動的影響。 ? ? Tb b b b b b b bC C a C b C c C c C e C f C g? ? ? ? ? ? ? ? ? ?PP （ 418） 由 （ 411） 、（ 412）和（ 418）得到 6 個校正參數(shù)： 222bxbbybbzbdoaeobfoc??? 王賓： 微型四旋翼控制系統(tǒng) 26 22 2 24xbybzbb b bbb b bg C ag C bg C cGCd e fga b c???????? ? ? （ 419） 所以校正加速度計的整體流程為：測量一批靜態(tài)數(shù)據(jù)，然后用這批數(shù)據(jù)根據(jù) （ 416）式生成方陣 B，然后 求方程 （ 417） 的近似解 P，再代入式（ 419），得到校正參數(shù)。當(dāng)處理到最后一個變量時，系數(shù)已經(jīng)變得很小了，強制令為 0，以釋放一個自由度，這樣就有非零解了。由于數(shù)據(jù)噪聲、運算精度限制等原因，方程（ 417）極少會出現(xiàn)非零解，所以轉(zhuǎn)向求近似解，解法為經(jīng)典的高斯消元法。因為 U 是 {a,b,c,d,e,f,g}的多項式函數(shù)，使 U最小的點必 為極值點，一階偏 導(dǎo) 為 0，得（ 415）式。 王賓： 微型四旋翼控制系統(tǒng) 24 ()()()c m x xc m y yc m z zx x o gy y o gz z o g? ? ?? ? ?? ? ? （ 47） 定義誤差 u 為測量值長度與重力加速度常數(shù) G 的平方差 2 2 2 2c c cu x y z G? ? ? ? （ 48） 把公式（ 47）代入 （ 48），得： 2 2 2 2 2 2 2 2 22 2 2 2 2 2 2222x m x x m x z y m y ym y y z m z z m z zu g x o y x o g g y o gy o g g z o g z o g G? ? ? ? ?? ? ? ? ? （ 49） 記 2 2 2 1 Tm m m m m mx y z x y z??? ??V （ 410） ? ?Ta b c d e f g?P （ 411） 2222222 2 2 2 2 2 2222xyzxxyyzzx x y y z zagbgcgd o ge o gf o gg o g o g o g G??????? ? ? ? （ 412） 則 u 可以表示成 2 2 2 Tm m m m m mu a x b y c z d x e y fz g? ? ? ? ? ? ? ? ?VP （ 413） 設(shè)目標(biāo)函數(shù) U，用來衡量整體誤差，這里用單個誤差的平方和。因此校正的任務(wù)為：尋找最佳的平移和縮放參數(shù)，使總體測量 的 數(shù)據(jù) 值更接近 重力加速度。 加速度計測量的對象是比力，也就是等效重力加速度和運動加速度的和，當(dāng)靜止時，運動加速度為 0，加速度計的測量值為等效重力加速度，可以利用這一點校正加速度計。 1 fnn? ?AX （ 45） ()ii gain? ? ?Y X A （ 46） （二） 加速度計 加速度計 是測量所在點的 重力 場的值的傳感器，靜態(tài)時加速度計測的是等效重力加速度場。實際使用時，把測量得到的值減去零偏，得到的值就是校正值。 傳感器的校正 （一） 陀螺儀 對于陀螺儀等靜止時 0 輸出的傳感器，可以很方便地校正零偏。在使用傳感器的值進(jìn)行測量姿態(tài)之前，有必要校正傳感器。 2222221 2 2 2 2 2 2( ) 2 2 1 2 2 2 22 2 2 2 1 2 2q q q q q q q q q qq q q q q q q q q qq q q q q q q q q qy z x y w z x z w yR x y w z x z y z w xx z w y y z w x x y??? ? ? ???? ? ? ? ?? ? ? ???q （ 44） 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 23 姿態(tài)測量 獲取當(dāng)前姿態(tài)是控制飛行器平穩(wěn)飛行的基礎(chǔ)，姿態(tài)的測量要求低噪聲、高輸出頻率，當(dāng)采用陀螺儀等需要積分的傳感器時，還需要考慮積分誤差等問題。運算過程見 （ 43） 式。 cos ( )2sin( )2sin( )2sin( )2qqwqwqwwxxyyzz??????????? （ 42） 記四元數(shù)乘法的符號為 ? 。飛行器的姿態(tài)計算是圍繞姿態(tài) 四元數(shù)進(jìn)行的，下面簡要介紹一下四元數(shù)的運算。如果用四元數(shù)表示旋轉(zhuǎn)，則由四元數(shù)的乘法來實現(xiàn)旋轉(zhuǎn)串聯(lián)。類比點和向量的概念，姿態(tài)相當(dāng)于點，旋轉(zhuǎn)相當(dāng)于向量，點可以通過加向量，變成另一個點。在本文中，矩陣用加粗大寫字母表示，如 EAR ，左上標(biāo)和左下標(biāo)表示從機體坐標(biāo)系 (Aircraft)變換到地理坐標(biāo)系 (Earth)；四元數(shù)用加粗小寫字母表示，如 EAq ，上下標(biāo)意義與變換矩陣一樣；向量用帶箭頭加粗小寫字母表示，如 Av ，左上標(biāo) A 表示向量 的值是在機體坐標(biāo)系的坐標(biāo)值。更深一層，姿態(tài)其實是一個旋轉(zhuǎn)變換，表示機體坐標(biāo)系與地理坐標(biāo) 系的旋轉(zhuǎn)關(guān)系，這里定義姿態(tài)為機體坐標(biāo)系向地理坐標(biāo)系的轉(zhuǎn)換。如圖 44 所示。地理坐標(biāo)系采用“東北天坐標(biāo)系”， X 軸指向東，為方便羅盤的使用， Y 軸指向地磁北， Z 軸指向天頂。地理坐標(biāo)系是固連在地面的坐標(biāo)系，機體坐標(biāo)系是固連在飛行器上的坐標(biāo)系。 NRF24L01 的發(fā)送和接收流程如圖 4圖 43。 在執(zhí)行過程中，本設(shè)計先寫入發(fā)送數(shù)據(jù)的目標(biāo)地址再寫入數(shù)據(jù)，然后再控制 NRF24L01 發(fā)送地址和數(shù)據(jù)。 STM32 通過 SPI接口與 NRF24L01 通信，將 ROM 中的數(shù)據(jù)發(fā)送出去。系統(tǒng)程序流程如圖 41 所示。本設(shè)計首先通過姿態(tài)傳感器的數(shù)據(jù)解算出飛行器的姿態(tài)，然后適當(dāng)?shù)倪x取控制量，運用控制理論中經(jīng)典的 PID 控制算法對飛行器系統(tǒng)進(jìn)行控制。 GND1VCC2CE3CSN4SCK5MOSI6MISO7IRQ8NRF24L01GND+PB10PA5PA6PA4PA7PB5 圖 316 NRF24L01接口原理圖 The schematic of Nrf24l01 interface 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 19 4 系統(tǒng)軟件設(shè)計 總體設(shè)計 四旋翼飛行器控制系統(tǒng)軟件設(shè)計的總體目標(biāo)是啟動飛行器 控制系統(tǒng)的各個功能模塊并使之正常工作，按照既定規(guī)劃實現(xiàn)穩(wěn)定飛行。 本設(shè)計通過上位機顯示并控制 飛行器的飛行姿態(tài) ， 遙控器接收來自飛行器的姿態(tài)信號 ，并把信號傳送至上位機 ， 上位機能夠顯示實時地顯示飛行器姿 態(tài) ，同時上位機能夠控制 飛行器飛行。C等共 9個外設(shè)接口，由圖 310可知， STM32F103C8T6處理器的外圍電路僅需要外部晶振和少量電容即可，內(nèi)部自帶的 USB 接口便于調(diào)試和程序下載。 無線遙控器 V B A TP C 1 3P C 1 4P C 1 5O S C _ I NO S C _ O U TN R S TG N DV D D AP A 0P A 1P A 2PA3PA4PA5PA6PA7PB0PB1BOOT1PB10PB11GND+3.3V+ 3 . 3 VG N DJ T A G _ T M SU S B _ D PU S B _ D MU 1 _ R XU 1 _ T XP A 8P B 1 2P B 1 3P B 1 4P B 1 5JTAG_TCKJTAG_TDIJTAG_TDOJTAG_