【正文】 ，一般硬件初始化步驟如下：①初始化系統(tǒng)控制（包括鎖相環(huán)、看門狗和外設(shè)時鐘等）②初始化GPIO③關(guān)全局中斷④初始化中斷控制寄存器⑤清除CPU中斷標(biāo)志寄存器和中斷使能寄存器⑥初始化中斷向量表⑦開中斷⑧初始化外設(shè)⑨使能CPU中斷和PIE中斷⑩使能全局中斷詳細(xì)信息可參考相關(guān)文獻或者TI官方的例程。 硬件系統(tǒng)初始化硬件系統(tǒng)初始化：包括外設(shè)的啟動與寄存器配置、中斷設(shè)置等等。因此，對DSP性能要求比較高，同時要合理利用DSP的系統(tǒng)資源。其中系統(tǒng)初始化自檢操作是由系統(tǒng)初始化模塊、傳感器數(shù)據(jù)采集模塊和無線通信模塊共同完成；導(dǎo)航解算操作由導(dǎo)航模塊完成；高度/姿態(tài)控制是由控制模塊完成；中斷處理部分由傳感器數(shù)據(jù)采集模塊和控制模塊共同完成。若是選擇自動飛行，系統(tǒng)根據(jù)控制目標(biāo)，自動完成飛行動作。主流程負(fù)責(zé)整個系統(tǒng)各部分的協(xié)調(diào)工作：上電之后，系統(tǒng)進行初始化自檢。 軟件系統(tǒng)總流程四旋翼無人直升機的軟件系統(tǒng)的總流程如下圖所示： 軟件系統(tǒng)總流程圖由上圖可以看出，整個軟件系統(tǒng)分為主流程和中斷處理兩部分。6 軟件系統(tǒng)設(shè)計 引言控制系統(tǒng)軟件一共分為六個模塊：系統(tǒng)初始化模塊、傳感器數(shù)據(jù)采集模塊、數(shù)據(jù)處理模塊、導(dǎo)航模塊、控制模塊和無線通訊模塊。在本章的最后設(shè)計了最常用的PID控制器來控制系統(tǒng)的姿態(tài)角。采用PID控制之后， 各姿態(tài)通道PID控制器參數(shù)的值通道KpKiKd俯仰814滾轉(zhuǎn)717航向1330 小結(jié)本章首先介紹了四旋翼無人直升機的建模方法，并對四旋翼無人直升機模型的一些參數(shù)進行辨識。因此有必要設(shè)計控制器來控制各姿態(tài)角，使系統(tǒng)性能滿足要求。由躍響應(yīng)曲線可以看出，四旋翼無人機的各姿態(tài)角的閉環(huán)性能很差。 四旋翼無人直升機內(nèi)部結(jié)構(gòu)圖上圖中motor為電機模塊，輸入、輸出量為電機的電壓和轉(zhuǎn)速；Mi為電機的扭矩模塊；Fi為旋翼推力模塊；Ff為機體坐標(biāo)系推力與導(dǎo)航坐標(biāo)系推力轉(zhuǎn)換模塊；VX是四旋翼無人直升機直線運動模塊；VX1是直升機轉(zhuǎn)動模塊。 系統(tǒng)仿真分析 仿真平臺搭建由本章前文分析，依據(jù)式()。當(dāng)M0為正時，左向電機電壓減少，右向電機電壓增大，并且增加量等于減少量，這樣便在滾轉(zhuǎn)通道上產(chǎn)生扭矩。 四旋翼無人機姿態(tài)控制結(jié)構(gòu)圖上圖中，前向通道(F)、后向通道(B)、左向通道(L)、右向通道(R)、電機電機2和電機4；‘+’表示電壓增大，‘’表示電壓減小。 PID控制器結(jié)構(gòu)簡單，使用靈活，本文選用PID控制器控制各個姿態(tài)。 四旋翼無人直升機的姿態(tài)控制結(jié)構(gòu)目前國內(nèi)外學(xué)者使用了多種控制方案來控制四旋翼無人直升機的姿態(tài)。（3）俯仰運動：，如果旋翼1和旋翼3的推力不同，則機體在這兩個電機所在的軸線方向受力就不平衡，從而產(chǎn)生俯仰運動。（2）水平轉(zhuǎn)動：四旋翼無人機的一對電機順時針旋轉(zhuǎn)，產(chǎn)生逆時針扭矩；另一對電機逆時針旋轉(zhuǎn)，產(chǎn)生順時針扭矩。（1）垂向運動：增加四個旋翼的轉(zhuǎn)速，旋翼的推力增大；減小四個旋翼的轉(zhuǎn)速，旋翼的推力減少。 辨識實驗獲得的參數(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ù)()式，得到最終模型如下： ()上式中各變量含義如下表所示。=105。令方差最小的值為。對應(yīng)于角頻率= rad/s；=（2）Ka的辨識采用試湊法對Ka進行辨識。為了確定轉(zhuǎn)動慣量陣J，： Z軸參數(shù)辨識的懸掛方法圖 X/Y軸參數(shù)辨識的懸掛方法圖 四旋翼無人機基本參數(shù)參數(shù)含義大小m質(zhì)量g重力加速度r懸點到重心距離l繩長震蕩周期則轉(zhuǎn)動慣量可由以下方程得出[22] ()空氣阻尼系數(shù)可以使用自由擺的表達(dá)式來求得At=A(0)exp?(λωt)cos?(1λ2ωt) ()其中阻尼 等于 ()Kaf可由式()求得。 模型參數(shù)辨識本節(jié)的主要內(nèi)容是介紹()式中轉(zhuǎn)動慣量陣J和空氣摩擦系數(shù)陣Kaf的辨識方法。四旋翼無人直升機系統(tǒng)是在華科爾的四旋翼遙控直升機上改造的。但是一些重要的參數(shù)，如質(zhì)量、轉(zhuǎn)動慣量等與原四旋翼直升機模型并不相同。將式()到式()分析，考慮到簡潔性，由于Jr的值很小()所以忽略陀螺效應(yīng)，便得到如式()所示的簡化系統(tǒng)模型 () 直流電機的模型[17]文獻[7]將小型直流電機近似為如下慣性環(huán)節(jié)模型 ()其中 為電機轉(zhuǎn)速，U為輸入電壓，a為系數(shù)根據(jù)以上分析，由式()、()和()可得四旋翼無人機的動態(tài)方程為： () 四旋翼無人機模型參數(shù)辨識，得到了如式()所示的系統(tǒng)模型。 式()中 是陀螺效應(yīng)。Mi是四旋翼的z軸力矩 ()Kd是旋翼的阻力系數(shù)。因為四旋翼機有對稱性，所以為對角陣。地理坐標(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ù)剛體運動定律可知 ()式()中m為四旋翼直升機的質(zhì)量，ξ為導(dǎo)航系中的位置[x y z]T，F(xiàn)f、Fdt、FG為旋翼推力、空氣阻力和重力，且 ()其中Kdt=diag(Kdtx,Kdty,Kdtz)為阻力系數(shù)陣。 四旋翼無人機的受力和運動圖四旋翼無人直升機直線運動的阻力一共有三個來源：重力、慣性和空氣阻力。 直線運動的模型[17] [22[[50] [51]，四旋翼直升機的控制相當(dāng)于對力和扭矩的平衡。本文所選用的是國產(chǎn)華科爾四旋翼無人直升機（Walker UFO4），它和Draganflyer結(jié)構(gòu)相似（），所以對Draganflyer的建模方法對本文的四旋翼機具有一定的指導(dǎo)意義。澳大利亞、法國和美國的學(xué)者共同對X4flyer的動態(tài)建模[6]。實際系統(tǒng)中可以使用數(shù)字羅盤對慣性導(dǎo)航系統(tǒng)進行定時補償，從補償?shù)慕Y(jié)果來看，導(dǎo)航的精度大大增加。本章最后給出了實際導(dǎo)航系統(tǒng)的姿態(tài)變化曲線。 小結(jié)本章介紹了四旋翼無人直升機的捷聯(lián)式慣性導(dǎo)航的原理和常用的姿態(tài)解算方法。本文對四旋翼無人機平臺的研究是在在理想環(huán)境下進行的，系統(tǒng)的動態(tài)特性、環(huán)境變化都很小，因此可以使用數(shù)字羅盤對慣性導(dǎo)航姿態(tài)校準(zhǔn)而不考慮數(shù)字羅盤的延時性。數(shù)字羅盤精度較高，可以進行導(dǎo)航校準(zhǔn)。另外各姿態(tài)角的精度也大大提高。因此可以采用數(shù)字羅盤對慣性導(dǎo)航系統(tǒng)進行每秒鐘8次的定時校準(zhǔn)。導(dǎo)航系統(tǒng)中，比慣性導(dǎo)航更精準(zhǔn)的傳感器是數(shù)字羅盤hmr3300，176。（2）定時的使用更精準(zhǔn)姿態(tài)傳感器按照()式進行四元數(shù)的初始化校準(zhǔn)。隨著時間的積累姿態(tài)角偏差越來越大，因此需要定時校正姿態(tài)偏差。/s、176。176。 靜態(tài)情況下姿態(tài)角變化曲線圖：在初始對準(zhǔn)到0176。A/D校正的軟件流程可參見第六章相關(guān)內(nèi)容。（2）利用線性方程y=xma+b和已知參考值xL，YL，xH，yH，計算實際增益誤差和實際失調(diào)誤差實際增益誤差 ()失調(diào)增益誤差 ()（3）定義輸入x=yCalGainCalOffset，則可以得到校正增益CalGain=1/ma=（xHxL）/（yHyL），校正失調(diào)CalOffset=b/ma=yL/maxL。補償?shù)脑硎峭ㄟ^ADC的兩個通道分別輸入基準(zhǔn)參考電壓，然后根據(jù)基準(zhǔn)值和轉(zhuǎn)換結(jié)果來修正偏差。 A/D轉(zhuǎn)換偏差的校正A/D轉(zhuǎn)換器有一個很大的缺陷，即存在增益誤差和偏移誤差。對采樣信號數(shù)據(jù)預(yù)處理時使用的防干擾平均濾波方法設(shè)置的隊列長度為20階，實際使用數(shù)據(jù)是16階。因此設(shè)計的低通濾波器的通帶邊界選擇歸一化頻率，阻帶邊界盡量靠近通帶邊界。FIR數(shù)字濾波器的差分方程描述為[67] ()對應(yīng)系統(tǒng)函數(shù)為 ()以下是設(shè)計FIR濾波器設(shè)計時應(yīng)注意的事項：（1）關(guān)于通頻帶：采樣頻率為763Hz對應(yīng)奈奎斯特頻率為1。所以根據(jù)以上原因。大多數(shù)算法能夠逼近任意頻率響應(yīng)。IIR的優(yōu)點是有現(xiàn)成的設(shè)計公式，設(shè)計簡單，但相位響應(yīng)不好。 FIR數(shù)字濾波器設(shè)計數(shù)字濾波器的主要功能是對數(shù)字信號處理，最常用的是保留數(shù)字信號中的有用頻率成分，濾除信號中的無用頻率成分。陀螺儀的帶寬40Hz之內(nèi)的幅值比較大，為有用信息，但是在40Hz到90Hz之間有一段幅值比較大的噪聲頻率。高頻噪聲幅值較小。首先去除原始信號中的直流趨勢項， 加速度計x軸振幅譜下邊是陀螺儀x軸的振幅譜，、。根據(jù)香農(nóng)采樣定理[63][64]，為了使采樣信號不失真，采樣頻率至少大于系統(tǒng)最大頻率的兩倍，因此設(shè)置采樣頻率為763Hz。定義采樣頻率的一半為歸一化頻率（即奈奎斯特頻率）為1，而將小于奈奎斯特頻率的頻率歸一化。通過傅里葉分解，我們可以看出數(shù)字序列中含有那種頻率成分。 原始采樣數(shù)據(jù) 采樣數(shù)據(jù)經(jīng)滑動均值濾波之后，基本可以去除采樣數(shù)據(jù)的脈沖干擾。剩余N4個數(shù)據(jù)取算術(shù)平均值。 傳感器采樣數(shù)據(jù)處理 中值平均濾波雖然在硬件電路設(shè)計中采用了抗干擾的措施，但是模擬傳感器和A/D接口之間的電路仍不免混入干擾信號。 慣性傳感器采樣慣性器件的傳感器帶寬最大為250Hz，根據(jù)香農(nóng)定理，設(shè)置采樣頻率為781Hz。對導(dǎo)航系統(tǒng)的靜態(tài)測試可以反應(yīng)出導(dǎo)航系統(tǒng)的穩(wěn)定性。靜態(tài)穩(wěn)定性是衡量導(dǎo)航系統(tǒng)的一個重要指標(biāo)。根據(jù)以上幾點分析，不難發(fā)現(xiàn)，地球運動對導(dǎo)航系統(tǒng)帶來的誤差并不起主導(dǎo)作用。（4）系統(tǒng)誤差：導(dǎo)航系統(tǒng)的誤差來源很多[45]，測量裝置的誤差（環(huán)境變化、轉(zhuǎn)換誤差等）、測量誤差、數(shù)學(xué)模型的近似誤差、初始對準(zhǔn)引起的誤差、迭代算法造成的誤差、元器件安裝誤差等等。/s（），理論上A/（12位），176。（3）傳感器精度不高：本文平臺所選用傳感器精度不是很高。（2）導(dǎo)航信息需求不多：目前系統(tǒng)設(shè)計的主要目標(biāo)是對四旋翼飛行器的姿態(tài)控制?！澹?Google earth數(shù)據(jù))。因此時間短、移動范圍小。但是對于本文所研究的小型四旋翼無人直升機的實驗平臺來說，導(dǎo)航算法的一些步驟是可以簡化的。上述（1）~（6）的內(nèi)容是經(jīng)典捷聯(lián)式慣性導(dǎo)航的基本原理。+M+180176。++M090176。（4）四元數(shù)歸一化[44][45]：姿態(tài)矩陣是正交矩陣，如果四元數(shù)法計算存在舍入誤差、截斷誤差，會導(dǎo)致捷聯(lián)矩陣成為非正交矩陣。在實際系統(tǒng)中使用數(shù)字羅盤獲取，然后采用式()確定四元數(shù)的初值。（2）四元數(shù)初值確定[44][47]：四元數(shù)的初值確定是捷聯(lián)慣導(dǎo)的重要的組成部分。為陀螺儀測量的載體角速度，為加速度計測量的加速度?？紤]到DSP的處理能力和系統(tǒng)實時性要求，本文采用四元數(shù)法進行導(dǎo)航姿態(tài)解算。歐拉角法的優(yōu)點是變量少，但是需要進行大量的三角函數(shù)運算，而且還存在奇點問題。本文選用捷聯(lián)式慣性導(dǎo)航算法。機構(gòu)簡單、但是存在計算復(fù)雜和計算量大的缺點。兩種導(dǎo)航方案各有優(yōu)缺點：平臺式導(dǎo)航使用復(fù)雜的機械結(jié)構(gòu)將慣性敏感元件與載體的角運動隔離開來，系統(tǒng)的精度高，但是成本也比較高、可靠性差，體積大。 四旋翼無人機捷聯(lián)慣導(dǎo)設(shè)計 慣性導(dǎo)航方案設(shè)計慣性導(dǎo)航[42][44]利用慣性傳感器測量載體相對于慣性空間的加速度和角速度，在給定初始運動的條件下，根據(jù)牛頓運動定律，推算載體的瞬時速度和瞬時位置。機體系（b）和導(dǎo)航系（n）之間的方向余弦陣可由下式表示[46] ()而。坐標(biāo)系之間的關(guān)系可采用四元數(shù)法或方向余弦陣描述[43]。相對于慣性坐標(biāo)系，非慣性坐標(biāo)系卻是普遍存在的，[43]。坐標(biāo)系的選擇非常重要，因為在不同坐標(biāo)系下飛行器的描述規(guī)律和運動形式是不同的。 導(dǎo)航坐標(biāo)系描述坐標(biāo)系是描述物體所在位置和物體的運動規(guī)律所選取的參考標(biāo)準(zhǔn)[43][44]。常用的導(dǎo)航系統(tǒng)[43]有慣性導(dǎo)航、路標(biāo)導(dǎo)航、天文導(dǎo)航、無線電導(dǎo)航、衛(wèi)星導(dǎo)航、推算導(dǎo)航、組合導(dǎo)航等。導(dǎo)航的根本任務(wù)就是引導(dǎo)飛行器從一個地點到另外一個地點[42]。最后給出了設(shè)計的抗干擾措施和硬件電路板的PCB布局。 PCB 正面布局圖 PCB背面布局圖 小結(jié)本章主要介紹了四旋翼無人機控制硬件系統(tǒng)的設(shè)計：包括元器件的選型和DSP最小系統(tǒng)的設(shè)計。另外，導(dǎo)線寬度突變和小于90度的拐角會點電路系統(tǒng)中產(chǎn)生反射噪聲，實際布線中可以采用135度拐角走線，另外可以使用水滴形焊盤來過度不同寬度的導(dǎo)線。為了增加穩(wěn)定性、減少干擾，應(yīng)當(dāng)加大布線的安全距離。（3）輸入輸出盡量避免平行：系統(tǒng)傳感器有很多采用雙線通信的方式，比如超聲傳感器、數(shù)字羅盤和無線模塊等等，為了防止發(fā)生反饋耦合，輸入輸出線路避免平行。比如專門為DSP供電的TPS767D318采用的封裝是TSSOP28。在布線時，將加速度計與DSP的A/D引腳之間連線盡量走最短路線，增加可靠性。（4）整齊美觀根據(jù)以上原則， 元器件布局圖 PCB布線布線基本原則[34]（1）連線盡量短：一個很好的抗干擾措施是減少PCB布線長度，從而減少干擾進入線路的機會。電機驅(qū)動芯片和開關(guān)元器件（比如78M05）也會產(chǎn)生很強的電氣噪聲，這兩種器件會對DSP產(chǎn)生干擾，系統(tǒng)設(shè)計中應(yīng)將電機驅(qū)動