摘 要： 針對小型固定翼無人機(jī)設(shè)計中對姿態(tài)測量系統(tǒng)小型化、低成本的需要,，設(shè)計了以STM32F405為處理核心的低成本的姿態(tài)測量系統(tǒng),。系統(tǒng)采用MARG傳感器方案，利用優(yōu)化的梯度遞減算法對采集的數(shù)據(jù)進(jìn)行處理,，將處理結(jié)果通過改進(jìn)的互補(bǔ)濾波算法完成數(shù)據(jù)的融合,，最終完成飛行姿態(tài)參數(shù)的解算。實(shí)驗驗證表明,，所設(shè)計的系統(tǒng)具有實(shí)時性,、低功耗,、低成本、小型化等特點(diǎn),，能較好地完成無人機(jī)的姿態(tài)角數(shù)據(jù)的測量,，具有較強(qiáng)實(shí)用性。
關(guān)鍵詞： 無人機(jī),；姿態(tài)測量,；STM32F405
0 引言
固定翼無人機(jī)廣泛用于航拍、測繪,、監(jiān)測等各個領(lǐng)域,。姿態(tài)測量系統(tǒng)作為無人機(jī)飛行控制系統(tǒng)的重要組成部分，為無人機(jī)飛行控制系統(tǒng)提供控制所需的數(shù)據(jù),，能否準(zhǔn)確地測量機(jī)體當(dāng)前姿態(tài)角直接影響到固定翼無人機(jī)飛控算法的設(shè)計的穩(wěn)定性,、可靠性和實(shí)現(xiàn)的難易程度[1],。
隨著無人機(jī)技術(shù)的不斷發(fā)展,，傳統(tǒng)的框架式姿態(tài)測量系統(tǒng)質(zhì)量大、體積大,、可靠性低,、成本高,，已經(jīng)不能滿足新的設(shè)計需求。近年來,，微電子及傳感器技術(shù)和加工工藝的進(jìn)步,，設(shè)計出體積小、成本低,、性能高的姿態(tài)測量系統(tǒng)成為可能,，以嵌入式為內(nèi)核的姿態(tài)測量系統(tǒng)已經(jīng)進(jìn)入實(shí)用階段[2]。本文設(shè)計一種以ARM內(nèi)核的STM32F405為處理核心的姿態(tài)測量系統(tǒng),，并采用梯度遞減算法對采集的數(shù)據(jù)進(jìn)行處理,，通過改進(jìn)的互補(bǔ)濾波算法進(jìn)行無人機(jī)的姿態(tài)解算。該算法相較于傳統(tǒng)的卡爾曼濾波算法具有運(yùn)算量小,、對采樣頻率要求低等優(yōu)點(diǎn)[3],，能夠滿足一般無人機(jī)對姿態(tài)測量的要求。
1 硬件設(shè)計
姿態(tài)測量系統(tǒng)是無人機(jī)的飛行控制系統(tǒng)的重要組成部分,，針對無人機(jī)飛行控制系統(tǒng)的要求和姿態(tài)測量系統(tǒng)設(shè)計的需要,，提出了無人機(jī)姿態(tài)測量系統(tǒng)的具體設(shè)計方案。系統(tǒng)主要由數(shù)據(jù)采集模塊和數(shù)據(jù)處理模塊組成,，采用IIC完成傳感器與處理器的連接，如圖1所示,。
數(shù)據(jù)采集模塊主要完成無人機(jī)姿態(tài)數(shù)據(jù)的獲取,。通過三軸加速度計和三軸陀螺儀組合傳感器MPU6050進(jìn)行加速度和角速度的測量,，免除了單獨(dú)使用陀螺儀和加速度計時的軸間差問題，提高了采集的精度,；采用帶有數(shù)字接口的弱磁傳感器HMC5883測量磁場強(qiáng)度,，其自帶的先進(jìn)的磁阻傳感器、集成電路放大器,、自動消磁器,、偏差校準(zhǔn)，能保證其磁方位的測量精度在1°~2°,。
數(shù)據(jù)處理模塊由微處理器,、電源電路、置位復(fù)位電路,、接口電路,、存儲電路組成。綜合運(yùn)算能力,、負(fù)載能力,、成本、輸入輸出接口等各方面因素,，采用主頻為168 MHz的STM32F405微處理器,。通過電源電路保證設(shè)計中電源的穩(wěn)定。
2 姿態(tài)解算設(shè)計
姿態(tài)解算算法測量系統(tǒng)設(shè)計的核心內(nèi)容關(guān)系到處理數(shù)據(jù)的精確程度,。所采用的算法通過四元數(shù)來對物體運(yùn)動狀態(tài)進(jìn)行描述,，四元數(shù)與歐拉角之間的轉(zhuǎn)換關(guān)系如下[4]：
地球坐標(biāo)系相對于機(jī)體坐標(biāo)系的方向可以用四元數(shù)向量q=[q1 q2 q3 q4]來描述，則變化率微分方程計算如下：
其中,，q表示歸一化的向量,，?茚表示四元數(shù)乘法，?棕x,、?棕y,、?棕z分別表示陀螺儀測得的機(jī)體角速度。則對進(jìn)行數(shù)字積分計算可以計算t時刻的q為：
設(shè)地球坐標(biāo)系下重力場的方向為g,，機(jī)體坐標(biāo)系下傳感器的測量結(jié)果為s,，那么利用四元數(shù)進(jìn)行參數(shù)轉(zhuǎn)換后的傳感器的測量誤差f為：
因此問題就簡化為找到一個迭代的方法求解q，使得f最小,。當(dāng)前,，求解fmin的方法較多，其中梯度下降算法是一種計算量較小并且收斂速度快的算法,，適合在嵌入式計算平臺上使用,，本文采用了n次迭代的梯度下降算法求q，可以表述如下：
其中,，J為q的雅克比矩陣,。
同理,，可以認(rèn)為地球坐標(biāo)系下磁場的方向為b，機(jī)體坐標(biāo)系下傳感器的測量結(jié)果為sm,。
在實(shí)際環(huán)境中,，磁場傳感器的誤差主要包括硬磁誤差和軟磁誤差，其中,，硬磁誤差主要通過校正消除,，軟磁誤差將由加速度計的計算來消除。b只包含水平和垂直分量,，這樣主要是為了消除由于傾斜帶來的測量誤差,。
利用四元數(shù)進(jìn)行參數(shù)轉(zhuǎn)換后的傳感器的測量誤差為f，可以得到磁場方向的誤差函數(shù)為：
q,t為迭代算法的估計值,，迭代的起點(diǎn)由數(shù)據(jù)融合的方式?jīng)Q定,。
t的值控制q的收斂速率，計算公式如式（16）所示,，其中q,t為陀螺儀測量到的四元數(shù)變化率加速度計和磁強(qiáng)計測量噪聲有關(guān),。
完成數(shù)據(jù)處理后需進(jìn)行數(shù)據(jù)的融合，由于陀螺儀本身存在的漂移,，短時間精度高,；加速度計短時間測量精度較差，但對長時間測量誤差不累積,。利用頻率上的互補(bǔ)特性進(jìn)行數(shù)據(jù)融合,，提高測量精度和動態(tài)響應(yīng)[4]。
融合的具體過程如式(17)所示：
綜上所述,，設(shè)計的算法軟件實(shí)現(xiàn)流程如圖2所示,。
3 實(shí)驗驗證與結(jié)果分析
為了較為全面地對設(shè)計的系統(tǒng)進(jìn)行驗證，將設(shè)計的系統(tǒng)固定在三軸慣導(dǎo)轉(zhuǎn)臺（如圖3所示）上進(jìn)行飛行模擬試驗,。
三軸慣導(dǎo)轉(zhuǎn)臺主要應(yīng)用于為慣性敏感器件和導(dǎo)航制導(dǎo)系統(tǒng)的性能測試等,，具有高精度、高穩(wěn)定性,、高分辨力和寬動態(tài)范圍,，轉(zhuǎn)臺測量精度為0.000 1，能夠完成對設(shè)計的姿態(tài)測量系統(tǒng)的性能測試,。設(shè)置并不斷改變測試轉(zhuǎn)臺的運(yùn)動角速度和運(yùn)動的方式,，模擬無人機(jī)的飛行運(yùn)動。讀取同一時刻設(shè)計的姿態(tài)測量系統(tǒng)和慣導(dǎo)平臺的姿態(tài)數(shù)據(jù)進(jìn)行記錄,，如圖4所示,，其中data1為測量值，data2為慣導(dǎo)轉(zhuǎn)臺測量值。
通過三軸慣導(dǎo)平臺的性能測試結(jié)果可知,，設(shè)計的姿態(tài)測量系統(tǒng),，俯仰角和滾轉(zhuǎn)角測試誤差小于1°,，偏航角測試誤差小于3°,，能夠滿足無人機(jī)對姿態(tài)的需求。
4 結(jié)論
本文設(shè)計了一個結(jié)構(gòu)簡單,、成本低的小型無人機(jī)姿態(tài)測量系統(tǒng),，并進(jìn)行了飛行參數(shù)的解算。性能測試結(jié)果表明,，設(shè)計的姿態(tài)測量系統(tǒng)誤差較小,，能夠滿足實(shí)際無人機(jī)對的姿態(tài)參數(shù)測量的需求。
參考文獻(xiàn)
[1] 粱延德,，程敏,，何福本，等．基于互補(bǔ)濾波器的飛行器姿態(tài)解算[J]．傳感器與微系統(tǒng),，2011,，30（11）：56-58．
[2] 陳文蓉，李杰,，蔣竅,，等．基于FPGA和DSP的嵌入式導(dǎo)航計算機(jī)的設(shè)計[J].計算機(jī)測量與控制，2012,，20（12）：3374-3376.
[3] 施聞名,，徐彬，陳利敏.捷聯(lián)式航姿系統(tǒng)中四元素算法Kalman濾波器的實(shí)現(xiàn)研究[J].自動化技術(shù)與應(yīng)用,，2005,，24（11）：6-8.
[4] 郭曉鴻，楊忠,，陳喆.EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用[J].傳感器與微系統(tǒng),，2011，30（11）：149-152.