【正文】 其思路為：陀螺儀不斷積分姿態(tài)；同時(shí)把加速度和磁場強(qiáng)度的誤差表示成姿態(tài)的函數(shù)，令誤差最小的點(diǎn)為極值點(diǎn)，然后用梯度下降法逼近誤差最小的點(diǎn)，即糾正了陀螺的積分誤差。姿態(tài)插值法的流程如圖 45。因?yàn)閮蓚€(gè)部分算出的姿態(tài)相差小 ，用普通線性插值即可。 為了達(dá)到最接近原則，把處理的量由加速度和磁場強(qiáng)度，轉(zhuǎn)成它們的平面法線和角平分線。姿態(tài)的定義為 兩個(gè)坐標(biāo)系統(tǒng)的旋轉(zhuǎn)變換，因此只要知道兩對(duì)在兩個(gè)坐標(biāo)系對(duì)應(yīng)的向量，就可以把姿態(tài)求出來。陀螺的輸出為時(shí)間離散的角速度，要對(duì)時(shí)間積分才得到角度。 （一） 姿態(tài)插值法 首先介紹“姿態(tài)插值法”，過程分兩部分：一、如果已知初始姿態(tài)，可以利用陀螺儀積分，不斷推算出下一個(gè)姿態(tài)，這部分動(dòng)態(tài)性能好，但誤差會(huì)累積；二、利用加速度計(jì)，可以直接計(jì)算出一個(gè)姿態(tài)，這個(gè)姿態(tài)的期望是正確的，但正如上文所述，這個(gè)姿態(tài)噪聲大，不穩(wěn)定。 數(shù)據(jù)融合 有了 傳感器的數(shù)據(jù)，就可以用來計(jì)算姿態(tài)了。為了減小誤差，行的處理順序有講究， 先處理待 消變量系數(shù)最大的行。 2U u?? （ 414） 校正的具體任務(wù)為：尋找參數(shù) {a,b,c,d,e,f,g}，使 U 最小。加速度計(jì)的校正的思路為：對(duì)測量值平移和縮放，把測量值擬合到重力加速度。實(shí)際應(yīng)用的公式如（ 46）， A 為零偏值， 31 矩陣 ，單位： LSB； Yi 為校正好的值， 31 矩陣，單位： rad/s； Xi 為測量原始值，單位： LSB；gain 為轉(zhuǎn)換系數(shù)，單位： (rad/s)/LSB，由傳感器的數(shù)據(jù)手冊(cè)給出。 由于實(shí)驗(yàn)條件限制，傳感器的校正有兩項(xiàng)，分別對(duì)兩種類型的傳感器：陀螺儀 靜止時(shí) 0 輸出的傳感器、加速度計(jì) 測量某向量場強(qiáng)度的傳感器。 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）式。 1 個(gè)四元數(shù)由 4 個(gè)實(shí)數(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)軸。如果用矩陣表示旋轉(zhuǎn)，旋轉(zhuǎn)的串聯(lián)由矩陣乘法來實(shí)現(xiàn)。旋轉(zhuǎn)變換有多種表示方式，包括變換矩陣、姿態(tài)角、轉(zhuǎn)軸轉(zhuǎn)角、四元數(shù)等。機(jī)體坐標(biāo)系原點(diǎn)在飛行器中心， xy 平面為電機(jī)所在平面，電機(jī)分布在 {|x|=|y|,z=0}的直線上，第一象限的電機(jī) 帶正槳， z 軸指向飛行器上方。 待 機(jī)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ù)啟 動(dòng) 發(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 置 低否是是是否否待 機(jī)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è)計(jì)（論文） 21 坐標(biāo)系統(tǒng) 飛行器涉及兩個(gè)空間直角坐標(biāo)系統(tǒng)：地理坐標(biāo)系和機(jī)體坐標(biāo)系。 STM32 控制 NRF24L01 發(fā)送數(shù)據(jù)，主要分為兩個(gè)步驟：一是 STM32 先向 NRF24L01 中寫入數(shù)據(jù)，二是控制 NRF24L01 發(fā)送數(shù)據(jù)。 飛行控制系統(tǒng)的中央控制模塊主要完成系統(tǒng)初始化、系統(tǒng)自檢、解算傳感器數(shù)據(jù)、執(zhí)行控制算法、計(jì)算并輸出控制量等功能。 遙控器采用 STM32 處理器，通過 NRF24L01 無線模塊傳輸數(shù)據(jù)，并用 USB 轉(zhuǎn)串口線連接至電腦， 圖 315為 STM32 最小系統(tǒng)原理圖，圖 316 為處理器與無線模塊的接口電路。STM32F103C8T6 是一款基 于 ARMv7M 體系結(jié)構(gòu)的 32 位標(biāo)準(zhǔn)處理器，具有 處理能力；擁有 USB、 USART、 SPI、 I178。 TP4056 的其他特點(diǎn)包括電池溫度檢測、欠壓閉鎖、自動(dòng)再充電和兩個(gè)用于指示充電、結(jié)束的 LED狀態(tài)引腳。 充電電壓固定于 ， 而充電電流可通過一個(gè)電阻器進(jìn)行外部設(shè)置。其底部帶有散熱片的 DOP8/MSOP8 封裝與較少的外部引腳數(shù)目使得 TP4056 成為便攜式應(yīng)用的理想選擇。 Q1NMOS210KR4Res2GNDD1D3C1CapPWM_UPBAT12J1MOTOR 圖 312 電機(jī)驅(qū)動(dòng)模塊 driver module of electric motor 電源模塊 穩(wěn)壓芯片采用低壓差線性穩(wěn)壓器 SP6205， 此芯片噪聲低 ， 紋波小 ， 電路簡單。直流電機(jī)是功率器件，需要很大的驅(qū)動(dòng)電流，控制器的驅(qū)動(dòng)能力有限，因此必須選用專門的驅(qū)動(dòng)芯片。如果要發(fā)送多個(gè)字節(jié)，連續(xù)發(fā)送數(shù)據(jù)位即可。當(dāng) NRF24L01 沒有被片選信號(hào)啟動(dòng)時(shí)，它的數(shù)據(jù)輸出線（ SDO）保持為高阻態(tài)。原理圖如圖 39。 CE1CSN2SCK3MOSI4MISO5IRQ6VDD17VSS18XC29XC110VDD_PA11ANT112ANT213VSS214VDD215IREF16VSS317VDD318DVDD19VSS420U6NRF24L01VCCGND12Y2XTALGND22pFC2522pFC30GNDVCC33nFC2322KR16GNDGNDC31CapC26CapC28CapC27CapL2InductorL4InductorL3InductorGND GNDGNDC24CapGNDC29CapGND12T1 TPGND1MR17Res2c24,c29,c31 無，不上ANT 天線C33Cap 圖 39 無線數(shù)據(jù)傳輸模塊 Wireless data transmission module 王賓： 微型四旋翼控制系統(tǒng) 14 極低的電流消耗當(dāng)工作在發(fā)射模式下發(fā)射功率為 6dBm 時(shí)電流消耗為 接收模式時(shí)為 。 SETP8SETC12C110NC13NC25NC36NC47NC514VDD2VDD_IO13S14SCL1SDA16DRDY15GND19GND211U2HMC5883LC3CapC4CapGNDVCC2K2R2Res22K2R3Res2GNDSCL_SENSORSDA_SENSOR 38 數(shù)字羅盤模塊 Digital pass module 無線數(shù)據(jù)傳輸模塊 無線數(shù)據(jù)傳輸模塊是四旋翼無人飛行器和控制中心之間通信的橋梁。 原理圖如圖38。 HMC5883L 包括最先進(jìn)的高分辨率 HMC118X 系列磁阻傳感器 ， 并附帶霍尼韋爾專利的集成電路包括放大器、自動(dòng)消磁驅(qū)動(dòng)器、偏差校準(zhǔn)、能使羅盤精度控制在 1176。然后，作為從設(shè)備的 MPU6050 產(chǎn)生應(yīng)答信號(hào)并開始發(fā)送寄存器數(shù)據(jù)。在第九個(gè)時(shí)鐘周期（高電平）時(shí)， MPU6050 產(chǎn)生應(yīng)答信號(hào)。數(shù)據(jù)傳輸總是以停止標(biāo)志（ P）結(jié)束，然后釋放通信線路。 R/W 位表示處理器是在接受 MPU6050 的數(shù)據(jù)還是在向其寫數(shù)據(jù)。對(duì)每一次傳輸必須伴有一個(gè)應(yīng)答（ ACK）信號(hào)，其時(shí)鐘由處理器提供，而真正的應(yīng)答信號(hào)由從 MPU6050發(fā)出，在時(shí)鐘為高時(shí)，通過拉低并保持 SDA 的值來實(shí)現(xiàn)。 當(dāng) SCL 線為高電平時(shí)， SDA 線由高到低的下降沿，為傳輸開始標(biāo)志（ S） 。連接到 I2C 接口的設(shè)備可做主設(shè)備或從設(shè)備。 微型四旋翼飛行器在某個(gè)時(shí)刻的狀態(tài)由 6 個(gè)物理量來描述，包括三維坐標(biāo)整的 3 個(gè)位置量和沿 3個(gè)軸的姿態(tài)兩（即稱為六自由度）。和所有設(shè)備寄存器王賓： 微型四旋翼控制系統(tǒng) 10 之間的通信采用 400kHz 的 I2C 接口或 1MHz 的 SPI 接口（ SPI 僅 MPU6000 可用）。 4， 177。 1000， 177。 MPU6050 對(duì)陀螺儀和加速度計(jì)分別用了三個(gè) 16 位的 ADC， 將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。 BOOT035NRST4OSC_IN/PD02OSC_OUT/PD13PA0WKUP7PA18PA1022PA1123PA1224PA13/JTMS/SWDIO25PA14/JTCK/SWCLK28PA15/JTDI29PA29PA310PA411PA512PA613PA714PA820PA921PB015PB116PB2/BOOT117PB3/JTDO30PB4/JNTRST31PB532PB633PB734VDD_119VDD_227VDD_31VDDA6VSS_118VSS_226VSS_336VSSA5U3STM32F103T8U612Y1XTALGND22pFC9Cap22pFC10Cap10kR8Res21uFC19CapVCCGNDVCCGNDARM_RSTJPWM_LEFTPWM_DOWNPWM_RIGHTPWM_UPPWM_LEFTPWM_DOWNPWM_RIGHTPWM_UPIRDA_ADBAT_ADIRDA_ADBAT_AD123P1COM DEBUG PORTTXDVCCGNDGNDVCC2K2R10Res2VCCINT_SENSORCOM_TXDINT_SENSORUSB_DUSB_D+SWD_DATASWD_CLKLEDSDA_LCDSCL_LCDRST_LCDSCL_SENSORSDA_SENSOR10KR9Res2GNDBOOT0123P2Header 32K2R13Res2VCCSCL_LCDSDA_LCDRST_LCDD9LED_RED4K7R11Res2VCC 圖 33 stm32最小系統(tǒng)原理圖 Stm32 minimum system schematic diagram 加速度計(jì)和陀螺儀模塊 MPU6050 是全球首例 9 軸運(yùn)動(dòng)處理傳感器。工作于 40℃至 +105 ℃的溫度范圍 ， 供電電壓遼寧工程技術(shù)大學(xué)畢業(yè)設(shè)計(jì)（論文） 9 至 ， 一系列的省電模式保證低功耗應(yīng)用的要求。 本系統(tǒng)分為兩大部分，第一部分為四旋翼飛行器（圖 31）。 采用場效應(yīng)管驅(qū)動(dòng)四個(gè)電機(jī)旋轉(zhuǎn)，并增加了二極管續(xù)流，防止電機(jī)反電動(dòng)勢擊穿場效應(yīng)管。 采用 NRF24L01+芯片作為系統(tǒng)的數(shù)據(jù)傳輸芯片，此芯片是一款工作在 世界通用ISM 頻段的單片無線收發(fā)器模塊。 采用霍尼韋爾的 HM5883L 芯片系統(tǒng)的羅盤芯片，此芯片是 帶有數(shù)字接口的弱磁傳感器芯片 ， 應(yīng)用于低成本羅盤和磁場檢測領(lǐng)域。 ΔT 是螺旋槳的功率變化量，為 41 矩陣，每行分別對(duì)應(yīng) 0 到 3 號(hào)螺旋槳； m 是力矩，為 31 矩陣。首先對(duì)螺旋槳編號(hào)：第一象限的為 0 號(hào)，然后逆時(shí)針依次遞增，如圖 (21) 。 螺旋槳旋轉(zhuǎn)時(shí)，把空氣對(duì)螺旋槳的壓力在軸向和側(cè)向兩個(gè)方向分解，得到兩種