張　鵬，王　鍵，段照斌（.中國民航大學 工程技術訓練中心，天津 300300；.中國民航大學　航空自動化學院，天津　300300）

一種小型固定翼無人機姿態(tài)測量系統(tǒng)的設計*

張鵬1，王鍵2，段照斌2
（1.中國民航大學 工程技術訓練中心，天津 300300；2.中國民航大學航空自動化學院，天津300300）

針對小型固定翼無人機設計中對姿態(tài)測量系統(tǒng)小型化、低成本的需要，設計了以STM32F405為處理核心的低成本的姿態(tài)測量系統(tǒng)。系統(tǒng)采用MARG傳感器方案，利用優(yōu)化的梯度遞減算法對采集的數(shù)據(jù)進行處理，將處理結果通過改進的互補濾波算法完成數(shù)據(jù)的融合，最終完成飛行姿態(tài)參數(shù)的解算。實驗驗證表明，所設計的系統(tǒng)具有實時性、低功耗、低成本、小型化等特點，能較好地完成無人機的姿態(tài)角數(shù)據(jù)的測量，具有較強實用性。

無人機；姿態(tài)測量；STM32F405

0　引言

固定翼無人機廣泛用于航拍、測繪、監(jiān)測等各個領域。姿態(tài)測量系統(tǒng)作為無人機飛行控制系統(tǒng)的重要組成部分，為無人機飛行控制系統(tǒng)提供控制所需的數(shù)據(jù)，能否準確地測量機體當前姿態(tài)角直接影響到固定翼無人機飛控算法的設計的穩(wěn)定性、可靠性和實現(xiàn)的難易程度［1］。

隨著無人機技術的不斷發(fā)展，傳統(tǒng)的框架式姿態(tài)測量系統(tǒng)質(zhì)量大、體積大、可靠性低、成本高，已經(jīng)不能滿足新的設計需求。近年來，微電子及傳感器技術和加工工藝的進步，設計出體積小、成本低、性能高的姿態(tài)測量系統(tǒng)成為可能，以嵌入式為內(nèi)核的姿態(tài)測量系統(tǒng)已經(jīng)進入實用階段［2］。本文設計一種以ARM內(nèi)核的STM32F405為處理核心的姿態(tài)測量系統(tǒng)，并采用梯度遞減算法對采集的數(shù)據(jù)進行處理，通過改進的互補濾波算法進行無人機的姿態(tài)解算。該算法相較于傳統(tǒng)的卡爾曼濾波算法具有運算量小、對采樣頻率要求低等優(yōu)點［3］，能夠滿足一般無人機對姿態(tài)測量的要求。

1　硬件設計

姿態(tài)測量系統(tǒng)是無人機的飛行控制系統(tǒng)的重要組成部分，針對無人機飛行控制系統(tǒng)的要求和姿態(tài)測量系統(tǒng)設計的需要，提出了無人機姿態(tài)測量系統(tǒng)的具體設計方案。系統(tǒng)主要由數(shù)據(jù)采集模塊和數(shù)據(jù)處理模塊組成，采用IIC完成傳感器與處理器的連接，如圖1所示。

數(shù)據(jù)采集模塊主要完成無人機姿態(tài)數(shù)據(jù)的獲取。通過三軸加速度計和三軸陀螺儀組合傳感器MPU6050進行加速度和角速度的測量，免除了單獨使用陀螺儀和加速度計時的軸間差問題，提高了采集的精度；采用帶有數(shù)字接口的弱磁傳感器HMC5883測量磁場強度，其自帶的先進的磁阻傳感器、集成電路放大器、自動消磁器、偏差校準，能保證其磁方位的測量精度在 1°～2°。

圖1　硬件系統(tǒng)框架圖

數(shù)據(jù)處理模塊由微處理器、電源電路、置位復位電路、接口電路、存儲電路組成。綜合運算能力、負載能力、成本、輸入輸出接口等各方面因素，采用主頻為168 MHz的STM32F405微處理器。通過電源電路保證設計中電源的穩(wěn)定。

2　姿態(tài)解算設計

姿態(tài)解算算法測量系統(tǒng)設計的核心內(nèi)容關系到處理數(shù)據(jù)的精確程度。所采用的算法通過四元數(shù)來對物體運動狀態(tài)進行描述，四元數(shù)與歐拉角之間的轉(zhuǎn)換關系如下［4］：

地球坐標系相對于機體坐標系的方向可以用四元數(shù)向量 q=［q1q2q3q4］來描述，則變化率q.微分方程計算如下：

其中，q?表示歸一化的向量，?表示四元數(shù)乘法，ωx、ωy、ωz分別表示陀螺儀測得的機體角速度。則對q.進行數(shù)字積分計算可以計算t時刻的q為：

設地球坐標系下重力場的方向為g，機體坐標系下傳感器的測量結果為s，那么利用四元數(shù)進行參數(shù)轉(zhuǎn)換后的傳感器的測量誤差f為：

因此問題就簡化為找到一個迭代的方法求解q，使得 f最小。當前，求解 fmin的方法較多，其中梯度下降算法是一種計算量較小并且收斂速度快的算法，適合在嵌入式計算平臺上使用，本文采用了n次迭代的梯度下降算法求q，可以表述如下：

其中，J為q的雅克比矩陣。

用四元數(shù)法表示的地球坐標系下的姿態(tài)計算誤差為：

同理，可以認為地球坐標系下磁場的方向為b，機體坐標系下傳感器的測量結果為sm。

在實際環(huán)境中，磁場傳感器的誤差主要包括硬磁誤差和軟磁誤差，其中，硬磁誤差主要通過校正消除，軟磁誤差將由加速度計的計算來消除。b只包含水平和垂直分量，這樣主要是為了消除由于傾斜帶來的測量誤差。

利用四元數(shù)進行參數(shù)轉(zhuǎn)換后的傳感器的測量誤差為f，可以得到磁場方向的誤差函數(shù)為：

q▽，t為迭代算法的估計值，迭代的起點由數(shù)據(jù)融合的方式?jīng)Q定。

μt的值控制q的收斂速率，計算公式如式（16）所示，其中 qω，t為陀螺儀測量到的四元數(shù)變化率，α與加速度計和磁強計測量噪聲有關。

完成數(shù)據(jù)處理后需進行數(shù)據(jù)的融合，由于陀螺儀本身存在的漂移，短時間精度高；加速度計短時間測量精度較差，但對長時間測量誤差不累積。利用頻率上的互補特性進行數(shù)據(jù)融合，提高測量精度和動態(tài)響應［4］。

融合的具體過程如式（17）所示：

γt要保證由陀螺儀積分漂移帶來的誤差能夠被q▽，t的收斂速率抵消掉，則有：

其中，β表示 q▽，t的發(fā)散速率。

由于 α≥ μt，可以化簡如下：

綜上所述，設計的算法軟件實現(xiàn)流程如圖2所示。

圖2　姿態(tài)解算算法軟件實現(xiàn)

3　實驗驗證與結果分析

為了較為全面地對設計的系統(tǒng)進行驗證，將設計的系統(tǒng)固定在三軸慣導轉(zhuǎn)臺 （如圖3所示）上進行飛行模擬試驗。

三軸慣導轉(zhuǎn)臺主要應用于為慣性敏感器件和導航制導系統(tǒng)的性能測試等，具有高精度、高穩(wěn)定性、高分辨力和寬動態(tài)范圍，轉(zhuǎn)臺測量精度為 0.000 1，能夠完成對設計的姿態(tài)測量系統(tǒng)的性能測試。設置并不斷改變測試轉(zhuǎn)臺的運動角速度和運動的方式，模擬無人機的飛行運動。讀取同一時刻設計的姿態(tài)測量系統(tǒng)和慣導平臺的姿態(tài)數(shù)據(jù)進行記錄，如圖4所示，其中data1為測量值，data2為慣導轉(zhuǎn)臺測量值。

通過三軸慣導平臺的性能測試結果可知，設計的姿態(tài)測量系統(tǒng)，俯仰角和滾轉(zhuǎn)角測試誤差小于1°，偏航角測試誤差小于3°，能夠滿足無人機對姿態(tài)的需求。

圖3　三軸慣導平臺

4　結論

本文設計了一個結構簡單、成本低的小型無人機姿態(tài)測量系統(tǒng)，并進行了飛行參數(shù)的解算。性能測試結果表明，設計的姿態(tài)測量系統(tǒng)誤差較小，能夠滿足實際無人機對的姿態(tài)參數(shù)測量的需求。

圖4　姿態(tài)測量的結果

［1］粱延德，程敏，何福本，等.基于互補濾波器的飛行器姿態(tài)解算［J］.傳感器與微系統(tǒng)，2011，30（11）：56-58.

［2］陳文蓉，李杰，蔣竅，等.基于 FPGA和DSP的嵌入式導航計算機的設計［J］.計算機測量與控制，2012，20（12）：3374-3376.

［3］施聞名，徐彬，陳利敏.捷聯(lián)式航姿系統(tǒng)中四元素算法Kalman濾波器的實現(xiàn)研究［J］.自動化技術與應用，2005，24（11）：6-8.

［4］郭曉鴻，楊忠，陳喆.EKF和互補濾波器在飛行姿態(tài)確定中的應用［J］.傳感器與微系統(tǒng)，2011，30（11）：149-152.

Design of attitude measurement system in mini fixed-wing UAV

Zhang Peng1，Wang Jian2，Duan Zhaobin2
（1.Engineering Techniques Training Center，Civil Aviation University of China，Tianjin 300300，China；2.Aeronautical Automation College，Civil Aviation University of China，Tianjin 300300，China）

For the requirements of miniaturized and low-cost of the attitude measurement system to the mini fixed-wing unmanned aerial vehicle（UAV）design，an attitude measurement system based on STM32F405 was designed.The system calculated the flight parameters by using the MARG（Magnetic，Angular Rate，and Gravity）sensors，the collected data disposed by an optimized gradient descent algorithm and the result of dispose would be fused by improved complementary filter.The experimental data shows that the attitude measurement system has the characteristics of real-time，low power consumption，miniaturization and low cost.It can better finish the measurement of UAV′attitude angle data and has strong practicability.

UAV；attitude measurement；STM32F405

TP212

A

1674-7720（2015）02-0035-03

民航局科技項目（MHRD201004）

（2014-08-22）

張鵬（1963-），男，碩士，教授，碩士生導師，主要研究方向：民用航空器導航、控制及信息處理技術。

王鍵（1988-），男，碩士研究生，主要研究方向：導航、制導與控制。