捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法

史 凱,劉馬寶

(1.西安交通大學(xué)航天學(xué)院,陜西 西安 710049；2.西安機(jī)電信息技術(shù)研究所，陜西 西安710065)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)不需要任何外來信息,也不會向外輻射任何信息,僅靠慣性導(dǎo)航系統(tǒng)本身就能在全天候條件下進(jìn)行三維定向[1],通過彈載三軸正交陀螺儀直接解算彈體姿態(tài),其結(jié)構(gòu)簡單,抗干擾能力強(qiáng)。姿態(tài)解算是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù),目前姿態(tài)解算的精度一方面受制于陀螺儀自身器件的水平;另一方面是受姿態(tài)解算算法的約束,捷聯(lián)式慣導(dǎo)姿態(tài)更新算法的精度以及復(fù)雜程度直接影響整個(gè)姿態(tài)測量系統(tǒng)的解算精度。

目前姿態(tài)解算的主要方法有歐拉角法、四元數(shù)法和方向余弦法。歐拉角法不能用于飛行器全姿態(tài)解算而難以廣泛應(yīng)用于工程實(shí)踐,且實(shí)時(shí)計(jì)算困難。方向余弦法避免了歐拉角法的“奇點(diǎn)”現(xiàn)象,但方程的計(jì)算量大,工作效率低。四元數(shù)法只需要求解四個(gè)未知量的線性微分方程組,計(jì)算量比方向余弦法小,且算法簡單,易于操作,是較實(shí)用的工程方法。關(guān)于四元數(shù)的求解方法,很多文獻(xiàn)都采用數(shù)字積分的方法解算載體的姿態(tài)[2],或只將龍格庫塔算法應(yīng)用到彈體的滾轉(zhuǎn)角解算,并沒有進(jìn)行三個(gè)方向的全姿態(tài)角解算[3]。文獻(xiàn)[3]中僅介紹了姿態(tài)更新微分方程,應(yīng)用了龍格庫塔法進(jìn)行姿態(tài)更新,但其算法結(jié)構(gòu)復(fù)雜,進(jìn)行了兩次迭代,運(yùn)算量較大,實(shí)時(shí)性和精度不能保證。為了提高姿態(tài)解算的精度,根據(jù)捷聯(lián)慣導(dǎo)姿態(tài)更新算法要求高精度、結(jié)構(gòu)復(fù)雜度低的需求[4],解決捷聯(lián)慣導(dǎo)姿態(tài)更新算法能夠滿足實(shí)際工程化需求之間的矛盾,提出了捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法。

1 坐標(biāo)系定義

首先介紹兩個(gè)坐標(biāo)系:導(dǎo)航坐標(biāo)系和載體坐標(biāo)系。

導(dǎo)航坐標(biāo)系:用Oxnynzn表示,它是慣導(dǎo)系統(tǒng)在求解導(dǎo)航參數(shù)時(shí)作采用的坐標(biāo)系,一般選導(dǎo)航坐標(biāo)系為地理坐標(biāo)系,原點(diǎn)為載體中心,xn指向東,yn軸指向北,zn軸指向天(當(dāng)不考慮地球自轉(zhuǎn)對陀螺輸出的影響)時(shí)Oxnynzn即發(fā)射坐標(biāo)系原點(diǎn)即炮位,yn軸指向射向,xn軸與yn在一個(gè)水平面指向右,zn軸指向天。

載體坐標(biāo)系:用Oxbybzb表示,原點(diǎn)為載體重心,xb沿載體橫軸向右,yn軸沿載體縱軸向前,zb軸沿載體立軸向上。載體坐標(biāo)系與地理坐標(biāo)系的方位關(guān)系可以用姿態(tài)矩陣或者姿態(tài)角來表示。

兩個(gè)坐標(biāo)系具體關(guān)系圖如圖1所示。

本文所有的公式推導(dǎo)及算法均是基于上述坐標(biāo)系定義的基礎(chǔ)上進(jìn)行的,按照圖中所示的坐標(biāo)系各

軸所示,本文繞三軸旋轉(zhuǎn)的基本坐標(biāo)系(右手系)轉(zhuǎn)換矩陣,如下所示[5]:

(1)

圖1 坐標(biāo)系示意圖Fig.1 Coordinate diagram

2 四元數(shù)的四階龍格庫塔姿態(tài)算法

2.1 四元數(shù)姿態(tài)更新算法

根據(jù)第1節(jié)三次基本轉(zhuǎn)換坐標(biāo)變換陣可以得出由n系至b系的坐標(biāo)轉(zhuǎn)換陣如式(2)所示。

(2)

則根據(jù)矩陣的性質(zhì)可以得出:

(3)

根據(jù)四元數(shù)可確定出b系至n系的坐標(biāo)變換陣,如式(4)所示。

(4)

綜合式(2)、式(4)可以根據(jù)四元數(shù)確定的矩陣解算出三個(gè)歐拉角如式(5)所示:

(5)

2.2 四階龍格庫塔法姿態(tài)解算

四元數(shù)描述了剛體的定點(diǎn)轉(zhuǎn)動,即當(dāng)只關(guān)心b系相對R系的角位置時(shí),可認(rèn)為b系是由R系經(jīng)過中間過程的一次性等效旋轉(zhuǎn)形成的,Q包含了這種等效旋轉(zhuǎn)的全部信息,uR為旋轉(zhuǎn)瞬軸和旋轉(zhuǎn)方向,θ為轉(zhuǎn)過的角度。根據(jù)2.1節(jié)可以看出姿態(tài)解算的實(shí)質(zhì)是如何更新四元數(shù),四元數(shù)微分方程如式(6)所示：

(6)

寫成矩陣形式展開即:

(7)

四階龍格庫塔法解四元數(shù)微分方程的數(shù)值解法思想是在積分區(qū)間內(nèi)進(jìn)行插值,優(yōu)化總的斜率得到更新結(jié)果,所以其計(jì)算精度高于直接積分,龍格庫塔算法階數(shù)越高,截?cái)嗾`差越小,計(jì)算精度越高。在工程上,四階龍格庫塔算法的精度已經(jīng)能夠滿足大多數(shù)精度要求,并且計(jì)算復(fù)雜度也適宜多數(shù)處理器性能。因此,四階龍格庫塔法在工程上被廣泛使用,一般情況下彈道仿真大都采用四階龍格庫塔法。針對式(7)所示的四元數(shù)微分方程,相對應(yīng)的四階龍格庫塔方程公式如下[8]:

(8)

式(8)中，q為四元數(shù)向量,h為仿真步長,f為四元數(shù)微分方程。經(jīng)過四階龍格庫塔解四元數(shù)微分方程后可以得出下一時(shí)刻的四元數(shù)數(shù)值,由于計(jì)算誤差等因素,計(jì)算過程中四元數(shù)會逐漸失去規(guī)范化特性,因此若干次更新后,必須對四元數(shù)進(jìn)行規(guī)一化處理，即：

(9)

從計(jì)算過程可以看出四階龍格庫塔姿態(tài)算法避免了直接積分算法復(fù)雜的計(jì)算過程、精度差等缺點(diǎn),其實(shí)質(zhì)是更新四元數(shù)微分方程,進(jìn)而根據(jù)更新的四元數(shù)求解彈丸的姿態(tài)角。該方法只需迭代一次即可更新四元數(shù),算法運(yùn)算量小,四階龍格庫塔姿態(tài)算法相對于直接積分姿態(tài)算法中通過級數(shù)展開略去高次項(xiàng)從而保證計(jì)算精度的方法。其姿態(tài)解算的精度可以直接通過仿真步長進(jìn)行控制,簡單易行,便于調(diào)試[9]。

3 算法驗(yàn)證

3.1 仿真分析

為了驗(yàn)證算法的正確性和精度,本文在迫彈平臺上對算法進(jìn)行驗(yàn)證,從而為后續(xù)在迫彈平臺上應(yīng)用捷聯(lián)式慣導(dǎo)系統(tǒng)提供理論依據(jù)。仿真模型:120迫彈6 DOF彈道模型,初速200 m/s,初始射角20°,仿真步長0.1 ms,則陀螺四元數(shù)解算彈丸姿態(tài)與6 DOF彈道模型原始數(shù)據(jù)外彈道姿態(tài)角對比如下圖所示。

圖2 解算偏航角與理論值對比Fig.2 The calculated yaw angle compared with the theoretical value

圖3 解算滾轉(zhuǎn)角與理論值對比Fig.3 The calculated roll angle compared with the theoretical value

圖4 解算俯仰角與理論值對比Fig.4 The calculated pitch angle compared with the theoretical value

通過仿真結(jié)果可以看出,捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法在迫彈全彈道環(huán)境下全向姿態(tài)解算精度很高,誤差量級為10-2。由于采樣率對算法的影響也不能忽略,故對不同采樣率對算法精度的影響展開分析,通過改變采樣頻率,對不同采樣率下對滾轉(zhuǎn)角、偏航角、俯仰角誤差進(jìn)行了統(tǒng)計(jì)如表1，表2所示。

通過表1，表2可以看出120迫彈平臺彈丸轉(zhuǎn)速在10轉(zhuǎn)以內(nèi)采樣率100 Hz已經(jīng)可以滿足系統(tǒng)要求,同時(shí)可以看出隨著采樣率的提高,姿態(tài)解算精度也隨之提高。

表1 轉(zhuǎn)速4 r/s下不同采樣率算法解算誤差

表2 轉(zhuǎn)速14 r/s下不同采樣率算法解算誤差

3.2 實(shí)驗(yàn)驗(yàn)證

根據(jù)仿真結(jié)果設(shè)計(jì)了相關(guān)實(shí)驗(yàn),開發(fā)了彈道飛行姿態(tài)測試系統(tǒng),其結(jié)構(gòu)外形如圖5所示。彈道飛行參數(shù)測試裝置實(shí)時(shí)獲得彈體的三軸角速度,根據(jù)三軸角速度信息解算出彈體的姿態(tài),測試裝置如圖所示,其中捷聯(lián)慣導(dǎo)三軸陀螺儀的安裝正方向如圖6所示。

圖5 彈道飛行姿態(tài)測試裝置Fig.5 Ballistic flight attitude testing device

圖6 三軸陀螺儀安裝正方向Fig.6 The three-axis gyro’s positive direction

按照要求設(shè)計(jì)了彈丸的拋灑試驗(yàn),即將彈道飛行參數(shù)測試裝置固定于距離地面一定高度的裝置中,點(diǎn)火后將測試裝置彈出,此時(shí)通過彈載三軸陀螺儀實(shí)時(shí)采集到的角速度信息通過彈載記錄儀進(jìn)行采集試驗(yàn)后回收測試裝置,讀取彈載存儲器中數(shù)據(jù),獲得彈體的三軸角速度,進(jìn)而通過地面計(jì)算機(jī)計(jì)算軟件解算出彈丸的姿態(tài),完成彈道姿態(tài)測試進(jìn)行彈丸姿態(tài)解算同時(shí)與高速錄像觀測結(jié)果比對,其中實(shí)時(shí)采集的彈丸三軸角速度和解算得到的彈丸姿態(tài)如圖7—圖9所示。

圖7 三軸角速度原始信號Fig.7 Triaxial angular velocity original signal

圖8 三軸加速度原始信號Fig.8 Triaxial acceleration original signal

圖9 姿態(tài)角解算結(jié)果Fig.9 Attitude angle solution results

通過結(jié)果可以看出在拋撒過程中姿態(tài)變化不大,俯仰角有40°,滾轉(zhuǎn)角和偏航角基本在20°以內(nèi),這與高速錄像觀測的彈丸姿態(tài)變化基本吻合,可以真實(shí)反映彈丸在拋撒過程中的飛行姿態(tài)變化,從而驗(yàn)證了捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法具有工程實(shí)用性。

4 結(jié)論

本文提出了捷聯(lián)慣導(dǎo)四元數(shù)的四階龍格庫塔姿態(tài)算法。該算法僅需一次迭代即可以計(jì)算出三個(gè)姿態(tài)角,通過120迫彈平臺對算法進(jìn)行了驗(yàn)證,仿真結(jié)果表明該算法結(jié)構(gòu)簡單,精度較高。根據(jù)工程實(shí)用性和彈上實(shí)時(shí)性要求,設(shè)計(jì)開發(fā)了彈道飛行姿態(tài)測試裝置,同時(shí)設(shè)計(jì)了相關(guān)試驗(yàn),實(shí)驗(yàn)結(jié)果表明算法可以實(shí)時(shí)計(jì)算出彈丸的飛行姿態(tài),可工程化使用,為后續(xù)在迫彈平臺上應(yīng)用低成本捷聯(lián)式慣導(dǎo)進(jìn)行姿態(tài)解算奠定了理論基礎(chǔ)。