【正文】 一步的工作研究。 小結(jié)本章介紹了四旋翼無(wú)人直升機(jī)的捷聯(lián)式慣性導(dǎo)航的原理和常用的姿態(tài)解算方法。簡(jiǎn)要介紹了傳感器采樣數(shù)據(jù)的預(yù)處理和A/D偏差補(bǔ)償?shù)姆椒?。本章最后給出了實(shí)際導(dǎo)航系統(tǒng)的姿態(tài)變化曲線(xiàn)。由曲線(xiàn)可以看出，本系統(tǒng)若使用純捷聯(lián)式慣導(dǎo)，各姿態(tài)角會(huì)出現(xiàn)積分漂移。實(shí)際系統(tǒng)中可以使用數(shù)字羅盤(pán)對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行定時(shí)補(bǔ)償，從補(bǔ)償?shù)慕Y(jié)果來(lái)看，導(dǎo)航的精度大大增加。5 四旋翼無(wú)人機(jī)建模與仿真 引言國(guó)內(nèi)外有很多研究單位都對(duì)四旋翼飛機(jī)無(wú)人機(jī)進(jìn)行了模型分析工作：McKerrow對(duì)Draganflyer進(jìn)行了精確建模[19]。澳大利亞、法國(guó)和美國(guó)的學(xué)者共同對(duì)X4flyer的動(dòng)態(tài)建模[6]。斯坦福對(duì)STARMAC進(jìn)行了建模和參數(shù)辨識(shí)工作[21]，文獻(xiàn)[22]詳述了Draganflyer的建模與參數(shù)辨識(shí)。本文所選用的是國(guó)產(chǎn)華科爾四旋翼無(wú)人直升機(jī)（Walker UFO4），它和Draganflyer結(jié)構(gòu)相似（），所以對(duì)Draganflyer的建模方法對(duì)本文的四旋翼機(jī)具有一定的指導(dǎo)意義。 Draganflyer V Walker UFO4本文對(duì)四旋翼無(wú)人直升機(jī)建模與系統(tǒng)仿真的步驟如下：（1）根據(jù)牛頓運(yùn)動(dòng)定律建立系統(tǒng)的直線(xiàn)運(yùn)動(dòng)模型（2）對(duì)直線(xiàn)運(yùn)動(dòng)模型中各作用力進(jìn)行分析（3）建立四旋翼無(wú)人機(jī)的轉(zhuǎn)動(dòng)模型（4）分析轉(zhuǎn)動(dòng)模型中各主要部分（5）給出直流小電機(jī)的簡(jiǎn)化模型（6）對(duì)系統(tǒng)模型參數(shù)進(jìn)行試驗(yàn)辨識(shí)（7）搭建仿真平臺(tái)，設(shè)計(jì)控制器 系統(tǒng)建模動(dòng)力學(xué)基礎(chǔ)知識(shí)已經(jīng)超出了的本文的范圍，本文僅對(duì)四旋翼無(wú)人機(jī)動(dòng)力學(xué)做一些基本分析，然后給出建模的相關(guān)結(jié)論。 直線(xiàn)運(yùn)動(dòng)的模型[17] [22[[50] [51]，四旋翼直升機(jī)的控制相當(dāng)于對(duì)力和扭矩的平衡。四旋翼所受外力和重力平衡時(shí)就可以實(shí)現(xiàn)盤(pán)旋飛行[52]。 四旋翼無(wú)人機(jī)的受力和運(yùn)動(dòng)圖四旋翼無(wú)人直升機(jī)直線(xiàn)運(yùn)動(dòng)的阻力一共有三個(gè)來(lái)源：重力、慣性和空氣阻力。重力作用于垂向，空氣阻力同時(shí)影響直線(xiàn)和滾轉(zhuǎn)運(yùn)動(dòng)。根據(jù)剛體運(yùn)動(dòng)定律可知 ()式()中m為四旋翼直升機(jī)的質(zhì)量，ξ為導(dǎo)航系中的位置[x y z]T，F(xiàn)f、Fdt、FG為旋翼推力、空氣阻力和重力，且 ()其中Kdt=diag(Kdtx,Kdty,Kdtz)為阻力系數(shù)陣。FG=mG ()上式中G=[0 0 g]T為重力向量。地理坐標(biāo)系下四旋翼的推力Ff =[ Fx Fy Fz]可由下式獲得 ()式()中Fi是旋翼i產(chǎn)生的推力，它和旋翼的角速度ωi有如下關(guān)系 ()Kl推力常量，和空氣密度ρ，推力系數(shù)Cz以及旋翼參數(shù)相關(guān)的參數(shù)關(guān)系如下關(guān)系 ()由()到()系統(tǒng)直線(xiàn)運(yùn)動(dòng)的動(dòng)態(tài)方程變?yōu)? () 旋轉(zhuǎn)運(yùn)動(dòng)的模型作用在四旋翼無(wú)人機(jī)上的主要物理作用有：空氣動(dòng)力學(xué)效應(yīng)、空氣摩擦阻力、慣性力矩和陀螺效應(yīng)。根據(jù)牛頓歐拉方程，可以得出 ()其中J=diag（IX,IY,IZ）為四旋翼直升機(jī)轉(zhuǎn)動(dòng)慣量陣。因?yàn)樗男頇C(jī)有對(duì)稱(chēng)性，所以為對(duì)角陣。 ()為機(jī)體系內(nèi)歐拉角速度，它和導(dǎo)航系內(nèi)姿態(tài)角的關(guān)系可以由下式得出 ()式()中是機(jī)體系四旋翼所受力矩 ()上式中，d是四旋翼重心到旋翼轉(zhuǎn)軸的距離。Mi是四旋翼的z軸力矩 ()Kd是旋翼的阻力系數(shù)。 式()中是空氣阻力扭矩 () Kaf=diag(Kafx,Kafy,Kafz)是空氣阻力系數(shù)。 式()中 是陀螺效應(yīng)。電機(jī)以每分以幾千轉(zhuǎn)的速度旋轉(zhuǎn)[5]，電機(jī)和旋翼的轉(zhuǎn)軸與機(jī)體系z(mì)軸平行，當(dāng)飛機(jī)俯仰或橫滾時(shí)，會(huì)改變旋轉(zhuǎn)物體角動(dòng)量向量的方向，從而產(chǎn)生力矩 ()Wi=[0,0,(1)i+1ωi]T， 是第i個(gè)旋翼的角速度， 是旋翼和電機(jī)的慣量。將式()到式()分析，考慮到簡(jiǎn)潔性，由于Jr的值很小()所以忽略陀螺效應(yīng)，便得到如式()所示的簡(jiǎn)化系統(tǒng)模型 () 直流電機(jī)的模型[17]文獻(xiàn)[7]將小型直流電機(jī)近似為如下慣性環(huán)節(jié)模型 ()其中 為電機(jī)轉(zhuǎn)速，U為輸入電壓，a為系數(shù)根據(jù)以上分析，由式()、()和()可得四旋翼無(wú)人機(jī)的動(dòng)態(tài)方程為： () 四旋翼無(wú)人機(jī)模型參數(shù)辨識(shí)，得到了如式()所示的系統(tǒng)模型。有些模型的參數(shù)可以從廠(chǎng)家獲得。但是一些重要的參數(shù)，如質(zhì)量、轉(zhuǎn)動(dòng)慣量等與原四旋翼直升機(jī)模型并不相同。所以，有必要對(duì)實(shí)際系統(tǒng)參數(shù)進(jìn)行辨識(shí)，一個(gè)可以反應(yīng)四旋翼無(wú)人機(jī)本質(zhì)特性的模型對(duì)控制器設(shè)計(jì)仍有很大的幫助。四旋翼無(wú)人直升機(jī)系統(tǒng)是在華科爾的四旋翼遙控直升機(jī)上改造的。使用數(shù)字羅盤(pán)測(cè)量參數(shù)辨識(shí)所需的信息之后，通過(guò)無(wú)線(xiàn)模塊發(fā)回上位機(jī)，由上位機(jī)處理系統(tǒng)數(shù)據(jù)。 模型參數(shù)辨識(shí)本節(jié)的主要內(nèi)容是介紹()式中轉(zhuǎn)動(dòng)慣量陣J和空氣摩擦系數(shù)陣Kaf的辨識(shí)方法。，X/。為了確定轉(zhuǎn)動(dòng)慣量陣J，： Z軸參數(shù)辨識(shí)的懸掛方法圖 X/Y軸參數(shù)辨識(shí)的懸掛方法圖 四旋翼無(wú)人機(jī)基本參數(shù)參數(shù)含義大小m質(zhì)量g重力加速度r懸點(diǎn)到重心距離l繩長(zhǎng)震蕩周期則轉(zhuǎn)動(dòng)慣量可由以下方程得出[22] ()空氣阻尼系數(shù)可以使用自由擺的表達(dá)式來(lái)求得At=A(0)exp?(λωt)cos?(1λ2ωt) ()其中阻尼 等于 ()Kaf可由式()求得。 辨識(shí)結(jié)果與分析（1） Z軸自由擺動(dòng)幅值曲線(xiàn)圖 Z軸采樣數(shù)據(jù)傅里葉分析對(duì)采樣之后的的數(shù)據(jù)進(jìn)行傅里葉分析。，對(duì)應(yīng)于角頻率= rad/s；=（2）Ka的辨識(shí)采用試湊法對(duì)Ka進(jìn)行辨識(shí)。設(shè)初始值A(chǔ)(0)=，每秒鐘8Hz采樣，將采樣數(shù)據(jù)與仿真所得的值相減，然后取方差。令方差最小的值為。 =。=105。 Z軸辨識(shí)曲線(xiàn) Z軸辨識(shí)曲線(xiàn)和原始曲線(xiàn)對(duì)比（3）其他軸的辨識(shí)曲線(xiàn)：— Y軸自由擺動(dòng)曲線(xiàn) Y軸采樣數(shù)據(jù)傅里葉分析 Y軸辨識(shí)曲線(xiàn) Y軸辨識(shí)曲線(xiàn)和原始曲線(xiàn)對(duì)比 X軸自由擺動(dòng)曲線(xiàn) X軸采樣數(shù)據(jù)傅里葉分析 X軸辨識(shí)曲線(xiàn) X軸辨識(shí)曲線(xiàn)和原始曲線(xiàn)對(duì)比（4）辨識(shí)結(jié)果。 辨識(shí)實(shí)驗(yàn)獲得的參數(shù)參數(shù)大小說(shuō)明m/g質(zhì)量/重力加速度r中心離旋翼中心距離Jdiag(, ,)轉(zhuǎn)動(dòng)慣量陣Kafdiag( , , ) N/rad/s空氣阻力系數(shù)陣 模型其他參數(shù)參數(shù)大小說(shuō)明Kdtdiag( , , )N/m/s阻力系數(shù)Kl推力常量Kd旋翼的阻力系數(shù)Jr旋翼慣量a/b電機(jī)參數(shù)()式，得到最終模型如下： ()上式中各變量含義如下表所示。 系統(tǒng)數(shù)學(xué)模型的參數(shù)含義變量含義變量含義xx向位置Fx導(dǎo)航系x軸推力yy向位置Fy導(dǎo)航系y軸推力zz向位置Fz導(dǎo)航系z(mì)軸推力滾轉(zhuǎn)角Mi第i個(gè)電機(jī)扭矩俯仰角電機(jī)角速度航向角Fi第i個(gè)旋翼的推力 四旋翼無(wú)人直升機(jī)的控制器設(shè)計(jì) 四旋翼直升機(jī)的基本運(yùn)動(dòng)狀態(tài)，四旋翼直升機(jī)有四個(gè)基本的運(yùn)動(dòng)狀態(tài)[51]：垂向運(yùn)動(dòng)、水平轉(zhuǎn)動(dòng)、俯仰運(yùn)動(dòng)和滾轉(zhuǎn)運(yùn)動(dòng)。（1）垂向運(yùn)動(dòng)：增加四個(gè)旋翼的轉(zhuǎn)速，旋翼的推力增大；減小四個(gè)旋翼的轉(zhuǎn)速，旋翼的推力減少。由旋翼的推力可以控制直升機(jī)的垂向運(yùn)動(dòng)。（2）水平轉(zhuǎn)動(dòng)：四旋翼無(wú)人機(jī)的一對(duì)電機(jī)順時(shí)針旋轉(zhuǎn)，產(chǎn)生逆時(shí)針扭矩；另一對(duì)電機(jī)逆時(shí)針旋轉(zhuǎn)，產(chǎn)生順時(shí)針扭矩。兩個(gè)扭矩共同作用從而控制機(jī)體轉(zhuǎn)動(dòng)。（3）俯仰運(yùn)動(dòng)：，如果旋翼1和旋翼3的推力不同，則機(jī)體在這兩個(gè)電機(jī)所在的軸線(xiàn)方向受力就不平衡，從而產(chǎn)生俯仰運(yùn)動(dòng)。（4）滾轉(zhuǎn)運(yùn)動(dòng)：由于四旋翼直升機(jī)的對(duì)稱(chēng)性，滾轉(zhuǎn)運(yùn)動(dòng)的產(chǎn)生原因和俯仰運(yùn)動(dòng)的產(chǎn)生原因相同。 四旋翼無(wú)人直升機(jī)的姿態(tài)控制結(jié)構(gòu)目前國(guó)內(nèi)外學(xué)者使用了多種控制方案來(lái)控制四旋翼無(wú)人直升機(jī)的姿態(tài)。很多文獻(xiàn)給出的都是傳統(tǒng)的PID[52][53][54][55]控制和狀態(tài)空間控制[56]，還有一部分是滑模[57] [58]和 控制[59]等等。 PID控制器結(jié)構(gòu)簡(jiǎn)單，使用靈活，本文選用PID控制器控制各個(gè)姿態(tài)。PID控制器的傳遞函數(shù)為 ()。 四旋翼無(wú)人機(jī)姿態(tài)控制結(jié)構(gòu)圖上圖中，前向通道(F)、后向通道(B)、左向通道(L)、右向通道(R)、電機(jī)電機(jī)2和電機(jī)4；‘+’表示電壓增大，‘’表示電壓減小。以滾轉(zhuǎn)通道為例，滾轉(zhuǎn)控制通道M0的執(zhí)行器件是左向和右向兩個(gè)直流電機(jī)。當(dāng)M0為正時(shí)，左向電機(jī)電壓減少，右向電機(jī)電壓增大，并且增加量等于減少量，這樣便在滾轉(zhuǎn)通道上產(chǎn)生扭矩。同時(shí)因?yàn)殡姍C(jī)2減小的扭矩等于電機(jī)4增加的扭矩，這就可以避免滾轉(zhuǎn)控制對(duì)航向通道的干擾。 系統(tǒng)仿真分析 仿真平臺(tái)搭建由本章前文分析，依據(jù)式()。 四旋翼無(wú)人機(jī)控制系統(tǒng)圖上圖中theta、gamma和psi模塊分別對(duì)應(yīng)三個(gè)姿態(tài)角的PID控制器；；fourrotors模塊對(duì)應(yīng)于式()所示的四旋翼無(wú)人直升機(jī)的系統(tǒng)模型。 四旋翼無(wú)人直升機(jī)內(nèi)部結(jié)構(gòu)圖上圖中motor為電機(jī)模塊，輸入、輸出量為電機(jī)的電壓和轉(zhuǎn)速；Mi為電機(jī)的扭矩模塊；Fi為旋翼推力模塊；Ff為機(jī)體坐標(biāo)系推力與導(dǎo)航坐標(biāo)系推力轉(zhuǎn)換模塊；VX是四旋翼無(wú)人直升機(jī)直線(xiàn)運(yùn)動(dòng)模塊；VX1是直升機(jī)轉(zhuǎn)動(dòng)模塊。由躍響應(yīng)曲線(xiàn)可以看出，四旋翼無(wú)人機(jī)的各姿態(tài)角的閉環(huán)性能很差。另外，上圖中各姿態(tài)角的震蕩周期與轉(zhuǎn)動(dòng)慣量有關(guān)，轉(zhuǎn)動(dòng)慣量越大，周期越大。因此有必要設(shè)計(jì)控制器來(lái)控制各姿態(tài)角，使系統(tǒng)性能滿(mǎn)足要求。采用臨界比例度法[60]。采用PID控制之后， 各姿態(tài)通道PID控制器參數(shù)的值通道KpKiKd俯仰814滾轉(zhuǎn)717航向1330 小結(jié)本章首先介紹了四旋翼無(wú)人直升機(jī)的建模方法，并對(duì)四旋翼無(wú)人直升機(jī)模型的一些參數(shù)進(jìn)行辨識(shí)。接下來(lái)在simulink平臺(tái)上搭建四旋翼無(wú)人直升機(jī)的系統(tǒng)仿真模型。在本章的最后設(shè)計(jì)了最常用的PID控制器來(lái)控制系統(tǒng)的姿態(tài)角。從仿真圖像上看出，本章設(shè)計(jì)的PID控制器可以實(shí)現(xiàn)對(duì)姿態(tài)角控制，改善了系統(tǒng)系能。6 軟件系統(tǒng)設(shè)計(jì) 引言控制系統(tǒng)軟件一共分為六個(gè)模塊：系統(tǒng)初始化模塊、傳感器數(shù)據(jù)采集模塊、數(shù)據(jù)處理模塊、導(dǎo)航模塊、控制模塊和無(wú)線(xiàn)通訊模塊。本章主要內(nèi)容是軟件系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)，在總流程之后分別介紹各子模塊的功能和實(shí)現(xiàn)方法。 軟件系統(tǒng)總流程四旋翼無(wú)人直升機(jī)的軟件系統(tǒng)的總流程如下圖所示： 軟件系統(tǒng)總流程圖由上圖可以看出，整個(gè)軟件系統(tǒng)分為主流程和中斷處理兩部分。中斷處理部分的作用是解決低速外設(shè)和高速DSP之間的通信問(wèn)題。主流程負(fù)責(zé)整個(gè)系統(tǒng)各部分的協(xié)調(diào)工作：上電之后，系統(tǒng)進(jìn)行初始化自檢。如果系統(tǒng)各器件工作正常，則判斷飛行模式，如果選擇手動(dòng)控制，則導(dǎo)航解算和自動(dòng)控制程序?qū)⒉粓?zhí)行。若是選擇自動(dòng)飛行，系統(tǒng)根據(jù)控制目標(biāo)，自動(dòng)完成飛行動(dòng)作。其中系統(tǒng)初始化自檢操作是由系統(tǒng)初始化模塊、傳感器數(shù)據(jù)采集模塊和無(wú)線(xiàn)通信模塊共同完成；導(dǎo)航解算操作由導(dǎo)航模塊完成；高度/姿態(tài)控制是由控制模塊完成；中斷處理部分由傳感器數(shù)據(jù)采集模塊和控制模塊共同完成。 系統(tǒng)初始化自檢模塊整個(gè)控制器只采用單片DSP進(jìn)行數(shù)據(jù)采集、處理和飛行控制。因此，對(duì)DSP性能要求比較高，同時(shí)要合理利用DSP的系統(tǒng)資源。 軟件系統(tǒng)初始化軟件初始化操作包含以下幾方面（1）頭文件引用（2）系統(tǒng)全局變量定義及初始化（3）函數(shù)的聲明等。 硬件系統(tǒng)初始化硬件系統(tǒng)初始化：包括外設(shè)的啟動(dòng)與寄存器配置、中斷設(shè)置等等。另外對(duì)于硬件系統(tǒng)初始化應(yīng)該有以下四點(diǎn)注意事項(xiàng)：（1）硬件系統(tǒng)初始化必須按照一定的順序進(jìn)行，如果順序不當(dāng)，硬件系統(tǒng)有可能工作不正常。，一般硬件初始化步驟如下：①初始化系統(tǒng)控制（包括鎖相環(huán)、看門(mén)狗和外設(shè)時(shí)鐘等）②初始化GPIO③關(guān)全局中斷④初始化中斷控制寄存器⑤清除CPU中斷標(biāo)志寄存器和中斷使能寄存器⑥初始化中斷向量表⑦開(kāi)中斷⑧初始化外設(shè)⑨使能CPU中斷和PIE中斷⑩使能全局中斷詳細(xì)信息可參考相關(guān)文獻(xiàn)或者TI官方的例程。 硬件系統(tǒng)初始化流程圖（2）系統(tǒng)暫時(shí)使用不到的外設(shè)，比如本文未使用的外設(shè)有I2C接口、SPI接口和ECAN口等，都可以在硬件系統(tǒng)初始化時(shí)關(guān)閉相應(yīng)的外設(shè)時(shí)鐘，以降低功耗。這一點(diǎn)常常被一般開(kāi)發(fā)者忽略。（3）如果采用TI官方提供的軟件系統(tǒng)框架編寫(xiě)程序。在使用FLASH燒寫(xiě)時(shí)，應(yīng)避免使用文件中的匯編函數(shù)（如DELAYUS（）），因?yàn)檫@些函數(shù)是專(zhuān)門(mén)為RAM編寫(xiě)的，F(xiàn)LASH和RAM的內(nèi)存分配方式不同，運(yùn)行該匯編程序會(huì)使程序跑飛，跳到一個(gè)未知的內(nèi)存地址。（3）DSP采用三級(jí)中斷機(jī)制，分別為外設(shè)級(jí)、PIE級(jí)和CPU級(jí)。系統(tǒng)初始化的時(shí)候應(yīng)該正確設(shè)置各級(jí)中斷，否則有可能不能進(jìn)入中斷服務(wù)子程序。比如本文使用的ECAP1中斷，在外設(shè)級(jí)中應(yīng)當(dāng)將寄存器ECCLR的INT置1；在PIE級(jí)中，將PIE使能寄存器PIEIER4的INTx1位置1；在CPU級(jí)中，開(kāi)啟第4組中斷IER|=M_INT4。（4）不同功能模塊都要使用的外設(shè)，應(yīng)該只初始化一次，否則有可能造成誤操作。如F28335的ADC中斷觸發(fā)源是ePWM1模塊的時(shí)基TB1，同時(shí)ePWM模塊負(fù)責(zé)驅(qū)動(dòng)兩路直流電機(jī)。因此對(duì)于TB1的設(shè)置和修改需要謹(jǐn)慎，否則有可能造成錯(cuò)誤。 數(shù)據(jù)采集模塊和無(wú)線(xiàn)通信模塊第三章介紹了控制系統(tǒng)選用的各種傳感器，由于每種傳感器的傳輸速度、通信方式和工作原理都各不相同。因此DSP在采集相關(guān)數(shù)據(jù)時(shí)的操作也并不一樣。