【正文】 EA ac c m agq1q2q ： 四 元 數(shù) 乘 法： 其 他 運(yùn) 算 圖 45 姿態(tài)插值法數(shù)據(jù)流程圖 Data flowchart of interpolation （二） 梯度下降法 從實(shí)際的使用 效果來(lái)說(shuō)，姿態(tài)插值法已經(jīng)滿足要求，但用了三角函數(shù)，即使經(jīng)過(guò)優(yōu)化，運(yùn)算量還是很大， 不方便應(yīng)用到低性能 MCU或提高運(yùn)算頻率。 amp。假定采樣間隔足夠短，在一個(gè)采樣時(shí)間間隔里，角速度不變，且轉(zhuǎn)角足夠小，忽略三角函數(shù)高階項(xiàng)，各軸間相互的影響忽略不計(jì)，根據(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)。而陀螺儀受外部影響弱 ，穩(wěn)定性好，但輸出量為角速度，需要積分才能得到姿態(tài)，無(wú)法避免誤差的累積。 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計(jì)（論文） 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）的齊次線性方程組。下面 介紹加速度計(jì)的校正 方法 。近年來(lái) MEMS 傳感器越來(lái)越成熟、應(yīng)用越來(lái)越廣泛，稱(chēng)為低成本 姿態(tài)測(cè)量的首選器件 [4]，因此本設(shè)計(jì)使用的傳感器全部都是 MEMS 傳感器。 用四元數(shù)來(lái)表示旋轉(zhuǎn)，組合旋轉(zhuǎn)時(shí)比用其他方法運(yùn)算量更少，所以無(wú)論在計(jì)算機(jī)圖形學(xué)、飛行器控制等涉及剛體旋轉(zhuǎn)的領(lǐng)域，四元數(shù)都有舉足輕重的地位 [3]。 圖 44 四軸的模型圖 The model of four axis 姿態(tài)的運(yùn)算 飛行器的姿態(tài)，是指飛行器的指向，一般用三個(gè)姿態(tài)角表示，包括偏航角 (yaw)、俯仰角 (pitch)和滾轉(zhuǎn)角 (roll)。 NRF24L01 發(fā)送模式會(huì)自動(dòng)產(chǎn)生字頭和 CRC 校驗(yàn)碼，當(dāng)發(fā)送過(guò)程完成后，數(shù)據(jù)準(zhǔn)備好引腳通知 STM32 數(shù)據(jù)發(fā)送完畢。由于四 旋翼飛行器為六自由度的系統(tǒng)，而其控制量只有 4 個(gè)，這就意味著被控量 之間存在耦合關(guān)系，所設(shè)計(jì)的控制算法應(yīng)該能夠?qū)@種欠驅(qū)動(dòng)系統(tǒng)足夠有效，用 4個(gè)控制量對(duì) 3個(gè)角位移和 3個(gè)線位移進(jìn)行穩(wěn)態(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è)計(jì)（論文） 17 電池 本設(shè)計(jì)的電池選用的是飛行航模專(zhuān)用的動(dòng)力電池，可以提供足夠的電流來(lái)保證飛行器的飛行，參數(shù)如下： 電量： 400mAh 電池電壓： 放電倍率： 25C 充電終止電壓： 放電終止電壓： 尺寸： 39mm ? 重量： 11g 放電倍率指的是放電電流，以電池容量的倍數(shù)計(jì)算，此電池的 最大 放電電流可根據(jù)下面的公式計(jì) 算： 400mAh ? 25C = 10A，足以滿足飛行器飛行需要。由于采用了內(nèi)部 PMOSFET 架構(gòu) ， 加上防倒充電路 ， 所以不需要外部隔離二極管。由于飛行器的電機(jī)在轉(zhuǎn)動(dòng)過(guò)程中會(huì)產(chǎn)生比較大的沖擊電流，所以加二極管 D D3 給電機(jī)的反電動(dòng)勢(shì)提供放電回路（圖 312）。傳輸?shù)牡谝粋€(gè)字節(jié)為 SPI 地址，第二個(gè)字節(jié)為 SPI 數(shù)據(jù)。 本系統(tǒng)通過(guò)無(wú)線模塊發(fā)送數(shù)據(jù) ， 無(wú)線遙控器接收數(shù)據(jù)并傳送至電腦上位機(jī)。它的工作電壓時(shí) 5V，采用 USART 協(xié)議通信，通過(guò)轉(zhuǎn)換電路之后可以和 STM32的 SCI 口通信，并且自帶模數(shù)轉(zhuǎn)換，以便于微處理器直接通信。拒絕應(yīng)答信號(hào)（ NACK）產(chǎn)生定義為 SDA 數(shù)據(jù)在第 9 個(gè)時(shí)鐘周期一直為高。綜上可知，所有的 SDA 信號(hào)變化都要在 SCL 時(shí)鐘為低電平時(shí)進(jìn)行，除了開(kāi)始和結(jié)束標(biāo)志。當(dāng) MPU6050 空閑后，并且釋放時(shí)鐘線，原來(lái)的數(shù)據(jù)傳輸才會(huì)繼續(xù)。 當(dāng) MPU6050 連接到處理器時(shí)， MPU6050 作為從設(shè)備。另外 ， 片上還內(nèi)嵌了一個(gè)溫度傳感器和在工作環(huán)境下僅有177。 /秒（ dps） ， 加速度計(jì)可測(cè)范圍為177。擴(kuò)展之后就可以通過(guò)其 I2C 或 SPI接口輸出一個(gè) 9 軸的信號(hào)（ SPI 接口僅在 MPU6000 可用）。 王賓： 微型四旋翼控制系統(tǒng) 8 M C US T M 3 2 F 1 0 3M P U 6 0 5 0 傳 感 器N R F 2 4 L 0 1 無(wú) 線 模 塊H M C 5 8 8 3 L 傳 感器電 源電 機(jī) X 4 圖 31 四旋翼飛行器原理框圖 Four rotor aircraft principle block diagram N R F 2 4 L 0 1無(wú) 線 模 塊電 源電 腦 U S B 端 口上 位 機(jī) 顯 示U S B 轉(zhuǎn) 串 口 模 塊M C US T M 3 2 F 1 0 3 圖 32 無(wú)線遙控器原理框圖 Wireless controller principle diagram 四旋翼飛行器 微控制器模塊 本 設(shè)計(jì) 采用 STM32F103 增強(qiáng)型系列使用高性能的 ARM/CortexM3/32 位的 RISC 內(nèi)核 ，工作頻率為 72MHz， 內(nèi)置高速存儲(chǔ)器 (高達(dá) 128K 字節(jié)的閃存和 20K 字節(jié)的 SRAM)， 豐富的增強(qiáng) I/O 端口和聯(lián)接到兩條 APB 總線的外設(shè)。輸出功率、頻道選擇和協(xié)議都可以由用戶進(jìn)行設(shè)置。 1 1 1111 d ia g ( , , )1 1 11 1 1x y zT m m m?????? ? ? ?????????m （ 21） 各個(gè)電機(jī)實(shí)際輸出的功率記為 outputT ，推力油門(mén)對(duì)應(yīng)的功率量為 baseT ，則有： output baseT T T? ?? （ 22） 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計(jì)（論文） 7 3 系統(tǒng)硬件 設(shè)計(jì) 系統(tǒng)總體介紹 本系統(tǒng)采用 STM32F103 處理器作為主控芯片，此處理器是基于 arm 內(nèi)核的 cortexm3系列處理器，具有運(yùn)算速度快，內(nèi)置高速緩存 ， 3個(gè)通用 16 位定時(shí)器和一個(gè) PWM 定時(shí)器 以及豐富的外設(shè)和 I/O 等特點(diǎn)。當(dāng)飛行器運(yùn)動(dòng)時(shí)，因?yàn)橥屏χ荒苎剌S向，所以只能通過(guò)傾斜姿態(tài)來(lái)提供水平的動(dòng)力，控制運(yùn)動(dòng)由控制姿態(tài)來(lái)間接實(shí)現(xiàn)。此 IMU 存在一定程度的漂移 ，但足以保證飛行器盤(pán)旋狀態(tài)下的穩(wěn)定。該項(xiàng)目對(duì)四旋翼飛行器高度進(jìn)行實(shí)時(shí)估測(cè)時(shí) ， 使用一個(gè)卡爾曼 濾波器。最近 ， 四旋翼飛行器新添加了一個(gè)板載攝像機(jī) ， 聯(lián)合地面的攝像機(jī)來(lái)估計(jì)飛行器的位置。三個(gè)板載傳感器用來(lái)提供飛行器內(nèi)環(huán)穩(wěn)定控制的數(shù)據(jù)。 X4 Flyers 研制人員稱(chēng)商用 IMU 較大的重量會(huì)影響飛行器的性能 ， 因此最終采用 EiMU。最新出品的 Draganflyer 還包括四個(gè)紅外熱傳感裝置來(lái)幫助飛行器在室外飛行時(shí)的平衡。此飛行器的每個(gè)旋翼直徑達(dá) 19 英尺 ， 并通過(guò)用兩個(gè)引擎來(lái)改變每個(gè)旋翼的升力來(lái)控制飛行器。 1923 年 1 月 19 日的另一次試飛 ， 飛行器將兩人抬至 米的高度。雖然自主飛行并未實(shí)現(xiàn) ， 但同時(shí)使用順時(shí)針旋轉(zhuǎn)旋翼和逆時(shí)針旋轉(zhuǎn)旋翼的思想是 BreguetRichet 四旋翼飛行器的顯著特點(diǎn)。飛行器可以飛至離目標(biāo)更近的區(qū)域 ， 而不像傳統(tǒng)直升機(jī)由于其巨大的單旋翼而不能近距離靠近目標(biāo)。而旋翼式飛行器與固定翼飛行器相比 ， 其優(yōu)勢(shì)還包括：飛行器起飛和降落所需空間少 ， 在障礙物密集環(huán)境下的可控性強(qiáng) ， 以及飛行器姿態(tài)保持能力高。由國(guó)際無(wú)人運(yùn)輸系統(tǒng)協(xié) 會(huì) (International Association for Unmanned Vehicle Systems）組織的一年一度的國(guó)際空中機(jī)器人競(jìng)賽 (International Aerial Robotics Competition),為自主旋翼式飛行器的應(yīng)用潛力研究提供了一個(gè)很好的展示平臺(tái)。 同 時(shí) ， 小型四旋翼飛行器研究也為自動(dòng)控制 ， 先進(jìn)傳感技術(shù)以及計(jì)算機(jī)科學(xué)等諸多領(lǐng)域的融合研究提供了一個(gè)平臺(tái)。 1921 年 1 月 ， 美國(guó)空軍軍團(tuán)（ US Army Air Corps）與 Gee de Bothezat 和 Ivan Jerome 簽訂合約共同建造垂直飛行器。截止 1923 年底 ， 該飛行器于俄亥俄州代頓市共試飛約 100 次。經(jīng)試飛驗(yàn)證 ， Convertawings 飛行器在空中飛行性能良好 ， 但由于 當(dāng)時(shí)人們對(duì)此種飛行器缺乏興趣而停止生產(chǎn)。 EADS Quattrocopter 原本是用來(lái)當(dāng)作研制微型飛行器控制單元的測(cè)試平臺(tái) ， 而如今因其良好的性能被工業(yè)界大量生產(chǎn)。一個(gè)雙核的板載計(jì)算機(jī)用來(lái)紀(jì)錄來(lái)自 R/C 接收器的輸入命令 ， IMU 上的串行接口是數(shù)據(jù)以 120Hz 的頻率被紀(jì)錄下來(lái) ， 而另一個(gè)串行接口用來(lái)實(shí)現(xiàn) IMU 和地面計(jì) 算系統(tǒng)之間的通信。置于地面的攝像機(jī)作為主傳感器來(lái)使用。在原先有地面攝像機(jī)觀測(cè)的五個(gè)標(biāo)記的基礎(chǔ)上 ， 另一個(gè)標(biāo)記被至于地面攝像機(jī)上 ， 供板載攝像機(jī)觀測(cè)。此濾波器主要用于保留高頻的傳感器數(shù)據(jù)（ 300Hz ）和低頻的視頻信號(hào)（ 10Hz） ， 并過(guò)濾掉其他頻率成分的干擾信號(hào)。 遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計(jì)（論文） 5 2 微型四旋翼飛行器結(jié)構(gòu)和控制原理簡(jiǎn)介 目前飛行 器 控制方式 主要有：遙控飛行、自主飛行以及半自主飛行三種方式。 0號(hào)3號(hào)1號(hào)2號(hào) 圖 21 四旋翼飛行器旋翼旋轉(zhuǎn)方向示意圖 Four rotor aircraft rotor rotation direction 王賓： 微型四旋翼控制系統(tǒng) 6 假設(shè)四軸為剛體，根據(jù)質(zhì)點(diǎn)系動(dòng)量矩定理，角速度和角加速度由外力矩決定 [2]，通過(guò)控制四個(gè)螺旋槳，可以產(chǎn)生需要的力矩。 采用 MPU6050 作為姿態(tài)測(cè)量芯片，此芯片為系統(tǒng)的核心芯片，它 集成了 3軸 MEMS 陀螺儀 ， 3 軸 MEMS 加速度計(jì) ，對(duì) 陀螺儀和加速度計(jì)分別用了三個(gè) 16 位的 ADC， 將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了減小體積和減輕重量，本系統(tǒng)把無(wú)線通信模塊固化在了 PCB 上。所有型號(hào)的器件都包含 2個(gè) 12位的 ADC、 3個(gè)通用 16 位定時(shí)器和一個(gè) PWM 定時(shí)器 ， 還包含標(biāo)準(zhǔn)和先進(jìn)的通信接口：多達(dá) 2 個(gè) I2C 和SPI、 3 個(gè) USART、一個(gè) USB 和一個(gè) CAN。 MPU6050也可以通過(guò)其 I2C 接口連接非慣性的數(shù)字傳感器 ， 比如壓力傳感器。 2，177。 1%變動(dòng)的振蕩器。 MPU6050 的 Slave 地址為 b110110X，7 位字長(zhǎng)，最低有效位 X由 A0 管教上的邏輯電平?jīng)Q定，本設(shè)計(jì) A0 接地，所以設(shè)備地址為b1101100。 SD A T A O U T P U T B Y T R A N S M I T T E R ( S D A )S C L F R O M M A S T E RS D A1 2 8 9D A T A O U T P U T B Y R E C E I V E R ( S D A )n o t a c k n o w l e d g e