【正文】 很大的驅(qū)動電流，控制器的驅(qū)動能力有限，因此必須選用專門的驅(qū)動芯片。由于飛行器的電機在轉(zhuǎn)動過程中會產(chǎn)生比較大的沖擊電流，所以加二極管 D D3 給電機的反電動勢提供放電回路（圖 312）。 Q1NMOS210KR4Res2GNDD1D3C1CapPWM_UPBAT12J1MOTOR 圖 312 電機驅(qū)動模塊 driver module of electric motor 電源模塊 穩(wěn)壓芯片采用低壓差線性穩(wěn)壓器 SP6205， 此芯片噪聲低 ， 紋波小 ， 電路簡單。原理圖如圖 313。其底部帶有散熱片的 DOP8/MSOP8 封裝與較少的外部引腳數(shù)目使得 TP4056 成為便攜式應(yīng)用的理想選擇。由于采用了內(nèi)部 PMOSFET 架構(gòu) ， 加上防倒充電路 ， 所以不需要外部隔離二極管。 充電電壓固定于 ， 而充電電流可通過一個電阻器進行外部設(shè)置。當(dāng)輸入電壓（交流適配器或 USB 電源）被拿掉時 ， TP4056 自動進入一個低電流狀態(tài) ， 將電池漏電流降至2uA 以下。 TP4056 的其他特點包括電池溫度檢測、欠壓閉鎖、自動再充電和兩個用于指示充電、結(jié)束的 LED狀態(tài)引腳。 T E M P1P R O G2G N D3V C C4B A T5S T D B Y J6C H R G J7C E8H E A T _ P A D9U 7T P 4 0 5 6G N DG N D1 K 2R 1 9R e s 20 . 1 u FC 3 2C a pG N DG N DD 1 1L E D _ B L U E1 K 2R 1 8R e s 2+ 5D 1 0D i o d e4 7 u FB C 3C a p P o l 1G N DNC1COM2EXT3INT4J 6 S W 4B A T12J 7L I _ B A T _ 2 P I NG N DB A T _ I NG N D 圖 314 充電模塊 charger module 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 17 電池 本設(shè)計的電池選用的是飛行航模專用的動力電池，可以提供足夠的電流來保證飛行器的飛行，參數(shù)如下： 電量： 400mAh 電池電壓： 放電倍率： 25C 充電終止電壓： 放電終止電壓： 尺寸： 39mm ? 重量： 11g 放電倍率指的是放電電流，以電池容量的倍數(shù)計算，此電池的 最大 放電電流可根據(jù)下面的公式計 算： 400mAh ? 25C = 10A，足以滿足飛行器飛行需要。STM32F103C8T6 是一款基 于 ARMv7M 體系結(jié)構(gòu)的 32 位標(biāo)準(zhǔn)處理器，具有 處理能力；擁有 USB、 USART、 SPI、 I178。主控制器通過 1 個 SPI 與無線通信模塊連接， 1 個 USART 與 USB 轉(zhuǎn)串口模塊連接，USB 轉(zhuǎn)串口模塊連接至電腦。 遙控器采用 STM32 處理器，通過 NRF24L01 無線模塊傳輸數(shù)據(jù)，并用 USB 轉(zhuǎn)串口線連接至電腦， 圖 315為 STM32 最小系統(tǒng)原理圖，圖 316 為處理器與無線模塊的接口電路。由于四 旋翼飛行器為六自由度的系統(tǒng)，而其控制量只有 4 個，這就意味著被控量 之間存在耦合關(guān)系，所設(shè)計的控制算法應(yīng)該能夠?qū)@種欠驅(qū)動系統(tǒng)足夠有效，用 4個控制量對 3個角位移和 3個線位移進行穩(wěn)態(tài)控制。 飛行控制系統(tǒng)的中央控制模塊主要完成系統(tǒng)初始化、系統(tǒng)自檢、解算傳感器數(shù)據(jù)、執(zhí)行控制算法、計算并輸出控制量等功能。 各 傳 感 器 初 始 化通 訊 模 塊 初 始 化姿 態(tài) 初 始 化控 制 初 始 化無 線 通 信 檢 測姿 態(tài) 檢 測姿 態(tài) 控 制是開 始電 池 電 壓 正 常 ？結(jié) 束否 圖 41 系統(tǒng) 程序 流程 System process 王賓： 微型四旋翼控制系統(tǒng) 20 無線通信模塊 無線通信模塊實現(xiàn)上位機對四旋翼飛行器的飛行控制和跟蹤定位，實時與上位機控制系統(tǒng)交換信息，接收上位機控制系統(tǒng)傳輸?shù)娘w行控制指令信號、向上位機控制系統(tǒng)發(fā)送當(dāng)前飛行器實時飛行和姿態(tài)數(shù)據(jù)等相應(yīng)信息，以完成指定的飛行控制任務(wù)。 STM32 控制 NRF24L01 發(fā)送數(shù)據(jù)，主要分為兩個步驟：一是 STM32 先向 NRF24L01 中寫入數(shù)據(jù)，二是控制 NRF24L01 發(fā)送數(shù)據(jù)。 NRF24L01 發(fā)送模式會自動產(chǎn)生字頭和 CRC 校驗碼，當(dāng)發(fā)送過程完成后，數(shù)據(jù)準(zhǔn)備好引腳通知 STM32 數(shù)據(jù)發(fā)送完畢。 待 機T R X _ C E = 1 ， T X _ E N = 1 ，P W R _ U P = 1初 始 化 S P I ， 模 塊 裝 載 a d d r 和發(fā) 送 數(shù) 據(jù)啟 動 發(fā) 送T R X _ C E = 1 ？生 成 C R C 和 前 導(dǎo) 碼 ， 并 發(fā) 送數(shù) 據(jù) 和 置 D R 為 1T R X _ C E = 1 ？A U T O _ T E T = 1 ?前 導(dǎo) 碼 完 成 后 D R 置 低否是是是否否待 機T R X _ C E = 1 ， T X _ E N = 0T R X _ C E = 1 ？接 收 部 分 上 電接 收 部 分 檢 測 載 波 ， 置 C D 為 高A D D R 正 確 ？A M 置 為 高接 收 數(shù) 據(jù)C R C 正 確 ？D R 置 為 高T R X _ C E = 1 ？A M 為 底A R M 從 S P I 接 收數(shù) 據(jù)否否否否是是是是 圖 42 發(fā)送數(shù)據(jù)流程圖 圖 43 接收數(shù)據(jù)流程圖 The flow diagram of send data The flow diagram of receive data 姿態(tài) 的表示 方法 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 21 坐標(biāo)系統(tǒng) 飛行器涉及兩個空間直角坐標(biāo)系統(tǒng)：地理坐標(biāo)系和機體坐標(biāo)系。四軸飛行器運動范圍小，可以不考慮地面曲率，且假設(shè)地面為慣性系。機體坐標(biāo)系原點在飛行器中心， xy 平面為電機所在平面，電機分布在 {|x|=|y|,z=0}的直線上，第一象限的電機 帶正槳， z 軸指向飛行器上方。 圖 44 四軸的模型圖 The model of four axis 姿態(tài)的運算 飛行器的姿態(tài)，是指飛行器的指向，一般用三個姿態(tài)角表示，包括偏航角 (yaw)、俯仰角 (pitch)和滾轉(zhuǎn)角 (roll)。旋轉(zhuǎn)變換有多種表示方式，包括變換矩陣、姿態(tài)角、轉(zhuǎn)軸轉(zhuǎn)角、四元數(shù)等。 王賓： 微型四旋翼控制系統(tǒng) 22 因為姿態(tài)實質(zhì)是一個旋轉(zhuǎn)變換，根據(jù)剛體有限轉(zhuǎn)動的歐拉定理，旋轉(zhuǎn)變換是可以串聯(lián)的，所以一個姿態(tài)可以經(jīng)過一個旋轉(zhuǎn)變換，變成另一個姿態(tài)。如果用矩陣表示旋轉(zhuǎn)，旋轉(zhuǎn)的串聯(lián)由矩陣乘法來實現(xiàn)。 用四元數(shù)來表示旋轉(zhuǎn)，組合旋轉(zhuǎn)時比用其他方法運算量更少，所以無論在計算機圖形學(xué)、飛行器控制等涉及剛體旋轉(zhuǎn)的領(lǐng)域，四元數(shù)都有舉足輕重的地位 [3]。 1 個四元數(shù)由 4 個實數(shù)組成： Tq q q qw x y z????q= （ 41） 規(guī)范化的四元數(shù) 可以表示旋轉(zhuǎn)，見 下 式 （ 42） ， θ 為旋轉(zhuǎn)的轉(zhuǎn)角，單位向量 ? ?Tw w wx y z為旋轉(zhuǎn)的轉(zhuǎn)軸。四元數(shù)乘法跟矩陣一樣，有結(jié)合律，但沒有交換律。 r p q p q p q p qr p q p q p q p qr p q p q p q p qr p q p q p q p qw w w x x y y z zx w x x w y z z yy w y x z y w z xz w z x y y x z w? ? ? ?? ? ? ?? ? ? ? ? ? ?? ? ? ?r p q （ 43） 四元數(shù)轉(zhuǎn)換成矩陣的函數(shù)記為 R( )， 具體過程見（ 44）式。近年來 MEMS 傳感器越來越成熟、應(yīng)用越來越廣泛，稱為低成本 姿態(tài)測量的首選器件 [4]，因此本設(shè)計使用的傳感器全部都是 MEMS 傳感器。 由于實驗條件限制，傳感器的校正有兩項，分別對兩種類型的傳感器：陀螺儀 靜止時 0 輸出的傳感器、加速度計 測量某向量場強度的傳感器。把傳感器固定好，這時對輸出值 Xf求平均值，得到的 A 即為零偏，如 （ 45） 式。實際應(yīng)用的公式如（ 46）， A 為零偏值， 31 矩陣 ，單位： LSB； Yi 為校正好的值， 31 矩陣，單位： rad/s； Xi 為測量原始值，單位： LSB；gain 為轉(zhuǎn)換系數(shù)，單位： (rad/s)/LSB，由傳感器的數(shù)據(jù)手冊給出。下面 介紹加速度計的校正 方法 。加速度計的校正的思路為：對測量值平移和縮放，把測量值擬合到重力加速度。 記測量值為 ? ?Tm m mx y z ，校正后的值為 ? ?Tc c cx y z ，平移參數(shù)為 Tx y zo o o????，縮放參 數(shù)為 Tx y zg g g????，他們之間的關(guān)系為（ 47）式。 2U u?? （ 414） 校正的具體任務(wù)為：尋找參數(shù) {a,b,c,d,e,f,g}，使 U 最小。 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計（論文） 25 2222 2 02 2 02 2 02 2 02 2 02 2 02 2 0mmmmmmUuu uxaaUuu uybbUuu uzccUuu uxddUuu uyeeUuu uzffUuuugg??? ? ?????? ? ?????? ? ?????? ? ?????? ? ?????? ? ?????? ? ????????????????? （ 415） 記 T???B V V （ 416） 由式 （ 410）、（ 411）和（ 415）： ()TTu? ? ? ? ? ? ? ? ?? ? ?V V V P V V P B P = 0 （ 417） 為了求出 P，需要解式 （ 417）的齊次線性方程組。為了減小誤差，行的處理順序有講究， 先處理待 消變量系數(shù)最大的行。解出的解帶一個任意常數(shù)，設(shè)為 C， Pb為某基礎(chǔ)解系，見（ 418）式。 數(shù)據(jù)融合 有了 傳感器的數(shù)據(jù)，就可以用來計算姿態(tài)了。而陀螺儀受外部影響弱 ，穩(wěn)定性好，但輸出量為角速度，需要積分才能得到姿態(tài)，無法避免誤差的累積。 （一） 姿態(tài)插值法 首先介紹“姿態(tài)插值法”，過程分兩部分：一、如果已知初始姿態(tài)，可以利用陀螺儀積分，不斷推算出下一個姿態(tài)，這部分動態(tài)性能好，但誤差會累積；二、利用加速度計，可以直接計算出一個姿態(tài)，這個姿態(tài)的期望是正確的，但正如上文所述，這個姿態(tài)噪聲大，不穩(wěn)定。 下面介紹“姿態(tài)插值法” 的計算過程。陀螺的輸出為時間離散的角速度，要對時間積分才得到角度。假定采樣間隔足夠短，在一個采樣時間間隔里，角速度不變，且轉(zhuǎn)角足夠小，忽略三角函數(shù)高階項，各軸間相互的影響忽略不計，根據(jù)式 (42)，該間隔里的旋轉(zhuǎn)可以用四元數(shù)表示成 12 2 2 21 ( )4 2 2 2n nTA An t x t y t zt x y z? ??? ? ? ? ? ??? ? ? ?????rg g gg g g （ 420） 更新前的姿態(tài)為1nEA?q， 1nnAAn?r為在1nEA?q的基礎(chǔ)上的旋轉(zhuǎn)，則把他們相乘，即得到新的當(dāng)前的姿態(tài)。姿態(tài)的定義為 兩個坐標(biāo)系統(tǒng)的旋轉(zhuǎn)變換，因此只要知道兩對在兩個坐標(biāo)系對應(yīng)的向量，就可以把姿態(tài)求出來。由于傳感器系統(tǒng)誤差、噪聲等影響，測量出的加速度和磁場的夾角不是恒定值，不可能精確旋轉(zhuǎn)到與常量一樣，只能求最接近的旋轉(zhuǎn)。 為了達到最接近原則，把處理的量由加速度和磁場強度，轉(zhuǎn)成它們的平面法線和角平分線。 amp。因為兩個部分算出的姿態(tài)相差小 ，用普通線性插值即可。 amp。姿態(tài)插值法的流程如圖 45。EA ac c m agq1q2q ： 四 元 數(shù) 乘 法： 其 他 運 算 圖 45 姿態(tài)插值法數(shù)據(jù)流程圖 Data flowchart of interpolation （二） 梯度下降法 從實際的使用 效果來說，姿態(tài)插值法已經(jīng)滿足要求，但用了三角函數(shù)，即使經(jīng)過優(yōu)化，運算量還是很大， 不方便應(yīng)用到低性能 MCU或提高運算頻率。其思路為：陀螺儀不斷積分姿態(tài)；同時把加速度和磁場強度的誤差表示成姿態(tài)的函數(shù)，令誤差最小的點為極值點，然后用梯度下降法逼近誤差最小的點，即糾正了陀螺的積分誤差。