本發(fā)明屬于空間飛行器導(dǎo)航與控制領(lǐng)域，具體涉及到一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法。

背景技術(shù)：

現(xiàn)代軍事技術(shù)的發(fā)展和國防建設(shè)的需求對飛行器導(dǎo)航和控制的精度提出了越來越高的要求。通常情況下，飛行器的姿態(tài)控制是其軌道控制的前提，而其姿態(tài)的描述則是其姿態(tài)控制的基礎(chǔ)，飛行器姿態(tài)確定的精度和快速性是其導(dǎo)航和姿態(tài)控制的關(guān)鍵步驟。因此如何更有效和快速地確定飛行器的姿態(tài)具有重要意義。常用的姿態(tài)描述參數(shù)有方向余弦矩陣、歐拉角、姿態(tài)四元數(shù)、旋轉(zhuǎn)矢量、羅德里格參數(shù)、修正羅德里格參數(shù)、凱萊-哈密頓參數(shù)等。

歐拉角與四元數(shù)姿態(tài)表示方法是目前工程上最常用的兩種方法。歐拉角表示法具有簡便、幾何意義明顯等優(yōu)點，同時姿態(tài)敏感器可以直接測出這些參數(shù)，能較方便地求解用這些姿態(tài)參數(shù)描述的姿態(tài)動力學(xué)方程。姿態(tài)四元數(shù)具有計算量小、非奇異性、可全姿態(tài)工作的特點，因此目前工程上開始采用四元數(shù)來描述飛行器運動及動力學(xué)方程中的姿態(tài)。

由于歐拉角的直觀性，在設(shè)計飛行器控制規(guī)律時仍然采用歐拉角描述飛行器的姿態(tài)，但用歐拉角表示的飛行器姿態(tài)運動學(xué)方程在大角度時會出現(xiàn)奇異現(xiàn)象，而采用四元數(shù)來表示，則可以避免這個問題。因此，飛行器運動學(xué)方程都采用四元數(shù)來表示，而飛行器的控制規(guī)律都采用歐拉角來表示，且歐拉角表示姿態(tài)角比起四元數(shù)更加形象，更易于被人理解，所以，在飛行器控制系統(tǒng)仿真設(shè)計的時候，需要四元數(shù)與歐拉角之間的轉(zhuǎn)換，但這里就存在由姿態(tài)四元數(shù)向歐拉角的轉(zhuǎn)換問題。

給定1個歐拉角，對應(yīng)1個四元數(shù)，因而歐拉角到四元數(shù)之間這種一一對應(yīng)的關(guān)系使得歐拉角到四元數(shù)的轉(zhuǎn)換比較容易。但是，1個四元數(shù)通常有1個或者2個歐拉角與之對應(yīng)，它們之間不是一一的對應(yīng)關(guān)系，因而，四元數(shù)到歐拉角之間的轉(zhuǎn)換比較困難。

通過檢索發(fā)現(xiàn)，目前專利庫中并沒有與“四元數(shù)轉(zhuǎn)換歐拉角”的相關(guān)專利，在搜索到的介紹“四元數(shù)轉(zhuǎn)換歐拉角”的相關(guān)文獻中，許佩珍、董長虹所著的“四元數(shù)在戰(zhàn)斗機飛行仿真中的應(yīng)用”闡述了在戰(zhàn)斗機的飛行仿真中采用四元數(shù)的原因和四元數(shù)的幾何意義，并按照中國戰(zhàn)斗機飛行仿真中采用的坐標(biāo)系，介紹了四元數(shù)與坐標(biāo)變換矩陣的關(guān)系、用四元數(shù)表示的歐拉角以及四元數(shù)與坐標(biāo)變換矩陣的關(guān)系，其中提到，常規(guī)的方法只能完成±90°以內(nèi)的四元數(shù)轉(zhuǎn)換歐拉角，但無法實現(xiàn)大姿態(tài)機動的歐拉角表示。

張帆、曹喜濱、鄒經(jīng)湘等所著的“一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法”中提出了一個新的轉(zhuǎn)換算法,該算法能使3個軸的歐拉角都在±180°之間進行四元數(shù)與歐拉角的轉(zhuǎn)換，其中提出了俯仰角取值范圍在[-180°,-90°]或[90°,180°]以內(nèi)、滾動角和偏航角都在[-180°,180°]以內(nèi)的轉(zhuǎn)換方法，但這不是真正意義上[0°,360°]內(nèi)的三個歐拉角的計算，如果實現(xiàn)三個歐拉角都在[0°,360°]以內(nèi)的轉(zhuǎn)換，需要在兩種轉(zhuǎn)換算法之間切換。

因此，目前行業(yè)內(nèi)亟需解決的問題在于怎樣找到一種能夠基于姿態(tài)矩陣各元素在全角域內(nèi)直接計算三個歐拉角的統(tǒng)一方法，而不需要進行復(fù)雜的歐拉角各區(qū)域判斷與不同算法之間的切換；如果能利用上一時刻的俯仰角及其旋轉(zhuǎn)方向確定俯仰角的角域，再通過一種統(tǒng)一的轉(zhuǎn)換算法計算[0°,360°]范圍內(nèi)的三個歐拉角，那么就可以在全角域范圍內(nèi)解算飛行器的姿態(tài)角問題。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于提供一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法。

本發(fā)明的目的是這樣實現(xiàn)的：

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，其具體的設(shè)計步驟如下：

(1)k＝0時刻，由空間飛行器捷聯(lián)慣導(dǎo)的初始對準(zhǔn)所確定的初始?xì)W拉角計算初始姿態(tài)矩陣，獲得初始姿態(tài)四元數(shù)q0；

(2)對空間飛行器的捷聯(lián)陀螺輸出數(shù)據(jù)進行預(yù)處理；

(3)定時采樣k時刻的捷聯(lián)陀螺輸出的角增量數(shù)據(jù)并進行預(yù)處理，將預(yù)處理后的捷聯(lián)陀螺輸出數(shù)據(jù)與上一時刻的姿態(tài)四元數(shù)qk-1相結(jié)合，計算該時刻的空間飛行器姿態(tài)四元數(shù)qk；

(4)對姿態(tài)四元數(shù)qk進行規(guī)范化，計算該時刻的姿態(tài)矩陣cnb,k；

(5)根據(jù)俯仰角θk的范圍和姿態(tài)矩陣cnb,k元素計算獲得偏航角ψk的正弦值和余弦值，由偏航角ψk的正弦值和余弦值計算偏航角ψk；

(6)根據(jù)步驟(5)計算得出的偏航角ψk和姿態(tài)矩陣cnb,k元素獲得橫滾角的正弦值和余弦值，由橫滾角的正弦值和余弦值計算橫滾角

(7)根據(jù)步驟(5)計算得出的偏航角ψk和姿態(tài)矩陣cnb,k元素計算獲得俯仰角θk的正弦值和余弦值，由俯仰角θk的正弦值和余弦值重新計算俯仰角θk；

(8)k＝k+1，轉(zhuǎn)步驟(3)，進行下一時刻姿態(tài)四元數(shù)及其轉(zhuǎn)換歐拉角的計算。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(1)中所述的空間飛行器捷聯(lián)慣導(dǎo)的初始對準(zhǔn)所確定的初始?xì)W拉角計算初始姿態(tài)四元數(shù)q0的具體實現(xiàn)方式如下：

(1.1)將z、x和y坐標(biāo)軸經(jīng)三次順序旋轉(zhuǎn)得到飛行器的姿態(tài)矩陣，設(shè)初始對準(zhǔn)確定的姿態(tài)角分別為ψ0、和θ0的初始?xì)W拉角，ψ0、和θ0分別對應(yīng)航行器繞z軸、x軸和y軸的旋轉(zhuǎn)角度；

(1.2)由初始?xì)W拉角ψ0、和θ0確定飛行器的初始姿態(tài)矩陣cnb,0；

(1.3)計算捷聯(lián)矩陣s的元素，

其中，符號t表示矩陣的轉(zhuǎn)置；

(1.4)計算初始姿態(tài)四元數(shù)q0；

其中，sign(·)表示計算該值的符號。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(2)中所述的對空間飛行器的捷聯(lián)陀螺輸出數(shù)據(jù)進行預(yù)處理的具體實施方式如下：

(2.1)根據(jù)空間飛行器的實際應(yīng)用需求計算出空間飛行器的角增量范圍；

(2.2)結(jié)合捷聯(lián)陀螺本身的測量誤差確定捷聯(lián)陀螺輸出數(shù)據(jù)的合理范圍，由捷聯(lián)陀螺輸出數(shù)據(jù)的合理范圍確定輸出數(shù)據(jù)的閾值，剔除超出閾值的粗大誤差數(shù)據(jù)；

(2.3)利用捷聯(lián)慣導(dǎo)的輸出信息和采樣時間間隔，通過捷聯(lián)陀螺的輸出補償?shù)舻乩硐档男D(zhuǎn)角速度。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(3)中所述的捷聯(lián)陀螺輸出是采樣時間間隔內(nèi)飛行器的角增量，利用定時采樣得到的角增量和上一時刻k-1的四元數(shù)qk-1確定該時刻k的四元數(shù)qk，每一次角增量的采樣對應(yīng)有相同的時間間隔。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(4)中所述的姿態(tài)四元數(shù)通過以下公式進行規(guī)范化：

其中，||·||表示求euclidean長度。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(5)中所述的由姿態(tài)矩陣cnb,k計算空間飛行器的偏航角ψk的具體實現(xiàn)方式如下：

姿態(tài)矩陣cnb,k為

(5.1)設(shè)定一個小量εθ′，當(dāng)|cnb,k(3,3)|＜εθ′時，

否則，

(5.2)根據(jù)飛行器的k-1時刻俯仰角θk-1及此時刻的俯仰角預(yù)估值θk′確定參數(shù)λθk；如果確定θk的范圍為[0°,180°]，則λθk＝1；否則，λθk＝-1；

(5.3)分別計算cψk和sψk，

cψk＝cosψk＝λθkcnb,k(1,1)cosθk′+λθkcnb,k(3,1)sinθk′

sψk＝sinψk＝λθkcnb,k(1,2)cosθk′+λθkcnb,k(3,2)sinθk′

其中，cψk為ψk的余弦值，sψk為ψk的正弦值。

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(6)中所述的由姿態(tài)矩陣cnb,k計算空間飛行器的橫滾角的具體實現(xiàn)方式如下：

(6.1)給定一個小量εψ，當(dāng)|cψk|＜εψ時，分別計算ψk與

否則，

(6.2)更新計算ψk；

(6.3)給定一個小量當(dāng)時；

否則，

其中，

(6.4)更新計算

對于一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，步驟(7)中所述的由姿態(tài)矩陣cnb,k計算空間飛行器的俯仰角θk的具體實現(xiàn)方式如下：

(7.1)計算cθk和sθk，

cθk＝cnb,k(1,1)cosψk+cnb,k(1,2)sinψk

sθk＝cnb,k(3,1)cosψk+cnb,k(3,2)sinψk

其中，cθk為θk的余弦值，sθk為θk的正弦值；

(7.2)給定一個小量εθ，當(dāng)|cθk|＜εθ時，

否則，

(7.3)更新計算θk，

本發(fā)明的有益效果在于：

本發(fā)明提供的一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法涉及到一種由姿態(tài)矩陣各元素在全角域內(nèi)直接計算三個歐拉角的統(tǒng)一方法，無需進行復(fù)雜的歐拉角各區(qū)域判斷，具有全角域轉(zhuǎn)換、無需在不同轉(zhuǎn)換算法之間相互切換等優(yōu)點，簡化了四元數(shù)轉(zhuǎn)換歐拉角的計算步驟，節(jié)省了轉(zhuǎn)換算法的計算時間。解決現(xiàn)有技術(shù)中不能實現(xiàn)的全角域四元數(shù)轉(zhuǎn)換歐拉角的問題，或者需在不同角域轉(zhuǎn)換算法之間切換實現(xiàn)全角域轉(zhuǎn)換的問題。

本發(fā)明所涉及的由姿態(tài)四元數(shù)全角域地轉(zhuǎn)換歐拉角的統(tǒng)一方法，無需進行繁雜的歐拉角各區(qū)域判斷與不同轉(zhuǎn)換算法之間的切換，簡化了四元數(shù)轉(zhuǎn)換歐拉角的計算步驟，節(jié)省了轉(zhuǎn)換算法的計算時間。

本發(fā)明提供一種統(tǒng)一的轉(zhuǎn)換算法計算[0°,360°]范圍內(nèi)的三個歐拉角，可以在全角域范圍內(nèi)計算飛行器的姿態(tài)角；另外，通過設(shè)置三個閾值由反正切函數(shù)切換到反余切函數(shù)，克服了歐拉角在90°或270°附近的解算奇異問題。

本發(fā)明進行了由四元數(shù)全角域轉(zhuǎn)換歐拉角以及由捷聯(lián)陀螺角增量輸出計算歐拉角的仿真實驗，實驗結(jié)果表明在一定的測量噪聲范圍歐拉角轉(zhuǎn)換誤差較小，本發(fā)明涉及的計算方法能夠?qū)崿F(xiàn)四元數(shù)全角域轉(zhuǎn)換歐拉角的功能。

附圖說明

圖1是空間飛行器角位置的歐拉角表示的笛卡爾坐標(biāo)系示意圖。

圖2是面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法的方框圖。

圖3是面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法的算法流程圖。

圖4是由四元數(shù)全角域轉(zhuǎn)換歐拉角的仿真實驗結(jié)果圖。

圖5是由捷聯(lián)陀螺的輸出全角域轉(zhuǎn)換歐拉角的仿真實驗結(jié)果圖。

圖6為三個歐拉角的真值表圖，角度單位為度。

具體實施方式：

下面結(jié)合附圖對本發(fā)明做進一步的描述：

本發(fā)明涉及一種面向大姿態(tài)機動的四元數(shù)全角域轉(zhuǎn)換歐拉角的獲取方法，該方法根據(jù)初始對準(zhǔn)結(jié)果計算初始姿態(tài)矩陣，獲得初始姿態(tài)四元數(shù)；由預(yù)處理后的捷聯(lián)陀螺輸出數(shù)據(jù)和上一時刻的姿態(tài)四元數(shù)更新得到這一時刻的姿態(tài)四元數(shù)；由姿態(tài)四元數(shù)計算飛行器的姿態(tài)矩陣，根據(jù)俯仰角的范圍和姿態(tài)矩陣元素計算獲得偏航角的正弦值和余弦值，由偏航角的正弦值和余弦值計算偏航角；再由偏航角和姿態(tài)矩陣元素獲得橫滾角的正弦值和余弦值，由橫滾角的正弦值和余弦值計算橫滾角；最后根據(jù)偏航角和姿態(tài)矩陣元素計算獲得俯仰角的正弦值和余弦值，由俯仰角的正弦值和余弦值重新計算俯仰角。

該方法的具體實現(xiàn)步驟如下：

(1)k＝0時刻，由空間飛行器捷聯(lián)慣導(dǎo)的初始對準(zhǔn)結(jié)果計算初始姿態(tài)四元數(shù)q0。

飛行器的姿態(tài)矩陣按照z、x和y坐標(biāo)軸的順序三次旋轉(zhuǎn)而得到，其相應(yīng)的笛卡爾坐標(biāo)系及三個歐拉角如圖1所示。設(shè)初始對準(zhǔn)確定的姿態(tài)角分別為ψ0、和θ0的初始?xì)W拉角，ψ0、和θ0分別對應(yīng)航行器繞z軸、x軸和y軸的旋轉(zhuǎn)角度。

(1.1)由初始?xì)W拉角ψ0、和θ0，按式(1)確定飛行器的初始姿態(tài)矩陣cnb,0。

(1.2)按式(2)計算捷聯(lián)矩陣s的元素

式中，符號t表示矩陣的轉(zhuǎn)置。

(1.3)按式(3)計算初始姿態(tài)四元數(shù)q0。

式中，sign(·)表示計算該值的符號，以下公式中的sign(·)具有相同的意義。

(2)空間飛行器的捷聯(lián)陀螺輸出數(shù)據(jù)的預(yù)處理。

根據(jù)空間飛行器的實際應(yīng)用需求計算其角增量范圍，再結(jié)合捷聯(lián)陀螺本身的測量誤差確定捷聯(lián)陀螺輸出數(shù)據(jù)的合理范圍，從而確定輸出數(shù)據(jù)的閾值，對超出閾值的粗大誤差數(shù)據(jù)予以剔除。利用捷聯(lián)慣導(dǎo)的輸出信息和采樣時間間隔，從捷聯(lián)陀螺的輸出補償?shù)舻乩硐档男D(zhuǎn)角速度。

(3)由k時刻的捷聯(lián)陀螺輸出數(shù)據(jù)和上一時刻的姿態(tài)四元數(shù)，計算該時刻的空間飛行器姿態(tài)四元數(shù)。

一般情況下，經(jīng)補償后的捷聯(lián)陀螺輸出是采樣時間間隔內(nèi)飛行器的角增量。為避免噪聲的微分放大，利用定時采樣得到的角增量和上一時刻k-1的四元數(shù)qk-1確定該時刻k的四元數(shù)qk，每一次角增量的采樣對應(yīng)有相同的時間間隔。設(shè)k時刻的角增量矢量的三分量分別為δθxk、δθyk和δθzk，四元數(shù)的遞推計算公式為

式中，i4表示4×4的單位矩陣，矩陣δθk的計算公式為

當(dāng)δθ接近于0時，式(4)變?yōu)閝k＝qk-1。

(4)規(guī)范化姿態(tài)四元數(shù)，利用規(guī)范化后的姿態(tài)四元數(shù)計算該時刻的姿態(tài)矩陣。

由于計算誤差等因素，四元數(shù)的范數(shù)不再是嚴(yán)格歸一化的，為提高轉(zhuǎn)換歐拉角的精度，按式(6)規(guī)范化姿態(tài)四元數(shù)。

式中，||·||表示求euclidean長度。

由k時刻的姿態(tài)四元數(shù)計算該時刻的姿態(tài)矩陣cnb,k。計算公式為

(5)由姿態(tài)矩陣cnb,k計算此時刻空間飛行器的三個歐拉角，即偏航角ψk、橫滾角俯仰角θk。

姿態(tài)矩陣cnb,k為

(5.1)設(shè)定一個小量εθ′，如果|cnb,k(3,3)|＜εθ′，則按式(9)計算θk′，

否則，按式(10)計算θk′，

(5.2)根據(jù)飛行器的k-1時刻俯仰角θk-1及此時刻的俯仰角預(yù)估值θk′確定參數(shù)λθk；如果確定θk的范圍為[0°,180°]，則λθk＝1；否則，λθk＝-1。

(5.3)將θk′分別代入式(11)和式(12)，計算cψk和sψk。

cψk＝cosψk＝λθkcnb,k(1,1)cosθk′+λθkcnb,k(3,1)sinθk′(11)

sψk＝sinψk＝λθkcnb,k(1,2)cosθk′+λθkcnb,k(3,2)sinθk′(12)

(5.4)給定一個小量εψ，如果|cψk|＜εψ，根據(jù)式(13)計算ψk，再根據(jù)式(14)計算

否則，根據(jù)式(15)計算ψk，再根據(jù)式(16)計算

(5.5)根據(jù)式(17)，更新計算ψk

(5.6)給定一個小量如果根據(jù)式(18)計算

否則，根據(jù)式(19)計算

式中，

(5.7)根據(jù)式(20)，更新計算

(5.8)分別根據(jù)式(21)和式(22)，計算cθk和sθk，

cθk＝cnb,k(1,1)cosψk+cnb,k(1,2)sinψk(21)

sθk＝cnb,k(3,1)cosψk+cnb,k(3,2)sinψk(22)

(5.9)給定一個小量εθ，如果|cθk|＜εθ，根據(jù)式(23)計算θk，

否則，根據(jù)式(24)計算θk，

(5.10)根據(jù)式(25)，更新計算θk

(6)k＝k+1，轉(zhuǎn)步驟(3)，進行下一時刻姿態(tài)四元數(shù)的計算，由下一時刻姿態(tài)四元數(shù)計算空間飛行器下一時刻的歐拉角。

利用姿態(tài)矩陣各元素與歐拉角之間的關(guān)系，得到在全角域內(nèi)直接計算三個歐拉角的統(tǒng)一方法，即四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法，其方框圖如圖2所示。四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法的算法流程圖如圖3所示。

本發(fā)明進行了兩個由四元數(shù)全角域轉(zhuǎn)換歐拉角以及由捷聯(lián)陀螺角增量輸出計算歐拉角的仿真實驗，為反映四元數(shù)全角域轉(zhuǎn)換歐拉角的轉(zhuǎn)換效果，定義k時刻三個歐拉角的轉(zhuǎn)換誤差δψk、和δθk。和其中為k時刻偏航角ψk的轉(zhuǎn)換值，為k時刻橫滾角的轉(zhuǎn)換值，為k時刻俯仰角θk的轉(zhuǎn)換值。

實驗一：設(shè)空間飛行器偏航角ψk、橫滾角和俯仰角θk的取值如圖6所示，k＝1,2,…,36。三個歐拉角的測量噪聲均為零均值、方差為1.45°的隨機白噪聲，對四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法進行50次蒙特卡洛仿真實驗。先將具有測量噪聲的三個歐拉角轉(zhuǎn)換為四元數(shù)，再由四元數(shù)全角域轉(zhuǎn)換三個歐拉角，轉(zhuǎn)換誤差的實驗結(jié)果如圖4所示，其中三種不同符號標(biāo)志的線分別為偏航角、橫滾角和俯仰角的計算誤差曲線。由圖4可知，δψk的均值和標(biāo)準(zhǔn)差分別為2.4290°和0.4634°，的均值和標(biāo)準(zhǔn)差分別為2.4308°和0.4633°，δθk的均值和標(biāo)準(zhǔn)差分別為2.3795°和0.4482°。

實驗二：設(shè)空間飛行器的橫滾角為45°，俯仰角θk為27.5°，偏航角ψk的變化規(guī)律為ψk＝360sin(πt/30)，采樣時間為0.01s，總時間為15s。角速度陀螺的測量噪聲ηv的功率譜密度為陀螺漂移為隨機游走，其驅(qū)動噪聲的功率譜密度為陀螺采樣周期為0.01s。三個歐拉角的初始誤差均為零均值、方差為1°的高斯白噪聲，對四元數(shù)全角域轉(zhuǎn)換歐拉角的計算方法進行50次蒙特卡洛仿真實驗。得到四元數(shù)全角域轉(zhuǎn)換歐拉角誤差的實驗結(jié)果如圖5所示，其中三種不同符號標(biāo)志的線分別為偏航角、橫滾角和俯仰角的計算誤差曲線。由圖5可知，δψk的均值和標(biāo)準(zhǔn)差分別為0.0109°和0.0080°，的均值和標(biāo)準(zhǔn)差分別為0.0016°和0.0016°，δθk的均值和標(biāo)準(zhǔn)差分別為0.0034°和0.0051°。