野瑩瑩，張艷珠，鄒心宇，陳 蕾，左 越

（沈陽(yáng)理工大學(xué)自動(dòng)化與電氣工程學(xué)院，遼寧 沈陽(yáng)110159）

姿態(tài)解算在飛行器動(dòng)力學(xué)計(jì)算中具有舉足輕重的地位。姿態(tài)解算的精度和速度會(huì)直接影響飛行控制算法的穩(wěn)定行、可靠性和實(shí)現(xiàn)的難易程度，因此對(duì)飛行器姿態(tài)解算方法的研究具有重要意義。目前，通常采用陀螺儀獲得飛行器的姿態(tài)角，但是長(zhǎng)期測(cè)量精度較差，因此需要對(duì)陀螺儀的輸出數(shù)據(jù)進(jìn)行有效的處理才能獲得可靠和穩(wěn)定的姿態(tài)角信息，陀螺儀輸出的數(shù)據(jù)通過(guò)運(yùn)動(dòng)學(xué)微分方程可以推導(dǎo)出飛行器的微分方程[1]。

在飛行力學(xué)研究中，用于求解歐拉角的微分方程中包含大量的三角運(yùn)算，會(huì)給實(shí)時(shí)解算帶來(lái)了一定的困難，而且存在的“GimbalLock”現(xiàn)象[2]。所以歐拉角方法不適用于全姿態(tài)飛行器的姿態(tài)確定。本文引入四元數(shù)，進(jìn)而使用了一階龍哥庫(kù)塔求解四元數(shù)微分方程，最終轉(zhuǎn)化為歐拉角得到飛行器空中姿態(tài)。

1 坐標(biāo)變換

首先進(jìn)行坐標(biāo)變換，利用歐拉角描述一次平面旋轉(zhuǎn)，見(jiàn)圖1.

圖1 坐標(biāo)系間的變換關(guān)系[3]

設(shè)坐標(biāo)系繞旋轉(zhuǎn)α角后得到坐標(biāo)系X2OY2，空間中有一個(gè)矢量在坐標(biāo)系X1OY1中的投影為rx2，在X2OY2內(nèi)的投影為ry2，由于旋轉(zhuǎn)繞進(jìn)行，所以Z坐標(biāo)未變，即有：

上面僅僅是繞一根軸的旋轉(zhuǎn)，三維空間中的歐拉角要進(jìn)行三次這樣的坐標(biāo)旋轉(zhuǎn)變換，得到（2）式。

2 求解姿態(tài)角

經(jīng)過(guò)三次旋轉(zhuǎn)，到了一個(gè)表示旋轉(zhuǎn)的方向余弦矩陣。再利用歐拉微分方程，進(jìn)行下面的姿態(tài)解算：

式（3）左邊是本次更新后的翻滾角 Φ（roll）、俯仰角 θ（pitch）、偏航角 ψ（yaw），式（3）右邊是上個(gè)周期測(cè)算出來(lái)的角度，角速度單位為弧度，計(jì)算間隔時(shí)T陀螺角速度。因此求解這個(gè)微分方程就能解算出當(dāng)前的歐拉角。

3 利用四元數(shù)求解姿態(tài)角

利用四元數(shù)代替歐拉角對(duì)方向余弦矩陣的描述：

引入一階龍哥庫(kù)塔方程并求解。最后根據(jù)四元數(shù)方向余弦陣和歐拉角的轉(zhuǎn)換關(guān)系，把四元數(shù)轉(zhuǎn)換成歐拉角，進(jìn)而得到用四元數(shù)表示的四軸飛行器空中姿態(tài)參數(shù) γ、θ、ψ：

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

根據(jù)上述推算方法進(jìn)行實(shí)驗(yàn)，并在飛行器上位機(jī)上采集到如下數(shù)據(jù)。結(jié)果如圖2和圖3所示。其中加速度曲線橫坐標(biāo)表示時(shí)間（ms）；縱坐標(biāo)表示加速度（g）。陀螺儀曲線橫坐標(biāo)表示時(shí)間（ms）；縱坐標(biāo)表示陀螺儀（%）。

（續(xù)下圖）

（接上圖）

圖 2 分別為加速度 X（g）、Y（g）、Z（g）曲線

圖3 陀螺儀X、Y、Z軸曲線

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

本文對(duì)歐拉角的姿態(tài)解算進(jìn)行改進(jìn)，引入四元數(shù)進(jìn)行解算。一方面避免了利用歐拉方程中的復(fù)雜三角運(yùn)算，另一方面解決了俯仰角為90°時(shí)方程式會(huì)出現(xiàn)的“GimbalLock”現(xiàn)象。經(jīng)過(guò)試驗(yàn)驗(yàn)證該方法進(jìn)行姿態(tài)解算效率高而且結(jié)果準(zhǔn)確，在姿態(tài)解算領(lǐng)域有良好的應(yīng)用前景。

參考文獻(xiàn)：

[1]張 帆，曹喜濱，鄒經(jīng)湘.一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法[J].南京理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2002（04）：376-380.

[2]趙學(xué)杰.數(shù)值方法中Runge-Kutta方法改進(jìn)的探討[J].衡水學(xué)院學(xué)報(bào)，2014，16（04）：23-26.

[3]呂志鵬，伍吉倉(cāng)，公 羽.基于四元數(shù)的大旋轉(zhuǎn)角坐標(biāo)變換模型的改進(jìn)[J].武漢大學(xué)學(xué)報(bào)（信息科學(xué)版），2016（02）：1-7.