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