魏宗康，高榮榮

（北京航天控制儀器研究所，北京 100854）

捷聯(lián)式慣性系統(tǒng)與載體直接固連，通過陀螺儀測量角速度并經(jīng)數(shù)學(xué)解算后給出三個(gè)姿態(tài)角的值。一般情況下，確定姿態(tài)信息的方法有方向余弦運(yùn)動(dòng)法、歐拉-克雷洛夫角法和四元數(shù)法等。方向余弦法的優(yōu)點(diǎn)是直接可求得動(dòng)系和定系之間的坐標(biāo)變換矩陣，但缺點(diǎn)是計(jì)算量較大，因此，該方法并沒有應(yīng)用于工程中。目前，應(yīng)用最多的是四元數(shù)法[1-3]，其思路是先根據(jù)4個(gè)元素求解動(dòng)系相對(duì)定系的坐標(biāo)變換矩陣，然后再依據(jù)該坐標(biāo)變換矩陣求解三個(gè)姿態(tài)角，但缺點(diǎn)是在求解姿態(tài)角過程中存在多值問題和奇異值問題。歐拉-克雷洛夫角法雖可直接通過3個(gè)角速度微分方程解算出姿態(tài)角，但缺點(diǎn)是解算值的大小與不同的3次轉(zhuǎn)動(dòng)順序相關(guān)，且中間旋轉(zhuǎn)角度接近90 °時(shí)方程會(huì)出現(xiàn)退化現(xiàn)象，導(dǎo)致精度下降，因此，歐拉-克雷洛夫角法不適用于運(yùn)載體全姿態(tài)運(yùn)動(dòng)時(shí)的姿態(tài)解算[4,5]。

工程中為了回避姿態(tài)角解算過程中的多值問題，通過限定載體的角運(yùn)動(dòng)范圍以確保解算的姿態(tài)角與真實(shí)值一致[6,7]。比如，對(duì)于航天器等非大機(jī)動(dòng)飛行場合，通過限定姿態(tài)角范圍既回避了多值問題，又規(guī)避了奇異值問題，將俯仰角或航向角限定在（-90 °, +90 °）范圍內(nèi)[8]。

但對(duì)于取消滾動(dòng)控制的大機(jī)動(dòng)全姿態(tài)飛行場合，很顯然繼續(xù)采用限定姿態(tài)角范圍將不滿足使用要求。目前，解決多值問題和奇異值問題的主要措施是雙歐拉角法，其主要思想都是通過改變不同歐拉角組合、相互切換的方法克服奇異值問題。例如，在文獻(xiàn)[9]中，為了解決魚雷俯仰角為90 °時(shí)正確的偏航角和橫滾角的解算問題，將雙歐拉法引入到空投魚雷的六自由度方程中，通過交替使用雙歐拉方程正確反映了空投魚雷的運(yùn)動(dòng)規(guī)律，為魚雷大幅度全姿態(tài)運(yùn)動(dòng)的空投彈道研究提供了有意義的理論參考。在文獻(xiàn)[10]中，針對(duì)垂直發(fā)射的地空導(dǎo)彈在大機(jī)動(dòng)飛行時(shí)姿態(tài)解算出現(xiàn)的奇異值問題，將正、反歐拉角微分方程切換的雙歐拉全姿態(tài)解算方法應(yīng)用到姿態(tài)解算過程中，并通過半物理試驗(yàn)驗(yàn)證了該方法在工程上的有效性。在文獻(xiàn)[11]中，同樣利用正、反歐拉方程在傘、彈系統(tǒng)中克服了歐拉方程的奇異性，并通過與四元數(shù)法的姿態(tài)解算精度進(jìn)行對(duì)比，認(rèn)為雙歐拉法在空間飛行時(shí)間較長的傘-彈系統(tǒng)來說，雙歐拉法更具優(yōu)勢(shì)。

上述雙歐拉全姿態(tài)方法雖然解決了俯仰角在±90 °時(shí)的奇異值問題，但是此方法的第一個(gè)缺點(diǎn)是需要根據(jù)俯仰角范圍選定正或反歐拉微分方程，因此，在計(jì)算方面，運(yùn)用到6個(gè)歐拉角和2套歐拉微分方程，且需要實(shí)時(shí)判定俯仰角的范圍，一方面增加了較大計(jì)算量、存儲(chǔ)量和切換流程，另一方面對(duì)系統(tǒng)的解算和控制周期有嚴(yán)格的要求[12]。第二個(gè)缺點(diǎn)是單個(gè)歐拉方程無論怎樣順序變換，一個(gè)歐拉方程都必然會(huì)出現(xiàn)一個(gè)姿態(tài)角在± 90 °時(shí)處于奇異值狀態(tài)，因此，正歐拉方程中俯仰角在± 90 °時(shí)奇異，那么反歐拉方程解決了俯仰角為± 90 °時(shí)的奇異問題，但是出現(xiàn)了偏航角或者滾轉(zhuǎn)角不能工作在± 90 °時(shí)的問題。以上兩個(gè)缺點(diǎn)說明雙歐拉角并不能實(shí)現(xiàn)真正意義上的全姿態(tài)。

針對(duì)四元數(shù)法、克雷洛夫角法以及雙歐拉角法存在的不足之處，本文提出了基于擴(kuò)展克雷洛夫角的全姿態(tài)導(dǎo)航解算方法，該導(dǎo)航解算的姿態(tài)矩陣由四個(gè)旋轉(zhuǎn)角度依次旋轉(zhuǎn)得到。針對(duì)該姿態(tài)矩陣的冗余性，采用因果控制策略以實(shí)現(xiàn)唯一的結(jié)果響應(yīng)。在整個(gè)解算過程中，不會(huì)發(fā)生某一旋轉(zhuǎn)角度在90 °時(shí)引起奇異值的現(xiàn)象，實(shí)現(xiàn)了全姿態(tài)高精度導(dǎo)航解算。

1 基于四元數(shù)法求解姿態(tài)角時(shí)存在的不足之處

采用四元數(shù)求解的坐標(biāo)變換矩陣是唯一的，但是，變換矩陣中的四個(gè)元素只是中間變量，在通過坐標(biāo)變換矩陣解算出姿態(tài)角的過程中存在以下兩個(gè)不足之處。

1.1 多值問題

導(dǎo)航坐標(biāo)系相對(duì)本體坐標(biāo)系按照相同的旋轉(zhuǎn)順序時(shí)，姿態(tài)角也不唯一，即一個(gè)坐標(biāo)變換矩陣可以用兩組不同的姿態(tài)角表述。比如，除一組姿態(tài)角φ、ψ、γ外，還可用另一組姿態(tài)角φ-π、π-ψ、γ-π表述，即：

1.2 奇異值問題

在式(1)中，當(dāng)ψ=90 °時(shí)，坐標(biāo)變換矩陣為：

從式(2)可以看出，不能求出姿態(tài)角φ、γ獨(dú)立的值，而只能求出二者之差γ-φ。滿足該差值的φ和γ有無窮多個(gè)，因此，在工程實(shí)際中可把其中一個(gè)姿態(tài)角設(shè)置為零，比如φ= 0 °，而另外一個(gè)姿態(tài)角取為γ=γ-φ。但這種方法的缺點(diǎn)是φ的真值如果不為零時(shí)，則兩個(gè)角度的值都與真值不相符。

2 基于克雷洛夫角求解姿態(tài)角時(shí)存在的不足之處

采用克雷洛夫角可描述導(dǎo)航坐標(biāo)系經(jīng)過三次轉(zhuǎn)動(dòng)后到達(dá)本體坐標(biāo)系的過程，坐標(biāo)變換矩陣也是唯一的，但受限于轉(zhuǎn)動(dòng)順序，導(dǎo)致具體轉(zhuǎn)動(dòng)過程中角度值不同。

2.1 以z→y→x順序轉(zhuǎn)動(dòng)的克雷洛夫角法

以z→y→x順序轉(zhuǎn)動(dòng)的具體過程為：導(dǎo)航坐標(biāo)系Oxyz繞Oz軸轉(zhuǎn)動(dòng)φ1角，到達(dá)OLNz；OLNz再繞ON軸轉(zhuǎn)動(dòng)ψ1角，到達(dá)Ox′NM；最后，Ox′NM繞Ox′軸轉(zhuǎn)動(dòng)γ1角，到達(dá)Ox′y′z′。列寫出用姿態(tài)角φ1、ψ1、γ1描述的坐標(biāo)變換矩陣為：

則三個(gè)克雷洛夫角對(duì)應(yīng)的角速度方程為：

其中，ωx1、ωy1、ωz1分別為捷聯(lián)式慣性系統(tǒng)的本體沿X、Y、Z三個(gè)方向相對(duì)導(dǎo)航坐標(biāo)系的角速度。

由式(4)可知，偏航角ψ1在± 90 °時(shí)奇異，無法滿足此導(dǎo)航模式下的高精度解算需求。

2.2 以z→x→y順序轉(zhuǎn)動(dòng)的克雷洛夫角法

以z→x→y順序轉(zhuǎn)動(dòng)的具體過程為：導(dǎo)航坐標(biāo)系Oxyz繞Oz軸轉(zhuǎn)動(dòng)φ2角，到達(dá)OLNz；OLNz再繞OL軸轉(zhuǎn)動(dòng)γ2角，到達(dá)OLy′M；最后，OLy′M繞Oy′軸轉(zhuǎn)動(dòng)ψ2角，到達(dá)Ox′y′z′。列寫出用姿態(tài)角φ2、ψ2、γ2描述的坐標(biāo)變換矩陣為：

三個(gè)克雷洛夫角γ2、ψ2、φ2對(duì)應(yīng)的角速度方程為

由式(6)可知，橫滾角γ2在± 90 °時(shí)奇異，無法滿足此導(dǎo)航模式下的高精度解算需求。

3 基于雙歐拉法求解姿態(tài)角時(shí)存在的不足之處

為避免采用式(4)或式(6)的微分方程進(jìn)行導(dǎo)航解算時(shí)克雷洛夫角的奇異值，提出了一種基于雙歐拉法的姿態(tài)解算方法[7-9]，主要思路為：

(a)當(dāng)ψ1∈[-45 °, +45 °]或ψ1∈[135 °, 225 °]時(shí)，式(4)不存在奇異值，則可直接采用該式計(jì)算得到γ1、ψ1、φ1，以及坐標(biāo)變換矩陣如式(3)。

由于式(3)表示的坐標(biāo)變換矩陣也可表示為式(5)，因此，可解算出：

(b)當(dāng)區(qū)間ψ1∈[-135 °, -45 °]或ψ1∈[45 °, 135 °]時(shí)，式(4)可能存在奇異值，為此，可采用式(6)得到γ2、ψ2、φ2以及坐標(biāo)變換矩陣如式(5)。由于式(5)表示的坐標(biāo)變換矩陣也可表示為式(3)，因此，在限定|ψ1|＜90 °的前提下，可求得：

上述基于雙歐拉法的姿態(tài)解算方法主要是避免了ψ1→90 °的情況，但不能保證γ2不會(huì)趨于90 °。換句話說，如果以γ2作為區(qū)間切換基準(zhǔn)角度，當(dāng)γ2∈[-45 °, +45 °]或γ2∈[135 °, 225 °]時(shí)，可直接采用式(6)得 到γ2、ψ2、φ2；而 當(dāng)γ2∈[-135 °, -45 °]或γ2∈[45 °, 135 °]時(shí)，采用式(4)解算γ1、ψ1、φ1，此時(shí)的量化誤差又很大。因此，需要事先已知哪種轉(zhuǎn)動(dòng)順序不會(huì)出現(xiàn)奇異值。既然如此，只需采用不會(huì)出現(xiàn)奇異值的轉(zhuǎn)動(dòng)順序的克雷洛夫角進(jìn)行姿態(tài)解算即可，而不需要所謂的“雙歐拉法”。另外，采用雙歐拉法需要兩組克雷洛夫角、共6個(gè)參數(shù)，計(jì)算量增大。

4 基于四次旋轉(zhuǎn)的擴(kuò)展克雷洛夫角全姿態(tài)解算

基于三次轉(zhuǎn)動(dòng)的克雷洛夫角姿態(tài)解算方法在中間角度接近90 °時(shí)會(huì)引起動(dòng)態(tài)誤差，且通過雙歐拉角法仍無法完全實(shí)現(xiàn)全部三個(gè)姿態(tài)角的全姿態(tài)導(dǎo)航解算。本節(jié)提出一種基于四個(gè)旋轉(zhuǎn)角度的擴(kuò)展克雷洛夫角全姿態(tài)解算方法，可有效避免該現(xiàn)象的發(fā)生。

4.1 擴(kuò)展克雷洛夫角的冗余性

擴(kuò)展克雷洛夫角是在原三次轉(zhuǎn)動(dòng)定義的克雷洛夫角基礎(chǔ)上再增加一次轉(zhuǎn)動(dòng)后形成的四個(gè)轉(zhuǎn)動(dòng)角度。如圖1描述的具體轉(zhuǎn)動(dòng)過程為：導(dǎo)航坐標(biāo)系Oxyz繞Oz軸轉(zhuǎn)動(dòng)φ角，到達(dá)OLNz；OLNz再繞ON軸轉(zhuǎn)動(dòng)ψ角，到達(dá)OQNM；OQNM繞OQ軸轉(zhuǎn)動(dòng)γ角，到達(dá)OQy′P；最后，OQy′P繞Oy′轉(zhuǎn)動(dòng)ξ角，到達(dá)Ox′y′z′。

圖1 基于擴(kuò)展克雷洛夫角的載體系相對(duì)導(dǎo)航系的轉(zhuǎn)動(dòng)Fig.1 rotation of the body system relative to navigation system based on extended Krylov angle

列寫出用姿態(tài)角φ、ψ、γ、ξ描述的坐標(biāo)變換矩陣為：

根據(jù)式(14)可解出兩組框架角的制約關(guān)系為：

圖2 矩陣不變時(shí)的擴(kuò)展克雷洛夫角Fig.2 Krylov angle when matrix is invariant

而當(dāng)φ=0 °、ψ=60 °、γ=60 °、ξ=0 °時(shí)，滿足矩陣保持不變的擴(kuò)展克雷洛夫角如圖3中藍(lán)色曲線和紅色曲線所示。

圖3 矩陣不變時(shí)的擴(kuò)展克雷洛夫角Fig.3 Krylov angle when matrix is invariant

根據(jù)四次擴(kuò)展克雷洛夫角的旋轉(zhuǎn)順序，寫出擴(kuò)展克雷洛夫角對(duì)應(yīng)的角速度方程為：

從擴(kuò)展克雷洛夫角的坐標(biāo)變換矩陣和角速度方程可以看出，四個(gè)角度φ、ψ、γ、ξ存在冗余，必須進(jìn)行約束。

4.2 擴(kuò)展克雷洛夫角因果約束方法

從擴(kuò)展克雷洛夫角的坐標(biāo)變換矩陣和角速度方程可以看出，必須對(duì)四個(gè)角度φ、ψ、γ、ξ進(jìn)行因果約束以實(shí)現(xiàn)響應(yīng)的唯一性。所謂的“因果約束”就是把γ角作為因變量，根據(jù)γ角所處的區(qū)間采用不同的控制策略[10,11]。具體因果約束的策略如圖4和圖5所示。

圖4 φ-γ的具體因果約束策略示意圖Fig.4 Diagram of causal constraint strategy of φ-γ

圖5 ψ-γ的具體因果約束策略示意圖Fig.5 Diagram of causal constraint strategy of ψ-γ

從圖4和圖5中可以看出，根據(jù)φ、ψ、γ所處的區(qū)間，對(duì)擴(kuò)展克雷洛夫角進(jìn)行因果約束。具體方法為：（1）當(dāng)時(shí)

由于四個(gè)角度φ、ψ、γ、ξ的初值為隨機(jī)狀態(tài)，必須進(jìn)行狀態(tài)轉(zhuǎn)移使得φ′=0，此時(shí)，ξ為：

把式(17)代入式(18)，

在狀態(tài)轉(zhuǎn)移完成后，φ′= 0，ψ′、γ′、ξ′作為微分方程的初值進(jìn)行姿態(tài)角ψ、γ、ξ的解算，微分方程為：

注意到，雖然cotγ、cscγ等函數(shù)在γ∈ [ -1 80°, +180°]的全局范圍內(nèi)存在奇異值，但在的限制范圍內(nèi)不存在奇異值。此時(shí)，坐標(biāo)變換矩陣為：

進(jìn)行狀態(tài)轉(zhuǎn)移使得ψ′=0，此時(shí)，ξ′為：

把式(21)代入式(22)：

在狀態(tài)轉(zhuǎn)移完成后，ψ′=0，φ′、γ′、ξ′作為微分方程的初值進(jìn)行姿態(tài)角φ、γ、ξ的解算，微分方程為：

注意到，雖然tanγ、secγ等函數(shù)在γ∈ [ -1 80°, +180°]的全局范圍內(nèi)存在奇異值，但在或的限制范圍內(nèi)不存在奇異值。此時(shí)，坐標(biāo)變換矩陣為：

由以上分析可以看出，通過四次角度旋轉(zhuǎn)，根據(jù)γ角的范圍，進(jìn)行狀態(tài)轉(zhuǎn)移使得同軸的φ′角或者ψ′=0為零，再將狀態(tài)轉(zhuǎn)移后的初值代入式(16)得到不同γ角條件下的姿態(tài)角微分方程，對(duì)其進(jìn)行積分即可得到載體姿態(tài)角。而且，基于擴(kuò)展克雷洛夫角的因果約束策略只與γ所處區(qū)間有關(guān)系，而與載體角運(yùn)動(dòng)無關(guān)，具有自主性，這是其另外一個(gè)優(yōu)點(diǎn)。

5 試驗(yàn)驗(yàn)證

5.1 采用克雷洛夫角的全姿態(tài)試驗(yàn)驗(yàn)證

下面通過分析在大動(dòng)態(tài)試驗(yàn)設(shè)備（如圖6）的慣組測試數(shù)據(jù)來驗(yàn)證克雷洛夫角的全姿態(tài)功能及在不同轉(zhuǎn)動(dòng)順序下的導(dǎo)航結(jié)果。該測試設(shè)備可實(shí)現(xiàn)俯仰角、橫滾角的±180 °轉(zhuǎn)動(dòng)，在試驗(yàn)過程中繞俯仰軸共轉(zhuǎn)了3個(gè)大圈，在每一圈繞橫滾軸轉(zhuǎn)了1個(gè)大圈，下面分別為按照克雷洛夫角的兩種轉(zhuǎn)動(dòng)順序進(jìn)行導(dǎo)航解算。

圖6 大動(dòng)態(tài)試驗(yàn)設(shè)備Fig.6 Large dynamic test equipment

以式(4)作為姿態(tài)角更新方程，以東北天地理坐標(biāo)系作為導(dǎo)航系進(jìn)行導(dǎo)航解算，三個(gè)姿態(tài)角如圖7所示，作為旋轉(zhuǎn)過程中的中間角度，ψ1角工作于（-55 °, +89.3 °）區(qū)間。

圖7 三個(gè)角度的變化過程Fig.7 Changing process of the three angles

當(dāng)ψ1角接近90 °時(shí)，姿態(tài)角誤差最大（如圖8所示），γ1和φ1的瞬時(shí)角度誤差超過20 °，經(jīng)過三次轉(zhuǎn)圈后ψ1角累計(jì)誤差超過0.2 °，造成位置誤差偏大的原因是離散化采樣時(shí)間引起的量化誤差。三維空間的位置如圖9所示，從圖中可以看出，位置誤差可達(dá)2000 m。

圖8 三個(gè)角度誤差的變化過程Fig.8 Changing process of the three angle errors

圖9 三維位置Fig.9 The three-dimensional position of the three angles

以式(6)作為姿態(tài)角更新方程，以東北天地理坐標(biāo)系作為導(dǎo)航系進(jìn)行導(dǎo)航解算，三個(gè)姿態(tài)角如圖10所示，作為旋轉(zhuǎn)過程中的中間角度，γ2角工作于（-81 °, +81 °）區(qū)間。

圖10 三個(gè)角度的變化過程Fig.10 Changing process of the three angles

從圖11的姿態(tài)角誤差曲線可以看出，γ2誤差小于0.005 °，ψ2和φ2角誤差為0.02 °，該誤差對(duì)導(dǎo)航結(jié)果的影響可忽略。三維空間的位置如圖12所示，從圖中可以看出，解算量較好地復(fù)現(xiàn)出在試驗(yàn)設(shè)備上的運(yùn)行軌跡，位置誤差約為20 m；雖然離散化采樣時(shí)間也會(huì)引起位置誤差，但該量化誤差相對(duì)圖9的位置誤差較小。

圖11 三個(gè)角度誤差的變化過程Fig.11 Changing process of the three angle errors

圖12 三維位置Fig.12 The three-dimensional position of the three angles

采用雙歐拉角法進(jìn)行導(dǎo)航時(shí)可采用方法：(1) 以式(4)中的ψ1作為切換依據(jù)，當(dāng)ψ1∈[-45 °, +45 °]時(shí)，對(duì)式(4)進(jìn)行積分解算；否則對(duì)式(6)進(jìn)行積分解算。仿真結(jié)果與圖10-12相同。(2) 以式(6)中的γ2作為切換依據(jù)，當(dāng)γ2∈[-45 °, +45 °]時(shí)，對(duì)式(6)進(jìn)行積分解算；否則對(duì)式(4)進(jìn)行積分解算。仿真結(jié)果與圖7-9相同，這說明采用雙歐拉角法仍未從根本上解決奇異值問題。因此，需要事先根據(jù)先驗(yàn)信息選取某一種特定的正反歐拉解算方法，但是在導(dǎo)航效果上與直接選取其中一種量化誤差相對(duì)較小的導(dǎo)航結(jié)果相同。

從以上導(dǎo)航結(jié)果可以看出，基于克雷洛夫角的姿態(tài)解算雖然可以實(shí)現(xiàn)全姿態(tài)導(dǎo)航的功能，但存在兩個(gè)問題：(1)轉(zhuǎn)動(dòng)過程中的中間角度越接近90 °，引起的動(dòng)態(tài)誤差會(huì)增大；(2)姿態(tài)解算精度與三次轉(zhuǎn)動(dòng)順序直接相關(guān)。因此，雖然通過補(bǔ)償可減小中間角度接近90 °時(shí)的動(dòng)態(tài)誤差，但應(yīng)用過程中應(yīng)通過改變旋轉(zhuǎn)順序等措施以提高使用精度。

5.2 采用擴(kuò)展克雷洛夫角的全姿態(tài)試驗(yàn)驗(yàn)證

以5.1節(jié)的大動(dòng)態(tài)試驗(yàn)數(shù)據(jù)為例，采用擴(kuò)展克雷洛夫角作為姿態(tài)角更新方程，導(dǎo)航解算的四個(gè)姿態(tài)角如圖13所示。從圖中可以看出，在1964.66 s之前，因此，ψ為0，而φ、γ、ξ按照式(23)進(jìn)行姿態(tài)解算。而在1964.66 s時(shí)刻，，進(jìn)行狀態(tài)轉(zhuǎn)移，從初始狀態(tài)φ=-90.0 °、ψ= 0、γ=-45.0 °、ξ=-13.1 °（如圖14中的紅色‘o’）瞬時(shí)轉(zhuǎn)移到末端狀態(tài)φ= 0 °、ψ=135.0 °、γ=90.0 °、ξ=-103.2 °（如圖14中的黑色‘*’）。在1964.66 s之后，，因此，φ為0，而ψ、γ、ξ按照式(19)進(jìn)行姿態(tài)解算。

圖13 四個(gè)角度的變化過程Fig.13 Changing process of the four angle errors

圖14 四個(gè)角度的狀態(tài)轉(zhuǎn)移示意圖Fig.14 Diagram of state transtion process of the four angle errors

四個(gè)角度的角度誤差曲線如圖15所示。

圖15 四個(gè)角度誤差的變化過程Fig.15 Changing process of the four angle errors

從圖15的姿態(tài)角誤差曲線可以看出，最大角度誤差不大于0.002°（7.2"），具備較高的精度。

對(duì)應(yīng)圖4和圖5，圖16中藍(lán)色曲線為大動(dòng)態(tài)試驗(yàn)數(shù)據(jù)下的φ-γ、ψ-γ因果約束關(guān)系。由圖16可以看出，當(dāng)時(shí)，ψ角為0 °；當(dāng)時(shí)，φ角為0 °，與3.2節(jié)的因果約束策略一致。

圖16 因果約束策略示意圖Fig.16 Diagram of causal constraint strategy

三維空間的位置如圖17所示。

圖17 擴(kuò)展克雷洛夫角參與導(dǎo)航解算的三維位置Fig.17 The three-dimensional position of the three angles based on extended Krylov angle

從圖17中可以看出，三維位置圖很好地復(fù)現(xiàn)出在試驗(yàn)設(shè)備上的運(yùn)行軌跡，位置誤差約為10 m，相比于克雷洛夫角法和雙歐拉法，無需通過變換或者切換不同的姿態(tài)角轉(zhuǎn)動(dòng)順序即可求解得到更高的導(dǎo)航精度。

6 結(jié) 論

對(duì)于取消滾動(dòng)控制的大機(jī)動(dòng)全姿態(tài)飛行場合，基于3個(gè)克雷洛夫角描述的姿態(tài)角存在奇異值，而如果在工程上采用限定姿態(tài)角范圍將不滿足全姿態(tài)飛行使用要求。雖然雙歐拉角法為解決多值問題和奇異值問題提供了一個(gè)思路，但需要依據(jù)先驗(yàn)信息判斷切換條件，兩套歐拉角并行解算的計(jì)算量較大，且仍沒從根本上解決奇異值問題。本文提出的基于擴(kuò)展克雷洛夫角的全姿態(tài)導(dǎo)航解算方法，在導(dǎo)航過程中不會(huì)發(fā)生奇異現(xiàn)象，具備全姿態(tài)高精度導(dǎo)航能力，綜合仿真及試驗(yàn)數(shù)據(jù)驗(yàn)證結(jié)果表明，基于四次旋轉(zhuǎn)的擴(kuò)展克雷洛夫角的姿態(tài)解算方法可完全滿足載體大機(jī)動(dòng)全姿態(tài)運(yùn)動(dòng)的需求。