茹巖，張欣

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

2.1 卡爾曼濾波

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

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

圖3 卡爾曼濾波算法框圖

2.2 互補濾波融合算法

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

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

圖4 陀螺儀與加速度互補

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

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

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

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

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

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

圖5 陀螺儀與磁力計互補

2.3 濾波實驗及分析

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

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

圖6 加速度計與互補濾波

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

圖7 陀螺儀與互補濾波

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

圖8 磁力計與互補濾波

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

圖9 互補濾波與卡爾曼濾波

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

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

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

（2）把地理坐標系的重力向量轉換到機體坐標系中。

（3）把地理坐標系轉換到機體坐標系的重力向量與機體坐標系測量的重力向量外積（叉積），就獲得了兩坐標系的誤差。

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

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

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

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

4 平衡控制及分析

4.1 串級PID控制器

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

4.2 PID參數(shù)整定

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

圖10 實際調(diào)試

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

4.3 實驗驗證

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

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

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

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

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

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

5 結束語

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

圖12 飛行高度擾動測試