【正文】 兩種：（1）使用更高精度傳感器和A/D轉(zhuǎn)換芯片，更理想的使用環(huán)境等。（2）定時(shí)的使用更精準(zhǔn)姿態(tài)傳感器按照()式進(jìn)行四元數(shù)的初始化校準(zhǔn)。由于各種元器件已經(jīng)選定，所以本文采用第二中方法。導(dǎo)航系統(tǒng)中，比慣性導(dǎo)航更精準(zhǔn)的傳感器是數(shù)字羅盤hmr3300，176。但是數(shù)字羅盤輸出頻率低（8Hz/s），不能滿足控制器對(duì)導(dǎo)航系統(tǒng)的實(shí)時(shí)性要求。因此可以采用數(shù)字羅盤對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行每秒鐘8次的定時(shí)校準(zhǔn)。校準(zhǔn)以后，、 俯仰角校準(zhǔn)之后的圖像 滾轉(zhuǎn)角校準(zhǔn)之后的圖像 航向角校準(zhǔn)之后的圖像由校準(zhǔn)之后的姿態(tài)曲線可以看出，雖然傳感器數(shù)據(jù)仍然含有趨勢(shì)項(xiàng)，但是經(jīng)定時(shí)校準(zhǔn)之后，各姿態(tài)角度不在發(fā)散。另外各姿態(tài)角的精度也大大提高。 結(jié)果分析，實(shí)驗(yàn)平臺(tái)使用單純的捷聯(lián)式慣性導(dǎo)航會(huì)不可避免地出現(xiàn)積分漂移，需要定時(shí)對(duì)導(dǎo)航系統(tǒng)精度校準(zhǔn)。數(shù)字羅盤精度較高，可以進(jìn)行導(dǎo)航校準(zhǔn)。但是高動(dòng)態(tài)的惡劣環(huán)境下，數(shù)字羅盤的延時(shí)性缺陷，會(huì)導(dǎo)致數(shù)學(xué)羅盤的輸出姿態(tài)和當(dāng)前實(shí)際姿態(tài)具有很大的偏差，因此對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的校準(zhǔn)信息就會(huì)出現(xiàn)偏差。本文對(duì)四旋翼無人機(jī)平臺(tái)的研究是在在理想環(huán)境下進(jìn)行的，系統(tǒng)的動(dòng)態(tài)特性、環(huán)境變化都很小，因此可以使用數(shù)字羅盤對(duì)慣性導(dǎo)航姿態(tài)校準(zhǔn)而不考慮數(shù)字羅盤的延時(shí)性。若想在高動(dòng)態(tài)環(huán)境下進(jìn)行姿態(tài)校準(zhǔn)，確認(rèn)數(shù)字羅盤的延時(shí)是一個(gè)很重要的研究?jī)?nèi)容[49]，有待進(jìn)一步的工作研究。 小結(jié)本章介紹了四旋翼無人直升機(jī)的捷聯(lián)式慣性導(dǎo)航的原理和常用的姿態(tài)解算方法。簡(jiǎn)要介紹了傳感器采樣數(shù)據(jù)的預(yù)處理和A/D偏差補(bǔ)償?shù)姆椒?。本章最后給出了實(shí)際導(dǎo)航系統(tǒng)的姿態(tài)變化曲線。由曲線可以看出，本系統(tǒng)若使用純捷聯(lián)式慣導(dǎo)，各姿態(tài)角會(huì)出現(xiàn)積分漂移。實(shí)際系統(tǒng)中可以使用數(shù)字羅盤對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行定時(shí)補(bǔ)償，從補(bǔ)償?shù)慕Y(jié)果來看，導(dǎo)航的精度大大增加。5 四旋翼無人機(jī)建模與仿真 引言國(guó)內(nèi)外有很多研究單位都對(duì)四旋翼飛機(jī)無人機(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)華科爾四旋翼無人直升機(jī)（Walker UFO4），它和Draganflyer結(jié)構(gòu)相似（），所以對(duì)Draganflyer的建模方法對(duì)本文的四旋翼機(jī)具有一定的指導(dǎo)意義。 Draganflyer V Walker UFO4本文對(duì)四旋翼無人直升機(jī)建模與系統(tǒng)仿真的步驟如下：（1）根據(jù)牛頓運(yùn)動(dòng)定律建立系統(tǒng)的直線運(yùn)動(dòng)模型（2）對(duì)直線運(yùn)動(dòng)模型中各作用力進(jìn)行分析（3）建立四旋翼無人機(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ì)四旋翼無人機(jī)動(dòng)力學(xué)做一些基本分析，然后給出建模的相關(guān)結(jié)論。 直線運(yùn)動(dòng)的模型[17] [22[[50] [51]，四旋翼直升機(jī)的控制相當(dāng)于對(duì)力和扭矩的平衡。四旋翼所受外力和重力平衡時(shí)就可以實(shí)現(xiàn)盤旋飛行[52]。 四旋翼無人機(jī)的受力和運(yùn)動(dòng)圖四旋翼無人直升機(jī)直線運(yùn)動(dòng)的阻力一共有三個(gè)來源：重力、慣性和空氣阻力。重力作用于垂向，空氣阻力同時(shí)影響直線和滾轉(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)直線運(yùn)動(dòng)的動(dòng)態(tài)方程變?yōu)? () 旋轉(zhuǎn)運(yùn)動(dòng)的模型作用在四旋翼無人機(jī)上的主要物理作用有：空氣動(dòng)力學(xué)效應(yīng)、空氣摩擦阻力、慣性力矩和陀螺效應(yīng)。根據(jù)牛頓歐拉方程，可以得出 ()其中J=diag（IX,IY,IZ）為四旋翼直升機(jī)轉(zhuǎn)動(dòng)慣量陣。因?yàn)樗男頇C(jī)有對(duì)稱性，所以為對(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ù)以上分析，由式()、()和()可得四旋翼無人機(jī)的動(dòng)態(tài)方程為： () 四旋翼無人機(jī)模型參數(shù)辨識(shí)，得到了如式()所示的系統(tǒng)模型。有些模型的參數(shù)可以從廠家獲得。但是一些重要的參數(shù)，如質(zhì)量、轉(zhuǎn)動(dòng)慣量等與原四旋翼直升機(jī)模型并不相同。所以，有必要對(duì)實(shí)際系統(tǒng)參數(shù)進(jìn)行辨識(shí)，一個(gè)可以反應(yīng)四旋翼無人機(jī)本質(zhì)特性的模型對(duì)控制器設(shè)計(jì)仍有很大的幫助。四旋翼無人直升機(jī)系統(tǒng)是在華科爾的四旋翼遙控直升機(jī)上改造的。使用數(shù)字羅盤測(cè)量參數(shù)辨識(shí)所需的信息之后，通過無線模塊發(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í)的懸掛方法圖 四旋翼無人機(jī)基本參數(shù)參數(shù)含義大小m質(zhì)量g重力加速度r懸點(diǎn)到重心距離l繩長(zhǎng)震蕩周期則轉(zhuǎn)動(dòng)慣量可由以下方程得出[22] ()空氣阻尼系數(shù)可以使用自由擺的表達(dá)式來求得At=A(0)exp?(λωt)cos?(1λ2ωt) ()其中阻尼 等于 ()Kaf可由式()求得。 辨識(shí)結(jié)果與分析（1） Z軸自由擺動(dòng)幅值曲線圖 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í)曲線 Z軸辨識(shí)曲線和原始曲線對(duì)比（3）其他軸的辨識(shí)曲線：— Y軸自由擺動(dòng)曲線 Y軸采樣數(shù)據(jù)傅里葉分析 Y軸辨識(shí)曲線 Y軸辨識(shí)曲線和原始曲線對(duì)比 X軸自由擺動(dòng)曲線 X軸采樣數(shù)據(jù)傅里葉分析 X軸辨識(shí)曲線 X軸辨識(shí)曲線和原始曲線對(duì)比（4）辨識(shí)結(jié)果。 辨識(shí)實(shí)驗(yàn)獲得的參數(shù)參數(shù)大小說明m/g質(zhì)量/重力加速度r中心離旋翼中心距離Jdiag(, ,)轉(zhuǎn)動(dòng)慣量陣Kafdiag( , , ) N/rad/s空氣阻力系數(shù)陣 模型其他參數(shù)參數(shù)大小說明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è)旋翼的推力 四旋翼無人直升機(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)：四旋翼無人機(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ī)所在的軸線方向受力就不平衡，從而產(chǎn)生俯仰運(yùn)動(dòng)。（4）滾轉(zhuǎn)運(yùn)動(dòng)：由于四旋翼直升機(jī)的對(duì)稱性，滾轉(zhuǎn)運(yùn)動(dòng)的產(chǎn)生原因和俯仰運(yùn)動(dòng)的產(chǎn)生原因相同。 四旋翼無人直升機(jī)的姿態(tài)控制結(jié)構(gòu)目前國(guó)內(nèi)外學(xué)者使用了多種控制方案來控制四旋翼無人直升機(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ù)為 ()。 四旋翼無人機(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ù)式()。 四旋翼無人機(jī)控制系統(tǒng)圖上圖中theta、gamma和psi模塊分別對(duì)應(yīng)三個(gè)姿態(tài)角的PID控制器；；fourrotors模塊對(duì)應(yīng)于式()所示的四旋翼無人直升機(jī)的系統(tǒng)模型。 四旋翼無人直升機(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是四旋翼無人直升機(jī)直線運(yùn)動(dòng)模塊；VX1是直升機(jī)轉(zhuǎn)動(dòng)模塊。由躍響應(yīng)曲線可以看出，四旋翼無人機(jī)的各姿態(tài)角的閉環(huán)性能很差。另外，上圖中各姿態(tài)角的震蕩周期與轉(zhuǎn)動(dòng)慣量有關(guān)，轉(zhuǎn)動(dòng)慣量越大，周期越大。因此有必要設(shè)計(jì)控制器來控制各姿態(tài)角，使系統(tǒng)性能滿足要求。采用臨界比例度法[60]。采用PID控制之后， 各姿態(tài)通道PID控制器參數(shù)的值通道KpKiKd俯仰814滾轉(zhuǎn)717航向1330 小結(jié)本章首先介紹了四旋翼無人直升機(jī)的建模方法，并對(duì)四旋翼無人直升機(jī)模型的一些參數(shù)進(jìn)行辨識(shí)。接下來在simulink平臺(tái)上搭建四旋翼無人直升機(jī)的系統(tǒng)仿真模型。在本章的最后設(shè)計(jì)了最常用的PID控制器來控制系統(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)航模塊、控制模塊和無線通訊模塊。本章主要內(nèi)容是軟件系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)，在總流程之后分別介紹各子模塊的功能和實(shí)現(xiàn)方法。 軟件系統(tǒng)總流程四旋翼無人直升機(jī)的軟件系統(tǒng)的總流程如下圖所示： 軟件系統(tǒng)總流程圖由上圖可以看出，整個(gè)軟件系統(tǒng)分為主流程和中斷處理兩部分。中斷處理部分的作用是解決低速外設(shè)和高速DSP之間的通信問題。主流程負(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ù)采集模塊和無線通信模塊共同完成；導(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)、看門狗和外設(shè)時(shí)鐘等）②初始化GPIO③關(guān)全局中斷④初始化中斷控制寄存器⑤清除CPU中斷標(biāo)志寄存器和中斷使能寄存器⑥初始化中斷向量表⑦開中斷⑧初始化外設(shè)⑨使能CPU中斷和PIE中斷⑩使能全局中斷詳細(xì)信息可參考相關(guān)文獻(xiàn)或者TI官方的例程。 硬件系統(tǒng)初始化流程圖