【文章內(nèi)容簡(jiǎn)介】 的結(jié)果是獲得的PWM波頻率為244HZ。而在調(diào)節(jié)PWM高電平時(shí)間的時(shí)候，只需分別向OCR1A、OCR1B、OCR2A、OCR1B內(nèi)寫入相應(yīng)數(shù)據(jù)即可從對(duì)應(yīng)的PWM輸出端口獲得不同高電平時(shí)間的244HZPWM波。輸出高電平時(shí)間 （單位：s）的計(jì)算公式如下所示（以O(shè)CR1A為例）。//**************** //PWM初始化模塊 //**************** voidpwm_init(void){ //***************8bit T/C2 初始化*******************/ TCCR2A = (1COM2A1) | (1COM2B1) | (1WGM21) | (1WGM20)。 //設(shè)置為快速PWM TCCR2B = (1CS22) | (1CS21)。 //256 prescaleing //***************16bit T/C1初始化*******************/ TCCR1A = (1COM1A1) | (1COM1B1) | (1WGM10) 。 //設(shè)置為快速PWM TCCR1B = (1WGM12) | (1CS11) 。//256 prescaleing } //**************** //電機(jī)1的PWM輸出 //**************** voidpwm_out(uint16_t pwm_base){ static uint16_t chan3。 if(pwm_base!=chan3) {OCR1A = (pwm_base+pwm_erro[0] 1024)*+63。} //重設(shè)OCR1A改變PWM占空比 chan3 = pwm_base。 } 傳感器模塊 與傳感器的IIC通信時(shí)序此模塊的作用是獲取MPU6050中加速度傳感器和陀螺儀的實(shí)時(shí)數(shù)據(jù)，核心就是使用AVR單片機(jī)自帶的硬件IIC模塊和外部模塊通信。查詢Atmega328p單片機(jī)的datasheet和MPU6050的datasheet，可以輕松的得到進(jìn)行IIC操作時(shí)應(yīng)該遵循的時(shí)序。讀取MPU6050數(shù)據(jù)的時(shí)序圖如下圖所示：圖44讀取MPU6050圖45寫MPU6050由圖知，在讀寫MPU6050的時(shí)候單片機(jī)需要在Master Transmitter Mode和Master Receiver Mode之間切換，在此不再贅述。 傳感器數(shù)據(jù)采集模塊關(guān)鍵程序段說明 該模塊的主要作用是通過IIC總線協(xié)議讀取MPU6050傳感器模塊獲得的加速度和陀螺儀原始數(shù)據(jù)，故只要按著上述讀寫時(shí)序一步步完成即可。但傳感器數(shù)據(jù)為16位，而IIC總線每個(gè)數(shù)據(jù)幀只能讀8位數(shù)據(jù)。MPU6050通過IIC先發(fā)送寄存器高8位數(shù)據(jù)，再發(fā)送低8位，如此一來，就需要利用位移語(yǔ)句，將首先接收到的8位數(shù)據(jù)緩存，再左移8位，再和后發(fā)送的低8位數(shù)據(jù)做位與運(yùn)算，使得他們?nèi)诤蠟橐粋€(gè)16位數(shù)據(jù)。要注意這里的16位數(shù)據(jù)是有符號(hào)的。 兩8位融合成16位數(shù)據(jù)程序段如下： if(reg==0x3B){ RAW_ACC_X = ((rawADC[0]8)|(rawADC[1]))。 RAW_ACC_Y = ((rawADC[2]8)|(rawADC[3]))。 RAW_ACC_Z = ((rawADC[4]8)|(rawADC[5]))。 }else{ RAW_GYRO_X = ((rawADC[0]8)|(rawADC[1]))。 RAW_GYRO_Y = ((rawADC[2]8)|(rawADC[3]))。 RAW_GYRO_Z = ((rawADC[4]8)|(rawADC[5]))。 } 姿態(tài)解算原理 姿態(tài)解算模塊的主要功能有兩個(gè)，一個(gè)是對(duì)傳感器獲得的加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行處理獲得各自所計(jì)算出來的飛行器此時(shí)的歐拉角，第二個(gè)是通過傳感器數(shù)據(jù)融合計(jì)算，得到飛行器此刻的姿態(tài)角準(zhǔn)確值。 利用加速度計(jì)數(shù)據(jù)計(jì)算歐拉角使用的是三角函數(shù)方法。其原理如下圖46所示圖46 加速度傳感器傾斜檢測(cè)示意圖利用三軸加速度計(jì)進(jìn)行傾斜檢測(cè)的一種方法是基于一個(gè)參照點(diǎn)分別確定加速度計(jì)各個(gè)軸的角度。參照點(diǎn)取器件的典型取向,其中X軸和Y軸位于水平面內(nèi)(0g場(chǎng)),Z軸與水平線垂直(1g場(chǎng))。,其中 為水平線與加速度計(jì)X軸的夾角, 為水平線與加速度計(jì)Y軸的夾角看, 為重力矢量與Z軸的夾角。在X軸和Y軸的起點(diǎn)0g處以及Z軸上1g處,計(jì)算得到的所有角度均為0度。由基本三角函數(shù)可知,可利用下列公式計(jì)算傾斜角度[12]。但是飛行器時(shí)刻是運(yùn)動(dòng)的,加速度計(jì)測(cè)得的數(shù)據(jù)中就不可避免的含有運(yùn)動(dòng)加速度,因此只利用加速度計(jì)求出旳飛行器姿態(tài)具有較大的誤差,所以就需要一定的算法來估計(jì)加速度計(jì)的誤差,實(shí)時(shí)修正飛行器的姿態(tài)[13]。 陀螺儀測(cè)量的是集體繞著某軸轉(zhuǎn)動(dòng)的角速度，由于原理較為復(fù)雜，這里不加敘述。 濾波的方法有很多，商業(yè)四軸飛行器為了保證穩(wěn)定性一般都使用復(fù)雜的卡爾曼濾波。但是卡爾曼濾波運(yùn)算復(fù)雜，加長(zhǎng)程序循環(huán)時(shí)間，會(huì)造成諸如陀螺儀積分漂移增加等情況，故本文采用互補(bǔ)濾波方式進(jìn)行濾波?；パa(bǔ)濾波計(jì)算簡(jiǎn)單，設(shè)計(jì)方便，可以很好的實(shí)現(xiàn)數(shù)據(jù)的融合。 互補(bǔ)濾波器是一種頻域特性濾波器,經(jīng)常用來融合由不同傳感器測(cè)量得到的具有相似物理意義的數(shù)據(jù),摒棄每個(gè)數(shù)據(jù)的缺點(diǎn)從而估算出準(zhǔn)確的參數(shù)。嚴(yán)格意義上講,互補(bǔ)濾波器包含兩種或兩種以上的頻率特性互補(bǔ)的傳遞函數(shù)。例如常見的兩輸入系統(tǒng),兩個(gè)輸入量的物理意義相同,但是某個(gè)輸入量也含高頻噪聲,需要加低通濾波器以濾除噪聲。另外一個(gè)輸入量包含低頻噪聲干擾,因此需要高頻濾波器除去低頻噪聲。低通濾波器與高通濾波器頻率特性互補(bǔ),所以最終的輸出是濾除噪聲的準(zhǔn)確的數(shù)據(jù)[14]。 陀螺儀響應(yīng)速度快,但是會(huì)受到零點(diǎn)隨溫度漂移產(chǎn)生低頻干擾；加速度傳感器用來測(cè)量重力加速度矢量,響應(yīng)速度也非常高,但是會(huì)因?yàn)槲⑿_動(dòng)產(chǎn)生高頻噪音干擾；數(shù)字羅盤測(cè)量當(dāng)?shù)氐卮艌?chǎng)矢量,數(shù)據(jù)測(cè)量比較準(zhǔn)確,但響應(yīng)速度較慢，也會(huì)受電機(jī)電磁場(chǎng)的高頻干擾而陀螺儀的數(shù)字積分又不可避免的帶來積分漂移問題，由此可知，三種傳感器測(cè)量相同的量在頻域內(nèi)特性互補(bǔ),濾波算法實(shí)現(xiàn)兩個(gè)信號(hào)的融合是很合適的[15]。 互補(bǔ)濾波主要是對(duì)加速度傳感器數(shù)據(jù)進(jìn)行低通濾波以削去高頻分量，一般而言高平分量即為加速度傳感器的擾動(dòng)量；對(duì)陀螺儀進(jìn)行高通濾波，濾去低頻分量，低頻分量意味著信號(hào)中頻率較低周期較長(zhǎng)的部分，這樣一來便可濾去加速度陀螺儀積分漂移產(chǎn)生的誤差，在完成濾波之后，就能獲得四軸飛行器的較為準(zhǔn)確的姿態(tài)角。 姿態(tài)解算模塊關(guān)鍵程序段說明 姿態(tài)解算模塊的作用就是實(shí)時(shí)獲得飛行器準(zhǔn)確的飛行姿態(tài)，由此這個(gè)模塊里最關(guān)鍵的程序段就是互補(bǔ)濾波算法了。互補(bǔ)濾波器已在前文中完成了設(shè)計(jì)，但可以從前文中發(fā)現(xiàn)，微分時(shí)間dt的大小嚴(yán)重影響該濾波器的性能，所以要嚴(yán)格控制dt大小不變。dt是每?jī)纱尾蓸拥臅r(shí)間間隔，通過如下程序段控制dt為STD_LOOP_TIME不變。 Time1 = time2。 // 上一次采樣時(shí)刻(ms) time2 = millis()。 // 當(dāng)前時(shí)刻(ms) if(dtSTD_LOOP_TIME0) delay(STD_LOOP_TIMEdt)。 //dt時(shí)間控制 get_rawADC(0x43)。 如下程序段即為互補(bǔ)濾波程序段，注意要減去陀螺儀的偏移角GYRO_BIAS_X、GYRO_BIAS_Y。GYRO_BIAS_X、GYRO_BIAS_Y在傳感器靜止時(shí)通過多次測(cè)量取平均值得到。 alpha=()*(*(RAW_GYRO_YGYRO_BIAS_Y)*dt)+ ()*(angle_acc_x)。 beta=()*(beta+*(RAW_GYRO_XGYRO_BIAS_X)*dt ) + ()*(angle_acc_y)。 信息綜合與決策控制模塊該模塊的主要功能就是集合無線遙控的輸入信息和測(cè)得的此時(shí)飛行器的空中姿態(tài)通過PID控制算法進(jìn)行控制決策，繼而產(chǎn)生控制輸出。其核心就是PID算法部分?？叵到y(tǒng)是四旋翼飛行器的核心。圖47 控制系統(tǒng)圖 高度控制系統(tǒng)設(shè)計(jì)高度控制系統(tǒng)十分簡(jiǎn)單，整個(gè)控制回路有人來完成。操作者可以通過遙控器給出此刻電機(jī)升力的基值pwm_base。姿態(tài)控制輸出量只不過是在該值上再改變PWM輸出。所以四軸飛行器受到升力的平均水平應(yīng)該是通過視覺反饋信息來人工控制。 姿態(tài)控制系統(tǒng)設(shè)計(jì)姿態(tài)控制的落腳點(diǎn)在于控制歐拉角。（1）控制規(guī)律選擇 工業(yè)領(lǐng)域最常用的控制器就是PID控制器，它結(jié)構(gòu)簡(jiǎn)單，在很多控制過程中都比較有效，對(duì)參數(shù)的調(diào)節(jié)已經(jīng)有很多成熟的經(jīng)驗(yàn)可以借鑒。雖然在控制領(lǐng)域，有很多先進(jìn)的控制算法，但囿于本設(shè)計(jì)鎖使用AVR單片機(jī)的運(yùn)算能力和設(shè)計(jì)者水平，本文采用PID控制規(guī)律來設(shè)計(jì)控制器。（2）數(shù)字PID控制器設(shè)計(jì)設(shè)計(jì)數(shù)字控制器時(shí)有兩種設(shè)計(jì)方式:連續(xù)化設(shè)計(jì)技術(shù)和離散化設(shè)計(jì)技術(shù)。數(shù)字控制器的連續(xù)化設(shè)計(jì)忽略系統(tǒng)中的零階保持器,將經(jīng)過釆樣后的系統(tǒng)仍然按照連續(xù)系統(tǒng)在S域中求出連續(xù)的控制器,最后通過近似將連續(xù)的控制器離散化為數(shù)字控制器。利用連續(xù)化的數(shù)字控制器,可以充分利用比較成熟的連續(xù)化設(shè)計(jì)技術(shù),但是這種方法要求系統(tǒng)有較高的釆樣率。根據(jù)計(jì)算機(jī)采樣控制理論來設(shè)計(jì)數(shù)字控制器,稱為數(shù)字控制器的離散化設(shè)計(jì),由于是根據(jù)系統(tǒng)采樣和控制特點(diǎn)計(jì)算相應(yīng)的控制率,因而這種方式允許系統(tǒng)有較大的采樣周期,對(duì)系統(tǒng)的控制質(zhì)量也較高。為了方便采用傳統(tǒng)的PID控制器我們采用數(shù)字控制器的連續(xù)化設(shè)計(jì)方法[18]。傳統(tǒng)的理想PID控制器原理圖如下圖：圖48 傳統(tǒng)理想PID控制器原理圖PID控制規(guī)律為 （41）其中。采用矩形法進(jìn)行數(shù)值積分，即以求和代替積分，以差分代替微分，可以得到數(shù)字形式的PID控制規(guī)律如公式42 （42）增量式PID數(shù)字控制器即為輸出。在實(shí)際情況中，由于實(shí)際的控制回路都可能存在高頻干擾，或者在控制量r變動(dòng)時(shí),微分環(huán)節(jié)會(huì)產(chǎn)生短暫的脈沖干擾,致使控制器的輸出飽和,破壞系統(tǒng)的穩(wěn)定性,尤其是在控制量頻繁調(diào)整的情況下。在此為避免該問題,選用增強(qiáng)型PID控制器,其結(jié)構(gòu)框圖如圖49所示。圖49 增強(qiáng)型PID控制器原理圖此圖中的y值分別取，r值分別為。俯仰角，翻滾角和偏航角的PID控制全部基于圖的增強(qiáng)型PID控制器[19]。依然用矩形法進(jìn)行數(shù)值積分，即以球和代替積分，以差分代替微分，得到的增量式數(shù)字控制器公式如下43 （43）由于實(shí)際中使用位置式PID算法可能產(chǎn)生輸出脈沖，影響安全，而且需要存儲(chǔ)數(shù)據(jù)的量巨大，所以本文采用增量式PID算法。數(shù)字PID控制算法程序框圖如圖316圖410 數(shù)字PID控制算法圖（3）PID參數(shù)整定 參數(shù)正定通常有兩種方法，即理論設(shè)計(jì)法和實(shí)驗(yàn)整定法。前者需要有被控對(duì)象的精確模型，然后采用最優(yōu)化方法確定PID參數(shù)。由于本文極大簡(jiǎn)化了本來復(fù)雜的四軸飛行器模型，所以顯然不適用與理論設(shè)計(jì)法。因此選擇實(shí)驗(yàn)整定方法（試湊法），該方法簡(jiǎn)便直觀，效果不錯(cuò)。PID參數(shù)對(duì)系統(tǒng)性能影響如下：1 放大倍數(shù)Kp 對(duì)系統(tǒng)性能的影響：Kp 增大，將使得系統(tǒng)動(dòng)作靈敏，響應(yīng)速度加快。而 Kp偏大則會(huì)使得衰減震蕩次數(shù)增多，調(diào)節(jié)時(shí)間變長(zhǎng)； Kp太小又會(huì)使得響應(yīng)速度緩慢。在系統(tǒng)穩(wěn)定的前提下，加大 有利于減小系統(tǒng)穩(wěn)態(tài)誤差，但不能消除穩(wěn)態(tài)誤差。2 積分時(shí)間 Ti對(duì)系統(tǒng)性能影響：積分時(shí)間通常影響系統(tǒng)穩(wěn)定性。 Ti太小，系統(tǒng)將不能穩(wěn)定。 Ti偏小振動(dòng)次數(shù)增多； Ti太大，系統(tǒng)動(dòng)態(tài)性能變差，且在系統(tǒng)穩(wěn)定的前提下，積分作用太弱，不利于減少穩(wěn)態(tài)誤差。3 微分時(shí)間 Td對(duì)系統(tǒng)性能的影響：微分時(shí)間常數(shù) Td的增加可以改善系統(tǒng)的動(dòng)態(tài)性能，如超調(diào)量減少、調(diào)節(jié)時(shí)間縮短、允許加大比例控制，使穩(wěn)態(tài)誤差減少?gòu)亩岣呖刂凭取５俏⒎肿饔糜钟锌赡芊糯笙到y(tǒng)噪聲，降低系統(tǒng)的抗干擾能力。 試湊法實(shí)現(xiàn)步驟如下：1. 按照先比例后積分最后微分的原則反復(fù)調(diào)整。 2. 首先只整定比例系數(shù)，將 Kp由小變大，使得系統(tǒng)響應(yīng)曲線略有超調(diào)。此時(shí)若系統(tǒng)無穩(wěn)態(tài)誤差或者穩(wěn)態(tài)誤差已經(jīng)小到允許范圍內(nèi)，并且響應(yīng)速度已經(jīng)滿意，那么，只需要用比例調(diào)節(jié)器即可，最優(yōu)比例系數(shù) Kp也已經(jīng)確定。 3. 若在比例調(diào)節(jié)的基礎(chǔ)上，系統(tǒng)穩(wěn)態(tài)誤差太大，則必須加入積分環(huán)節(jié)。整定時(shí)先將第一步所整定的比例系數(shù)略微縮小，再將積分時(shí)間常數(shù) 置為一個(gè)較大的數(shù)值并且連續(xù)減少，不斷觀察系統(tǒng)響應(yīng)情況，使得系統(tǒng)具有良好動(dòng)態(tài)性能的情況下消除穩(wěn)態(tài)誤差。反復(fù)不斷進(jìn)行這一步驟，直至獲得最佳的Kp 與Ti 。 ⒋ 如果使用了PI調(diào)節(jié)器消除了穩(wěn)態(tài)誤差，但系統(tǒng)動(dòng)態(tài)響應(yīng)經(jīng)過反復(fù)調(diào)整后任然不能讓人滿意，則可以加入微分環(huán)節(jié)，構(gòu)成PID調(diào)節(jié)器。在整定時(shí)候，先將微分時(shí)間常數(shù) Td設(shè)為零，再逐步增加 Td并同時(shí)進(jìn)行前面第二、第三步，以獲得滿意的調(diào)節(jié)結(jié)果[20]。 GPS模塊程序設(shè)計(jì) GPS導(dǎo)航設(shè)計(jì)將目標(biāo)點(diǎn)的經(jīng)緯度存入數(shù)組，并實(shí)時(shí)將接收到的當(dāng)前經(jīng)緯度與數(shù)組中的值進(jìn)行比較，控制飛行器前進(jìn)后退、或左飛右飛和45176。斜飛。 圖411導(dǎo)航流程圖由于沒有裝載電子羅盤，只能人工將飛行器起飛時(shí)的方向朝北。若目標(biāo)點(diǎn)在起飛點(diǎn)的右上方，則先45176。斜向飛至目標(biāo)點(diǎn)正下方或正左方，再前飛或右飛至目標(biāo)點(diǎn)。圖412單個(gè)目標(biāo)點(diǎn)的行走方向當(dāng)設(shè)置多個(gè)目標(biāo)點(diǎn)是，可近似直線飛至目標(biāo)點(diǎn)，目標(biāo)點(diǎn)設(shè)置越多則直線越平滑。圖413多個(gè)目標(biāo)點(diǎn)的行走方向 GPS數(shù)據(jù)格式(NMEA0183)例如：$GPRMC,A,N,E,150706,A*50其中各參數(shù)的意義:字段0：$GPRMC，語(yǔ)句ID，表明該語(yǔ)句為Remended Minimum SpecificGPS/TRANSIT Data(RMC)推薦最小定位信息字段1：UTC格林威治時(shí)間，格式是 hh mm 字段2：狀態(tài)，A代表定位，V代表未定位字段3：緯度，度分格式位數(shù)不足就補(bǔ)上0字段4：緯度S即南緯，緯度N即北緯字段5：經(jīng)度，度分格式位數(shù)不足