四軸實(shí)用教程

1、 小四軸飛行器制作過(guò)程電子1201 俞強(qiáng) 吳文杰 注：本文只講我們做四軸遇到的實(shí)際的真正困擾我們的一些問(wèn)題以及我們自身的經(jīng)驗(yàn)，可以結(jié)合我們的報(bào)告來(lái)看，對(duì)于前人的經(jīng)驗(yàn)以及我們覺(jué)得對(duì)我們有幫助的文章或帖子以及一些理論知識(shí)將在每一部分的講解中給出，對(duì)于理論，本文盡量少講甚至不講，建議在看本文的每個(gè)部分的時(shí)候，寫把給出的參考資料閱讀一番。一飛行器大體結(jié)構(gòu)1.飛行控制電路（飛控），稍后介紹，是飛行器設(shè)計(jì)的最關(guān)鍵部分。2.電機(jī)+槳：720空心杯電機(jī)，注意不人為要阻止電機(jī)轉(zhuǎn)動(dòng)，很容易損壞。720表示電機(jī)直徑：7 MM ；電機(jī)長(zhǎng)度：20 MM。根據(jù)飛行器的具體大小以及飛行測(cè)試的實(shí)際情況自行選擇其他規(guī)格電機(jī)。槳

2、要注意有正槳和反槳的區(qū)別，見(jiàn)下圖：注意：正槳逆時(shí)針轉(zhuǎn)動(dòng)，反槳逆時(shí)針轉(zhuǎn)動(dòng)。如果不確定，可以在安裝好電機(jī)后，開(kāi)電源看看是不是四個(gè)槳風(fēng)都是向下吹的。對(duì)于相同的電機(jī)，槳的大小不同，飛行效果也不一樣，小槳效率低，但抗風(fēng)性能好。大槳效率高（就是省電），但抗風(fēng)性能差。3.電池： 400mah，3.7v，20c鋰電池基本知識(shí):1) 1000mAh的意思是指充滿電的情況下用1安培的電流放電, 可以放電一個(gè)小時(shí).400mAh的電池在在1安培電流充放電情況下, 充放電時(shí)間大約是25分鐘. 如果采用4安培的電流放電, 放電時(shí)間大約在6分鐘(25/4)左右.2) 20c指放電倍率。放電倍率指的是放電電流, 以電池容量的

3、倍數(shù)計(jì)算. 上述電池的放電電流可根據(jù)下面的公式計(jì)算:400mAhX25C=10A。4. X 型四軸和 + 型四軸電機(jī)安放的區(qū)別：其實(shí)是要注意傳感器mpu6050和電機(jī)的放置，因?yàn)閭鞲衅魇怯休S向的，圖中大框?yàn)檎麎K飛控，小框?yàn)閙pu6050，飛行方向?yàn)樯舷伦笥?。X型四軸飛控和電機(jī)臂的安放：+ 型四軸：二飛控電路1.飛控電路主要器件:1）mcu的選擇：對(duì)于小四軸mcu實(shí)際上選擇很多，只要滿足：最少4路PWM,盡量有最少一路AD轉(zhuǎn)換，盡量有硬件spi和i2c接口，頻率夠快即可。如avr， msp430，stm32等。還有人用stc產(chǎn)的增強(qiáng)型51單片機(jī)也做出來(lái)了，可以去看看2）mpu6050：mpu60

4、50集成了三軸加速度計(jì)和三軸陀螺儀，使用起來(lái)更方便，軸向融合度更高。下面先來(lái)介紹陀螺儀和加速度計(jì)（可參考MPU6050原始數(shù)據(jù)分析）。陀螺儀：陀螺儀的作用是測(cè)量載體繞x，y，z三個(gè)軸轉(zhuǎn)動(dòng)的角速度；角速度對(duì)時(shí)間積分就可以得到繞三個(gè)軸轉(zhuǎn)過(guò)的角度。零點(diǎn)漂移：到飛機(jī)靜止不動(dòng)時(shí)，讀取陀螺儀的數(shù)據(jù)，發(fā)現(xiàn)繞三個(gè)軸旋轉(zhuǎn)的角速度不為0。由于零點(diǎn)漂移的存在，假設(shè)x軸的零漂為1度，積分360次后，累積的誤差為360度，顯然這是不合理的，因此傳感器的讀數(shù)需要進(jìn)行處理。加速度計(jì)：測(cè)量的是物體的加速度，當(dāng)物體靜止或做勻速運(yùn)動(dòng)時(shí)，加速度計(jì)的輸出是重力加速度在x，y，z三個(gè)軸上的分量。當(dāng)飛機(jī)做加速運(yùn)動(dòng)時(shí)，此時(shí)，顯然無(wú)法正確測(cè)

5、量重力加速度。因此加速度計(jì)的讀數(shù)也需要處理。下面介紹mpu6050：MPU-60X0 是全球首例 9 軸運(yùn)動(dòng)處理傳感器。它集成了 3 軸 MEMS 陀螺儀，3 軸 MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器 DMP（Digital Motion Processor），可用 I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過(guò)其 I2C 或 SPI 接口輸出一個(gè) 9 軸的信號(hào)（SPI 接口僅在 MPU-6000 可用）。MPU-60X0 也可以通過(guò)其 I2C 接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-60X0 對(duì)陀螺儀和加速度計(jì)分別用了三個(gè) 16 位的 ADC，將

6、其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶可控的，陀螺儀可測(cè)范圍為±250，±500，±1000，±2000°/秒（dps），加速度計(jì)可測(cè)范圍為±2，±4，±8，±16g。由于我們不使用它的dmp功能（該功能可以直接輸出飛行器的姿態(tài)數(shù)據(jù)），因此，我們簡(jiǎn)單介紹幾個(gè)常用的寄存器，詳細(xì)說(shuō)明請(qǐng)看芯片手冊(cè)和寄存器手冊(cè)：1.REGISTER 26 CONFIGURATION。EXT_SYNC_SET的值確定采樣的值將代替?zhèn)鞲衅鲾?shù)據(jù)寄存器中的最低有效位。替換如下表所示：D

7、LPF(數(shù)字低通濾波器)由DLPF_CFG 配置。加速度計(jì)和陀螺儀根據(jù)DLPF_CFG 的值被過(guò)濾。下表顯示過(guò)濾情況：2. REGISTER 25SAMPLE RATE DIVIDER。該寄存器用于 MPU-60X0的陀螺儀采樣頻率輸出設(shè)置。傳感器寄存器輸出，F(xiàn)ITO輸出，DMP采樣，Motion檢測(cè)，ZeroMotion檢測(cè)和 Free Fall檢測(cè)都基于這個(gè)采樣頻率。配置后SMPLRT_DIV=0。采樣頻率=（陀螺儀或加速度計(jì)等的）輸出頻率/（1+SMPLRT_DIV）。3 REGISTER 27 GYROSCOPE CONFIGURATION這寄存器是用來(lái)觸發(fā)陀螺儀自檢和配置陀螺儀的滿量

8、程范圍。量程設(shè)為±2000度/s,即FS_SEL=3。XG_ST 該位置位 X軸進(jìn)行自檢。YG_ST 該位置位 Y軸進(jìn)行自檢。ZG_ST 該位置位 Z軸進(jìn)行自檢。FS_SEL 2位無(wú)符號(hào)數(shù)值.。選擇陀螺儀的滿量程范圍。4 REGISTER 28ACCELEROMETER CONFIGURATION這寄存器是用來(lái)觸發(fā)加速度計(jì)自檢和配置加速度計(jì)的滿量程范圍。這個(gè)寄存器也可以用于配置數(shù)字高通濾波器（DHPF）。量程設(shè)為±8g，即AFS_SEL=2.AFS_SEL 用于選擇加速度計(jì)的滿量程范圍，如下表：XA_ST 該位置 1，加速度計(jì)的 X軸執(zhí)行自檢。YA_ST 該位置 1，加速度

9、計(jì)的Y軸執(zhí)行自檢。ZA_ST 該位置 1，加速度計(jì)的 Z軸執(zhí)行自檢。AFS_SEL 2位無(wú)符號(hào)值。 選擇加速度計(jì)的滿量程范圍。5.REGISTER 107 POWERMANAGEMENT 1。這個(gè)寄存器允許用戶配置電源模式和時(shí)鐘源。配置后喚醒mpu6000（設(shè)備剛上電處于睡眠模式，因此需要喚醒）。時(shí)鐘選擇內(nèi)部時(shí)鐘源的PLL with Z axis gyroscope reference。DEVICE_RESET=0， 該位置 1，重啟內(nèi)部寄存器到默認(rèn)值。復(fù)位完成后該位自動(dòng)清0。SLEEP=0， 該位置 1，MPU-60X0進(jìn)入睡眠模式。CYCLE 當(dāng)失能 SLEEP且 CYCLE位置 1，MP

10、U-60X0進(jìn)入循環(huán)模式。在循環(huán)模式，設(shè)備在睡眠模式和喚醒模式間循環(huán)，根據(jù) LP_WAKE_CTRL（寄存器 108）設(shè)定的速率從加速度計(jì)采集樣品數(shù)據(jù)。TEMP_DIS=0， 該位置 1，失能溫度傳感器。CLKSEL =3，3位無(wú)符號(hào)數(shù)值。指定設(shè)備的系統(tǒng)時(shí)鐘源。7.REGISTER 106 USER CONTROL這個(gè)寄存器允許用戶使能或失能 FIFO緩沖區(qū)，I2C主機(jī)模式和主要 I2C接口。FIFO緩沖區(qū)，I2C主機(jī)，傳感器信號(hào)通道和傳感器寄存器也可以使用這個(gè)寄存器復(fù)位。當(dāng) I2C_MST_EN置 1，I2C主機(jī)模式使能。在這個(gè)模式下，MPU-60X0作為 I2C主機(jī)通過(guò)輔助I2C總線連接外

11、部傳感器從機(jī)設(shè)備。當(dāng)該位清0，輔助I2C總線（AUX_DA 和AUX_CL）邏輯上由主 I2C總線（SDA和 SCL）驅(qū)動(dòng)。FIFO_EN=0，失能 FIFO緩沖區(qū)。 FIFO緩沖區(qū)不能讀取或?qū)懭?。FIFO緩沖區(qū)狀態(tài)不能改變除非 MPU-60X0重新上電。I2C_MST_EN=0，輔助 I2C總線（AUX_DA 和 AUX_CL）邏輯上由主 I2C總線（SDA和 SCL）驅(qū)動(dòng)。I2C_IF_DIS=1，MPU-6000:該位置 1，失能主 I2C接口并使能 SPI接口。FIFO_RESET=0， 當(dāng) FIFO_EN=0該位置 1，復(fù)位 FIFO緩沖區(qū)。復(fù)位后該位自動(dòng)清 0,，此處置0不復(fù)位FI

12、FO。不使用FIFO，0或1都可以。I2C_MST_RESET=0， 當(dāng) I2C_MST_EN=0該位置 1，復(fù)位 I2C主機(jī)。復(fù)位后該位自動(dòng)清 0。,SIG_COND_RESET=0，該位置 1，復(fù)位所有傳感器的信號(hào)通道 (陀螺儀, 加速度計(jì)和溫度傳感器)。這個(gè)操作會(huì)清除傳感器寄存器。復(fù)位后該位自動(dòng)清0。如果只想復(fù)位信號(hào)通道(不復(fù)位傳感器寄存器)，請(qǐng)使用寄存器 104，SIGNAL_PATH_RESET。8.REGISTER 117 WHOAMI（只讀）這個(gè)寄存器用于標(biāo)識(shí)設(shè)備的身份。WHO_AM_I的內(nèi)容是 MPU-60X0的 7位 I2C地址的頭 6位。最后一位地址由 AD0引腳確定。AD

13、0引腳的值跟寄存器無(wú)關(guān)。寄存器的默認(rèn)值為 0x68。位0 和7 保留。（硬編碼為0）。9.REGISTERS 59 TO 64 ACCELEROMETERMEASUREMENTS（只讀）這個(gè)寄存器存儲(chǔ)最近加速度計(jì)的測(cè)量值。加速度計(jì)根據(jù)采樣頻率（由寄存器 25定義）寫入到這些寄存器。每個(gè) 16位加速度計(jì)測(cè)量值的滿量程定義在 ACCEL_FS（寄存器 28）。對(duì)于每個(gè)滿量程的設(shè)置，ACCEL_xOUT里加速度計(jì)測(cè)量值的靈敏度最低分辨率（LSB）如下表：10.REGISTERS 67 TO 72 GYROSCOPEMEASUREMENTS（只讀）這個(gè)寄存器存儲(chǔ)最近陀螺儀的測(cè)量值。陀螺儀根據(jù)采樣頻率（

14、由寄存器 25定義）寫入到這些寄存器。每個(gè) 16 位陀螺儀測(cè)量值的滿量程定義在 FS_SEL（寄存器 27）。對(duì)于每個(gè)滿量程的設(shè)置，GYRO_xOUT里陀螺儀測(cè)量值的靈敏度最低分辨率（LSB）如下表： 對(duì)于寄存器，我們需要注意REGISTER 117 WHOAMI（只讀），這個(gè)寄存器與mpu6050的讀寫地址有關(guān)，AD0為mpu6050的一個(gè)引腳，由對(duì)該寄存器的描述可知，如果AD0接地，那么mpu6050的讀地址為0xd0，如果AD0接高，那么mpu6050的讀地址為0xd2。關(guān)于電源，MPU-60X0 可支持 VDD 范 圍 2.5V±5% ，3.0V±5%，或 3.3V

15、±5%。另外MPU-6050 還有一個(gè) VLOGIC 引腳，用來(lái)為 I2C 輸出提供邏輯電平。VLOGIC 電壓可取1.8±5%或者 VDD。對(duì)于VLOGIC引腳，最好接VDD，當(dāng)然，還要看使用的mcu的高低電平范圍是多少，否則，mpu6050的高低電平可能和mcu認(rèn)為的高低電平不同。3）磁力計(jì)：又叫電子羅盤，磁力測(cè)量的是磁場(chǎng)強(qiáng)度在三個(gè)軸上的分量，但是在小四軸上，沒(méi)有必要使用磁力計(jì)，因?yàn)殡姍C(jī)和飛控電路靠的很近，我們知道，電機(jī)內(nèi)部都是有磁鐵的，因此會(huì)有很大的干擾，沒(méi)有必要裝磁力計(jì)。4）數(shù)據(jù)傳輸恩，飛機(jī)做好，那用什么來(lái)控制呢，最容易想到的是藍(lán)牙，射頻和wifi，wifi由于功耗

16、較大，我們暫不考慮。我們做的飛行器支持用hc-05（藍(lán)牙）和nrf54l01+（射頻）來(lái)傳輸數(shù)據(jù)。藍(lán)牙模塊實(shí)際上就相當(dāng)于一個(gè)無(wú)線串口，藍(lán)牙與藍(lán)牙之間數(shù)據(jù)是透?jìng)鞯?，也就是說(shuō)配置好藍(lán)牙模塊之后，就可以把它當(dāng)串口對(duì)待了。而射頻接收則需要用另一塊mcu來(lái)控制進(jìn)行數(shù)據(jù)的接收并通過(guò)串口來(lái)發(fā)送數(shù)據(jù)給上位機(jī)。顯然藍(lán)牙更方便，但是射頻可以支持更遠(yuǎn)距離的傳輸，只是要注意，電機(jī)對(duì)于射頻的干擾是致命，因此設(shè)計(jì)電路的時(shí)候，電源部分要做好隔離，同時(shí)，射頻極力推薦使用挪威原產(chǎn)nrf24l01+。臺(tái)產(chǎn)的nrf24l01+，和普通的nrf24l01最好不要用。下面解釋原因： 我們知道，如果用藍(lán)牙或者串口收發(fā)數(shù)據(jù)的話，是不需要配

17、置某一方串口或藍(lán)牙處于發(fā)送狀態(tài)還是接受狀態(tài)的，只需將數(shù)據(jù)寫入發(fā)送緩沖區(qū)就可以發(fā)送數(shù)據(jù)，有數(shù)據(jù)接收只需要讀接收緩沖寄存器。而nrf24l01則不同，在發(fā)送前必須配置為發(fā)送模式，接收前必須配置為接收模式，這對(duì)于雙向通信顯然是很麻煩的。但是，接受方在接受數(shù)據(jù)之后會(huì)向發(fā)送方發(fā)送應(yīng)答信號(hào)表示已接收到數(shù)據(jù)，那么這個(gè)應(yīng)答信號(hào)要是能順便捎點(diǎn)其他信息給發(fā)送方，豈不是太方便了？挪威原產(chǎn)nrf24l01+完美解決了這個(gè)問(wèn)題。三程序設(shè)計(jì)1）四軸飛行器的程序框架如下：1.初始化mcu和各個(gè)傳感器2.讀取傳感器數(shù)據(jù)3對(duì)傳感器原始數(shù)據(jù)進(jìn)行處理4利用處理后的數(shù)據(jù)和上一次更新的姿態(tài)數(shù)據(jù)進(jìn)行姿態(tài)更新5根據(jù)遙控信號(hào)和姿態(tài)對(duì)電機(jī)進(jìn)行

18、控制，回到22）介紹陀螺儀和加速度計(jì)時(shí)說(shuō)了，陀螺儀由于積分作用，它的累積誤差來(lái)源于零漂。加速度計(jì)由于測(cè)量的是加速度，因此運(yùn)動(dòng)狀態(tài)以及電機(jī)震動(dòng)會(huì)干擾它對(duì)重力加速度的測(cè)量。對(duì)于以上干擾，我們對(duì)陀螺儀和加速度計(jì)進(jìn)行如下處理： 1.飛行器水平靜止放置時(shí)，讀取200次傳感器數(shù)據(jù)取平均，對(duì)于陀螺儀，這個(gè)數(shù)據(jù)就是零點(diǎn)漂移。 2.對(duì)于陀螺儀，以后每次用來(lái)姿態(tài)解算的數(shù)據(jù)都先減去1里獲得平均值。3.對(duì)于加速度計(jì)，以后每次姿態(tài)解算前都讀取20次加速度計(jì)數(shù)據(jù)并取平均，這樣就可以有效的濾除高頻分量，是加速度曲線稍微平滑。3）姿態(tài)解算在姿態(tài)解算前還得先講講四元數(shù)和歐拉角以及方向余弦矩陣，具體推導(dǎo)過(guò)程可以看秦永元的慣性導(dǎo)航

19、第一章緒論和第九章捷聯(lián)慣導(dǎo)系統(tǒng)。1.先建立兩個(gè)三維直角坐標(biāo)系，一個(gè)是機(jī)體坐標(biāo)系（設(shè)為b系），它就用來(lái)代表飛機(jī)，飛機(jī)怎么轉(zhuǎn)，它就怎么轉(zhuǎn)。一個(gè)是地理坐標(biāo)系（設(shè)為n系），他是固定不動(dòng)的，方便起見(jiàn)，設(shè)它的xyz指向東北天。然后，有一個(gè)概念：任意從n到b的旋轉(zhuǎn)都可以等效為三次旋轉(zhuǎn)（這里我們約定旋轉(zhuǎn)順序?yàn)閆-X-Y,值得注意的是，矩陣的乘法是不滿足交換律的，因此旋轉(zhuǎn)順序的不同對(duì)應(yīng)的旋轉(zhuǎn)矩陣不同，）：這里的，就是三個(gè)歐拉角yaw，pitch，roll（對(duì)于三個(gè)歐拉角的實(shí)際意義，可以看一下附帶的動(dòng)態(tài)圖片pitch.gif,roll.gif,yaw.gif）2用歐拉角的表示的由n到b的旋轉(zhuǎn)陣即姿態(tài)陣為：3.四元

20、數(shù)旋轉(zhuǎn)矩陣的四元數(shù)表示：4由于上面兩個(gè)旋轉(zhuǎn)矩陣式是等價(jià)的，那么可以用作為歐拉角和四元數(shù)的相互轉(zhuǎn)換 5.為什么要引入四元數(shù)我們知道，姿態(tài)更新的過(guò)程，主要的部分是陀螺儀的積分，如果不用四元數(shù)，我們可用7.2.3來(lái)計(jì)算，但是有缺點(diǎn)，看截圖部分。由此我們引入四元數(shù)，用四元數(shù)來(lái)更新姿態(tài)，然后再由四元數(shù)求得歐拉角，他們都可以用來(lái)表示姿態(tài)，且是可以相互轉(zhuǎn)換的。四元數(shù)微分方程四元數(shù)更新算法，一階龍庫(kù)法，同樣4個(gè)量（入、P1-3）為四元數(shù)的四個(gè)值,即上面的q0-3。Wx,y,z是三個(gè)陀螺的這個(gè)周期的角速度，比如歐拉角微分方程中的0.01弧度/秒，T為更新周期,比如0.02秒。 四元數(shù)微分方程更新后的規(guī)

21、范化每個(gè)周期更新完四元數(shù)，需要對(duì)四元數(shù)做規(guī)范化處理。因?yàn)樗脑獢?shù)本來(lái)就定義為四維單位向量。求q0-3的平方和，再開(kāi)根號(hào)算出的向量長(zhǎng)度length。然后每個(gè)q0-3除這個(gè)length。 6.姿態(tài)解算姿態(tài)解算就是姿態(tài)更新，說(shuō)白了就是獲得飛行器的姿態(tài)矩陣，在上述數(shù)學(xué)原理的講解中已經(jīng)說(shuō)明了用四元數(shù)來(lái)更新姿態(tài)矩陣的方法。但是，上述理論都是以陀螺儀為主體的，那么加速度計(jì)有什么用呢？盡管姿態(tài)更新之前的陀螺儀數(shù)據(jù)已經(jīng)經(jīng)過(guò)了一定的處理，但這顯然是不夠，陀螺儀的零點(diǎn)漂移受溫度等多種因素影響，并不是一個(gè)固定的值。因此如果依舊只用陀螺儀積分來(lái)進(jìn)行姿態(tài)解算，時(shí)間久了仍然是有很大誤差的。而加速度計(jì)不存在積分環(huán)節(jié)，

22、只不過(guò)電機(jī)擾動(dòng)并較大，造成測(cè)量的重力加速度數(shù)據(jù)不準(zhǔn)確。實(shí)際上，陀螺儀測(cè)量數(shù)據(jù)的零漂屬于低頻分量，加速度計(jì)的誤差屬于高頻分量。因此可以對(duì)陀螺儀數(shù)據(jù)進(jìn)行高通濾波，對(duì)加速度數(shù)據(jù)進(jìn)行低通濾波，然后進(jìn)行姿態(tài)解算，這就是互補(bǔ)濾波。其基本表達(dá)式為： angle=a*(angle+gyro*dt)+(1-a)*x_acc;(一個(gè)軸的表示，angle表示繞某個(gè)軸轉(zhuǎn)動(dòng)的姿態(tài)角，gyro*dt表示對(duì)陀螺儀數(shù)據(jù)積分，x_acc為加速度數(shù)據(jù)，a為參數(shù)，表示對(duì)陀螺儀數(shù)據(jù)和加速度數(shù)據(jù)的信任度。)上面的式子是互補(bǔ)濾波器的基本表達(dá)式，我們實(shí)際使用的是改進(jìn)型的帶PI修正的互補(bǔ)濾波器。詳細(xì)專業(yè)內(nèi)容可以參考論文基于改進(jìn)互補(bǔ)濾波器的低

23、成本微小飛行器姿態(tài)估計(jì)方法（注意：我們的程序中沒(méi)有對(duì)PI 補(bǔ)償?shù)脑鲆孢M(jìn)行分級(jí)調(diào)節(jié)）。姿態(tài)解算流程請(qǐng)看：互補(bǔ)濾波法.bmp,看上面一部分圖片即可，我們沒(méi)有用到磁力計(jì)，圖片放大后看比較清晰。或者參考7.最后要提的2點(diǎn)是互補(bǔ)濾波法也可以使用卡爾曼濾波，梯度下降法代替，但卡爾曼濾波太過(guò)復(fù)雜，因此我們并未使用。并且這三種方法是對(duì)姿態(tài)數(shù)據(jù)進(jìn)行誤差補(bǔ)償，使之更接近于真實(shí)姿態(tài)，而不屬于四元數(shù)姿態(tài)更新的范疇。Mpu6050讀取出來(lái)的數(shù)據(jù)在某些情況是不能直接使用，必須經(jīng)過(guò)單位轉(zhuǎn)換，至于怎么把原始數(shù)據(jù)轉(zhuǎn)換成度或是弧度，在這里就不廢話了，寄存器介紹那一部分有測(cè)量范圍。寫姿態(tài)更新程序的時(shí)候務(wù)必用定時(shí)器來(lái)產(chǎn)生精確地更新周

24、期。四PID控制器對(duì)于PID的詳細(xì)資料請(qǐng)參閱AN_SPMC75_0012PID調(diào)節(jié)控制做電機(jī)速度控制有入門到精通吃透PID1）采用位置式PID控制，位置式PID公式如下 其中ek為當(dāng)前實(shí)際姿態(tài)角減去期望姿態(tài)角和的差值，而uk則是作為pwm的占空比，來(lái)控制電機(jī)的轉(zhuǎn)速；Kp，Ki，Kd是三個(gè)參數(shù)，下面介紹這三個(gè)參數(shù)的作用：1. Kp是比例系數(shù)，強(qiáng)制回復(fù)力是系統(tǒng)平衡的主力，它總是使飛行器的實(shí)際姿態(tài)不斷逼近期望姿態(tài)，太小會(huì)導(dǎo)致飛行器回復(fù)力太小，導(dǎo)致飛行器無(wú)法回復(fù)，太大容易超調(diào)，所謂超調(diào)就是指恢復(fù)到平衡位置之后往另一個(gè)方向偏，從而引發(fā)低頻振蕩。要注意的是，如果只有Kp，那么四軸是肯定會(huì)震蕩的，

25、因此需要Kd來(lái)抑制這個(gè)低頻震蕩。2. Kd是阻尼力參數(shù)，始終是抑制飛行器姿態(tài)的變化，姿態(tài)變化的越快，阻尼力越大，因此Kd*（ek-ek-1）這一項(xiàng)是負(fù)的，這一點(diǎn)一定要注意，否則pid永遠(yuǎn)調(diào)不出來(lái)。Kd的乘積項(xiàng)可以直接用角速度代替。Kd項(xiàng)可以抑制Kp的低頻震蕩，Kd越大，抑制效果越好，但是太大會(huì)引起高頻震蕩。因此Kp和Kd具體的大小需要不斷地調(diào)試才能獲得。3. Ki是積分項(xiàng)的參數(shù)，他也是回復(fù)力，但比Kp柔和，Ki只能是輔助Kp，在小四軸上，甚至可以不用Ki這一項(xiàng)。2）調(diào)試PID 可以參閱： 調(diào)PID就是通過(guò)不斷地實(shí)驗(yàn)獲取合理的KP,KI,KD這三個(gè)參數(shù)的值，要注意，對(duì)于小四軸來(lái)說(shuō)，如果只有參數(shù)K

26、p，那么四軸要么一下子就倒下去再也起不來(lái)，要么就是倒向另一邊之后突然電機(jī)力量加大，朝另一個(gè)方向翻滾，不停地翻滾，因此必須適當(dāng)?shù)募尤隟d進(jìn)行抑制。在調(diào)PID之前，非常非常重要的一點(diǎn)，就是按照你的程序定好的機(jī)體坐標(biāo)系和飛行模式（X型or +型）安放你的電機(jī)。我們小四軸為X型，示意如下：1,2,3,4是電機(jī)編號(hào)以便說(shuō)明2143上圖對(duì)應(yīng)的機(jī)體坐標(biāo)系如下：z軸方向?yàn)榇怪鄙涑黾埫鎄Y同時(shí)，在我們的程序中，按照右手定則約定：右手握住x軸，拇指指向x軸正方向，那么另外四個(gè)手指指尖的朝向就是機(jī)體繞x軸旋轉(zhuǎn)的正方向，同時(shí)，我們約定橫滾角roll就是繞x軸。右手按同樣方法握住Y軸和Z軸可知繞這兩個(gè)軸旋轉(zhuǎn)的正方向，同

27、樣，我們約定繞Y軸旋轉(zhuǎn)為俯仰角pitch，繞z軸旋轉(zhuǎn)為航向角yaw。對(duì)于pid的程序，有一點(diǎn)需要說(shuō)明：Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;Moto_PWM_2 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;Moto_P

28、WM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;上面一段程序中，Moto_PWM_1/2/3/4是四個(gè)電機(jī)的pwm值，rc_in->THROTTLE是接收到的遙控信號(hào)的基礎(chǔ)油門值（這里要減1000是因遙控油門數(shù)據(jù)為1000到2000之間，而我們程序的PWM值得范圍是0到1000之間，不用深究），后面的PID_ROL.OUT ， PID_PIT.OUT ， PID_YAW.OUT才是三個(gè)軸的PID控制器的輸出。那么怎么來(lái)區(qū)分這個(gè)pid輸出是減還是加呢？我們假設(shè)現(xiàn)在要讓飛行器向右飛，那么按照

29、前面的分析我們應(yīng)該給個(gè)正的roll的期望角，記為exp_roll,那么ek=0-exp_roll(我們假設(shè)原來(lái)飛行器是水平的，即實(shí)際姿態(tài)角為0)。 然后，我們想象一下，飛行器如果要向右飛，顯然2，4號(hào)電機(jī)應(yīng)該加大轉(zhuǎn)速；1，3電機(jī)應(yīng)該減小轉(zhuǎn)速，這樣才能向右傾斜。那么表現(xiàn)在公式上：Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT;Moto_PWM_2 = rc_in->THROTTLE - 1000 - PID_ROL.OUT;Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT;Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT;可能有人覺(jué)得奇怪，這個(gè)式子怎么和上面的分析相反呢？怎么1,3是+ PID_ROL.OUT，而2,4卻是 - PID_ROL.OUT呢？這是因?yàn)槲覀兩厦媾e得例子中ek=-exp_roll是個(gè)小于0的值，那么PID的輸出肯定也是個(gè)負(fù)值。為什么呢，首先，ek帶入PID的公式