四軸飛行器控制系統(tǒng)設(shè)計與實(shí)現(xiàn)

茹巖，張欣

(柳州鐵道職業(yè)技術(shù)學(xué)院，廣西柳州，545616)

0 引言

四旋翼飛行器最早出現(xiàn)在公眾視野可能要追溯到2009年的著名印度電影《三傻》……2010年，法國Parrot公司發(fā)布了世界上首款流行的四旋翼飛行器AR.Drone。作為一個高科技玩具，它的性能非常優(yōu)秀：輕便、靈活、安全、控制簡單，還能通過傳感器懸停，用WIFI傳送相機(jī)圖像到手機(jī)上。在航拍領(lǐng)域有著其獨(dú)特的優(yōu)勢，目前在國內(nèi)做的比較好的就是深圳的大疆公司，它研發(fā)的飛行器操作相對比較簡單，飛行穩(wěn)定性和可玩性都比較高。但是價格比較貴，目前還無法普及。

本文設(shè)計了一種低成本的捷連貫性導(dǎo)航系統(tǒng)，通過多種傳感器對飛行姿態(tài)進(jìn)行實(shí)時采集，并利用合適的濾波算法對傳感器采集的數(shù)據(jù)進(jìn)行處理，提高姿態(tài)角度的精度。借助無線數(shù)傳模塊，實(shí)現(xiàn)姿態(tài)數(shù)據(jù)的實(shí)時傳輸，完成了四軸飛行器的實(shí)時姿態(tài)控制。

1 姿態(tài)檢測與控制系統(tǒng)組成

四軸飛行器主要無線通訊模塊，姿態(tài)采集模塊和電機(jī)驅(qū)動模塊組成，系統(tǒng)的硬件平臺如圖1所示。

圖1 硬件系統(tǒng)設(shè)計

本系統(tǒng)以STM32F103為主控芯片構(gòu)建中央處理器模塊，負(fù)責(zé)接收和處理傳感器數(shù)據(jù)，根據(jù)姿態(tài)信息計算出相應(yīng)驅(qū)動指令驅(qū)動四個電機(jī)達(dá)到想要的飛行效果。采用MPU9150模塊作為姿態(tài)采集傳感器，MPU9150集成了3軸MEMS陀螺儀、3軸MEMS加速度計和磁力計。模塊通過IIC總線將采集到的x、y、z三軸上的數(shù)字信號傳遞給中央處理器，處理器對原始數(shù)據(jù)進(jìn)行預(yù)處理得到初始角度和角速度值，再經(jīng)過濾波融合算法得到準(zhǔn)確穩(wěn)定的姿態(tài)數(shù)據(jù)。最終中央處理器通過接收遙控模塊發(fā)出的指令來驅(qū)動和控制四個直流無刷電機(jī)做出相應(yīng)的飛行動作和保持動態(tài)的平衡。

本次設(shè)計的硬件系統(tǒng)模型如圖2所示。

圖2 硬件系統(tǒng)模型

2 傳感器的數(shù)據(jù)融合

要使飛行器穩(wěn)定飛行需要獲取飛行器當(dāng)前精確的飛行姿態(tài)，得到準(zhǔn)確的姿態(tài)角就顯得非常重要。這里我們使用陀螺儀、加速度計和磁力計冗余實(shí)時測量飛行器當(dāng)前姿態(tài)。加速度計測量飛行器自身加速度，通過與重力加速度對比，計算當(dāng)前飛行器傾角。加速度計對外界干擾噪聲非常敏感，尤其是振動噪聲。陀螺儀是測量機(jī)體旋轉(zhuǎn)角速度的器件，通過積分運(yùn)算，可以得到機(jī)體的旋轉(zhuǎn)角度。由于陀螺儀的數(shù)據(jù)需要積分才能得到角度，存在積分漂移，隨著時間的不斷累積，誤差會不斷加大。而磁力計又容易受到外部磁場的干擾，解算出來的偏航角容易受到干擾。

由于傳感器測量值存在誤差和噪聲干擾，所以如果直接使用原始數(shù)據(jù)計算姿態(tài)角度，累積誤差會不斷增大，可以采用濾波手段除去數(shù)據(jù)中的噪聲干擾，并將陀螺儀、加速度計和陀螺儀三者進(jìn)行數(shù)據(jù)融合，修正陀螺儀的累積誤差，即可得到較為精確的飛行姿態(tài)。下面我們對卡爾曼濾波和互補(bǔ)濾波進(jìn)行了對比。

2.1 卡爾曼濾波

卡爾曼濾波器是利用目標(biāo)的動態(tài)信息，設(shè)法除去噪聲干擾，從包含噪聲的有限信號中預(yù)測出目標(biāo)真實(shí)值的濾波算法，可以對目標(biāo)位置的進(jìn)行最優(yōu)估計，可以對當(dāng)前目標(biāo)位置、將來目標(biāo)位置或者過去目標(biāo)位置進(jìn)行估計。同時，卡爾曼濾波器采用遞歸式估計，只需當(dāng)前狀態(tài)的觀測值和前一時刻的狀態(tài)估計值就可以計算出當(dāng)前狀態(tài)的估計值，不必浪費(fèi)大量內(nèi)存記錄觀測值和估計估計值的歷史數(shù)據(jù)，有效的節(jié)約系統(tǒng)資源，提高運(yùn)算速度。

圖3中Q為過程噪聲，R為測量噪聲，通過實(shí)驗Q增大，動態(tài)響應(yīng)變快，收斂穩(wěn)定性變壞，R增大，動態(tài)響應(yīng)變慢，收斂穩(wěn)定性變好。

圖3 卡爾曼濾波算法框圖

2.2 互補(bǔ)濾波融合算法

飛行器有三個姿態(tài)角度，俯仰偏航和橫滾角。由于飛行器的偏航不會受到重力加速度的方向，而且橫滾角和俯仰角也不會影響偏航角度，所以分成兩路分別進(jìn)行互補(bǔ)濾波。

利用加速度計的瞬時值來計算飛行器當(dāng)前 姿態(tài)角誤差相對較大；而陀螺儀積分得到的角度不受飛行器加速度的影響，噪聲干擾小，然而利用陀螺儀數(shù)據(jù)計算飛行器姿態(tài)角存在積分漂移和溫度漂移，而且誤差會隨著時間的不斷推移而不斷增大。因此可以使這兩個傳感器取長補(bǔ)短，彌補(bǔ)彼此的缺點(diǎn)。如圖4所示為俯仰角和橫滾角的互補(bǔ)濾波器原理。

圖4 陀螺儀與加速度互補(bǔ)

加速度計是測量機(jī)體加速度的器件，受外界環(huán)境干擾較大，噪聲干擾嚴(yán)重。四軸飛行器由于環(huán)境及材料等原因，飛行過程振動比較強(qiáng)烈，尤其是遇到大風(fēng)等情況，加速度計的測量很容易混入高頻噪聲。陀螺儀是測量角加速度的傳感器，受外界環(huán)境的干擾較小，通過積分獲得的角度在短時間內(nèi)比較準(zhǔn)確，但是存在積分誤差和溫度漂移，誤差隨著積分時間不斷增長而不斷增大。根據(jù)傳感器的不同特性，可通過高通濾波器濾掉加速度計的高頻信號，而通過低通濾波器濾掉陀螺儀低頻信號，然后兩個信號疊加即可得到整個頻帶的信號。互補(bǔ)濾波就是在短時間內(nèi)以陀螺儀測量得到的角度為主，定時對加速度采樣，利用三角函數(shù)計算得到的角度取平均值來修正陀螺儀的累積誤差，從而得到正確的角度。簡而言之，短時間內(nèi)陀螺儀測量計算角度受噪聲干擾小，相對準(zhǔn)備，以其為主，長時間內(nèi)加速度測量計算角度誤差較小，此時增大加速度計的權(quán)重以糾正陀螺儀的誤差。

假設(shè)Acc為加速度計的測量值，則有：

陀螺儀角速度為ω，經(jīng)過積分得到的角度為θt：

互補(bǔ)濾波更新飛行器姿態(tài)角公式：

θ是最終融合更新的姿態(tài)角度，其中，K1、K2是由系統(tǒng)時間常數(shù)計算得到的常數(shù)，且K1+K2=1。在姿態(tài)解算中，主要條件這兩個參數(shù)大小，在短時間內(nèi)以陀螺儀為主，長時間則趨近與加速度計的平均值。

而對于偏航角，如圖5 使用加速度和陀螺儀就無法準(zhǔn)確測出飛行器的偏航角度，就需要磁力計和陀螺儀進(jìn)行互補(bǔ)濾波，磁力計測得的航向角容易受到外部磁場的干擾，而陀螺儀不容易受到外部的干擾。互補(bǔ)的原理與加速度計和陀螺儀類似。

圖5 陀螺儀與磁力計互補(bǔ)

2.3 濾波實(shí)驗及分析

實(shí)驗分析：將加速度計、陀螺儀和磁力計的姿態(tài)角及濾波前后的數(shù)據(jù)導(dǎo)入MATLAB進(jìn)行仿真。

圖6中加入了瞬間傾斜(強(qiáng)加速度)干擾，可以看出，此時的加速度計解算的角度值在干擾下將出現(xiàn)較大的毛刺，最大的誤差達(dá)到了20度而互補(bǔ)濾波得到的角度值并不受干擾影響。

圖6 加速度計與互補(bǔ)濾波

圖7中而陀螺儀積分出來的角度值隨者時間的積累將產(chǎn)生逐步增大的誤差。最大 誤差將近10度。

圖7 陀螺儀與互補(bǔ)濾波

圖8中多次施加外部磁場干擾，可以看出只由磁力計解算出的偏航角在外部磁場靠近時，其偏航角度發(fā)生突變，其誤差在150度。但是經(jīng)過互補(bǔ)濾波后的基本不受影響。

圖8 磁力計與互補(bǔ)濾波

圖9中對互補(bǔ)濾波和卡爾曼濾波進(jìn)行了對比，由圖可以看出，互補(bǔ)濾波調(diào)節(jié)速度快，而卡爾曼濾波存在一定的滯后性，且靈敏度不高，這里我們選擇互補(bǔ)濾波實(shí)現(xiàn)姿態(tài)數(shù)據(jù)的濾波融合。

圖9 互補(bǔ)濾波與卡爾曼濾波

3 姿態(tài)解算算法

姿態(tài)解算涉及到坐標(biāo)變換，向量運(yùn)算、畢卡算法、四元數(shù)與歐拉角轉(zhuǎn)換和互補(bǔ)濾波等算法，姿態(tài)解算的具體實(shí)現(xiàn)步驟為：

（1）將濾波后的三軸加速度傳感器值歸一化，該值為機(jī)體坐標(biāo)系下重力向量測量值；

（2）把地理坐標(biāo)系的重力向量轉(zhuǎn)換到機(jī)體坐標(biāo)系中。

（3）把地理坐標(biāo)系轉(zhuǎn)換到機(jī)體坐標(biāo)系的重力向量與機(jī)體坐標(biāo)系測量的重力向量外積（叉積），就獲得了兩坐標(biāo)系的誤差。

（4）因為陀螺儀會有誤差，且四元數(shù)更新姿態(tài)是用陀螺儀來跟新的，所以陀螺儀的誤差是導(dǎo)致機(jī)體坐標(biāo)系產(chǎn)生誤差的根本原因。這里用兩坐標(biāo)系誤差的PI來補(bǔ)償陀螺儀使跟新后的機(jī)體坐標(biāo)系更加準(zhǔn)確

（5）對四元數(shù)進(jìn)行更新，使用四元數(shù)進(jìn)行微分方程的一階畢卡算法。

（6）由于誤差的引入使得四元數(shù)的模不為1，導(dǎo)致四元數(shù)不是規(guī)范化四元數(shù)，所以在使用四元數(shù)計算歐拉角時，必須對四元數(shù)進(jìn)行規(guī)范化處理

（7）四元數(shù)更新完成后即完成了飛行器的姿態(tài)解算過程，為方便直觀的控制飛行器姿態(tài)，將四元數(shù)轉(zhuǎn)換成歐拉角。

4 平衡控制及分析

4.1 串級PID控制器

四軸飛行器的平衡控制是通過負(fù)反饋來實(shí)現(xiàn)的，內(nèi)環(huán)采用的是PD控制，控制的是角速度變化。外環(huán)采用的是PID控制，控制角度變化。

4.2 PID參數(shù)整定

在實(shí)際調(diào)試過程中，遵循先內(nèi)環(huán)后外環(huán)，先比例后微分最后積分的原則。由于四軸飛行器機(jī)架的對稱性，橫滾角（roll）和俯仰角（pitch）參數(shù)一致，航向角的參數(shù)基本也與橫滾角參數(shù)一致。所以串級PID調(diào)試主要調(diào)試5個參數(shù)，即橫滾角的內(nèi)環(huán)PD和外環(huán)PID參數(shù)，實(shí)際調(diào)試圖如下圖10所示。

圖10 實(shí)際調(diào)試

在調(diào)試某個參數(shù)的時候，其它還未調(diào)的參數(shù)設(shè)定為零，根據(jù)施加干擾后實(shí)際效果來確定參數(shù)。內(nèi)環(huán)P小了四軸會往一邊傾斜，大了施加干擾四軸會晃動很厲害；內(nèi)環(huán)D小了抑制擾動的效果不明顯，大了四軸高頻振動很厲害。外環(huán)P小了施加干擾四軸會慢慢往一邊傾斜,大了四軸會在平衡位置晃動很厲害;外環(huán)D小了效果不明顯,大了會在平衡位置高頻振動;外環(huán)I小了四軸的平衡位置不在水平面上,外環(huán)I大了四軸會在平衡位置低頻振動。在這些參數(shù)附近進(jìn)行試驗，直到尋找到比較理想的參數(shù)。

4.3 實(shí)驗驗證

為了驗證飛行器的穩(wěn)定性，在飛行器上機(jī)架上系兩根繩子，用遙控器將飛行器起飛至一定高度，輕拽繩子，使飛行器失去平衡，觀察飛行器是否能自主調(diào)節(jié)平衡，及響應(yīng)速度。并將飛行的實(shí)時數(shù)據(jù)記錄到SD卡內(nèi)，用matlab生成飛行時姿態(tài)曲線。

實(shí)驗一：在飛行器上機(jī)架上系兩根繩子，用遙控器將飛行器起飛至一定高度，輕拽繩子，人為的對其施加干擾，使飛行器失去平衡，觀察飛行器是否能自主調(diào)節(jié)平衡，及響應(yīng)速度。

在實(shí)驗一中多次對飛行器的姿態(tài)角度施加干擾，最大的角度變化達(dá)到30度，但在較短時間內(nèi)能夠恢復(fù)平衡。

實(shí)驗二：飛行器飛到一定高度，用繩子拉著飛行器的底部，觀察高度的變化及其穩(wěn)定性。

在實(shí)驗二中，多次對其施加干擾，但是誤差能夠保持在30cm之內(nèi)，且能夠?qū)Ω蓴_做出迅速的響應(yīng)，基本上能保持在設(shè)定的高度上。但由于沒有對飛行器水平位置進(jìn)行控制，在飛行器受到干擾時，會發(fā)生較大的水平位移。

圖11 姿態(tài)角度測試

5 結(jié)束語

本文設(shè)計了一種低成本的捷連貫性導(dǎo)航系統(tǒng)，通過兩種濾波的對比，最終使用互補(bǔ)濾波對四軸飛行器的姿態(tài)進(jìn)行了融合，并通過串級PID的控制方法進(jìn)行了驗證，并且做了飛行試驗及抗干擾實(shí)驗，基本能夠滿足要求。 當(dāng)然所設(shè)計的系統(tǒng)還存在一定的不足之處，如水平位置沒有進(jìn)行控制，仍需要進(jìn)一步的完善。

圖12 飛行高度擾動測試