四旋翼飛行器程序

《四旋翼飛行器程序》由會(huì)員分享，可在線閱讀，更多相關(guān)《四旋翼飛行器程序（16頁(yè)珍藏版）》請(qǐng)?jiān)谘b配圖網(wǎng)上搜索。

1、四旋翼自主飛行器(B題) 摘要 系統(tǒng)以R5F100LE作為四旋翼自主飛行器控制的核心，由電源模塊、電機(jī)調(diào)速控制模塊、傳感器檢測(cè)模塊、飛行器控制模塊等構(gòu)成。飛行控制模塊包括角度傳感器、陀螺儀，傳感器檢測(cè)模塊包括紅外障礙傳感器、超聲波測(cè)距模塊、TLS1401-LF模塊，瑞薩MCU綜合飛行器模塊和傳感器檢測(cè)模塊的信息，通過(guò)控制4個(gè)直流無(wú)刷電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)飛行器的欠驅(qū)動(dòng)系統(tǒng)飛行。在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)PID控制回路，即位置控制回路和姿態(tài)控制回路。測(cè)試結(jié)果表明系統(tǒng)可通過(guò)各個(gè)模塊的配合實(shí)現(xiàn)對(duì)電機(jī)的精確控制，具有平均速度快、定位誤差小、運(yùn)行較為穩(wěn)定等特點(diǎn)。

2、 目 錄 1 系統(tǒng)方案論證與控制方案的選擇 - 2 - 1.1 地面黑線檢測(cè)傳感器 - 2 - 1.2 電機(jī)的選擇與論證 - 2 - 1.3 電機(jī)驅(qū)動(dòng)方案的選擇與論證 - 3 - 2 四旋翼自主飛行器控制算法設(shè)計(jì) - 3 - 2.1 四旋翼飛行器動(dòng)力學(xué)模型 - 3 - 2.2 PID控制算法結(jié)構(gòu)分析 - 3 - 3 硬件電路設(shè)計(jì)與實(shí)現(xiàn) - 5 - 3.1飛行控制電路設(shè)計(jì) - 5 - 3.2 電源模塊 - 6 - 3.3 電機(jī)驅(qū)動(dòng)模塊 - 6 - 3.4 傳感器檢測(cè)模塊 - 7 - 4 系統(tǒng)的程序設(shè)計(jì) - 8 - 5 測(cè)試

3、與結(jié)果分析 - 9 - 5.1 測(cè)試設(shè)備 - 9 - 5.2 測(cè)試結(jié)果 - 9 - 6 總結(jié) - 10 - 附錄A 部分程序清單 - 11 - 1 系統(tǒng)方案論證與控制方案的選擇 根據(jù)題目要求，對(duì)該系統(tǒng)的特點(diǎn)及其控制特性進(jìn)行了分析，進(jìn)行了幾種不同設(shè)計(jì)方案的比較。 1.1 地面黑線檢測(cè)傳感器 探測(cè)地面黑線的基本原理是：光線照射到路面并反射，由于黑線和白色地面對(duì)光的反射系數(shù)不同，所以可以根據(jù)接收到的反射光強(qiáng)弱來(lái)判斷黑線。可實(shí)現(xiàn)的方案有： 方案一：采用普通發(fā)光二極管及光敏電阻組成的發(fā)射接收方案。該方案在實(shí)際使用時(shí)，容易受到外界光源的干擾，有時(shí)甚至檢測(cè)不到。主要是因?yàn)?/p>

4、可見(jiàn)光的反射效果跟地表的平坦程度、地表材料的反射情況均對(duì)檢測(cè)效果產(chǎn)生直接影響。雖然可采取超高高度發(fā)光二極管降低一定的干擾，但這又增加額外的功率損耗。 方案二：紅外避障傳感器E18-D80NK。這是一種集發(fā)射與接收于一體的光電傳感器，發(fā)射光經(jīng)過(guò)調(diào)制后發(fā)出，接收頭對(duì)反射光進(jìn)行解調(diào)輸出，有效的避免了可見(jiàn)光的干擾。透鏡的使用，也使得這款傳感器最遠(yuǎn)可以檢測(cè)80厘米距離。檢測(cè)障礙物的距離可以根據(jù)要求通過(guò)尾部的電位器旋鈕進(jìn)行調(diào)節(jié)。并且具有探測(cè)距離遠(yuǎn)、受可見(jiàn)光干擾小、價(jià)格便宜、易于裝配、使用方便等特點(diǎn)。 比較以上二種方案，方案二占有很大優(yōu)勢(shì)，不但能準(zhǔn)確完成測(cè)量，而且能避免電路的復(fù)雜性，因此選擇方案二。

5、1.2 電機(jī)的選擇與論證 四旋翼無(wú)人飛行器是通過(guò)控制四個(gè)不同無(wú)刷直流電機(jī)的轉(zhuǎn)速，達(dá)到控制四旋翼無(wú)人飛行器的飛行姿態(tài)和位置，與傳統(tǒng)直升機(jī)通過(guò)控制舵機(jī)來(lái)改變螺旋槳的槳距角，達(dá)到控制直升機(jī)的目的不同。在電機(jī)的選型上，主要有直流有刷電機(jī)和直流無(wú)刷電機(jī)兩種。 方案一：直流有刷電機(jī)是當(dāng)前普遍使用的一種直流電機(jī)，它的驅(qū)動(dòng)電路簡(jiǎn)單、控制方法成熟，但是直流有刷電機(jī)使用電刷進(jìn)行換向，換向時(shí)電刷與線圈觸電存在機(jī)械接觸，電機(jī)長(zhǎng)時(shí)間高速轉(zhuǎn)動(dòng)使極易因磨損導(dǎo)致電氣接觸不良等問(wèn)題，而且有刷電機(jī)效率低、力矩小、重量大，不適合對(duì)功率重量比敏感的電動(dòng)小型飛行器。 方案二：直流無(wú)刷電機(jī)能量密度高、力矩大、重量輕，采用非接觸式的

6、電子換向方法，消除了電刷磨損，較好地解決了直流有刷電機(jī)的缺點(diǎn)，適用于對(duì)功率重量比敏感的用途，同時(shí)增強(qiáng)了電機(jī)的可靠性。 所以選擇直流無(wú)刷電機(jī)作為動(dòng)力源。 1.3 電機(jī)驅(qū)動(dòng)方案的選擇與論證 方案一:采用電阻網(wǎng)絡(luò)或數(shù)字電位器調(diào)整電動(dòng)機(jī)的分壓，從而達(dá)到調(diào)速的目的。但是電阻網(wǎng)絡(luò)只能實(shí)現(xiàn)有級(jí)調(diào)速，而數(shù)字電阻的元器件價(jià)格比較昂貴。更主要的問(wèn)題在于一般電動(dòng)機(jī)的電阻很小，但電流很大；分壓不僅會(huì)降低效率，而且實(shí)現(xiàn)很困難。 方案二:采用繼電器對(duì)電動(dòng)機(jī)的開(kāi)或關(guān)進(jìn)行控制，通過(guò)開(kāi)關(guān)的切換對(duì)小車的速度進(jìn)行調(diào)整。這個(gè)方案的優(yōu)點(diǎn)是電路較為簡(jiǎn)單，缺點(diǎn)是繼電器的響應(yīng)時(shí)間慢、機(jī)械結(jié)構(gòu)易損壞、壽命較 短、可靠性不高。

7、 方案三:采用全橋驅(qū)動(dòng)PWM電路。這種驅(qū)動(dòng)的優(yōu)點(diǎn)是使管子工作在占空比可調(diào)的開(kāi)關(guān)狀態(tài)，提高使用效率實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的微調(diào)。并且保證了可以簡(jiǎn)單的方式實(shí)現(xiàn)方向控制。 基于上述理論分析，選擇方案三。 2 四旋翼自主飛行器控制算法設(shè)計(jì) 2.1 四旋翼飛行器動(dòng)力學(xué)模型 設(shè)計(jì)的小型四旋翼飛行器適用于室內(nèi)低速飛行，因此忽略空氣阻力的影響。因此，簡(jiǎn)化后的飛行器動(dòng)力學(xué)模型為 式1-1 式中為四旋翼飛行器在導(dǎo)航坐標(biāo)系下的線位移，為運(yùn)動(dòng)加速度，m為飛行器質(zhì)量，分別為機(jī)體的偏航角、俯仰角和橫滾角，為旋翼面中心到四旋翼飛行器質(zhì)心的距離， 為軸向慣性主矩。該動(dòng)力學(xué)模型對(duì)

8、四旋翼飛行器的真實(shí)飛行狀態(tài)進(jìn)行了合理的簡(jiǎn)化，忽略了空氣阻力等對(duì)系統(tǒng)運(yùn)行影響較小的參數(shù)，使得飛行控制算法更加簡(jiǎn)潔。 2.2 PID控制算法結(jié)構(gòu)分析 在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)控制回路，即位置控制回路和姿態(tài)控制回路。算法結(jié)構(gòu)如圖B-1所示。 給定位置 位置控制 姿 態(tài) 控 制 電機(jī)控制 飛行器機(jī)體 姿態(tài)控制回路 位置控制回路 圖2-1 四旋翼飛行器控制算法結(jié)構(gòu)圖 使用經(jīng)典PID控制算法實(shí)現(xiàn)位置控制回路和姿態(tài)控制回路。PID算法簡(jiǎn)單可靠，理論體系完備，而且在長(zhǎng)期的應(yīng)用過(guò)程中積攢了大量的使用經(jīng)

9、驗(yàn)，在飛行器位置和姿態(tài)控制應(yīng)用中具有良好的控制效果和較強(qiáng)的魯棒性，能提供控制量的較優(yōu)解。 控制回路包含了三個(gè)控制量，因此設(shè)計(jì)3個(gè)獨(dú)立的PID控制器對(duì)位移進(jìn)行控制。根據(jù)PID控制器的原理，設(shè)分別為比例項(xiàng)、積分項(xiàng)和微分項(xiàng)系數(shù)，有 式1-2 其中，為航姿參考系統(tǒng)測(cè)量到的加速度積分得到的位移量。 姿態(tài)控制回路的作用是控制四旋翼飛行器的飛行姿態(tài)，使其實(shí)際姿態(tài)與設(shè)定的姿態(tài)一致。姿態(tài)控制回路有偏航角、俯仰角和橫滾角三個(gè)控制量，在此忽略三個(gè)通道之間的耦合效應(yīng)，設(shè)計(jì)3個(gè)獨(dú)立的PID控制器對(duì)每個(gè)量進(jìn)行獨(dú)立控制。

10、 式1-3 根據(jù)PID控制器的原理，設(shè)分別為比例項(xiàng)、積分項(xiàng)和微分項(xiàng)系數(shù)，有控制器方程如B-4。 式1-4 其中比例項(xiàng)系數(shù)為3.3積分項(xiàng)系數(shù)為0.14微分項(xiàng)系數(shù)為3.2。 3 硬件電路設(shè)計(jì)與實(shí)現(xiàn) 系統(tǒng)以R5F100LE單片機(jī)為核心，主要包括電源模塊、電機(jī)驅(qū)動(dòng)模塊、飛行控制模塊、傳感器檢測(cè)等功能模塊，該系統(tǒng)硬件結(jié)構(gòu)框圖如圖2-2所示。 飛 行 控 制 模 塊 電源模塊 數(shù)據(jù)轉(zhuǎn)發(fā)控制器 陀螺儀 角度傳感器 紅外避障傳感器 超聲波傳感器 電子調(diào)速器 無(wú)

11、刷直流電機(jī)1 電子調(diào)速器 無(wú)刷直流電機(jī)2 電子調(diào)速器 無(wú)刷直流電機(jī)3 電子調(diào)速器 無(wú)刷直流電機(jī)4 鍵盤 羅盤 圖 2-2 系統(tǒng)硬件結(jié)構(gòu)圖 3.1飛行控制電路設(shè)計(jì) 飛行控制模塊是控制系統(tǒng)的核心部分。它在每個(gè)控制周期內(nèi)實(shí)時(shí)處理傳感器采集的數(shù)據(jù)和飛行器的姿態(tài)信息，完成PID控制的算法，得到四旋翼飛行器的姿態(tài)和位置信息，計(jì)算出控制量，轉(zhuǎn)化為相應(yīng)的控制信號(hào)經(jīng)驅(qū)動(dòng)電路后驅(qū)動(dòng)四個(gè)電機(jī)工作，保持四旋翼飛行器穩(wěn)定飛行。電路圖如圖2-3所示。 圖2-3 飛行控制器電路原理圖 3.2 電源模塊 電源由一塊11.1V 2200ma的鋰電池（重量約為166克）供電，在由電調(diào)降壓給系統(tǒng)

12、中的各個(gè)模塊供5v電壓并給電機(jī)提供電流，這樣可滿足可滿足各個(gè)小系統(tǒng)的電源要求。 3.3 電機(jī)驅(qū)動(dòng)模塊 四電機(jī)驅(qū)動(dòng)模塊根據(jù)中心控制模塊指令驅(qū)動(dòng)各個(gè)電機(jī)到達(dá)指定轉(zhuǎn)速，將電機(jī)的速度通過(guò)測(cè)速反饋裝置反饋給飛行姿態(tài)控制模塊，控制無(wú)刷直流電機(jī)閉環(huán)控制轉(zhuǎn)速，從而控制飛行狀態(tài)，達(dá)到預(yù)期位置和姿態(tài)。通過(guò)電子調(diào)速器給電機(jī)提供電流，電子調(diào)速器模塊電路圖如圖2-4所示。 圖2-4 電子調(diào)速器電流圖 3.4 傳感器檢測(cè)模塊 傳感器模塊是為四旋翼飛行器的飛行控制提供各種飛行參數(shù)的裝置，包括測(cè)量機(jī)身三軸角速率的陀螺儀、測(cè)量機(jī)身三軸線加速度的加速度傳感器、測(cè)量機(jī)身航向及姿態(tài)信息的羅盤，電機(jī)轉(zhuǎn)速檢測(cè)的測(cè)速傳感

13、器、飛行高度傳感器和黑線檢測(cè)傳感器。其中角度傳感器和陀螺儀電路圖如圖2-5和2-6所示。 圖B-5 角度傳感器電路圖 圖2-6 陀螺儀電路圖 4 系統(tǒng)的程序設(shè)計(jì) 本系統(tǒng)以瑞薩MCU為核心，采用C語(yǔ)言對(duì)單片機(jī)進(jìn)行編程。主程序主要起導(dǎo)向和決策的作用，系統(tǒng)的控制總流程圖如圖所示。系統(tǒng)包括延時(shí)子程序，電機(jī)轉(zhuǎn)速控制子程序，檢測(cè)子程序，副翼子程序。系統(tǒng)控制的總流程圖如圖2-7所示。部分程序清單見(jiàn)附錄A。 按鍵開(kāi)始 系統(tǒng)初始化 上升定高 校正姿態(tài) 前行 紅外傳感器1是否檢測(cè)到黑線？ 計(jì)黑線值 減速降落 Y N 紅外傳感器2是否檢測(cè)到黑線？ 黑線值=3？

14、 尋點(diǎn) Y N Y 圖2-7 系統(tǒng)主程序流程圖 5 測(cè)試與結(jié)果分析 5.1 測(cè)試設(shè)備 模擬區(qū)域：按題目要求設(shè)計(jì)。 設(shè)備：秒表、卷尺。 測(cè)試環(huán)境為室內(nèi)。 5.2 測(cè)試結(jié)果 使系統(tǒng)運(yùn)行，參照題目的各項(xiàng)要求進(jìn)行多次實(shí)際定量測(cè)量，得到該系統(tǒng)的測(cè)量結(jié)果如表表B-1所示。 表B-1 測(cè)試功能、測(cè)試工具及實(shí)現(xiàn)的情況表 具體功能要求 實(shí)際測(cè)試結(jié)果 基本部分 四旋翼自主飛行器一鍵式啟動(dòng)，從A區(qū)飛向B區(qū)，在B區(qū)降落并停機(jī)；飛行時(shí)間不大于45s 可以滿足，飛行時(shí)間為15s 四旋翼自主飛行器一鍵式啟動(dòng)，從B區(qū)飛向A區(qū)，在A區(qū)降落并停機(jī)；飛行時(shí)間不大于45

15、s 可以滿足要求，飛行時(shí)間為17s 發(fā)揮部分 飛行器擺放在A區(qū)，飛行器下面擺放一薄鐵片，一鍵式啟動(dòng)，飛行器拾取薄鐵片并起飛 飛行器攜帶薄鐵片從示高線上方飛向B區(qū)，并在空中將薄鐵片投放到B區(qū)；飛行器從示高線上方返回A區(qū)，在A區(qū)降落并停機(jī) 以上往返飛行時(shí)間不大于30s 其他 最為完成發(fā)揮部分但以實(shí)現(xiàn)其在一個(gè)水平高度前行 經(jīng)測(cè)試，系統(tǒng)可以滿足基本要求和部分發(fā)揮部分，并具有穩(wěn)定前行的特點(diǎn)。 6 總結(jié) 本次設(shè)計(jì)按照題目要求，采用模塊化的硬件和軟件設(shè)計(jì)方法，運(yùn)用PID控制算法，成功地實(shí)現(xiàn)了飛行器尋黑帶自主飛行并定點(diǎn)降落的要求。具有飛行穩(wěn)定、運(yùn)動(dòng)迅速、接收信號(hào)靈敏等特點(diǎn)。完成

16、了題目基本部分和部分發(fā)揮部分的要求。 附錄A 部分程序清單 /*********************************************************************************************************************** * DISCLAIMER * This software is supplied by Renesas Electronics Corporation and is only * intended for use with Renesas products. No other

17、 uses are authorized. This * software is owned by Renesas Electronics Corporation and is protected under * all applicable laws, including copyright laws. * THIS SOFTWARE IS PROVIDED "AS IS" AND RENESAS MAKES NO WARRANTIES REGARDING * THIS SOFTWARE, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLU

18、DING BUT NOT * LIMITED TO WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE * AND NON-INFRINGEMENT. ALL SUCH WARRANTIES ARE EXPRESSLY DISCLAIMED. * TO THE MAXIMUM EXTENT PERMITTED NOT PROHIBITED BY LAW, NEITHER RENESAS * ELECTRONICS CORPORATION NOR ANY OF ITS AFFILIATED COMPANI

19、ES SHALL BE LIABLE * FOR ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES FOR * ANY REASON RELATED TO THIS SOFTWARE, EVEN IF RENESAS OR ITS AFFILIATES HAVE * BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. * Renesas reserves the right, without notice, to make changes to thi

20、s software * and to discontinue the availability of this software. By using this software, * you agree to the additional terms and conditions found by accessing the * following link: * * * Copyright (C) 2011, 2013 Renesas Electronics Corporation. All rights reserved. ********************

21、***************************************************************************************************/ /*********************************************************************************************************************** * File Name : r_cg_timer_user.c * Version : CodeGenerator for RL7

22、8/G13 V2.00.00.07 [22 Feb 2013] * Device(s) : R5F100LE * Tool-Chain : CA78K0R * Description : This file implements device driver for TAU module. * Creation Date: 2013/9/7 **********************************************************************************************************************

23、*/ /*********************************************************************************************************************** Pragma directive ***********************************************************************************************************************/ #pragma interrupt INTTM00 r_tau0

24、_channel0_interrupt #pragma interrupt INTTM01 r_tau0_channel1_interrupt #pragma interrupt INTTM02 r_tau0_channel2_interrupt #pragma interrupt INTTM03 r_tau0_channel3_interrupt #pragma interrupt INTTM04 r_tau0_channel4_interrupt #pragma interrupt INTTM05 r_tau0_channel5_interrupt #pragma interr

25、upt INTTM06 r_tau0_channel6_interrupt #pragma interrupt INTTM07 r_tau0_channel7_interrupt /* Start user code for pragma. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ /******************************************************************************

26、***************************************** Includes ***********************************************************************************************************************/ #include "r_cg_macrodriver.h" #include "r_cg_timer.h" /* Start user code for include. Do not edit comment generated here */

27、 /* End user code. Do not edit comment generated here */ #include "r_cg_userdefine.h" #include "r_cg_kongzhi.h" #include "r_cg_lcd.h" /*********************************************************************************************************************** Global variables and functions *******

28、****************************************************************************************************************/ /* For TAU0_ch0 pulse measurement */ volatile uint32_t g_tau0_ch0_width = 0U; /* Start user code for global. Do not edit comment generated here */ uint32_t oko=0; /* End user code.

29、Do not edit comment generated here */ /*********************************************************************************************************************** * Function Name: r_tau0_channel0_interrupt * Description : This function is INTTM00 interrupt service routine. * Arguments : None

30、* Return Value : None ***********************************************************************************************************************/ __interrupt static void r_tau0_channel0_interrupt(void) { if ((TSR00 & _0001_TAU_OVERFLOW_OCCURS) == 1U) /* overflow occurs */ {

31、 g_tau0_ch0_width = (uint32_t)(TDR00 + 1U) + 0x10000U; } else { g_tau0_ch0_width = (uint32_t)(TDR00 + 1U); } /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /*********************

32、************************************************************************************************** * Function Name: r_tau0_channel1_interrupt * Description : This function is INTTM01 interrupt service routine. * Arguments : None * Return Value : None ****************************************

33、*******************************************************************************/ __interrupt static void r_tau0_channel1_interrupt(void) { /* Start user code. Do not edit comment generated here */ TDR05=2980; delayms(500); TDR05=3025; dela

34、yms(100); TDR05=2980; delayms(500); TDR05=3040; delayms(50); oko++; if(oko++>=10){R_TAU0_Channel1_Stop();z(2040);} //xianshi(); /* End user code. Do not edit comment generated here */

35、 } /*********************************************************************************************************************** * Function Name: r_tau0_channel2_interrupt * Description : This function is INTTM02 interrupt service routine. * Arguments : None * Return Value : None ***********

36、************************************************************************************************************/ __interrupt static void r_tau0_channel2_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ }

37、/*********************************************************************************************************************** * Function Name: r_tau0_channel3_interrupt * Description : This function is INTTM03 interrupt service routine. * Arguments : None * Return Value : None ******************

38、*****************************************************************************************************/ __interrupt static void r_tau0_channel3_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /******

39、***************************************************************************************************************** * Function Name: r_tau0_channel4_interrupt * Description : This function is INTTM04 interrupt service routine. * Arguments : None * Return Value : None *************************

40、**********************************************************************************************/ __interrupt static void r_tau0_channel4_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /*************

41、********************************************************************************************************** * Function Name: r_tau0_channel5_interrupt * Description : This function is INTTM05 interrupt service routine. * Arguments : None * Return Value : None ********************************

42、***************************************************************************************/ __interrupt static void r_tau0_channel5_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /********************

43、*************************************************************************************************** * Function Name: r_tau0_channel6_interrupt * Description : This function is INTTM06 interrupt service routine. * Arguments : None * Return Value : None ***************************************

44、********************************************************************************/ __interrupt static void r_tau0_channel6_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /***************************

45、******************************************************************************************** * Function Name: r_tau0_channel7_interrupt * Description : This function is INTTM07 interrupt service routine. * Arguments : None * Return Value : None **********************************************

46、*************************************************************************/ __interrupt static void r_tau0_channel7_interrupt(void) { /* Start user code. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ } /* Start user code for adding. Do not edit comment generated here */ /* End user code. Do not edit comment generated here */ - 15 -