【文章內(nèi)容簡介】 從測試結果中沒有發(fā)現(xiàn)嚴重系統(tǒng)缺陷。 五、結論 本系統(tǒng)完成了硬件電路的設計并結合了軟件開發(fā)程序，使四旋翼飛行器能夠一鍵啟動后，平衡穩(wěn)定的飛行任務。通過各種方案的討論和嘗試，在經(jīng)過多次的整體軟硬件結合調(diào)試，不斷地對系統(tǒng)進行優(yōu)化，設計采用簡單的動力學原 理結合軟件開發(fā)平臺，就能夠簡單高效的完成任務，若采用其他類型的功能傳感器，功能會更加強大，且若將算法設計精準，功能完成會更加流暢。 六、參考文獻 ??1 黃智偉 .全國大學生電子設計競賽訓練教程 .電子工業(yè)出版社， 2022 ??2 李曉林 .單片機原理與接口技術 .電子工業(yè)出版社， 2022 ??3 黃智偉 .全國大學生電子設計競賽訓練教程 .電子工業(yè)出版社， 2022 ??4 黃智偉 .全國大學生電子設計競賽訓練教程 .電子工業(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ù)量 備注 1 四旋翼飛行器 1 2 MCU R5F100LEA 1 16位微處理器 3 陀螺儀 MPU6050 1 4 超聲傳感器 HCSR04 1 程序清單 1 主函數(shù) void main(void) { R_MAIN_UserInit()。 /* Start user code. Do not edit ment generated here */ /*????????*/ 17 while(get_systemtimer()5000)。 //wait 5 sec //clr_systemtimer()。 curatt_init()。 power=750。 delaytime=0。 while (1U) { task_mpu6050()。 if(OK==get_mpu6050_mode()) { atttitude_calculate(4000,fgyrox,fgyroy,fgyroz)。 if(flag!=0) { flag=0。 } else { flag=1。 q4_to_euler_rpy(amp。curattX,amp。currrpy)。 euler_control(amp。currrpy,amp。target)。 } control_task(power)。 delaytime++。 if(delaytime500) { delaytime=600。 //power=0。 } } 。 } /* End user code. Do not edit ment generated here */ } 子函數(shù) 1. include include Q4 curattX={1,0,0,0}。 RPY currrpy={0,0,0}。 18 float math_rsqrt(float number) { long i。 float x2, y。 x2 = number * 。 y = number。 i = * ( long * ) amp。y。 // evil floating point bit level hacking i = 0x5f3759df ( i 1 )。 // what the fuck y = * ( float * ) amp。i。 y = y * ( ( x2 * y * y ) )。 // 1st iteration return y。 } void curatt_init(void) { = 1。 = 0。 = 0。 = 0。 } void atttitude_calculate(unsigned int timer_us,float gx,float gy,float gz) { float a_rsqrt。 float qa,qb,qc。 float timesec=timer_us*。 gx *= ( * timesec)。 // premultiply mon factors gy *= ( * timesec)。 gz *= ( * timesec)。 qa = 。 qb = 。 qc = 。 += (qb * gx qc * gy * gz)。 += (qa * gx + qc * gz * gy)。 += (qa * gy qb * gz + * gx)。 += (qa * gz + qb * gy qc * gx)。 a_rsqrt = math_rsqrt(* + * + * + *)。 *= a_rsqrt。 *= a_rsqrt。 *= a_rsqrt。 19 *= a_rsqrt。 } void q4_to_euler_rpy(Q4 *dat,RPY* ang) { float gx, gy, gz,x_2,w_2。 gx = 2 * (datx*datz datw*daty)。 gy = 2 * (datw*datx + daty*datz)。 x_2 = datx*datx。 w_2 = datw*datw。 gz = w_2 x_2 daty*daty + datz*datz。 gz*= gz。 angyaw = 1*atan2(2*datx*daty 2*datw*datz, 2*w_2 + 2*x_2 1)。// * 180 / PI。 angroll =1* atan(gx* math_rsqrt(gy*gy + gz))。// * 180 / PI。 angpitch = atan(gy* math_rsqrt(gx*gx + gz))。// * 180 / PI。 } 2. static void updat_pid(PIDP* pid) { float ilimit1=pidilimit*(1)。 pidistate+=(piderrdat)*pidiGain。 if(pidistatepidilimit)pidistate=pidilimit。 if(pidistateilimit1)pidistate=ilimit1。 pidpidout = pidpGain*piderrdat+pidistate+(piderrdatpidperr)*piddGain。 pidperr=piderrdat。 } static void pid_init(PIDP* pid) { pidistate=0。 pidperr=0。 piderrdat=0。 pidpidout=0。 } void control_init(void) { curatt_init()。 check_mpu6050()。 pid_init(amp。Xaxespid)。 pid_init(amp。Yaxespid)。 pid_init(amp。Zaxespid)。 pid_init(amp。XGpid)。 20 pid_init(amp。YGpid)。 pid_init(amp。ZGpid)。 } /* 功能： X 型 馬達分配驅動分配 */ enum{ L_F, L_B, R_B, R_F }。 static void x_mode_updat_motodat(S16 basedat,S16 Xaxesdat,S16 Yaxesdat,S16 Zaxesdat) { S16 pwm[4]。 pwm[L_F]=basedat + Xaxesdat + Yaxesdat Zaxesdat。 pwm[L_B]=basedat Xaxesdat + Yaxesdat + Zaxesdat。 pwm[R_B]=basedat Xaxesdat Yaxesdat Zaxesdat。 pwm[R_F]=basedat + Xaxesdat Yaxesdat + Zaxesdat。 updat_pwm(pwm)。 }。 static void gpid_pro_xmode(float Xpidout,float Ypidout,float Zpidout) { = Xpidout fgyrox。 = Ypidout fgyroy。 = Zpidout fgyroz。 updat_pid(amp。XGpid)。 updat_pid(amp。YGpid)。 updat_pid(amp。ZGpid)。 } /* 功能：歐拉角控制 角度單環(huán)控制 */ define MAX_ERR U8 euler_control(RPY *cureuler,RPY *target) { /*取誤差 */ = (targetyaw) cureuleryaw。 = (targetroll) cureulerroll。 21 = (targetpitch) cureulerpitch。 /*限制誤差 */ if(MAX_ERR)=MAX_ERR。 else if(MAX_ERR)=MAX_ERR。 if(MAX_ERR)=MAX_ERR。 else if(MAX_ERR)=MAX_ERR。 if(MAX_ERR)=MAX_ERR。 else if(MAX_ERR)=MAX_ERR。 /*歐拉角 PID*/ updat_pid(amp。Xaxespid)。 updat_pid(amp。Yaxespid)。 updat_pid(amp。Zaxespid)。 return OK。 } U8 control_task(S16 basedat) { S16 pwmxout,pwmyout,pwmzout。 if(basedat150)/*油門太小不控制 */ { gpid_pro_xmode(,)。 pwmxout = 。 pwmyout = 。 pwmzout = 。 } else { pid_init(amp。Xaxespid)。 pid_init(amp。Yaxespid)。 pid_init(amp。Zaxespid)。 pid_init(amp。XGpid)。 pid_init(amp。YGpid)。 pid_init(amp。ZGpid)。 pwmxout =0。 pwmyout =0。 pwmzout =0。 } x_mode_updat_motodat(basedat,pwmxout,pwmyout,pwmzout)。