馬 敏，吳海超

MA Min,WU Hai-chao

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 偏航角真值表

表2 翻滾角真值表

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

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

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

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

在傳統(tǒng)的慣性導(dǎo)航系統(tǒng)中，慣性測(cè)量元件與運(yùn)載體固連，它們之間沒(méi)有相對(duì)運(yùn)動(dòng)。四元數(shù)自動(dòng)補(bǔ)償方法是將安裝有陀螺儀、加速度計(jì)和電子羅盤(pán)的慣導(dǎo)系統(tǒng)相對(duì)機(jī)體坐標(biāo)系旋轉(zhuǎn)，使慣性器件常值偏差沿著機(jī)體系敏感軸方向呈周期性變化?？捎行б种屏阄徽`差對(duì)系統(tǒng)的影響。

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

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

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

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

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

3 實(shí)驗(yàn)結(jié)果

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

圖2 搭建硬件平臺(tái)

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

圖3 未加入自補(bǔ)償曲線圖

圖4 加入自補(bǔ)償曲線圖

4 結(jié)束語(yǔ)

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

[1] 單海燕.四旋翼無(wú)人直升機(jī)飛行控制技術(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)對(duì)慣導(dǎo)系統(tǒng)誤差特性的影響[J].慣性技術(shù)學(xué)報(bào),2008,16(6)∶643-648.

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

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

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