呂維維，程向紅，邱 偉

(1. 東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，江蘇 南京 210096； 2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院， 江蘇 南京 210096； 3. 上海宇航系統(tǒng)工程研究所，上海 201109)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)是一種不依賴于任何外部信息，也不向外部輻射能量的自主導(dǎo)航系統(tǒng)。該系統(tǒng)具有隱蔽性好、導(dǎo)航精確的優(yōu)點，在戰(zhàn)術(shù)導(dǎo)彈、運(yùn)載火箭、宇宙飛船、飛機(jī)、艦船上有著廣泛應(yīng)用[1-2]。作為SINS的核心，姿態(tài)更新算法在SINS精確導(dǎo)航中發(fā)揮著重要作用，這些算法主要有方向余弦法、四元數(shù)法、歐拉角法[3-5]等。其中：方向余弦法更新姿態(tài)矩陣需求解9個微分方程，求解的姿態(tài)矩陣可全姿態(tài)工作，但計算量較大[3]；四元數(shù)法計算量小、計算精度高，但每次求解到的姿態(tài)矩陣必須經(jīng)過正交化的誤差修正處理，在飛行過程中會出現(xiàn)有界參數(shù)誤差出界的情況[6-8]；歐拉角法求解歐拉角只需求解3個微分方程，與其他算法相比，需要求解的方程個數(shù)最少，且求解到的姿態(tài)矩陣永遠(yuǎn)是正交陣，無需正交化處理，但歐拉角系統(tǒng)有1對奇異點，在奇異點附近會產(chǎn)生解算誤差，使輸出姿態(tài)角發(fā)生突變，無法準(zhǔn)確定值[2, 9-10]。

為解決歐拉角奇異性的問題，黃雪樵[6]最早對雙歐法進(jìn)行了理論論證，認(rèn)為雙歐法能有效消除單歐法奇異點。SINGLA等[11]對序貫旋轉(zhuǎn)法(MSR)進(jìn)行了詳細(xì)研究，并指出采取不同歐拉角組合、相互切換的方法比MSR方法更簡潔。陳廷楠等[12]將雙歐法與四元數(shù)法進(jìn)行了理論對比，證明四元數(shù)法存在較大的累計計算誤差，雙歐法在克服歐拉角奇異性的問題上優(yōu)于四元數(shù)法。以上學(xué)者主要進(jìn)行理論研究，對雙歐法的應(yīng)用研究較少，對四元數(shù)法與雙歐拉法解算精度的對比研究也較少。為適應(yīng)現(xiàn)代戰(zhàn)爭的需要，目前戰(zhàn)術(shù)導(dǎo)彈已逐漸采用大攻角控制方式，實現(xiàn)大機(jī)動飛行。對于垂直發(fā)射的地空導(dǎo)彈，為使導(dǎo)引頭能成功捕獲目標(biāo)，要求對導(dǎo)彈姿態(tài)進(jìn)行精確控制。傳統(tǒng)方法使用四元數(shù)表示彈體姿態(tài)，并對姿態(tài)控制進(jìn)行系統(tǒng)設(shè)計，然而四元數(shù)法在彈體俯仰角90°時只能進(jìn)行姿態(tài)矩陣更新，無法正常輸出姿態(tài)角，這給大機(jī)動地空導(dǎo)彈的姿態(tài)控制帶來了困難。

本文針對垂直發(fā)射的地空導(dǎo)彈在大機(jī)動飛行時姿態(tài)解算容易出現(xiàn)奇異值的問題，將正、反歐拉角切換的雙歐拉全姿態(tài)算法應(yīng)用到姿態(tài)解算過程中，通過半物理試驗驗證了該方法的有效性。

1 正、反歐拉角方程

1.1 歐拉角法

動坐標(biāo)系相對參考坐標(biāo)系方位，由動坐標(biāo)系依次繞3個不同軸轉(zhuǎn)動的3個轉(zhuǎn)角來確定[6]，如圖1所示。令O-XbYbZb為動坐標(biāo)系，O-ENU為參考坐標(biāo)系，則航向角H、縱搖角P、橫滾角R為1組歐拉角。

圖1 彈體空間角位置的確定Fig.1 Determination of missile’s space position

兩坐標(biāo)系間的變換矩陣等于基本旋轉(zhuǎn)確定變化矩陣的連乘，順序根據(jù)基本旋轉(zhuǎn)先后次序從右向左排列。圖中，彈體空間角位置確定依據(jù)為

令導(dǎo)航坐標(biāo)系為n，彈體坐標(biāo)系為b，則姿態(tài)矩陣可表示為

(1)

(2)

1.2 正、反歐拉角微分方程

彈體的姿態(tài)變化可看作依次繞航向軸、俯仰軸、橫滾軸進(jìn)行基本旋轉(zhuǎn)后的復(fù)合結(jié)果[1, 10]，即

(3)

(4)

(5)

將式(5)轉(zhuǎn)換為歐拉角微分方程，具體表示為

(6)

(7)

由式(6)可知，歐拉角微分方程包含三角函數(shù)運(yùn)算，給實時計算帶來困難，當(dāng)俯仰角P=90°時，歐拉角微分方程出現(xiàn)奇異點，使計算溢出。

(8)

(9)

(10)

式(8)～(10)中：P=±90°為方程組奇異點，但當(dāng)P=0或P≈±180°時，方程解的精度會提高，此求解范圍稱為正歐拉方程解的精華區(qū)。

(11)

將式(11)轉(zhuǎn)換為

(12)

(13)

(14)

(15)

從反歐拉方程來看，奇異性問題同樣存在，即Rr=±90°是方程組的奇異點；當(dāng)Rr=0或Rr=±180°時，方程解的精度會提高，此求解范圍稱為反歐拉角微分方程解的精華區(qū)。

2 正、反歐拉角的轉(zhuǎn)換關(guān)系

任意2個坐標(biāo)系之間的變換矩陣是唯一的，因此正、反歐拉角的變換矩陣對應(yīng)項相等，分別表示為

(16)

L(Pr,Rr,Hr)=

(17)

由式(16)，(17)可得，cosRr=sinP/sinPr，將cosRr代入式(13)～(15)中，則反歐拉角微分方程可變換為

(18)

(19)

(20)

圖2 正、反歐拉角微分方程的精華區(qū)與奇異區(qū)Fig.2 Essence areas and singular areas of positive and reverse Euler angle differential equations

由式(18)～(20)可見，反歐拉角微分方程與正歐拉角微分方程相反，其精華區(qū)在P=±90°附近，奇異區(qū)在P=0或P=±180°附近。若以±45°和±135°為界進(jìn)行劃分，則正、反歐拉角微分方程的精華區(qū)與奇異區(qū)如圖2所示。由圖可知：正、反歐拉角微分方程的精華區(qū)與奇異區(qū)正好呈倒置關(guān)系。充分利用該關(guān)系實現(xiàn)兩者的無縫切換：當(dāng)|P|≤45°或|P|>135°時，求解正歐拉角微分方程，發(fā)揮正歐拉角精華區(qū)解算精確的優(yōu)點；當(dāng)|P|>45°且|P|≤135°時，求解反歐拉角微分方程，利用反歐拉角精華區(qū)進(jìn)行姿態(tài)解算，有效避免俯仰角在±90°附近姿態(tài)角突變的問題。

由于正、反歐拉角變換矩陣的一致性，因此正歐拉角變換矩陣與反歐拉角變換矩陣的對應(yīng)項相等，其定義為

由式(16)～(17)得到正歐拉角到反歐拉角的變換關(guān)系式，即

(21)

式中：Rr≠±π/2。反之，反歐拉角到正歐拉角的變換關(guān)系式為

(22)

式中：P≠±π/2。針對地空導(dǎo)彈的大機(jī)動運(yùn)動，利用雙歐拉全姿態(tài)算法，交替采用正、反歐拉角來描述姿態(tài)。地空導(dǎo)彈正、反歐拉角微分方程的全姿態(tài)解算流程如圖3所示。由圖可知，其解算步驟為：1)初始化系統(tǒng)，輸入慣性測量單元(IMU)信息。2)對正歐拉角俯仰角P的絕對值進(jìn)行判斷，當(dāng)|P|≤45°或|P|>135°時，符號標(biāo)志位Flag=1，此時求解正歐拉角微分方程；當(dāng)|P|>45°且|P|≤135°時，F(xiàn)lag=-1，此時求解反歐拉角微分方程。整個姿態(tài)解算的持續(xù)時間設(shè)為T，當(dāng)導(dǎo)航時間達(dá)到T時解算結(jié)束。

圖3 地空導(dǎo)彈正、反歐拉角微分方程全姿態(tài)解算流程Fig.3 Flow chart of positive and reverse Euler angle differential equations’ whole attitude solution

3 試驗驗證

3.1 仿真驗證

為驗證地空導(dǎo)彈雙歐拉全姿態(tài)算法的非奇異性，進(jìn)行了仿真驗證。仿真參數(shù)設(shè)置情況見表1。姿態(tài)解算周期為0.01 s，仿真經(jīng)度為118.8° E，緯度為32.06° N。在仿真過程中，IMU航向角H=0°，橫滾角R=0°，俯仰角周期性搖擺，正弦函數(shù)為A·sin(2πf·t+φ0)+ψ0。其中：A為IMU的搖擺幅值，A=90°；f為搖擺頻率，f=(1/256)Hz；φ0為搖擺初始相位，φ0=0°；ψ0為搖擺中心，ψ0=0°。仿真時間為2 500 s。

表1 傳感器參數(shù)設(shè)置

整個仿真過程中，IMU俯仰角在±90°附近周期性搖擺，姿態(tài)解算采用四元數(shù)算法。經(jīng)過2 500 s的仿真，IMU實際解算姿態(tài)角與理論姿態(tài)角的對比如圖4～6所示。

圖4 四元數(shù)法航向角的理論值與實際值比較Fig.4 Comparison of heading angle’s theoretical value and actual value by using quaternion method

圖5 四元數(shù)法俯仰角的理論值與實際值比較Fig.5 Comparison of pitching angle’s theoretical value and actual value by using quaternion method

圖6 四元數(shù)法橫滾角理論值與實際值比較Fig.6 Comparison of rolling angle’s theoretical value and actual value by using quaternion method

由圖4～6可知：當(dāng)姿態(tài)解算采用四元數(shù)算法時，由于IMU俯仰角為±90°時只能進(jìn)行姿態(tài)更新，無法正常輸出姿態(tài)角，因此IMU輸出的實際航向角和橫滾角均發(fā)生突變，姿態(tài)角出現(xiàn)奇異現(xiàn)象。

為進(jìn)行對比，在同樣條件下采用雙歐拉全姿態(tài)算法進(jìn)行姿態(tài)解算，IMU實際解算的姿態(tài)角與理論姿態(tài)角的對比情況如圖7～9所示。

圖7 雙歐拉法航向角理論值與實際值比較Fig.7 Comparison of heading angle’s theoretical value and actual value by using dual-Euler method

圖8 雙歐拉法俯仰角理論值與實際值比較Fig.8 Comparison of pitching angle’s theoretical value and actual value by using dual-Euler method

圖9 雙歐拉法橫滾角理論值與實際值比較Fig.9 Comparison of rolling angle’s theoretical value and actual value by using dual-Euler method

由圖7～9可知：當(dāng)采用雙歐拉全姿態(tài)算法后，IMU實際解算的航向角、橫滾角與理論值基本重合，未出現(xiàn)突變?yōu)?180°或180°的現(xiàn)象。理論俯仰角與實際俯仰角基本重合，在±90°附近能正常輸出姿態(tài)，未出現(xiàn)突變現(xiàn)象。雙歐拉全姿態(tài)解算的實際姿態(tài)、理論姿態(tài)的誤差曲線分別如圖10～12所示。

圖10 雙歐拉法航向角誤差Fig.10 Error curve of heading angle by using dual-Euler method

圖11 雙歐拉法俯仰角誤差Fig.11 Error curve of pitching angle by using dual-Euler method

圖12 雙歐拉法橫滾角誤差Fig.12 Error curve of rolling angle by using dual-Euler method

由圖10～12可知：在解算過程中，航向角誤差緩慢變大，但誤差絕對值在0.5′以內(nèi)。俯仰角誤差因IMU周期性搖擺而上下波動，但誤差絕對值在1′以內(nèi)。橫滾角誤差隨時間增長而緩慢變大，但誤差絕對值在0.6′以內(nèi)。當(dāng)運(yùn)用正歐拉角更新時，F(xiàn)lag=1；當(dāng)運(yùn)用反歐拉角更新時，F(xiàn)lag=-1。俯仰角在±90°搖擺時Flag的變換情況如圖13所示。

圖13 雙歐拉法正、反歐拉角微分方程切換標(biāo)志Fig.13 Switch flag of positive and reverse Euler angle differential equations by using dual-Euler method

3.2 半物理驗證

為進(jìn)一步驗證地空導(dǎo)彈雙歐拉全姿態(tài)算法的有效性，在三軸轉(zhuǎn)臺上進(jìn)行了半物理試驗。轉(zhuǎn)臺試驗實物如圖14所示，該轉(zhuǎn)臺的轉(zhuǎn)動速率精度為±0.000 5(°)/s，角度測量精度為±0.000 1°，可認(rèn)為轉(zhuǎn)臺輸出是沒有誤差的理論姿態(tài)。在半物理試驗中，轉(zhuǎn)臺外框、中框、內(nèi)框分別控制IMU的航向角、俯仰角、橫滾角轉(zhuǎn)動。試驗采用某型撓性慣組作為IMU，其中三軸陀螺儀的隨機(jī)常值漂移為0.025 (°)/h，隨機(jī)噪聲為0.01(°)/√h，三軸加速度計的隨機(jī)常值偏置為0.1 mg，隨機(jī)噪聲為0.05 mg/√Hz。IMU的姿態(tài)更新周期為0.01 s。試驗中，轉(zhuǎn)臺外框和內(nèi)框始終保持靜止(H=0，R=0)，轉(zhuǎn)臺中框先靜止一段時間，然后以約0.015 (°)/s的固定轉(zhuǎn)動速率在±90°之間周期性轉(zhuǎn)動。

圖14 轉(zhuǎn)臺試驗實物Fig.14 Actual turntable test

試驗總共進(jìn)行2 500 s，導(dǎo)航計算機(jī)實時記錄轉(zhuǎn)臺輸出姿態(tài)、IMU輸出角速度、加速度信息。導(dǎo)航計算機(jī)采用雙歐拉全姿態(tài)算法對陀螺儀和加速度計輸出進(jìn)行捷聯(lián)解算。給出俯仰角姿態(tài)解算結(jié)果，解算得到的實際俯仰角與轉(zhuǎn)臺輸出理論俯仰角的比較如圖15所示。

圖15 雙歐拉法俯仰角理論值與實際值比較Fig.15 Comparison of pitching angle’s theoretical value and actual value by using dual-Euler method

圖16 雙歐拉法俯仰角誤差曲線Fig.16 Error curve of pitching angle by using dual-Euler method

由圖15可知：雙歐拉全姿態(tài)算法得到的IMU俯仰角曲線與轉(zhuǎn)臺輸出俯仰角曲線幾乎重合。即使轉(zhuǎn)臺輸出角速率因人為操作而出現(xiàn)小的波動，IMU解算的姿態(tài)角也能很好跟蹤。當(dāng)俯仰角接近±90°時，導(dǎo)航計算機(jī)能正常輸出俯仰角，未出現(xiàn)突變?yōu)?180°或180°的現(xiàn)象。雙歐拉全姿態(tài)算法解算的俯仰角與理論俯仰角的誤差曲線如圖16所示，解算出的俯仰角與理論俯仰角的誤差大致在±2′之間波動，誤差值保持穩(wěn)定。

半物理試驗正、反歐拉角微分方程切換時Flag的變化情況如圖17所示。由圖可見：Flag的變化與IMU俯仰角大小的變化保持一致。

圖17 雙歐拉法正、反歐拉角微分方程切換標(biāo)志Fig.17 Switch flag of positive and reverse Euler angle differential equations by using dual-Euler method

3.3 雙歐拉全姿態(tài)方法與四元數(shù)法的精度比較

垂直發(fā)射的地空導(dǎo)彈為了使導(dǎo)引頭能夠成功捕獲目標(biāo)，對導(dǎo)彈的姿態(tài)精度和導(dǎo)航精度提出了很高的要求。但目前，對雙歐拉方法進(jìn)行理論研究的文獻(xiàn)很少有雙歐拉方法與四元數(shù)法導(dǎo)航精度比較的報道。為比較本文提出的地空導(dǎo)彈雙歐拉全姿態(tài)方法與傳統(tǒng)四元數(shù)方法的導(dǎo)航精度，進(jìn)行了半物理純慣性導(dǎo)航試驗。試驗進(jìn)行了2 500 s，分別記錄不同時刻采用雙歐拉法和四元數(shù)法所得的純慣性導(dǎo)航位置誤差，對比情況見表2～4。

表2 純慣性導(dǎo)航東向位置誤差

表3 純慣性導(dǎo)航北向位置誤差

表4 純慣性導(dǎo)航位置誤差

由表2～4可知：在不同時刻采用雙歐法所得的純慣性導(dǎo)航東向位置誤差、北向位置誤差、位置誤差均小于采用四元數(shù)法所得的誤差。采用四元數(shù)法每次求解到的姿態(tài)矩陣必須經(jīng)過正交化的誤差修正，不可避免存在近似誤差，且隨著計算時間的增加，誤差逐漸變大。由于雙歐拉法無需正交化處理，因此無近似誤差的累積。由表4可知：當(dāng)時間為500 s時，雙歐拉法的位置誤差比四元數(shù)法的位置誤差減小了9.81%；當(dāng)時間為2 500 s時，雙歐拉法的位置誤差比四元數(shù)法的位置誤差減小了13.14%。隨著時間的增加，雙歐拉法的導(dǎo)航精度比四元數(shù)法的導(dǎo)航精度更高。

4 結(jié)束語

地空導(dǎo)彈在大機(jī)動飛行時，其俯仰角在±90°附近會因奇異性而造成姿態(tài)角輸出突變。為解決該問題，本文提出了雙歐拉全姿態(tài)解算方法。通過推導(dǎo)正、反歐拉角微分方程，證明正、反歐拉角微分方程的精華區(qū)與奇異區(qū)呈倒置關(guān)系。根據(jù)俯仰角將正、反歐拉角微分方程進(jìn)行切換，避免微分方程在奇異區(qū)內(nèi)求解所產(chǎn)生的奇異性。詳細(xì)分析了正、反歐拉角的內(nèi)在聯(lián)系，建立了兩者的轉(zhuǎn)換關(guān)系，實現(xiàn)兩者的無縫切換。為驗證地空導(dǎo)彈雙歐拉全姿態(tài)算法的有效性，進(jìn)行了仿真試驗與半物理試驗,結(jié)果表明：由雙歐拉全姿態(tài)算法得到的彈體俯仰角，在±90°附近與理論俯仰角一致，未出現(xiàn)姿態(tài)突變現(xiàn)象。將雙歐拉全姿態(tài)解算方法與四元數(shù)法的導(dǎo)航精度進(jìn)行試驗對比，結(jié)果表明：前者導(dǎo)航精度更高，未出現(xiàn)誤差累積現(xiàn)象。本文方法能保證地空導(dǎo)彈在大機(jī)動飛行時姿態(tài)角正常解算和輸出，對于彈載SINS在大機(jī)動飛行時的全姿態(tài)解算具有較高的工程應(yīng)用價值。后續(xù)將對大機(jī)動飛行實際系統(tǒng)中的導(dǎo)彈進(jìn)行進(jìn)一步測試，以驗證該方法的穩(wěn)定性和精確性。