【正文】 。 21 = (targetpitch) cureulerpitch。 } /* 功能：歐拉角控制 角度單環(huán)控制 */ define MAX_ERR U8 euler_control(RPY *cureuler,RPY *target) { /*取誤差 */ = (targetyaw) cureuleryaw。 updat_pid(amp。 updat_pid(amp。 updat_pid(amp。 = Ypidout fgyroy。 }。 pwm[R_F]=basedat + Xaxesdat Yaxesdat + Zaxesdat。 pwm[L_B]=basedat Xaxesdat + Yaxesdat + Zaxesdat。 static void x_mode_updat_motodat(S16 basedat,S16 Xaxesdat,S16 Yaxesdat,S16 Zaxesdat) { S16 pwm[4]。ZGpid)。YGpid)。XGpid)。Zaxespid)。Yaxespid)。Xaxespid)。 check_mpu6050()。 pidpidout=0。 pidperr=0。 pidperr=piderrdat。 if(pidistateilimit1)pidistate=ilimit1。 pidistate+=(piderrdat)*pidiGain。// * 180 / PI。// * 180 / PI。// * 180 / PI。 gz*= gz。 w_2 = datw*datw。 gy = 2 * (datw*datx + daty*datz)。 } void q4_to_euler_rpy(Q4 *dat,RPY* ang) { float gx, gy, gz,x_2,w_2。 *= a_rsqrt。 *= a_rsqrt。 += (qa * gz + qb * gy qc * gx)。 += (qa * gx + qc * gz * gy)。 qc = 。 qa = 。 // premultiply mon factors gy *= ( * timesec)。 float timesec=timer_us*。 } void atttitude_calculate(unsigned int timer_us,float gx,float gy,float gz) { float a_rsqrt。 = 0。 } void curatt_init(void) { = 1。 y = y * ( ( x2 * y * y ) )。 // what the fuck y = * ( float * ) amp。y。 y = number。 float x2, y。 RPY currrpy={0,0,0}。 } } 。 if(delaytime500) { delaytime=600。 } control_task(power)。currrpy,amp。currrpy)。 q4_to_euler_rpy(amp。 if(flag!=0) { flag=0。 while (1U) { task_mpu6050()。 power=750。 //wait 5 sec //clr_systemtimer()。 六、參考文獻 ??1 黃智偉 .全國大學(xué)生電子設(shè)計競賽訓(xùn)練教程 .電子工業(yè)出版社， 2022 ??2 李曉林 .單片機原理與接口技術(shù) .電子工業(yè)出版社， 2022 ??3 黃智偉 .全國大學(xué)生電子設(shè)計競賽訓(xùn)練教程 .電子工業(yè)出版社， 2022 ??4 黃智偉 .全國大學(xué)生電子設(shè)計競賽訓(xùn)練教程 .電子工業(yè)出版社， 2022 ??5 Zufferey, D. Floreano Evolving VisionBased Flying Robots. Proceedings of the 2nd International Workshop on Biologically Motivated Computer Vision, LNCS 2525, pp. 592600, Berlin, SpringerVerlag,2022. 16 ??6 A. Elfes, . Bueno and al, Robotic Airship for Exploration of plaary Bodies with an Atmosphere Autonomy Challenges. Autonomous Robots Journal: Kluwer Academic Publishers, 2022. ??7 S. Sastry, A mathematical introduction to robotic manipulation . Boca Raton, FL, 1994. ??8 P. Mllhaupt, Analysis and Control of Underactuated Mechanical Nonminimumphase Systems. Phd thesis, Department of MechanicalEngineering, EPFL, 1999. 七、附錄 儀器設(shè)備清單 序號 名稱 型號 數(shù)量 備注 1 四旋翼飛行器 1 2 MCU R5F100LEA 1 16位微處理器 3 陀螺儀 MPU6050 1 4 超聲傳感器 HCSR04 1 程序清單 1 主函數(shù) void main(void) { R_MAIN_UserInit()。 五、結(jié)論 本系統(tǒng)完成了硬件電路的設(shè)計并結(jié)合了軟件開發(fā)程序，使四旋翼飛行器能夠一鍵啟動后，平衡穩(wěn)定的飛行任務(wù)。 15 測試方法： 數(shù)字萬用表主要用來測試分立元件的電阻、壓降、漏電流、截止 /導(dǎo)通狀態(tài)等參數(shù)； 示波器用于測試各 PWM波形； 秒表用于產(chǎn)品測試，按照基本要求對制成的四旋翼飛行器進行飛行時間的測量； R5F100LEA仿真器用來測試軟件； 測試結(jié)果： 測試結(jié)果顯示本系統(tǒng)以實現(xiàn)需求中基本功能，能滿足在時間限度里飛行所要求的距離，不足之處在于系統(tǒng)功能相對不夠完善，在傳感器模塊還需要進行完善。但開關(guān)電源會產(chǎn)生較強的電磁干擾，而且存在電源波紋，這樣對 PPM的遙控方式影響就很大。由于四旋翼飛行器載重量有限，所以電源系統(tǒng)應(yīng)該盡可能 輕，能量密度大。采用比例控制規(guī)律能較快地克服擾動的影響，它的作用于輸出值較 快， 但不能很好穩(wěn)定在一個理想的數(shù)值，不良的結(jié)果是雖較能有效的克服擾動的影響， 但有 余差出現(xiàn)。積分，反應(yīng)系統(tǒng)的累計偏差，使系統(tǒng)消除穩(wěn)態(tài)誤差，提高無差度，因為有差，積分調(diào)節(jié)就進行，直至無誤差；微分，具有預(yù)見性，能預(yù)見偏差變化的趨勢，產(chǎn)生超前的控制作用，在偏差還沒有形成之前，已被微分調(diào)節(jié)作用消除，因此可以改善系統(tǒng) 的動態(tài)性能。但并不是必須同時具備這三種算法，也可以 是 PD,PI,甚至可以只有 P算法控制。 超聲波傳感器高度控制 超聲波傳感器高度控制是通過 PID 閉環(huán)控制算法實現(xiàn)的，這種控制采 用閉合反饋，通過超聲波傳感器發(fā)射的時間和收到反射波的時間差，計算距離，反饋給主控芯片，同時控制四個電機的轉(zhuǎn)速。在積分前，這個錯誤信號減去了初始角速度信號。這架旋翼機的姿態(tài)是確定利用互補濾波器為每個旋轉(zhuǎn)軸。 飛行姿態(tài)的測量 對無人機的控制來說測量無人機的飛行姿態(tài)是勢必不可少。(2)模塊自動發(fā)送 8個 40khz 的方波，自動檢測是否有信號返回； (3)有信號返回，通過 IO 口 ECHO 輸出一個高電平，高電平持續(xù)的時間就是超聲波從發(fā)射到返回的時間。 具有可提供 2cm400cm的非接觸式距離感測功能， 測距精度可達高到 3mm。 MPU6050中陀螺儀和加速度計分別用了三個 16位的 ADC，將其測量的模擬量轉(zhuǎn)化為數(shù)字量。 5%，邏輯接口 VVDIO 供電為 177。 5%、 177。產(chǎn)品傳輸可透過最高至 400kHz 的 IC 或最高達 20MHz 的 SPI（ MPU6050沒有 SPI）。 8g 與177。 2g、177。 2022176。 500、177。 MPU6000（ 6050）的角速度全格感測范圍為177。 MPU 6050 如圖 6 11 圖 6 MPU6050引腳圖 MPU6050為全球首例整合性 6軸運動處理組件，采用最小最薄包裝 ( QFN) 符合 RoHS 及環(huán)境標準，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝空間。本產(chǎn)品相較于 C8051有以下特點：擁有 15個端口，采用引腳復(fù)用方式，既節(jié)省了引腳資源，又拓展了功能。 2. 系統(tǒng)硬件芯片的選擇 飛行器控制板主控芯片 R5F100LEA，如圖 5所示。因此運動方程變成了： )s i ns i nc o ss i n( c o s1 ????? ?? ux?? lu2??? )s i ns i nc o ss i n( s i n1 ????? ?? uy?? lu3??? guz ?? )c o s(c o s1 ???? 4u??? 10 假設(shè)重心在對角線的交點處，若重心向上（或向下）移動 d單位，則角速度對力更不敏感，從而穩(wěn)定性增加了。為了簡單起見，我們定義輸入為： 34321424321314321243211/)(/)(/)(/)(JFFFFCuJFFFFuJFFFFumFFFFu?????????????????? iJ 是對 于軸的轉(zhuǎn)動慣量， C 是力與力矩的比例因子。兩個旋翼的旋轉(zhuǎn)方向是順時針方向，另外兩個是逆時針的，為了平衡力矩并且實現(xiàn)所需要的偏航運動 。這些力矩已通過實驗觀察到在低速情況下與力是呈線性的。因此一個好的控制器應(yīng)能達到預(yù)期的偏航角，同時保持固定的傾斜角度和高度。這也可以被用來產(chǎn)生預(yù)期的偏航運動。常規(guī)直升機有尾槳，以平衡由主旋翼產(chǎn)生的力矩。類似的，沿著 Y軸的運動與在 X軸方向上傾斜有關(guān)。這種傾斜可通過降低旋翼 2的速度，增加旋翼 4的速度來實現(xiàn)。 圖 4 四旋翼三維受力圖 直升機的垂直運動可以在同一時間通過改變所有的轉(zhuǎn)子速度來實現(xiàn)。 8 圖 3 四旋翼飛行器沿各自由度的運動 四旋翼直升機的動力學(xué)原理 不像普通的直升機有可變螺距角度，四旋翼直升機由固定俯仰角轉(zhuǎn)子和轉(zhuǎn)子速度控制，以產(chǎn)生預(yù)期的升力。當然在圖 b圖 c中，飛行器在產(chǎn)生俯仰、翻滾運動的同時也會產(chǎn)生沿 x、 y軸的水平運動。按圖 b的理論，飛行器首先發(fā)生一定程度的傾斜，從而使旋翼拉力產(chǎn)生水平分量，因此可以實現(xiàn)飛行器的前飛運動。 圖 3（ e）前后運動：要想實現(xiàn)飛行器在水平面內(nèi)前后、左右的運動，必須在水 平面內(nèi)對飛行器施加一定的力。反扭矩的大小與旋翼轉(zhuǎn)速有關(guān)，當四個電機轉(zhuǎn)速相同時，四個 7 旋翼產(chǎn)生的反扭矩相互平衡，四旋翼飛行器不發(fā)生轉(zhuǎn)動；當四個電機轉(zhuǎn)速不完全相同時，不平衡的反扭矩會引起四旋翼飛行器轉(zhuǎn)動。 圖 3（ d）偏航運動：四旋翼飛行器偏航運動可以借助旋翼產(chǎn)生的反扭矩來實現(xiàn)。由于旋翼 1的升力上 升，旋翼 3的升力下降，產(chǎn)生的不平衡力矩使機身繞 y軸旋轉(zhuǎn)（方向如圖所示），同理，當電機 1的轉(zhuǎn)速下降，電機 3的轉(zhuǎn)速上升，機身便繞 y軸向另一個方向旋轉(zhuǎn)， 實現(xiàn)飛行器的俯仰運動。 圖 3（ b）俯仰運動：電機 1的轉(zhuǎn)速上升，電機 3的轉(zhuǎn)速下降，電機 電機 4的轉(zhuǎn)速保持不變。當外界擾動量為零時，在旋翼產(chǎn)生的升力等于飛行器的自重時，飛行器便保持懸停狀態(tài)。圖 3（ a）垂直運動：垂直運動相對來說比較容易。 前 6 圖 2 四旋翼飛行器的結(jié)構(gòu)形式 基本運動狀態(tài)分別是： 1垂直運動； 2俯仰運動； 3滾轉(zhuǎn)運動； 4偏航運動； 5前后運動； 6側(cè)向運動。與傳統(tǒng)的直升機相比，四旋翼飛行器有下列優(yōu)勢：各個旋翼對機身所施加的反扭矩與旋翼的旋轉(zhuǎn)方向相反，因此當電機 1 和電機 3逆時針旋轉(zhuǎn)的同時，電機 2和電機 4順時針旋轉(zhuǎn)，可以平衡旋翼對機身的反扭矩。綜上所述，采用了十字模式算法。十字模式算法簡單，容易操作，飛行平穩(wěn)。由于飛行器是通過改變旋翼轉(zhuǎn)速實現(xiàn)升 力變化， 按照旋翼布置方式可分為十字模式和 X模式，如圖 l所示， 四旋翼飛行器只有四個輸入力，同時卻有六個狀態(tài)輸出，是一種六自由度的垂直升降機，因此這種欠驅(qū)動系統(tǒng)非常適合靜態(tài)和準靜態(tài)條件下飛行。飛行器動作依靠 4個電機的轉(zhuǎn)速差進行控制