提高飛行器姿態(tài)角計算精度的方法研究

范奎武，張　兵

（1．海軍駐航天一院代表室，北京100076；2．北京航天自動控制研究所，北京100854）

提高飛行器姿態(tài)角計算精度的方法研究

范奎武1，張兵2

（1．海軍駐航天一院代表室，北京100076；2．北京航天自動控制研究所，北京100854）

分析了傳統(tǒng)的通過方向余弦矩陣、四元數(shù)形式的飛行器轉(zhuǎn)動微分方程數(shù)值積分結(jié)果確定姿態(tài)角方法的不足，介紹了基于改進(jìn)的牛頓法確定飛行器姿態(tài)角的方法?；诙啻螠y量均值效應(yīng)原理，介紹多使用上述兩種方法中的元素提高飛行器姿態(tài)角計算精度的算法及仿真結(jié)果。

飛行器；姿態(tài)角；四元數(shù)；方向余弦；精度

0 引言

描述飛行器姿態(tài)的除了有歐拉角、方向余弦、四元數(shù)這3種常用的方法外，還有5元素法。歐拉角法簡單，物理意義直觀，但對于飛航式飛行器當(dāng)俯仰角接近時會出現(xiàn)奇異點(diǎn)，從而使描述姿態(tài)運(yùn)動的歐拉角微分方程發(fā)散。因此在實際求解飛行器轉(zhuǎn)動微分方程時常使用后兩種方法，然后再利用它們與歐拉角的關(guān)系換算出姿態(tài)角。但是在求解其余兩種方法下飛行器的姿態(tài)變化微分方程時，隨機(jī)誤差會隨時間積累，從而導(dǎo)致確定姿態(tài)角時的誤差［1?3］。

本文介紹推導(dǎo)根據(jù)方向余弦矩陣和四元數(shù)的元素提高姿態(tài)角計算精度的算法和仿真確認(rèn)結(jié)果［4］。

1 確定飛行器姿態(tài)的方法簡介

用體軸坐標(biāo)系Oxbybzb相對發(fā)射慣性系Oxiyizi的姿態(tài)表述飛行器的姿態(tài)。以傾斜發(fā)射的飛行器為例，設(shè)由發(fā)射慣性系Oxiyizi繞Oyi軸轉(zhuǎn)動偏航角ψ得到Ox1y1z1，第二次繞Oz1轉(zhuǎn)動俯仰角得到Ox2y2z2，第三次再繞Ox2軸轉(zhuǎn)滾動角γ得到體軸系Oxbybzb，如圖1所示，則從發(fā)射慣性系到體軸系的坐標(biāo)轉(zhuǎn)換矩陣為［3］：

根據(jù)方向余弦矩陣與四元數(shù)的關(guān)系，得出：

合成轉(zhuǎn)動的四元數(shù)：

圖1　從發(fā)射慣性系Oxiyizi到體軸坐標(biāo)系Oxbybzb轉(zhuǎn)換關(guān)系示意圖Fig.1 Schematic diagram of the transformation from the inertial frame Oxiyizito the shaft system Oxbybzb

根據(jù)式（1）和式（2）求出：

由速率陀螺測得飛行器角速度，據(jù)此可求解方向余弦矩陣微分方程和四元數(shù)微分方程［3］。

方向余弦矩陣是正交陣，根據(jù)這一性質(zhì)可以對計算出的矩陣進(jìn)行正交規(guī)范化，以減少數(shù)值積分誤差的積累。對四元數(shù)微分方程的求解結(jié)果也要進(jìn)行規(guī)范化。

從式（4）～式（6）可以看出，計算姿態(tài)角時僅使用了方向余弦矩陣中的3個元素。從普通的測量理論可知，在進(jìn)行有獨(dú)立隨機(jī)誤差的多次直接測量時，使用N次測量得出的均值，能使估值誤差的方差減少到原來的1/N。因此，利用平均效應(yīng)，盡可能多地使用方向余弦矩陣中的元素，就可以提高在隨機(jī)誤差條件下由方向余弦矩陣求取姿態(tài)角的精度。

2 基于改進(jìn)牛頓法的計算姿態(tài)角的算法

把時刻ti的方向余弦矩陣的9個元素排成維數(shù)為np=9的矢量y(ti)=yi，該矢量的元素與3個姿態(tài)角有關(guān)。一般情況下，對于任意時刻，得出方程：

其中，

這里，f（yi，ai）是九維的已知的九維矢量變量yi和三維矢量ai的函數(shù)。所能知道的是矢量變量yi的觀測值z（ti），它包含隨機(jī)誤差ξ（ti）=ξi。

那么，在式（7）的基礎(chǔ)上形成觀測模型：

式中，ξi是隨機(jī)誤差，它是獨(dú)立的標(biāo)準(zhǔn)的隨機(jī)向量序列，其數(shù)學(xué)期望為0，協(xié)方差陣是R(ti)。

形成最小化泛函：

假設(shè)在所研究的情況中誤差是獨(dú)立的，那么協(xié)方差陣是對角陣，進(jìn)一步假設(shè)觀測是等精度的，于是就可以把協(xié)方差陣取為單位陣R（ti）=I。這樣，式（10）就變成誤差的平方和，有：

為了求出估值，使用改進(jìn)的牛頓法［4］，遞推公式為：

對（11）式求導(dǎo)數(shù)，得出：

計算二次導(dǎo)數(shù)：

式中，第一項與誤差成比例，可以認(rèn)為在極值附近它小到可以忽略。

對于每個ti時刻計算歐拉角，使用式（12）～式（14）式進(jìn)行迭代計算，當(dāng)矢量ak的模的增量小于某個預(yù)先給定的該模值的一個較小的比例數(shù)ε時，迭代過程結(jié)束。例如，可以取ε=0．01，有：

當(dāng)滿足式（15）時，迭代過程結(jié)束，這時的矢量ak+1的3個分量就看作是所求的姿態(tài)角的精確值。

這種方法的優(yōu)點(diǎn)是最終結(jié)果與方向余弦矩陣的所有元素有關(guān)，并且在均方意義下是最優(yōu)的，但是，迭代過程的收斂與起始近似值的選取及函數(shù)的拓?fù)湫再|(zhì)有關(guān)，所以在飛行控制過程中需要實時計算姿態(tài)角時采用這種遞推算法不方便。

3 提高計算姿態(tài)角精度的代數(shù)公式

3.1 由方向余弦矩陣的元素確定姿態(tài)角的公式

由于方向余弦矩陣微分方程求解過程中積分誤差的積累，式（4）～式（6）計算姿態(tài)角過程中正弦與余弦的反三角函數(shù)的自變量的值可能超出其定義域，即超出［-1，1］的范圍。如果根據(jù)單位矢量的標(biāo)量積來確定姿態(tài)角，就可以回避上述缺點(diǎn)。除此之外，還要附加上使得出的姿態(tài)角位于合理值范圍內(nèi)的條件?？傊?，多使用方向余弦矩陣中的元素，既能提高精度，又可以使計算過程不發(fā)散且得出的角度值合理。

（1）計算俯仰角

體軸坐標(biāo)系Oxb軸上的單位矢量ib在Oxizi平面內(nèi)的分量在發(fā)射慣性系Oxiyizi內(nèi)的投影是（a11，0，a13），按下面的公式計算俯仰角

計算單位矢量ib與發(fā)射慣性系Oyi軸的標(biāo)量積，就可以求出確定俯仰角的另一個公式：

該公式直接給出俯仰角的值，不需要附加條件。

（2）計算偏航角

根據(jù)圖1中的幾何關(guān)系，得出計算偏航角的公式：

（3）計算滾動角

根據(jù)兩矢量的點(diǎn)乘公式，從圖1中可以看出，可以用Oyb與Oy2軸上單位矢量iyb和iy2、Ozb軸Oz2軸上單位矢量izb和iz2的標(biāo)量積求出滾動角γ。

由式（1）知，Oyb軸與Oy2軸上單位矢量在發(fā)射慣性系Oxiyizi內(nèi)的投影分別是（a21，a22，a23）、根據(jù)前面求出的姿態(tài)角的三角函數(shù)與方向余弦矩陣的元素之間的關(guān)系，由式（16）～式（18）得出：

Ozb軸與Oz2軸上單位矢量在發(fā)射慣性系Oxiyizi內(nèi)的投影分別是（a31，a32，a33）、（sinψ，0，cosψ），得出：

據(jù)此可得出滾動角γ，具體計算時還要考慮滾動角的范圍再進(jìn)一步細(xì)化。

3.2 由四元數(shù)的分量計算姿態(tài)角

在根據(jù)四元數(shù)計算姿態(tài)角的式（4）～式（6）中，已經(jīng)包含了四元數(shù)的4個分量，即已經(jīng)運(yùn)用了求均值效應(yīng)，但還存在精度更高的計算姿態(tài)角的公式。引入符號：

可以推導(dǎo)出：

從式（21）可以求出：

根據(jù)式（22），得出求姿態(tài)角的公式：

3.3 仿真驗證結(jié)果

文獻(xiàn)［4］以某型教練機(jī)在與鉛垂面夾角為0．5°～2°的平面內(nèi)做“斜筋斗”為例，驗證所介紹方法的有效性。仿真結(jié)果表明，在俯仰角為?∈（89°，89．5°）時，姿態(tài)角形式的微分方程已經(jīng)發(fā)散；當(dāng)俯仰角為?∈（60°，70°）時，方向余弦和四元數(shù)形式的微分方程的積分結(jié)果開始出現(xiàn)誤差；當(dāng)俯仰角接近90°時，誤差達(dá)到最大，通過正交規(guī)范化，可使微分方程求解精度提高。

根據(jù)方向余弦微分方程的積分結(jié)果按式（4）～式（6）求偏航角、俯仰角、滾動角時，隨著俯仰角從0°逐漸增加到89．5°，俯仰角的計算誤差從百分之幾度增加到1°～1．5°，偏航角的誤差逐漸增大直到出現(xiàn)振蕩式特征，滾動角的誤差已接近90°。而采用式（16）時，俯仰角誤差只有0．01°～0．02°；采用式（18）計算偏航角時，誤差最大為1．9°；采用式（19）計算滾動角時，誤差最大為2．0°；根據(jù)四元數(shù)微分方程的積分結(jié)果求出方向余弦矩陣的元素然后再采用式（16）時，誤差在0．04°左右。

4 結(jié)論

本文在比較分析計算飛行器姿態(tài)角各種方法的基礎(chǔ)上，介紹了基于改進(jìn)牛頓法的計算姿態(tài)角的算法?；诙啻螠y量均值效應(yīng)原理，介紹推導(dǎo)了根據(jù)剛體轉(zhuǎn)動的方向余弦矩陣形式、四元數(shù)形式微分方程的數(shù)值積分結(jié)果，盡量多使用這兩種方法中的元素，確定姿態(tài)角的方法。對于方向余弦矩陣的情況，本文介紹方法的效果更明顯。

［1］陸元九．慣性器件［M］．北京：中國宇航出版社，1990．LUYuan?jiu．Inertialdevice［M］．Beijing：China Aerospace Press，1990．

［2］秦永元．慣性導(dǎo)航［M］．北京：科學(xué)出版社，2006．QIN Yong?yuan．Inertial navigation［M］．Beijing：Science Press，2006．

［3］范奎武．用四元數(shù)描述飛行器姿態(tài)時的幾個基本問題［J］．航天控制，2012，30（4）：49?53．FAN Kui?wu．The key problems of describing rigid?body attitude by using quaternion［J］．Aerospace Control，2012，30（4）：49?53．

［4］В．В．Булгаков，О．Н．Корсун．Алгоритмы повышения точностирасчётаугловориентациилетательного аппарата［J］．Tеория и Cистемы Yправления，2016（1）：159?170．

The Research on Method for ImProving Calculating Precision of Attitude Angle in Flight Vehicle

FAN Kui?wu1，ZHANG Bing2
（1．Representatives Office of Navy of CALT，Beijing 100076；2．Beijing Aerospace Automatic Control Institute，Beijing 100854）

The deficiencies of the algorithm method for attitude angle in flight vehicle are analyzed in this paper．In this algorithm method，the attitude angle of flight vehicle is calculated by integral value of rotation＆motion differential e?quation，through the calculating of direction cosine and quaternion．Then the improved Newton method for attitude angle in flight vehicle is introduced as well．According to the mean value effect of multi?measurements，the algorithm method for im?proving attitude angle calculating?precision in flight vehicle by using maximumly the above two methods is de?duced．Simulation result shows the good effect of this method．

flight vehicle；attitude angle；quaternion；direction cosine；precision

U666．1

A

1674?5558（2016）01?01330

10．3969／j．issn．1674?5558．2016．06．007

2016?10?18

范奎武，男，博士后，研究方向為飛行器總體設(shè)計及導(dǎo)航、制導(dǎo)與控制。