1四旋翼無(wú)人機(jī)的姿態(tài)解算系統(tǒng)分析案例目錄TOC\o"1-3"\h\u4008四旋翼無(wú)人機(jī)的姿態(tài)解算系統(tǒng)分析案例 135461.1四旋翼無(wú)人機(jī)姿態(tài)角表示 1251081.2數(shù)據(jù)解算和控制 558651.2.1互補(bǔ)濾波融合算法 5189571.2.2PID控制 6187231.3小結(jié) 10研究分析無(wú)人機(jī)姿態(tài)檢測(cè)系統(tǒng)，建立機(jī)體運(yùn)動(dòng)方程，分析歐拉角和四元數(shù)的關(guān)系并建立數(shù)學(xué)模型，分析互補(bǔ)濾波算法和PID控制在系統(tǒng)中實(shí)現(xiàn)的作用。1.1四旋翼無(wú)人機(jī)姿態(tài)角表示圖3-1地面坐標(biāo)系圖3-2機(jī)體坐標(biāo)系建立地面坐標(biāo)系如圖3-1所示。Xg軸和水平地面平行，Yg軸垂直于Xg軸，Zg軸與水平地面始終保持垂直。建立機(jī)體坐標(biāo)系如圖3-2所示。點(diǎn)O與飛行器質(zhì)心重合，X軸是飛行器的對(duì)稱(chēng)軸線(xiàn)并始終指向飛行方向，Y軸與飛行器對(duì)稱(chēng)平面互相垂直，Z軸與X軸互相垂直且平行于對(duì)稱(chēng)平面，Z軸始終指向飛行器下方。無(wú)人機(jī)飛行時(shí)的姿態(tài)通過(guò)所建立的地面坐標(biāo)系和機(jī)體坐標(biāo)系之間形成的特定的角度來(lái)表示，具體可以分為俯仰、橫滾和偏航角。俯仰角(pitch)：機(jī)體坐標(biāo)系的X軸和水平面形成的夾角。當(dāng)X軸正半軸在坐標(biāo)原點(diǎn)水平面之上時(shí)俯仰角為正，機(jī)體俯沖時(shí)俯仰角為負(fù)。偏航角(yaw)：機(jī)體Y軸水平面投影與地面Y軸的夾角，機(jī)頭右偏航時(shí)偏航角為正，左偏航時(shí)為負(fù)。(3)橫滾角(roll)：機(jī)體坐標(biāo)系Z軸與Z軸鉛垂面之間形成的夾角，機(jī)體向右滾動(dòng)為正橫滾角，向左滾動(dòng)為負(fù)橫滾角。1.1.1歐拉角歐拉角表示三維空間坐標(biāo)系。參考系靜止不動(dòng)，坐標(biāo)系固定于剛體，伴隨剛體旋轉(zhuǎn)。歐拉角如圖3-3所示。圖3-3歐拉角圖中α是x軸與交點(diǎn)線(xiàn)的夾角，β是z軸與Z軸的夾角，γ是交點(diǎn)線(xiàn)與X軸的夾角。由于夾角的順序和標(biāo)記、夾角的兩個(gè)軸的指定沒(méi)有明確的規(guī)定，因此在使用歐拉角時(shí)必須明確好定義。1.1.2四元數(shù)四元數(shù)是由愛(ài)爾蘭數(shù)學(xué)家哈密頓于1843年發(fā)明的數(shù)學(xué)概念。準(zhǔn)確地說(shuō)，四元數(shù)是復(fù)數(shù)的不可交換延伸。如果把四元數(shù)的集合看做是多維度的實(shí)數(shù)空間，那么四元數(shù)就等價(jià)成一個(gè)四維空間，四元數(shù)具體可以用實(shí)數(shù)和四維向量表示。表示一個(gè)四元數(shù)：（3-1）四元數(shù)的長(zhǎng)度（模）和普通的向量相似：（3-2）四元數(shù)相乘，旋轉(zhuǎn)組合即：（3-3）（3-4）R(q)是四元數(shù)q對(duì)應(yīng)的矩陣，有：（3-5）“四元數(shù)表示”“轉(zhuǎn)”“歐拉角表示”。用顯示（3-6）（3-7）1.1.3姿態(tài)角解算當(dāng)Z坐標(biāo)不變時(shí)旋轉(zhuǎn)，矩陣表示為：（3-8）三維空間圍繞x、y、z軸旋轉(zhuǎn)：（3-9）（3-10）上式為方向余弦矩陣，公式左側(cè)是對(duì)應(yīng)的姿態(tài)角，右側(cè)是推算的角度。三個(gè)角速度通過(guò)陀螺儀在周期內(nèi)旋轉(zhuǎn)過(guò)角度計(jì)算出。只需要對(duì)這個(gè)微分方程計(jì)算就可得到姿態(tài)角度，但是pitch為90度時(shí)方程不存在，因此，歐拉角并不能滿(mǎn)足所有飛行情況下姿態(tài)角的表示。三維空間旋轉(zhuǎn)可以用單位四元數(shù)表示：（3-11）用四元數(shù)來(lái)表示歐拉角的方向余弦矩陣為：（3-12）1.2數(shù)據(jù)解算和控制四旋翼無(wú)人機(jī)飛行的穩(wěn)定需要精確的傳感器數(shù)據(jù)提供保障。本設(shè)計(jì)中使用到的姿態(tài)檢測(cè)模塊是MPU9250，它集成了加速度計(jì)、磁力計(jì)和陀螺儀這三個(gè)傳感器模塊。飛行器的飛行角度可由加速度計(jì)、陀螺儀計(jì)算出，但是加速計(jì)和陀螺儀都有一定的缺點(diǎn)，例如，加速度計(jì)對(duì)外部環(huán)境噪聲的干擾過(guò)于敏感，陀螺儀測(cè)量得到的原始值由于需要進(jìn)行積分運(yùn)算，且過(guò)程中會(huì)產(chǎn)生累積誤差，時(shí)間越久誤差越大。磁力計(jì)容易受到周?chē)艌?chǎng)的影響，所以，為了能獲取更加精確的姿態(tài)角度，這就需要對(duì)三種傳感器傳出的數(shù)據(jù)進(jìn)行處理來(lái)提升精確度，互補(bǔ)濾波算法是目前最為廣泛使用的濾波融合算法。1.2.1互補(bǔ)濾波融合算法互補(bǔ)濾波器就相當(dāng)于頻域特性濾波器?；パa(bǔ)濾波器是根據(jù)傳感器傳輸?shù)臄?shù)據(jù)在不同頻域上會(huì)產(chǎn)生不同的響應(yīng)效果。它的本質(zhì)將傳感器數(shù)據(jù)從頻域的角度進(jìn)行融合來(lái)達(dá)到獲得更加精準(zhǔn)的數(shù)據(jù)。加速度計(jì)和磁力計(jì)由于存在著一定的缺陷，例如加速度計(jì)在機(jī)體飛行時(shí)會(huì)受到震動(dòng)和空氣阻力的影響，磁力計(jì)會(huì)受到地球磁場(chǎng)影響，而陀螺儀雖然有更好的動(dòng)態(tài)性能，但是溫度和零點(diǎn)漂移直接影響了數(shù)據(jù)傳出的準(zhǔn)確性。單一的數(shù)據(jù)傳感器都會(huì)存在一定缺陷，達(dá)不到在所有頻率范圍內(nèi)都表現(xiàn)出良好的響應(yīng)特性的要求。然而，互補(bǔ)濾波融合算法卻可以從頻域的角度實(shí)現(xiàn)多傳感器數(shù)據(jù)的融合?；パa(bǔ)濾波融合算法流程如圖3-4所示。圖3-4互補(bǔ)濾波算法原理框圖以加速計(jì)、陀螺儀兩者的互補(bǔ)濾波融合為例，式中Acc是加速計(jì)測(cè)出的原始數(shù)據(jù)：（3-13）由反三角函數(shù)可以將轉(zhuǎn)換成為角度值A(chǔ)cc，則有：（3-14）w是初始角速度，對(duì)其積分得到角度θt：（3-15）經(jīng)由互補(bǔ)濾波，飛行器姿態(tài)角可以表示成：（3-16）經(jīng)由互補(bǔ)濾波算法融合得到最終的姿態(tài)角度θ，K1、K2是兩個(gè)常數(shù)且K1+K2=1。陀螺儀和磁力計(jì)之間的互補(bǔ)數(shù)據(jù)融合過(guò)程與上述過(guò)程類(lèi)似。1.2.2PID控制1.PID控制理論閉環(huán)自動(dòng)控制技術(shù)基于反饋來(lái)減少不確定性。PID控制由比例、積分和微分控制共同構(gòu)成。比例控制是PID理論的基礎(chǔ)，積分控制在一定程度上能消除系統(tǒng)穩(wěn)態(tài)誤差，但也會(huì)導(dǎo)致調(diào)節(jié)過(guò)度，微分控制能夠有效地提高控制系統(tǒng)響應(yīng)速度并減弱系統(tǒng)可能存在的調(diào)節(jié)過(guò)度。因此，PID控制能更好地糾正系統(tǒng)。PID控制原理如圖3-5所示。圖3-5傳統(tǒng)PID算法原理圖PID原理基本公式為：（3-17）其中r(t)是設(shè)定值也是期望值，u(t)是控制量，y(t)是控制系統(tǒng)實(shí)際輸出的值，e是y(t)與r(t)的差值，u(t)由各項(xiàng)線(xiàn)性組合得出。Kp、Ki、Kd分別表示比例、積分和微分系數(shù)，G(s)是傳遞函數(shù)：（3-18）1.比例（P）控制PID控制器中比例項(xiàng)能夠?qū)ο到y(tǒng)產(chǎn)生的誤差擴(kuò)大化、明顯化，使誤差值能夠迅速、明顯地反映出來(lái)，并為系統(tǒng)存在的偏差進(jìn)行失調(diào)補(bǔ)償。比例項(xiàng)P的輸出公式：（3-19）當(dāng)積分微分增益ki、kd不變時(shí)，比例增益kp改變，受控變量階躍響應(yīng)如圖3-6和3-7所示。圖3-6階躍響應(yīng)曲線(xiàn)圖3-7階躍響應(yīng)曲線(xiàn)控制系統(tǒng)中加入比例控制器能夠提高系統(tǒng)反應(yīng)的速度，但是比例系數(shù)大小控制不好會(huì)導(dǎo)致系統(tǒng)失調(diào)而引發(fā)震蕩，使系統(tǒng)進(jìn)入不穩(wěn)定狀態(tài)，這就失去了比例控制的意義，所以控制比例系數(shù)值的大小也是關(guān)鍵。2.積分（I）控制積分能消除系統(tǒng)穩(wěn)態(tài)誤差。積分項(xiàng)I輸出公式：（3-20）當(dāng)只有積分增益ki變化，其余增益不變，受控變量階躍響應(yīng)如圖3-8和圖3-9所示。圖3-8階躍響應(yīng)曲線(xiàn)圖3-9階躍響應(yīng)曲線(xiàn)PID控制器中積分項(xiàng)能提高控制精度，有效減弱系統(tǒng)存在的靜態(tài)誤差。在一定范圍內(nèi)積分的作用和Ti的大小成正比關(guān)系，但是，對(duì)Ti過(guò)度調(diào)節(jié)會(huì)導(dǎo)致系統(tǒng)積分飽和并且超調(diào)增大，使系統(tǒng)進(jìn)入不穩(wěn)定狀態(tài)，最終導(dǎo)致積分控制響應(yīng)速度相位滯后。1.微分（D）控制在PID控制的微分環(huán)節(jié)，輸出和控制器的輸入誤差的微分成正比，微分調(diào)節(jié)在PID控制器中能夠使系統(tǒng)控制量提前。微分項(xiàng)輸出：（3-21）微分增益kd變化，增益kp和ki不變，階躍響應(yīng)如圖3-10和3-11所示。圖3-10階躍響應(yīng)曲線(xiàn)圖3-11階躍響應(yīng)曲線(xiàn)微分控制器能夠預(yù)知系統(tǒng)的偏差并抑制偏差擴(kuò)大的趨勢(shì)，從而克服系統(tǒng)產(chǎn)生的震蕩，改善系統(tǒng)動(dòng)態(tài)性能。1.3小結(jié)本章研究分析四旋翼無(wú)人機(jī)的姿態(tài)解算系統(tǒng)，對(duì)機(jī)體建立地理坐標(biāo)系，分析歐拉角和四元數(shù)與無(wú)人機(jī)姿態(tài)角之間存