6自由度在剛體初始軌跡中的數(shù)值研究

楊 磊，肖天航

(南京航空航天大學(xué) 飛行器先進(jìn)設(shè)計(jì)技術(shù)國防重點(diǎn)學(xué)科實(shí)驗(yàn)室， 江蘇 南京 210016)

6自由度在剛體初始軌跡中的數(shù)值研究

楊 磊，肖天航

(南京航空航天大學(xué) 飛行器先進(jìn)設(shè)計(jì)技術(shù)國防重點(diǎn)學(xué)科實(shí)驗(yàn)室， 江蘇 南京 210016)

采用6自由度方法對(duì)剛體的運(yùn)動(dòng)姿態(tài)進(jìn)行了仿真研究，通過求解剛體的動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程確定剛體的質(zhì)心位置。在分析研究傳統(tǒng)求解剛體歐拉角缺點(diǎn)的基礎(chǔ)上，采用全角度轉(zhuǎn)換的四元數(shù)法求解歐拉角，解決了非全角度轉(zhuǎn)換的四元數(shù)法在大攻角情況下轉(zhuǎn)換不準(zhǔn)確的問題。對(duì)6自由度方法進(jìn)行了算例驗(yàn)證，結(jié)果表明該方法正確、實(shí)用，與已有的模型相比具有更好的工程應(yīng)用價(jià)值。

6自由度;歐拉角;四元數(shù);角度轉(zhuǎn)換;機(jī)彈分離

在研究物體(如飛行器外掛物、導(dǎo)彈等)6自由度運(yùn)動(dòng)姿態(tài)[1]時(shí)，本文引入“固化原理”，即忽略物體運(yùn)動(dòng)時(shí)的變形，這樣物體的姿態(tài)可看作繞各個(gè)坐標(biāo)軸的轉(zhuǎn)動(dòng)。物體的運(yùn)動(dòng)是關(guān)于時(shí)間的連續(xù)性問題，可以通過求解運(yùn)動(dòng)剛體的動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程確定其質(zhì)心的運(yùn)動(dòng)軌跡。在對(duì)剛體運(yùn)動(dòng)姿態(tài)數(shù)值仿真中，一般采用的是方向余弦法和歐拉角法，但方向余弦法求解方程個(gè)數(shù)眾多并涉及矩陣正交優(yōu)化問題，求解難度大，耗時(shí)長；歐拉角法只有在攻角不大的情況下，才有較高的精度，在大攻角的情況下會(huì)出現(xiàn)奇點(diǎn)問題，求解結(jié)果會(huì)有很大偏差。因此需要研究人員研究新的方法描述剛體運(yùn)動(dòng)姿態(tài)角。

近年來，隨著計(jì)算機(jī)[2]技術(shù)的飛速發(fā)展，以及航空航天、兵器等領(lǐng)域的需求，四元數(shù)法獲得了廣泛的應(yīng)用，例如，載人飛船和航天飛機(jī)中的姿態(tài)控制、高性能飛機(jī)的空間機(jī)動(dòng)、戰(zhàn)術(shù)導(dǎo)彈對(duì)目標(biāo)實(shí)施全方位攻擊時(shí)采用的垂直發(fā)射技術(shù)等[3]。非全角度轉(zhuǎn)換的四元數(shù)法在求解剛體運(yùn)動(dòng)姿態(tài)時(shí)雖然不會(huì)出現(xiàn)奇點(diǎn)問題，但因三角函數(shù)值域的局限會(huì)出現(xiàn)轉(zhuǎn)換偏差。因此，本文采用全角度[4]轉(zhuǎn)換四元數(shù)法求解剛體的歐拉角。

1 6 自由度模型的建立

1.1剛體質(zhì)心運(yùn)動(dòng)軌跡方程

描述剛體質(zhì)心運(yùn)動(dòng)軌跡通常采用慣性坐標(biāo)系，其動(dòng)力學(xué)方程為：

F=m·a

(1)

由于上述方程為矢量方程，所以將式(1)分解為沿三個(gè)坐標(biāo)方向的方程組進(jìn)行求解。應(yīng)當(dāng)注意的是有些量是慣性坐標(biāo)系下的量(如重力等)，有些量是體坐標(biāo)系下的量(如阻力、推力等)，這樣就需要將體坐標(biāo)系O-x1y1z1下的量轉(zhuǎn)換成慣性坐標(biāo)系O-xyz下的量，轉(zhuǎn)換矩陣如下：

(2)

1.2剛體姿態(tài)角模型的建立

采用四元數(shù)[5]法可以確定剛體相對(duì)于全局坐標(biāo)系的轉(zhuǎn)動(dòng)位置。在已知初始?xì)W拉角的前提下，經(jīng)過初始余弦矩陣可以求出初始四元數(shù)；給定時(shí)間步長Δt，通過求解四元數(shù)微分方程可求出下一步長的四元數(shù)值，再反求初始?xì)W拉角到初始四元數(shù)的轉(zhuǎn)換公式，最終可得到此時(shí)刻的歐拉角。在進(jìn)行坐標(biāo)轉(zhuǎn)換時(shí)，本文規(guī)定按3-2-1的順序轉(zhuǎn)換。

a.初始?xì)W拉角到初始四元數(shù)的轉(zhuǎn)換。

(3)

通過式(3)就可以將初始?xì)W拉角轉(zhuǎn)換成初始四元數(shù)的值。

b.歐拉角和四元數(shù)的坐標(biāo)變換矩陣。

無論是歐拉角的坐標(biāo)變換還是四元數(shù)表示的坐標(biāo)變換，只是同一變換的不同形式而已，所以這兩種變換矩陣的對(duì)應(yīng)元素相等，由此就可以用四元數(shù)來表示歐拉角。

四元數(shù)表示的坐標(biāo)變換矩陣為：

(4)

(5)

不難看出，式(5)只能表示3個(gè)軸在±90°范圍的角度，又因?yàn)閟in、tan函數(shù)在[-180°,+180°]范圍內(nèi)并不單調(diào)，不能準(zhǔn)確地完成四元數(shù)和歐拉角之間的全角度轉(zhuǎn)換，所以需要進(jìn)一步討論，其中T00～T33為坐標(biāo)變換矩陣的元素。

當(dāng)θ∈(-90°，+90°)，γ,ψ∈(-180°,+180°)時(shí)，四元數(shù)到歐拉角的轉(zhuǎn)換公式為：

(6)

當(dāng)θ∈(-180°，-90°)∪(+90°，+180°)，γ,ψ∈(-180°,+180°)時(shí)，四元數(shù)到歐拉角的轉(zhuǎn)換公式為：

(7)

式(6)就是非全角度轉(zhuǎn)換四元數(shù)法公式，也是目前廣泛使用的一種方法。但俯仰角不能夠?qū)崿F(xiàn)全角度轉(zhuǎn)換，所以本文結(jié)合式(7)提出全角度轉(zhuǎn)換的四元數(shù)法，即θ，ψ，γ∈(-180°，+180°)。在已知(t+Δt)時(shí)刻的四元數(shù)的情況下，根據(jù)式(6)和式(7)可分別求出歐拉角A6(θ，ψ，γ)、A7(θ，ψ，γ)。因?yàn)楸疚姆抡娴臄?shù)學(xué)模型是動(dòng)態(tài)連續(xù)模型，歐拉角在Δt時(shí)間內(nèi)的變化幅度比較小，不會(huì)發(fā)生角度突變，所以將歐拉角A6(θ，ψ，γ)、A7(θ，ψ，γ)分別與上一時(shí)刻的歐拉角At(θ，ψ，γ)比較，最接近的那個(gè)即為(t+Δt)時(shí)刻的大歐拉角At+Δt(θ，ψ，γ)。圖1為四元數(shù)到歐拉角的全角度變換流程圖。

圖1 四元數(shù)到歐拉角的全角度變換流程

2 四元數(shù)微分方程

通過求解四元數(shù)微分方程可以得到下一時(shí)間步長的歐拉角，但利用不同的坐標(biāo)變換順序得到的四元數(shù)微分方程也是不同的。本文以3-2-1的坐標(biāo)變換順序?yàn)槔浞匠倘缦拢?/p>

(8)

式中：ωx,ωy,ωz為剛體角速度在慣性坐標(biāo)系上的分量。

3 剛體運(yùn)動(dòng)姿態(tài)的仿真分析

在數(shù)學(xué)模型建立后，利用計(jì)算機(jī)編程來模擬仿真，并得到0～15s的歐拉角曲線圖。如圖2所示，主要流程是：輸入初始?xì)W拉角、角速度和時(shí)間步長—計(jì)算機(jī)模擬仿真—輸出下一時(shí)間步長時(shí)刻的歐拉角—輸出歐拉角曲線圖。在實(shí)例驗(yàn)證中，本文取(θ，ψ，γ)=(45°,60°,0°)，(ωx,ωy,ωz)=(0.1,0，0)，單位是(°)/s，仿真時(shí)間t=15s，并繪制0～15s內(nèi)的曲線。

圖2 計(jì)算機(jī)模擬流程

在初始條件一樣的情況下，分別繪制成非全角度模型轉(zhuǎn)換和全角度模型轉(zhuǎn)換的曲線圖。從圖3和圖4不難看出，在0～7.8s,兩種方法模擬的歐拉角度完全一致；由于非全角度轉(zhuǎn)換四元數(shù)法的取值范圍θ∈(-90°,+90°)，因此當(dāng)俯仰角超過其取值范圍時(shí)，非全角度轉(zhuǎn)換四元數(shù)法無法準(zhǔn)確判斷角度，從而出現(xiàn)了θ=90°時(shí)，ψ和γ的角度突變。從整個(gè)仿真時(shí)間段來看，θ角度的曲線出現(xiàn)尖點(diǎn)，隨后歐拉角的轉(zhuǎn)換發(fā)生錯(cuò)誤。從全角度仿真曲線圖中可以看出上述情況并沒有出現(xiàn)，且較好地模擬了歐拉角的變化。

圖3 非全角度轉(zhuǎn)換的歐拉角曲線圖

4 6-DOF算例驗(yàn)證

由于剛體質(zhì)心位移模型只是確定剛體的位置，所以可以把剛體看作一質(zhì)點(diǎn)。圖5和圖6分別是拋射角為0°、30°，拋射速度為1m/s，時(shí)間步長為0.100s，不考慮阻力情況下剛體的水平方向和垂直方向的位移圖。通過和真實(shí)值比較，雖然有所偏差，但曲線符合度較好。

圖4 全角度轉(zhuǎn)換的歐拉角曲線圖

圖5 拋射角為0°的質(zhì)點(diǎn)位移圖

圖6 拋射角為30°的質(zhì)點(diǎn)位移圖

為了研究時(shí)間步長和計(jì)算精度的關(guān)系，本文通過比較不同時(shí)間步長下的誤差值發(fā)現(xiàn)，在其他條件不變的情況下，選擇的時(shí)間步長越小，計(jì)算值與真實(shí)值的誤差越小。圖7是拋射角為-30°，拋射速度為1m/s，時(shí)間步長分別為0.001s、0.050s、0.100s，不考慮阻力情況下剛體的水平方向和垂直方向的位移圖。若選取時(shí)間步長過大，由于時(shí)間的積累導(dǎo)致誤差越來越大，最終會(huì)導(dǎo)致錯(cuò)誤的發(fā)生；若時(shí)間步長選取很小，計(jì)算時(shí)間就會(huì)增加。在誤差范圍內(nèi)，并有較快的計(jì)算速度，本文選取的時(shí)間步長為0.002s。

通過對(duì)二維平面內(nèi)算例的驗(yàn)證，確定了時(shí)間步長，但是還不能夠反映三維空間內(nèi)質(zhì)點(diǎn)的運(yùn)動(dòng)軌跡。本文對(duì)剛體質(zhì)心位移模型進(jìn)一步驗(yàn)證，分別給三個(gè)方向加力Fx=1N、Fy=5N、Fz=1N，重力大小為9.8N，x方向的速度為1m/s、其他兩個(gè)方向速度均為0，時(shí)間步長為0.002s。圖8為質(zhì)點(diǎn)在三個(gè)方向力及重力作用下的空間軌跡圖。

圖7 不同的時(shí)間步長的質(zhì)點(diǎn)位移圖

圖8 三維空間內(nèi)質(zhì)點(diǎn)受力運(yùn)動(dòng)軌跡圖

通過對(duì)二維和三維算例的分析，不僅證明了剛體質(zhì)心位移模型的正確性，而且確定了求解運(yùn)動(dòng)方程的時(shí)間步長。

本文對(duì)剛體的姿態(tài)角模型進(jìn)行了驗(yàn)證。該模型主要是為了描述剛體在受到外力矩的情況下，其姿態(tài)角繞質(zhì)心的變化情況。假設(shè)剛體的質(zhì)心坐標(biāo)是(0，0，0)，在y軸方向加一對(duì)大小相等(Fy=20N)、方向相反、對(duì)質(zhì)心的作用距離均為0.5m的力，作用時(shí)間1s。從圖9可知,0～1s剛體在力的作用下加速度不斷增加，當(dāng)力矩消失后加速度保持不變；而z方向的歐拉角則做周期性的變化，如圖10所示。

圖9 繞z軸角速度的曲線圖

圖10 繞z軸歐拉角的曲線圖

5 結(jié)束語

本文對(duì)剛體的運(yùn)動(dòng)姿態(tài)進(jìn)行了研究(包括剛體質(zhì)心的位移和姿態(tài)角)。質(zhì)心位移通過剛體運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程求得；本文主要?jiǎng)?chuàng)新點(diǎn)是通過與前一時(shí)刻角度信息的對(duì)比，修正了已有的四元數(shù)轉(zhuǎn)換方法的角度局限，采用全角度轉(zhuǎn)換四元數(shù)法對(duì)剛體姿態(tài)角進(jìn)行了解算；采用四階龍格-庫塔法求解四元數(shù)微分方程，將求得的四元數(shù)進(jìn)行歸一化處理，降低了計(jì)算機(jī)浮點(diǎn)誤差對(duì)結(jié)果的干擾，提高了精確度。本文提出的新型仿真模型對(duì)飛行器和衛(wèi)星的姿態(tài)描述有重要意義。

[1] 鐘山.基于機(jī)器學(xué)習(xí)的衛(wèi)星姿態(tài)控制律設(shè)計(jì)[J].航天控制，2011(8)：82-85.

[2] 王夙娟.衛(wèi)星的姿態(tài)控制研究[J].電子技術(shù)，2012(6)：120-121.

[3] 李軼.一種衛(wèi)星姿態(tài)軌道確定系統(tǒng)及方法[M].北京：北京航空航天大學(xué)，2012.

[4] 張帆.一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法[J].南京理工大學(xué)學(xué)報(bào)，2002(6)：20-23.

ARigidBodyNumericalSimulationBasedonSixDegreesofFreedomMethod

YANG Lei, XIAO Tianhang

(Nanjing University of Aeronautics and Astronautics, Jiangsu Nanjing, 210016, China)

It introduces the simulation of rigid body motion attitude with six degrees of freedom method. In order to determine the CG position of the rigid body, it solves the equation of rigid body dynamics and kinematics equations. Based on the shortcomings analysis of traditional methods and comparing with non-point conversion quaternion method, it shows that the proposed method can avoid the problem of inaccurate conversion for solving speed and angle of attack. The results prove that this method is correct and practical, and has better value in engineering applications than the existing other methods.

Six-DOF; Euler Angle; Quaternion; Angle Conversion; Wing-Store Separation

10.3969/j.issn.2095-509X.2014.03.002

2013-11-29

航空科學(xué)基金資助項(xiàng)目(20100152002);江蘇高校優(yōu)勢(shì)學(xué)科建設(shè)工程資助項(xiàng)目

楊磊(1988—)，男，江蘇南通人，南京航空航天大學(xué)碩士研究生，主要研究方向?yàn)轱w行器總體設(shè)計(jì)。

V211.3

A

2095-509X(2014)03-0005-05