馬 敏，吳海超

MA Min,WU Hai-chao

（中國民航大學(xué) 航空自動化學(xué)院，天津 300300）

0 引言

四旋翼(Quad-Rotor)無人飛行器是由四螺旋槳驅(qū)動的、能夠?qū)崿F(xiàn)垂直起降的飛行器，由前后和左右兩組旋翼組成，旋轉(zhuǎn)時對應(yīng)方向上電機旋轉(zhuǎn)方向相反，其與一般直升機最主要的不同點是四旋翼飛行器不需要用尾槳來平衡機體。特別適合在近地面環(huán)境中執(zhí)行監(jiān)視和偵察等任務(wù)，具有廣闊的軍事和民用前景[1]。

其中，姿態(tài)解算的精度是實現(xiàn)飛行器穩(wěn)定飛行的前提。而體積小、功耗低及易于集成的MEMS 傳感器是實現(xiàn)低成本航姿測量的最佳選擇，主要由三軸陀螺儀、三軸加速度計和三軸電子羅盤組成。MEMS 陀螺儀測量精度較低，雖然動態(tài)性能良好，但存在累計漂移誤差，加速度計和電子羅盤有著較好的靜態(tài)特性，沒有漂移問題，但容易受機體振動和外部磁場等因素的干擾，使其動態(tài)可信度降低[2]；為有效抑制慣性器件常值偏差對慣導(dǎo)系統(tǒng)的影響，本文設(shè)計了四元數(shù)自補償方法。由于旋轉(zhuǎn)的引入，慣導(dǎo)系統(tǒng)中陀螺儀和加速度計的常值偏差被調(diào)制成正弦信號，通過積分運算可以有效地消除常值偏差對慣系統(tǒng)的影響。實現(xiàn)了長時間穩(wěn)定地輸出準(zhǔn)確姿態(tài)數(shù)據(jù)。

1 導(dǎo)航坐標(biāo)系描述

所謂姿態(tài)解算是指將運載體上慣性單元的輸出，實時轉(zhuǎn)換成運載體的姿態(tài)。這里的姿態(tài)通常指機體坐標(biāo)系(b系)相對于導(dǎo)航坐標(biāo)系(n系)的角位置。

導(dǎo)航坐標(biāo)系(n系)，它的原點位于載體質(zhì)心，x、y和z 軸的方向選為東北天(ENU)右手坐標(biāo)系。機體坐標(biāo)系（b系）的原點與載體質(zhì)心重合，對于飛行器而言，x 軸沿載體橫軸向右，y軸沿載體縱軸向前，z 軸沿載體豎軸向上，即“右前上”坐標(biāo)系，可見，機體坐標(biāo)系是與載體固連的[3]。根據(jù)機體坐標(biāo)系和地理坐標(biāo)系之間的相對角位置關(guān)系，可以定義載體的 3 個姿態(tài)角，分別為偏航角j、俯仰角q和橫滾角g，如圖1所示。

圖1 機體坐標(biāo)系和導(dǎo)航坐標(biāo)系角度關(guān)系

機體系(b)和導(dǎo)航系(n)之間的方向余弦陣可由式(1)表示：

2 四元數(shù)自補償算法研究

2.1 姿態(tài)解算的四元數(shù)法

四元數(shù)，是由一個實數(shù)和三個虛數(shù)組成的“超復(fù)數(shù)”，表示為：

在計算運載體姿態(tài)時，當(dāng)只關(guān)心機體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系的角位置時，可以認(rèn)為機體坐標(biāo)系(b系)是由導(dǎo)航坐標(biāo)系(n系)經(jīng)過一次性等效旋轉(zhuǎn)形成的，四元素Q包含了這種等效旋轉(zhuǎn)的全部信息[4]。其姿態(tài)變換公式如式（4）所示：

剛體轉(zhuǎn)動理論中，依歐拉定理，式（1）的四元數(shù)表示為：

由旋轉(zhuǎn)矩陣式(1)得到的歐拉角求解公式如下：

偏航角j、橫滾角g真值通過表1和表2確定。

表1 偏航角真值表

表2 翻滾角真值表

由于四旋翼飛行器飛行高度低，飛行速度慢，可以忽略地球自轉(zhuǎn)等因素的影響，那么四元數(shù)Q具有如下微分方程關(guān)系：

設(shè)T為采樣周期，那么四元數(shù)微分方程的一階龍格-庫塔法計算式為：

每個采樣周期讀取陀螺儀、加速度計和電子羅盤的數(shù)據(jù)，對上式進行迭代運算，即可實現(xiàn)四元數(shù)隨時間的更新，進而求得3個姿態(tài)角。而在實際中常值偏差是變化的，即在一次啟動后隨著陀螺儀、加速度計和電子羅盤運轉(zhuǎn)的時間的增長，慣性測量器件的常值偏差也緩慢的變化到導(dǎo)航系統(tǒng)精度無法允許的程度。

2.2 四元數(shù)自補償算法研究

在傳統(tǒng)的慣性導(dǎo)航系統(tǒng)中，慣性測量元件與運載體固連，它們之間沒有相對運動。四元數(shù)自動補償方法是將安裝有陀螺儀、加速度計和電子羅盤的慣導(dǎo)系統(tǒng)相對機體坐標(biāo)系旋轉(zhuǎn)，使慣性器件常值偏差沿著機體系敏感軸方向呈周期性變化。可有效抑制零位誤差對系統(tǒng)的影響。

由于自補償算法在三個軸方向具有相同的應(yīng)用，本文僅講解單軸旋轉(zhuǎn)的自補償。

設(shè)初始時刻導(dǎo)航坐標(biāo)系(n系)與機體坐標(biāo)系重合(b系)，然后機體坐標(biāo)系以角速度w繞ozb軸旋轉(zhuǎn)。機體坐標(biāo)系和導(dǎo)航坐標(biāo)系的夾角為wt，兩個坐標(biāo)系之間的關(guān)系用方向余弦表示為[6]：

陀螺儀和加速度計的輸出從導(dǎo)航坐標(biāo)系到機體坐標(biāo)系的轉(zhuǎn)換關(guān)系表示為：

導(dǎo)航坐標(biāo)系中陀螺儀和加速度計的輸出為：

式（17）、（18）可以看出當(dāng)機體坐標(biāo)系以角速度w繞ozb軸旋轉(zhuǎn)時，常值陀螺漂移ex、ey和常值加速度計零位誤差?x、?y在導(dǎo)航坐標(biāo)系oxb和oxb軸上的分量呈周期變化[7]。所以，ozb軸方向上的陀螺常值漂移和加速度計零位誤差沒有得到調(diào)制，有效抑制了慣性器件的偏差對解算精度的影響。

3 實驗結(jié)果

實驗中使用陀螺儀 L3G4200D，加速度計ADXL345和電子羅盤 HMC5883L 構(gòu)成姿態(tài)解算單元，采用 TMS320F28335做為主控制器，通過I2C接口讀取3個傳感器數(shù)據(jù)進行實時姿態(tài)解算，得到的姿態(tài)角通過 SCI 接口與上位機進行通訊[8]。實驗平臺如圖2所示。

圖2 搭建硬件平臺

上位機將相應(yīng)的數(shù)據(jù)繪制成曲線顯示出來。未加入補償算法得出曲線如圖 3 所示。可以看出一些時間段內(nèi)翻滾角度超出了基準(zhǔn)范圍[-90～90]，所以，直接運用四元數(shù)法進行姿態(tài)解算，動態(tài)性能良好，但存在一定的漂移。圖4給出了加入自補償算法后曲線圖，其解算姿態(tài)角，既能保持在姿態(tài)角基準(zhǔn)范圍內(nèi)，又具有四元數(shù)法的良好動態(tài)特性。

圖3 未加入自補償曲線圖

圖4 加入自補償曲線圖

4 結(jié)束語

本文分析了MEMS器件常值偏差的變化對姿態(tài)解算的影響[9]，針對MEMS器件精度低、易發(fā)散的問題提出四元數(shù)自補償算法，并在小型四旋翼飛行器姿態(tài)解算系統(tǒng)中進行了實驗。實驗結(jié)果表明：該算法能有效抑制常值誤差的變動對姿態(tài)解算精度的影響，且保證了四元數(shù)法的良好動態(tài)特性。

[1] 單海燕.四旋翼無人直升機飛行控制技術(shù)研究[D].南京航空航天大學(xué).2008.

[2] 聶博文,馬宏緒,王劍,等.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].電光與控制,2007,14(6)∶113-117.

[3] 劉俊,石云波,李杰.微慣性技術(shù)[M].北京∶電子工業(yè)出版社,2005.

[4] YANG Y,MIAO L J,SHEN J.Method of improving the navigation accuracy of SINS by continuous rotation[J].Journa1 of Beijing Institute ofTechno1ogy,2005,14(1)∶45-49.

[5] HUANG W Q,ZHAO G L,TAN ZH F,et a1.Research of Gyro case rotating monitor technique[J].Ship Bui1ding of China,2002,43∶54-59.

[6] 于旭東,王宇,張鵬飛,等.單軸旋轉(zhuǎn)對慣導(dǎo)系統(tǒng)誤差特性的影響[J].慣性技術(shù)學(xué)報,2008,16(6)∶643-648.

[7] 王其,徐曉蘇.旋轉(zhuǎn)IMU在光纖捷聯(lián)航姿系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報,2007,15(3)∶265-368.

[8] 蘇奎峰,呂強,鄧志東,湯霞青.TMS320x28xxx原理與開發(fā)[M].北京∶電子工業(yè)出版社.2009.

[9] 洪森濤,金智慧,李志強.四旋翼飛行器建模及姿態(tài)穩(wěn)定性分析[J].航空制造技術(shù).2006(5).91-94.