【正文】 (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的四旋翼 無人機控制 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的四旋翼 無人機控制 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 }。也感謝同學的 四年 陪伴。 感謝 Arduino 項目對本試驗開發(fā)帶來的便利，愿他們未來能發(fā)展順利。 同時大學四年所有老師都 間接參與了本論文的實踐與寫作，他們在不同方向，不同力度的影響了本實驗 的思考與實踐。 基于 AVR的四旋翼 無人機控制 33 致謝 此論文的完成受到逯鵬老師的指導與關切，提供了遇到困難時的方向與心靈幫助，逯老師的哲學性指導是 本 畢業(yè)設計中得到的最大收獲。 PID 控制在一定條件下可以使飛行器得到穩(wěn)定控制 ， PID 參數(shù)在沒有完整數(shù)學模型的情況下需要反復調試，本實驗 PID參數(shù) 在自然環(huán)境中 并沒有達到理想要求。 主要核心任務為姿態(tài)傳感器 DMP 四元數(shù)到歐拉角的轉換和 PID 控制部分。 基于 AVR的四旋翼 無人機控制 31 圖 510： 本實驗 四旋翼 低 高度觀察圖 圖 511： 本實驗 四旋翼 中等高度觀察圖 圖 512： 本實驗 四旋翼 高高度觀察圖 ? 結論：四旋翼完成了空中停留的目標，但 仍不能 可靠 控制。飛行穩(wěn)定，但 實際飛行中 方向 控制過程并不理想。 整機測試 通過 姿態(tài)傳感器獲得實時姿態(tài)，遙控器獲得期望姿態(tài)并通過 PID 控制完成姿態(tài)變化。 基于 AVR的四旋翼 無人機控制 30 實際 整定 方式 如圖 59將飛行器固定并只打開需要的通道，測試飛行器在模擬空中狀態(tài)的變化 。 3) 若此時震蕩經反復調節(jié) PI 項無法消除，可加入微分調節(jié)。 2) 若 此時 靜態(tài) 誤差較大，可加入積分調節(jié)。 ? 微分系數(shù) ????： 微分項 由式（ ）當角度變化快 （角速度 大 ） 時 輸出調節(jié)量較大 ， 飛行器震蕩時在平衡位置出現(xiàn)角速度峰值，微分項 在本實驗中有“阻尼”的作用。 ? 積分系數(shù) ????： 積分調節(jié)隨時間的積累增大調節(jié)量， 當系統(tǒng) 有 小幅度靜態(tài)偏差時隨時間積累會引起積分項調節(jié)， 在控制過程中起到消除靜態(tài)誤差的作用，是比例系數(shù)輔助調節(jié)項。比例系數(shù)過大時會使衰減震蕩次數(shù)增多，過小時姿態(tài)調整緩慢。 圖 58： PID過渡 過程狀態(tài)圖 比例項，積分項，微分項 對飛行 控制有如下影響： ? 比例系數(shù) ????： 比例 調響應 速度 快， 是調節(jié)控制主要參數(shù)。 d) 非周期震蕩 ： 被控系統(tǒng)姿態(tài)與給定系統(tǒng)姿態(tài)偏差隨時間無規(guī)則變化。 b) 等幅振蕩 ：被控系統(tǒng)姿態(tài)與給定系統(tǒng)姿態(tài)偏差隨時間基本保持不變。 參數(shù)整定 整定方法 控制 過程由不穩(wěn)定狀態(tài)到穩(wěn)定狀態(tài)可能會 出現(xiàn) 如下 4個 狀態(tài) 。 ? 操作 ：保持 Pitch、 Yaw角度不變， 只 改變翻滾角 Roll 基于 AVR的四旋翼 無人機控制 27 圖 52：姿態(tài)數(shù)據(jù)結果 —— 翻滾角 Roll 圖 53為 改變飛機俯仰姿勢 即 俯仰角 Pitch 角度值 ? 操作：保持 Roll、 Yaw 角度不變，只改變 俯仰角 Pitch 圖 53：姿態(tài)數(shù)據(jù)結果 —— 俯仰角 Pitch 圖 54為改變飛機偏航方向 即 偏航角 Yaw 角度值 ? 操作：保持 Roll、 Pitch 角度不變，只改變 偏航角 Yaw 圖 54：姿態(tài)數(shù)據(jù)結果 —— 偏航角 Yaw 圖 55為 歐拉角各方向角度綜合改變 ? 操作：同時改變歐拉角各角度值 基于 AVR的四旋翼 無人機控制 28 圖 55：姿態(tài)數(shù)據(jù)結果 ? 結論： 根據(jù)以上數(shù)據(jù)測試，姿態(tài)數(shù)據(jù)獲得正常，且能正確反映機體姿態(tài)。 可視化軟件為 SerialChart。 圖 51：遙控器信號解碼結果 ? 結論： 圖中可以看出遙控器 各通道 信號接收解碼正常，高電平時間記錄無誤。 遙控解碼 測試 主控芯片接受到遙控器接收機發(fā)送的 PWM 信號并記錄高電平時常， 單位為 微秒， 最后通過串口發(fā)送給計算機顯示各通道信息。 轉化為 C語言為 基于 AVR的四旋翼 無人機控制 26 第五 章 測試 與試驗 本章根據(jù)實際測試資料獲得硬件、程序正確與否，通過串口或設備查看控制各個部分工作情況。 電調接受到 PWM 信號后轉化成電壓的變化控制電機的轉速。 在 本實驗 整個系統(tǒng)中的 PID 控制 如圖49所示 PID+ +被 控 對 象設 定 值M P U 6 0 5 0反 饋 值e ( t )u ( t ) 圖 49： 本實驗 PID控制圖 PID控制公式為 0 ()( ) ( ) ( )tp i d d e tu t K e t T e t d t T dt? ? ? ? () 其中 ????為比例系數(shù)， ????為積分系數(shù)， ????為微分系數(shù)。 控 制 器 執(zhí) 行 機 構 被 控 對 象給 定 值 被 控 參 數(shù) 圖 47：開環(huán)控制系統(tǒng) 控 制 器 執(zhí) 行 機 構 被 控 對 象測 量 與 反 饋 單 元+反 饋 量給 定 量 被 控 參 數(shù) 圖 48：閉環(huán)控制系統(tǒng) 基于 AVR的四旋翼 無人機控制 23 本實驗控制系統(tǒng)為閉環(huán)控制， PID 控制器接受遙控器給定值控制馬達轉速大小產生升力 F，控制被控對象整個飛行器姿態(tài)，由姿態(tài)傳感器測量反饋現(xiàn)時姿態(tài)與遙控器對比，作為新 輸入 參與下一輪控制 過程 。 控制系統(tǒng)可分為開環(huán)控制和閉環(huán)控制兩種如圖 47，圖 48 所示 ，開環(huán)控制輸出量對控制系統(tǒng)不產生影響 ，如子彈飛行 。 決策 模塊 遙控器控制信息和當前姿態(tài)信息獲得之后就到了將他們綜合起來計算控制量了 。四元數(shù)獲得代碼為： 基于 AVR的四旋翼 無人機控制 21 此時四元數(shù) Q = w +x +y+ z全部數(shù)據(jù)在 Quaternion 數(shù)據(jù)結構中。 IIC 總線信息傳輸需要遵循規(guī)則，根據(jù) MPU6050 數(shù)據(jù)手冊可得到如圖 45，46信息 基于 AVR的四旋翼 無人機控制 20 圖 45： IIC總線數(shù)據(jù)發(fā)送規(guī)則 圖 46： IIC總線數(shù)據(jù)接收規(guī)則 傳送數(shù)據(jù)中系統(tǒng)處理器始終為主機， MPU6050 傳感器始終為從機。 圖 44為信號解碼流程圖 開 始結 束上 升 沿記 錄 時 間 T1是下 降 沿記 錄 時 間 T2是否否返 回 T=T2T1 圖 44：信號解