【正文】 ，同時減小四個電機的輸出功率，四旋翼飛行器則垂直下降，直至平衡落地，實現了沿 z軸的垂直運動。保證四個旋翼轉速同步增加或減小是垂直運動的關鍵。為了不因為旋翼轉速的改變引起四旋翼飛行器整體扭矩及總拉力改變，旋翼 1與旋翼 3轉速該變量的大小應相等。 圖 3（ c）滾轉運動：與圖 b的原理相同，在圖 c中，改變電機 2和電機 4的轉速，保持電機 1和電機 3的轉速不變，則可使機身繞 x軸旋轉（正向和反向），實現飛行器的滾轉運動。旋翼轉動過程中由于空氣阻力作用會形成與轉動方向相反的反扭矩，為了克服反扭矩影響，可使四個旋翼中的兩個正轉 ，兩個反轉，且對角線上的來年各個旋翼轉動方向相同。在圖 d中，當電機 1和電機 3的轉速上升，電機 2和電機 4的轉速下降時，旋翼 1和旋翼 3對機身的反扭矩大于旋翼 2和旋翼 4對機身的反扭矩，機身便在富余反扭矩的作用下繞 z軸轉動，實現飛行器的偏航運動，轉向與電機 電機 3的轉向相反。在圖 e中，增加電機 3轉速，使拉力增大，相應減小電機 1轉速，使拉力減小，同時保持其它兩個電機轉速不變，反扭矩仍然要保持平衡。向后飛行與向前飛行正好相反。 圖 3（ f）傾向運動：由于結構對稱，所以傾向飛行的工作原理與前后運動完全一樣。四旋翼的基本運動可以用圖 4來描述。沿著 X 軸的運動與在 Y軸方向上傾斜有關。這種傾斜也可以沿 X軸的加速度。偏航運動是利用旋翼產生的力矩來實現的。但在四旋翼的情況下，旋翼的旋轉方向是用來平衡和減少這些力矩的。為了在順時針方向上轉向，必須增加旋翼 4的速度以克服旋翼 1和 3產生的力矩。假設一個固定在支架上的結構在該直升機重心處，此處 Z軸是指向上的，它的身體軸心是與慣性系的位置矢量（ x,y,z)和三個分別代表了俯仰滾 9 轉與偏航的歐拉角（θ，Ψ，Φ），有關的式 1 用來表示旋 轉?????????????????????????????????????????????ccscssccssccssscssscsccssscccR 在上式中 ?c 和 ?s 分別表示了 ?cos 和 ?sin 每個旋翼產生的力矩相當于縱向的力。這里有 4個輸入的力與 6個輸出的參數（ ??? , zyx ）因此直升機是一個欠驅動系統(tǒng)。根據力和力矩平衡所得方程如下： m xKFx i i ??? 141 )s i ns i nc oss i n)(c os( ??? ? ? ????? myKFy i i ??? 24 1 )s i nc o sc o ss i n) ( s i n( ??? ? ? ?????m zKmgFz i i ??? 341 )c os)(c os( ??? ? ? ?? 364321254321144321/)(/)(/)(JKMMMMJKFFFFlJKFFFFl???????????????????????????????? 上面的 iK 是阻尼系數，接下來我們假設阻力為 0，因為阻力在低速時可以忽略的。 1U 表示了在 Z軸方向上的加速度， 2U 、 3U 表示 the roll and pitch inputs 4U 表示了一個偏航力矩。使旋翼的力指向中心同樣增加穩(wěn)定性，同時減少 The roll and pitch moments 和 垂直方向上的總推力。 圖 5 主控芯片 R5F100LEA引腳圖 主控芯片 R5F100LEA是瑞薩公司生產的一款單片機芯片，采用 64引腳 LQFP封裝，64KB的 ROM容量，擁有 Flash存儲器。適合在復雜應用且微型飛行器上應用。 MPU6000（ 6050）整合了 3軸陀螺儀、 3軸加速器，并含可藉由第二個 I2C端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數位運動處理 (DMP: Digital Motion Processor)硬件加速引擎，由主要 I2C 端口以單一數據流的形式，具有向 應用端輸出完整的 9軸融合演算技術 InvenSense的運動處理資料庫，可處理運動感測的復雜數據，降低了運動處理運算對操作系統(tǒng)的負荷，并為應用開發(fā)提供架構化的 API等功能。 250、177。 1000與177。 /sec (dps)，可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為177。 4g177。 16g。 MPU6000可在不同電壓下 工作， VDD 供電電壓介為 177。 5%或 177。 5%（ MPU6000僅用VDD）。 HCSR04 如圖 7 12 圖 7 HCSR04實物圖 HCSR04超聲波測距模塊 包括超聲波發(fā)射器、接收器與控制電路， 性能穩(wěn)定，測度距離精確，模塊高精度，盲區(qū)小。 基本工 作原理： (1)采用 IO口 TRIG觸發(fā)測距，給至少 10us 的高電平信號 。測試距離 =(高電平時間 *聲速 (340M/S))/2。我們用一個慣性測量裝置（陀螺儀 mpu6050）來執(zhí)行這一任務。該過濾器的工程計算誤差信號之間 的估計角（ ? ），并參考角（ ref? ）在直接從加速度計算。應該指出的是，比率傳感器可以測量在體內固定軸，而加速度測量傾斜，在地球上的固定軸。 PID 是比例 (P)、積分 (I)、微分 (D)控制算法。 13 采用了以上三種算法 比例 (P)、積分 (I)、微分 (D)控制超聲波傳感器反饋的脈沖，以控制四個電機的轉速，越過障礙。但是微分對噪聲干擾有放大作用，加強微分對系統(tǒng)抗干擾不利 積分和微分都不能單 獨起作用，必須與比例控制配合。 電源模塊 為四旋翼飛行器飛行控制系統(tǒng)、無刷電機等提供電源?？紤]到線性穩(wěn)壓方式轉換效率不高，能量損耗大的缺點，本系統(tǒng)改用開關電源的方式來穩(wěn)壓。 硬件電路圖 整 體連接電路如圖 8 圖 8 硬件連接電路圖 14 三、程序設計 程序流程圖 如圖 9 圖 9 流程圖 四、系統(tǒng)測試 基本部分測試 測試儀器：秒表、數字萬用表、示波器、 R5F100LEA仿真機等。此次測試目的在于發(fā)現系統(tǒng)缺陷與漏洞，從測試結果中沒有發(fā)現嚴重系統(tǒng)缺陷。通過各種方案的討論和嘗試，在經過多次的整體軟硬件結合調試，不斷地對系統(tǒng)進行優(yōu)化，設計采用簡單的動力學原 理結合軟件開發(fā)平臺，就能夠簡單高效的完成任務，若采用其他類型的功能傳感器，功能會更加強大，且若將算法設計精準，功能完成會更加流暢。 /* Start user code. Do not edit ment generated here */ /*????????*/ 17 while(get_systemtimer()5000)。 curatt_init()。 delaytime=0。 if(OK==get_mpu6050_mode()) { atttitude_calculate(4000,fgyrox,fgyroy,fgyroz)。 } else { flag=1。curattX,amp。 euler_control(amp。target)。 delaytime++。 //power=0。 } /* End user code. Do not edit ment generated here */ } 子函數 1. include include Q4 curattX={1,0,0,0}。 18 float math_rsqrt(float number) { long i。 x2 = number * 。 i = * ( long * ) amp。 // evil floating point bit level hacking i = 0x5f3759df ( i 1 )。i。 // 1st iteration return y。 = 0。 = 0。 float qa,qb,qc。 gx *= ( * timesec)。 gz *= ( * timesec)。 qb = 。 += (qb * gx qc * gy * gz)。 += (qa * gy qb * gz + * gx)。 a_rsqrt = math_rsqrt(* + * + * + *)。 *= a_rsqrt。 19 *= a_rsqrt。 gx = 2 * (datx*datz datw*daty)。 x_2 = datx*datx。 gz = w_2 x_2 daty*daty + datz*datz。 angyaw = 1*atan2(2*datx*daty 2*datw*datz, 2*w_2 + 2*x_2 1)。 angroll =1* atan(gx* math_rsqrt(gy*gy + gz))。 angpitch = atan(gy* math_rsqrt(gx*gx + gz))。 } 2. static void updat_pid(PIDP* pid) { float ilimit1=pidilimit*(1)。 if(pidistatepidilimit)pidistate=pidilimit。 pidpidout = pidpGain*piderrdat+pidistate+(piderrdatpidperr)*piddGain。 } static void pid_init(PIDP* pid) { pidistate=0。 piderrdat=0。 } void control_init(void) { curatt_init()。 pid_init(amp。 pid_init(amp。 pid_init(amp。 pid_init(amp。 20 pid_init(amp。 pid_init(amp。 } /* 功能： X 型 馬達分配驅動分配 */ enum{ L_F, L_B, R_B, R_F }。 pwm[L_F]=basedat + Xaxesdat + Yaxesdat Zaxesdat。 pwm[R_B]=basedat Xaxesdat Yaxesdat Zaxesdat。 updat_pwm(pwm)。 static void gpid_pro_xmode(float Xpidout,float Ypidout,float Zpidout) { = Xpidout fgyrox。 = Zpidout fgyroz。XGpid)。YGpid)。ZGpid)。 = (targetroll) cureulerroll。 /*限制誤差 */ if(MAX_ERR)=MAX_ERR。 if(MAX_ERR)=MAX_ERR。 if(MAX_ERR)=MAX_ERR。 /*歐拉角 PID*/ updat_pid(amp。 updat_pid(amp。 updat_pid(amp。 return OK。 if(basedat150)/*油門太小不控制 */ { gpid_pro_xmode(,)。 pwmyout = 。 } else { pid_init(amp。 pid_init(amp。 pid_init(amp。 pid_init(amp。 pid_init(amp。 pid_init(amp。 pwmxout =0。 pwmzout =0。 return OK。s Shaanxi province pass through a stop on the ancient Silk Road, Gansu39。80s. We sat on the back of pickup trucks for hours. The sky was blue, and we couldn39。t have a formal stage. The audience just sat on the grass. Usually, the performances became a big party with local people joining in. For him, the rewarding part about touring isn39。s performers of the troupe sti