【文章內(nèi)容簡介】 050中加速度傳感器和陀螺儀的實(shí)時(shí)數(shù)據(jù)，核心就是使用AVR單片機(jī)自帶的硬件IIC模塊和外部模塊通信。查詢Atmega328p單片機(jī)的datasheet和MPU6050的datasheet，可以輕松的得到進(jìn)行IIC操作時(shí)應(yīng)該遵循的時(shí)序。讀取MPU6050數(shù)據(jù)的時(shí)序圖如圖36所示。圖36讀取MPU6050向MPU6050寫數(shù)據(jù)的時(shí)序圖如圖37圖37寫MPU6050由圖知，在讀寫MPU6050的時(shí)候單片機(jī)需要在Master Transmitter Mode和Master Receiver Mode之間切換。兩個(gè)狀態(tài)的狀態(tài)標(biāo)志寄存器TWSR代表含義分別分別如下圖38和39所示。圖38Master Transmitter Mode下TWSR圖39Master Receiver Mode下TWSRMPU6050最高支持SCL為400Hz，為增加數(shù)據(jù)傳輸速率，在單片機(jī)的設(shè)置上，采用SCL獨(dú)立使用的分頻器獲得400Hz的SCL速率。具體速率計(jì)算和配置方式如公式35所示。 （35） 傳感器數(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位，如此一來，就需要利用位移語句，將首先接收到的8位數(shù)據(jù)緩存，再左移8位，再和后發(fā)送的低8位數(shù)據(jù)做位與運(yùn)算，使得他們?nèi)诤蠟橐粋€(gè)16位數(shù)據(jù)。要注意這里的16位數(shù)據(jù)是有符號的。兩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)解算原理姿態(tài)解算模塊的主要功能有兩個(gè)，一個(gè)是對傳感器獲得的加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行處理獲得各自所計(jì)算出來的飛行器此時(shí)的歐拉角，第二個(gè)是通過傳感器數(shù)據(jù)融合計(jì)算，得到飛行器此刻的姿態(tài)角準(zhǔn)確值。利用加速度計(jì)數(shù)據(jù)計(jì)算歐拉角使用的是三角函數(shù)方法。其原理如下圖310所示圖310加速度傳感器傾斜檢測示意圖利用三軸加速度計(jì)進(jìn)行傾斜檢測的一種方法是基于一個(gè)參照點(diǎn)分別確定加速度計(jì)各個(gè)軸的角度。參照點(diǎn)取器件的典型取向,其中X軸和Y軸位于水平面內(nèi)(0g場),Z軸與水平線垂直(1g場)。,其中為水平線與加速度計(jì)X軸的夾角,為水平線與加速度計(jì)Y軸的夾角看,為重力矢量與Z軸的夾角。在X軸和Y軸的起點(diǎn)0g處以及Z軸上1g處,計(jì)算得到的所有角度均為0度。由基本三角函數(shù)可知,可利用下列公式計(jì)算傾斜角度[12]。 （36）上述第三個(gè)公式中的運(yùn)算為逆運(yùn)算,這是因?yàn)槌跏嘉恢脼镮g場,如果需要將水平線作為Z軸的參照,則可進(jìn)行逆運(yùn)算。正角表示加速度計(jì)對應(yīng)的正軸指向水平線上方,負(fù)角表示該軸指向水平線下方。由于使用了反正切函數(shù)和加速比來求傾角,因此有效增量靈敏度為常數(shù),可以精確測量單位球面周圍所有點(diǎn)的角度。利用加速度計(jì)可以求出控制的絕對傾角,但是飛行器時(shí)刻是運(yùn)動(dòng)的,加速度計(jì)測得的數(shù)據(jù)中就不可避免的含有運(yùn)動(dòng)加速度,因此只利用加速度計(jì)求出旳飛行器姿態(tài)具有較大的誤差,所以就需要一定的算法來估計(jì)加速度計(jì)的誤差,實(shí)時(shí)修正飛行器的姿態(tài)[13]。陀螺儀測量的是集體繞著某軸轉(zhuǎn)動(dòng)的角速度，由于原理較為復(fù)雜，這里不加敘述。濾波的方法有很多，商業(yè)四軸飛行器為了保證穩(wěn)定性一般都使用復(fù)雜的卡爾曼濾波。但是卡爾曼濾波運(yùn)算復(fù)雜，加長程序循環(huán)時(shí)間，會造成諸如陀螺儀積分漂移增加等情況，故本文采用互補(bǔ)濾波方式進(jìn)行濾波。互補(bǔ)濾波計(jì)算簡單，設(shè)計(jì)方便，可以很好的實(shí)現(xiàn)數(shù)據(jù)的融合?；パa(bǔ)濾波器是一種頻域特性濾波器,經(jīng)常用來融合由不同傳感器測量得到的具有相似物理意義的數(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)速度快,但是會受到零點(diǎn)隨溫度漂移產(chǎn)生低頻干擾；加速度傳感器用來測量重力加速度矢量,響應(yīng)速度也非常高,但是會因?yàn)槲⑿_動(dòng)產(chǎn)生高頻噪音干擾；數(shù)字羅盤測量當(dāng)?shù)氐卮艌鍪噶?數(shù)據(jù)測量比較準(zhǔn)確,但響應(yīng)速度較慢，也會受電機(jī)電磁場的高頻干擾而陀螺儀的數(shù)字積分又不可避免的帶來積分漂移問題，由此可知，三種傳感器測量相同的量在頻域內(nèi)特性互補(bǔ),濾波算法實(shí)現(xiàn)兩個(gè)信號的融合是很合適的[15]?；パa(bǔ)濾波的原理圖如圖311所示圖311互補(bǔ)濾波原理圖互補(bǔ)濾波是對加速度傳感器數(shù)據(jù)進(jìn)行低通濾波以削去高頻分量，一般而言高平分量即為加速度傳感器的擾動(dòng)量；對陀螺儀進(jìn)行高通濾波，濾去低頻分量，低頻分量意味著信號中頻率較低周期較長的部分，這樣一來便可濾去加速度陀螺儀積分漂移產(chǎn)生的誤差。假設(shè)互補(bǔ)濾波器的兩個(gè)輸入分別為,表示對量X的兩種測量數(shù)據(jù)。表示包含的高頻噪聲,表示包含的低頻噪聲。選擇一對頻率特性互補(bǔ)的濾波器使和使得+=1,其中為低通濾波器,為高通濾波器,則由互補(bǔ)濾波器估算的X如公式(37)所示[16]。 （37）假定和形式如公式(38)和(39)所示。如果具有全通性，則具有低通濾波特性，具有高通濾波特性。若表示加速度傳感器數(shù)據(jù)表示陀螺儀數(shù)據(jù)，由此可得濾波器通?？梢匀。礊榛パa(bǔ)濾波器的時(shí)間常數(shù)，則為轉(zhuǎn)折頻率，單位rad/s[17]化簡公式（310）得到化為時(shí)域模型得程序中使用時(shí)間循環(huán)時(shí)間限制函數(shù)將控制在30ms， 。最后得到數(shù)字濾波器如下在完成濾波之后，就能獲得四軸飛行器的較為準(zhǔn)確的姿態(tài)角。 姿態(tài)解算模塊關(guān)鍵程序段說明姿態(tài)解算模塊的作用就是實(shí)時(shí)獲得飛行器準(zhǔn)確的飛行姿態(tài)，由此這個(gè)模塊里最關(guān)鍵的程序段就是互補(bǔ)濾波算法了?；パa(bǔ)濾波器已在前文中完成了設(shè)計(jì)，但可以從前文中發(fā)現(xiàn)，微分時(shí)間dt的大小嚴(yán)重影響該濾波器的性能，所以要嚴(yán)格控制dt大小不變。dt是每兩次采樣的時(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í)通過多次測量取平均值得到。alpha=()*(*(RAW_GYRO_YGYRO_BIAS_Y)*dt)+ ()*(angle_acc_x)。beta=()*(beta+*(RAW_GYRO_XGYRO_BIAS_X)*dt ) + ()*(angle_acc_y)。該模塊的主要功能就是集合無線遙控的輸入信息和測得的此時(shí)飛行器的空中姿態(tài)通過PID控制算法進(jìn)行控制決策，繼而產(chǎn)生控制輸出。其核心就是PID算法部分?？叵到y(tǒng)是四旋翼飛行器的核心。本課題的飛行器控制系統(tǒng)主要由高度控制和姿態(tài)控制組成?？刂葡到y(tǒng)總體框圖如圖313所示圖313 控制系統(tǒng)總體框 高度控制系統(tǒng)設(shè)計(jì)高度控制系統(tǒng)十分簡單，整個(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)簡單，在很多控制過程中都比較有效，對參數(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)有較大的采樣周期,對系統(tǒng)的控制質(zhì)量也較高。為了方便采用傳統(tǒng)的PID控制器我們采用數(shù)字控制器的連續(xù)化設(shè)計(jì)方法[18]。傳統(tǒng)的理想PID控制器原理圖如下圖314圖314傳統(tǒng)理想PID控制器原理方框圖PID控制規(guī)律為其中。采用矩形法進(jìn)行數(shù)值積分，即以求和代替積分，以差分代替微分，可以得到數(shù)字形式的PID控制規(guī)律如公式315增量式PID數(shù)字控制器即為輸出.在實(shí)際情況中，由于實(shí)際的控制回路都可能存在高頻干擾，或者在控制量r變動(dòng)時(shí),微分環(huán)節(jié)會產(chǎn)生短暫的脈沖干擾,致使控制器的輸出飽和,破壞系統(tǒng)的穩(wěn)定性,尤其是在控制量頻繁調(diào)整的情況下。在此為避免該問題,選用增強(qiáng)型PID控制器,其結(jié)構(gòu)框圖如圖315所示。圖315增強(qiáng)型PID控制器此圖中的y值分別取，r值分別為。俯仰角，翻滾角和偏航角的PID控制全部基于圖311的增強(qiáng)型PID控制器[19]。依然用矩形法進(jìn)行數(shù)值積分，即以求和代替積分，以差分代替微分，得到的增量式數(shù)字控制器公式如下316由于實(shí)際中使用位置式PID算法可能產(chǎn)生輸出脈沖，影響安全，而且需要存儲數(shù)據(jù)的量巨大，所以本文采用增量式PID算法。數(shù)字PID控制算法程序框圖如圖316316 數(shù)字PID算法框圖（3）PID參數(shù)整定參數(shù)正定通常有兩種方法，即理論設(shè)計(jì)法和實(shí)驗(yàn)整定法。前者需要有被控對象的精確模型，然后采用最優(yōu)化方法確定PID參數(shù)。由于本文極大簡化了本來復(fù)雜的四軸飛行器模型，所以顯然不適用與理論設(shè)計(jì)法。因此選擇實(shí)驗(yàn)整定方法（試湊法），該方法簡便直觀，效果不錯(cuò)。PID參數(shù)對系統(tǒng)性能影響如下：① 放大倍數(shù)對系統(tǒng)性能的影響：增大，將使得系統(tǒng)動(dòng)作靈敏，響應(yīng)速度加快。而偏大則會使得衰減震蕩次數(shù)增多，調(diào)節(jié)時(shí)間變長；太小又會使得響應(yīng)速度緩慢。在系統(tǒng)穩(wěn)定的前提下，加大有利于減小系統(tǒng)穩(wěn)態(tài)誤差，但不能消除穩(wěn)態(tài)誤差。② 積分時(shí)間對系統(tǒng)性能影響：積分時(shí)間通常影響系統(tǒng)穩(wěn)定性。太小，系統(tǒng)將不能穩(wěn)定。偏小振動(dòng)次數(shù)增多；太大，系統(tǒng)動(dòng)態(tài)性能變差，且在系統(tǒng)穩(wěn)定的前提下，積分作用太弱，不利于減少穩(wěn)態(tài)誤差。③ 微分時(shí)間對系統(tǒng)性能的影響：微分時(shí)間常數(shù)的增加可以改善系統(tǒng)的動(dòng)態(tài)性能，如超調(diào)量減少、調(diào)節(jié)時(shí)間縮短、允許加大比例控制，使穩(wěn)態(tài)誤差減少從而提高控制精度。但是微分作用又有可能放大系統(tǒng)噪聲，降低系統(tǒng)的抗干擾能力。試湊法實(shí)現(xiàn)步驟如下：① 按照先比例后積分最后微分的原則反復(fù)調(diào)整。② 首先只整定比例系數(shù)，將由小變大，使得系統(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ù)也已經(jīng)確定。③ 若在比例調(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)行這一步驟，直至獲得最佳的與。④ 如果使用了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ù)設(shè)為零，再逐步增加并同時(shí)進(jìn)行前面第二、第三步，以獲得滿意的調(diào)節(jié)結(jié)果。[20] PID算法程序段說明 依照以上程序流程圖，有r_k即r（k）,y_k即為y（k）,y_k1即為y（k1）,y_k2即為y（k2），Dupi即為，指的是用PI控制部分得到的輸出增量，其與Dud即線性相加之后得到輸出增量//****************//PID control//****************void PID(void){static uint16_t pwm_base,pwm_err[4]。pwm_base = ppmInValue[2]。PID_algorithm(alpha,ppmInValue[16])。pwm_out[0] = pwm_base + pwm_err[0]。pwm_out[1] = pwm_base + pwm_err[1]。pwm_out[2] = pwm_base + pwm_err[2]。pwm_out[3] = pwm_base + pwm_err[3]。 }voidPID_algorithm(float a,uint16_t b){stati