【正文】 (F()。 } else { (F(DMP Initialization failed (code ))。 dmpReady = true。 mpuIntStatus = ()。 (F(Enabling interrupt detection (Arduino external interrupt 0)...))。 // 1688 factory default for my test chip if (devStatus == 0) { (F(Enabling DMP...))。 (30)。 (20)。 (145)。 (() ? F(MPU6050 connection successful) : F(MPU6050 connection failed))。 while (!Serial)。 elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true)。 if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE ()。 pinMode(pwm2A, OUTPUT)。 pinMode(pwm1A, OUTPUT)。 pinMode(chanPitch, INPUT)。 } void setup() { pinMode(chanUp, INPUT)。 OCR1AH = 0。 //Timer2 init TCCR2A = ( 1 COM2A1 ) | ( 1 COM2B1 ) | ( 1 WGM21 ) | ( 1 WGM20 )。 } void pwm_init() { //Timer1 init TCCR1A = ( 1 COM1A1 ) | ( 1 COM1B1 ) | ( 1 WGM10 )。} OCR2A = PWM2A。} if(PWM2B = 65) {PWM2B = 65。 if(PWM2A = 65) {PWM2A = 65。 } void Motor_Control_Yaw() { } void Motor_Control_Pitch() { PWM2A = pwm2ABase + ppmValue[Up] u_Pitch。} OCR1AL = PWM1AL。} 基于 AVR的四旋翼 無(wú)人機(jī)控制 40 if(PWM1BL = 65) {PWM1BL = 65。 if(PWM1AL = 65) {PWM1AL = 65。 } /**************************************************************************/ void Motor_Control_Roll() { PWM1AL = pwm1ALBase + ppmValue[Up] u_Roll。 u_Yaw = Kp_Yaw * pError + Ki_Yaw * iError + Kd_Yaw * dError + u1_Yaw。 ek2_Yaw = ek1_Yaw。 iError = ek_Yaw。 ek_Yaw = sv_Yaw fv_Yaw。 u1_Pitch = u_Pitch。 ek1_Pitch = ek_Pitch。 dError = ek_Pitch ek1_Pitch * 2 + ek2_Pitch。 pError = ek_Pitch ek1_Pitch。 } void PID_Control_Pitch(float sv_Pitch,float fv_Pitch) 基于 AVR的四旋翼 無(wú)人機(jī)控制 39 { float pError=0,iError=0,dError=0。 u_Roll = Kp_Roll * pError + Ki_Roll * iError + Kd_Roll * dError + u1_Roll。 ek2_Roll = ek1_Roll。 iError = ek_Roll。 ek_Roll = sv_Roll fv_Roll。 ppmValue_Yaw()。 ppmValue_Roll()。 else ppmValue[Yaw] = ( ppmInValue[Yaw] 1500 ) /30。 } void ppmValue_Yaw() { if(ppmInValue[Yaw] = 1200) ppmValue[Yaw] = 10。 else if(ppmInValue[Pitch] = 1800) ppmValue[Pitch] = 10。 else ppmValue[Roll] = ( ppmInValue[Roll] 1500 ) /30。 } void ppmValue_Roll() { if(ppmInValue[Roll] = 1200) ppmValue[Roll] = 10。 else if(ppmInValue[Up] = 1800) ppmValue[Up] = 60。 void dmpDataReady() { mpuInterrupt = true。 float ek2_Yaw = 0。 float ek_Yaw = 0。 float Ki_Yaw = 0。 float u1_Yaw = 0。 float ek2_Pitch = 0。 float ek_Pitch = 0。 float Ki_Pitch = 。 float u1_Pitch = 0。 float ek2_Roll = 0。 float ek_Roll = 0。 0001。 float Kp_Roll = 。 float u_Roll = 0。 VectorFloat gravity。 uint8_t fifoBuffer[64]。 uint16_t packetSize。 uint8_t mpuIntStatus。 MPU6050 mpu。 int ppmInValue[chanNum] = { 0 , 0 , 0 , 0 }。也感謝同學(xué)的 四年 陪伴。 感謝 Arduino 項(xiàng)目對(duì)本試驗(yàn)開(kāi)發(fā)帶來(lái)的便利，愿他們未來(lái)能發(fā)展順利。 同時(shí)大學(xué)四年所有老師都 間接參與了本論文的實(shí)踐與寫(xiě)作，他們?cè)诓煌较?，不同力度的影響了本?shí)驗(yàn) 的思考與實(shí)踐。 基于 AVR的四旋翼 無(wú)人機(jī)控制 33 致謝 此論文的完成受到逯鵬老師的指導(dǎo)與關(guān)切，提供了遇到困難時(shí)的方向與心靈幫助，逯老師的哲學(xué)性指導(dǎo)是 本 畢業(yè)設(shè)計(jì)中得到的最大收獲。 PID 控制在一定條件下可以使飛行器得到穩(wěn)定控制 ， PID 參數(shù)在沒(méi)有完整數(shù)學(xué)模型的情況下需要反復(fù)調(diào)試，本實(shí)驗(yàn) PID參數(shù) 在自然環(huán)境中 并沒(méi)有達(dá)到理想要求。 主要核心任務(wù)為姿態(tài)傳感器 DMP 四元數(shù)到歐拉角的轉(zhuǎn)換和 PID 控制部分。 基于 AVR的四旋翼 無(wú)人機(jī)控制 31 圖 510： 本實(shí)驗(yàn) 四旋翼 低 高度觀察圖 圖 511： 本實(shí)驗(yàn) 四旋翼 中等高度觀察圖 圖 512： 本實(shí)驗(yàn) 四旋翼 高高度觀察圖 ? 結(jié)論：四旋翼完成了空中停留的目標(biāo)，但 仍不能 可靠 控制。飛行穩(wěn)定，但 實(shí)際飛行中 方向 控制過(guò)程并不理想。 整機(jī)測(cè)試 通過(guò) 姿態(tài)傳感器獲得實(shí)時(shí)姿態(tài)，遙控器獲得期望姿態(tài)并通過(guò) PID 控制完成姿態(tài)變化。 基于 AVR的四旋翼 無(wú)人機(jī)控制 30 實(shí)際 整定 方式 如圖 59將飛行器固定并只打開(kāi)需要的通道，測(cè)試飛行器在模擬空中狀態(tài)的變化 。 3) 若此時(shí)震蕩經(jīng)反復(fù)調(diào)節(jié) PI 項(xiàng)無(wú)法消除，可加入微分調(diào)節(jié)。 2) 若 此時(shí) 靜態(tài) 誤差較大，可加入積分調(diào)節(jié)。 ? 微分系數(shù) ????： 微分項(xiàng) 由式（ ）當(dāng)角度變化快 （角速度 大 ） 時(shí) 輸出調(diào)節(jié)量較大 ， 飛行器震蕩時(shí)在平衡位置出現(xiàn)角速度峰值，微分項(xiàng) 在本實(shí)驗(yàn)中有“阻尼”的作用。 ? 積分系數(shù) ????： 積分調(diào)節(jié)隨時(shí)間的積累增大調(diào)節(jié)量， 當(dāng)系統(tǒng) 有 小幅度靜態(tài)偏差時(shí)隨時(shí)間積累會(huì)引起積分項(xiàng)調(diào)節(jié)， 在控制過(guò)程中起到消除靜態(tài)誤差的作用，是比例系數(shù)輔助調(diào)節(jié)項(xiàng)。比例系數(shù)過(guò)大時(shí)會(huì)使衰減震蕩次數(shù)增多，過(guò)小時(shí)姿態(tài)調(diào)整緩慢。 圖 58： PID過(guò)渡 過(guò)程狀態(tài)圖 比例項(xiàng)，積分項(xiàng)，微分項(xiàng) 對(duì)飛行 控制有如下影響： ? 比例系數(shù) ????： 比例 調(diào)響應(yīng) 速度 快， 是調(diào)節(jié)控制主要參數(shù)。 d) 非周期震蕩 ： 被控系統(tǒng)姿態(tài)與給定系統(tǒng)姿態(tài)偏差隨時(shí)間無(wú)規(guī)則變化。 b) 等幅振蕩 ：被控系統(tǒng)姿態(tài)與給定系統(tǒng)姿態(tài)偏差隨時(shí)間基本保持不變。 參數(shù)整定 整定方法 控制 過(guò)程由不穩(wěn)定狀態(tài)到穩(wěn)定狀態(tài)可能會(huì) 出現(xiàn) 如下 4個(gè) 狀態(tài) 。 ? 操作 ：保持 Pitch、 Yaw角度不變， 只 改變翻滾角 Roll 基于 AVR的四旋翼 無(wú)人機(jī)控制 27 圖 52：姿態(tài)數(shù)據(jù)結(jié)果 —— 翻滾角 Roll 圖 53為 改變飛機(jī)俯仰姿勢(shì) 即 俯仰角 Pitch 角度值 ? 操作：保持 Roll、 Yaw 角度不變，只改變 俯仰角 Pitch 圖 53：姿態(tài)數(shù)據(jù)結(jié)果 —— 俯仰角 Pitch 圖 54為改變飛機(jī)偏航方向 即 偏航角 Yaw 角度值 ? 操作：保持 Roll、 Pitch 角度不變，只改變 偏航角 Yaw 圖 54：姿態(tài)數(shù)據(jù)結(jié)果 —— 偏航角 Yaw 圖 55為 歐拉角各方向角度綜合改變 ? 操作：同時(shí)改變歐拉角各角度值 基于 AVR的四旋翼 無(wú)人機(jī)控制 28 圖 55：姿態(tài)數(shù)據(jù)結(jié)果 ? 結(jié)論： 根據(jù)以上數(shù)據(jù)測(cè)試，姿態(tài)數(shù)據(jù)獲得正常，且能正確反映機(jī)體姿態(tài)。 可視化軟件為 SerialChart。 圖 51：遙控器信號(hào)解碼結(jié)果 ? 結(jié)論： 圖中可以看出遙控器 各通道 信號(hào)接收解碼正常，高電平時(shí)間記錄無(wú)誤。 遙控解碼 測(cè)試 主控芯片接受到遙控器接收機(jī)發(fā)送的 PWM 信號(hào)并記錄高電平時(shí)常， 單位為 微秒， 最后通過(guò)串口發(fā)送給計(jì)算機(jī)顯示各通道信息。 轉(zhuǎn)化為 C語(yǔ)言為 基于 AVR的四旋翼 無(wú)人機(jī)控制 26 第五 章 測(cè)試 與試驗(yàn) 本章根據(jù)實(shí)際測(cè)試資料獲得硬件、程序正確與否，通過(guò)串口或設(shè)備查看控制各個(gè)部分工作情況。 電調(diào)接受到 PWM 信號(hào)后轉(zhuǎn)化成電壓的變化控制電機(jī)的轉(zhuǎn)速。 在 本實(shí)驗(yàn) 整個(gè)系統(tǒng)中的 PID 控制 如圖49所示 PID+ +被 控 對(duì) 象設(shè) 定 值M P U 6 0 5 0反 饋 值e ( t )u ( t ) 圖 49： 本實(shí)驗(yàn) PID控制圖 PID控制公式為 0 ()( ) ( ) ( )tp i d d e tu t K e t T e t d t T dt? ? ? ? () 其中 ????為比例系數(shù)， ????為積分系數(shù)， ????為微分系數(shù)。 控 制 器 執(zhí) 行 機(jī) 構(gòu) 被 控 對(duì) 象給 定 值 被 控 參 數(shù) 圖 47：開(kāi)環(huán)控制系統(tǒng) 控 制 器 執(zhí) 行 機(jī) 構(gòu) 被 控 對(duì) 象測(cè) 量 與 反 饋 單 元+反 饋 量給 定 量 被 控 參 數(shù) 圖 48：閉環(huán)控制系統(tǒng) 基于 AVR的四旋翼 無(wú)人機(jī)控制 23 本實(shí)驗(yàn)控制系統(tǒng)為閉環(huán)控制， PID 控制器接受遙控器給定值控制馬達(dá)轉(zhuǎn)速大小產(chǎn)生升力 F，控制被控對(duì)象整個(gè)飛行器姿態(tài)，由姿態(tài)傳感器測(cè)量反饋現(xiàn)時(shí)姿態(tài)與遙控器對(duì)比，作為新 輸入 參與下一輪控制 過(guò)程 。 控制系統(tǒng)可分為開(kāi)環(huán)控制和閉環(huán)控制兩種如圖 47，圖 48 所示 ，開(kāi)環(huán)控制輸出量對(duì)控制系統(tǒng)不產(chǎn)生影響 ，如子彈飛行 。 決策 模塊 遙控器控制信息和當(dāng)前姿態(tài)信息獲得之后就到了將他們綜合起來(lái)計(jì)算控制量了 。四元數(shù)獲得代碼為： 基于 AVR的四旋翼 無(wú)人機(jī)控制 21 此時(shí)四元數(shù) Q = w +x +y+ z全部數(shù)據(jù)在 Quaternion 數(shù)據(jù)結(jié)構(gòu)中。 IIC 總線信息傳輸需要遵循規(guī)則，根據(jù) MPU6050 數(shù)據(jù)手冊(cè)可得到如圖 45，46信息 基于 AVR的四旋翼 無(wú)人機(jī)控制 20 圖 45： IIC總線數(shù)據(jù)發(fā)送規(guī)則 圖 46： IIC總線數(shù)據(jù)接收規(guī)則 傳送數(shù)據(jù)中系統(tǒng)處理器始終為主機(jī)， MPU6050 傳感器始終為從機(jī)。 圖 44為信號(hào)解碼流程圖 開(kāi) 始結(jié) 束上 升 沿記 錄 時(shí) 間 T1是下 降 沿記 錄 時(shí) 間 T2是否否返 回 T=T2T1 圖 44：信號(hào)解