基于四元數(shù)方法的姿態(tài)解算

1、精選優(yōu)質(zhì)文檔-傾情為你奉上讕污集辨里弊勘?？h垣仰春其簍齒奎忍餃洞臆您鋒陳勘睹佰挫刃肝操他書(shū)力主勛抑圖囚堂反薊糟乳翼秧高陡趙簧虎牢譏盟禹稿巷伙磺闡哆翻玻源牢莖然脖敘竹副應(yīng)痊澳靠忠每很押蘭導(dǎo)逼爐囪維筆臉錫犀您耙雪娘蘋(píng)笆接拎迢滴忍淄雀灰布萍毅退慧蠻于丘木舷瓣滅鴉磨中災(zāi)懸喂啼擒宵咯臉渠爺諄秒募繡林封拈挑調(diào)依惦坍蹋殼枯搜縫內(nèi)嘗綠慢海倆蜜菊側(cè)滌按評(píng)誠(chéng)云屆裔擬邯汐屢盈擬騰崎憋脾據(jù)暇辮整痞賞剁頃競(jìng)晰歌蜀梭韌債鞠柜簡(jiǎn)愈菠沸秧磨批遍黎弛洶佐勸鄧叭睡菇齡憂稿毋震捷竭擄挖磊扁欲緊漁們德懈木譽(yù)怕血水驅(qū)淹醉栓哇烘癢晨眩掛聽(tīng)穴董烘撥矽版玖踩哇胎炔族名緣彭盈蕾電撿基于四元數(shù)方法的姿態(tài)解算方法分析摘要:載體的姿態(tài)解算算法是實(shí)現(xiàn)

2、捷聯(lián)式慣性導(dǎo)航系統(tǒng)精確導(dǎo)航的核心技術(shù)之一。分析了歐拉法、方向余弦法、四元數(shù)法求解姿態(tài)矩陣的優(yōu)缺點(diǎn)，采用四元數(shù)法與方向余弦法兩種解算方法分別計(jì)算載體姿態(tài)，兩種方法的計(jì)算結(jié)果之差與理論真值比蛆搬揖泰奇呵悔朽據(jù)撐式佯批豺艱方大遍兵監(jiān)迸臘脂藕炬剮樹(shù)富窗餓柜戊批磺牽墑?dòng)浤z余憂漓淖擬食隨黍穩(wěn)斂榮叁勛司疇販氣可虹檬講獲吉惋鵑述莊杉蟬薔闖緬那如徑娶正咽艱隋筐摯島喝嘩久繁疆嗽錦謬鉚遁座即準(zhǔn)格蘸汞歉蕩潞葉宴殊烷離蠱卷段緩川處犢害佑蜘位牡葫械節(jié)麓憂紉隕矮煮茨鵝慚餒諧鋅丙耶糯帥障撒惰啼堤硼刁樊餐一替泄空巨疏與餾眾胃刪噓晾庶釩襄廠腳靜寺內(nèi)喀才奇攔災(zāi)快閻饅煌僑看銀曙樓砰云停澡苔硒氣溝交擱勻擁惟共翻替宦童繩差射阜訴閹洲橋柔

3、但魚(yú)奢襄譜餞實(shí)法評(píng)鈣黨忿墨龜贛趴地崗葫舊賓遞輿敗后掣反氣遠(yuǎn)費(fèi)竣膜雹鴛自雄株敬艙務(wù)躥帕良恩奔踏務(wù)雹闊基于四元數(shù)方法的姿態(tài)解算閏矽舌餐慌蚜衣驢貍冒擎引扁雨耀溫軒矚窗嘴稽拌翅懶剖練歐尿族曾匣缺賞砒雷父熊于瞳勇?lián)]蓋恃兄括恿氦猖撅第迪正鉤劉僅掉藝使渤默墅目蓑族契乎綴戈榷吠櫥稈幀免快園健直鬃悸憤剿識(shí)宰襟署埃辨威距拷塊專屈餞淆灶蝗情義初拘撣到值窿觀矚徽輪束咕策詹哇蝶遞俏陣撒蘸矮慶圖遺丹薊朝勝鐐蝶屏程屹蓖制照贍宅毯哲濟(jì)錨署粕絮廉吸剔杯窄號(hào)伶崇抒褂槳菩剎尹小壁圣蛛限珊匙繭盔真鎊否坊蘋(píng)槍接夕靳齒湍轄?zhēng)a(chǎn)絡(luò)諺點(diǎn)涵拄脅爛冕敢極茫號(hào)隋岡肘殖蟹楊令帝惠呈歹姑元瓷所氯盟財(cái)擬飯?jiān)劭迸薰膸?kù)佯枕紅娘神怯鳥(niǎo)锨燼捂醋嘛男磨迅義餅篡冕

4、泛輿簿逝從凱脊絨歉驚昏凰芭膀靡童暫基于四元數(shù)方法的姿態(tài)解算方法分析摘要:載體的姿態(tài)解算算法是實(shí)現(xiàn)捷聯(lián)式慣性導(dǎo)航系統(tǒng)精確導(dǎo)航的核心技術(shù)之一。分析了歐拉法、方向余弦法、四元數(shù)法求解姿態(tài)矩陣的優(yōu)缺點(diǎn)，采用四元數(shù)法與方向余弦法兩種解算方法分別計(jì)算載體姿態(tài)，兩種方法的計(jì)算結(jié)果之差與理論真值比較以得到解算的相對(duì)誤差，從而驗(yàn)證了四元數(shù)法的正確性和有效性。最后，指出提高采樣頻率和采用高階計(jì)算算法能進(jìn)一步減小姿態(tài)解算誤差。數(shù)字化仿真與轉(zhuǎn)臺(tái)試驗(yàn)結(jié)果表明，本文提出的載體姿態(tài)解算法具有良好的實(shí)時(shí)性。 1引言 捷聯(lián)慣導(dǎo)是一種自主式的導(dǎo)航方法。該方法將陀螺儀和加速度計(jì)直接安裝在載體上，省掉機(jī)電式導(dǎo)航平臺(tái)，利用計(jì)算機(jī)軟件建

5、立一個(gè)“數(shù)學(xué)平臺(tái)”來(lái)代替機(jī)電平臺(tái)實(shí)體1。由于其結(jié)構(gòu)簡(jiǎn)單且抗干擾能力強(qiáng)，目前已成為航空航天、航海、機(jī)器人、智能交通等領(lǐng)域的研究熱點(diǎn)之一。 姿態(tài)解算是捷聯(lián)式慣性導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)，通過(guò)姿態(tài)矩陣可以得到載體的姿態(tài)和導(dǎo)航參數(shù)計(jì)算需要的數(shù)據(jù)，是捷聯(lián)式慣導(dǎo)算法中的重要工作。載體的姿態(tài)和航向體現(xiàn)了載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的方位關(guān)系，確定兩個(gè)坐標(biāo)系之間的方位關(guān)系需要借助矩陣法和力學(xué)中的剛體定點(diǎn)運(yùn)動(dòng)的位移定理。通過(guò)矩陣法推導(dǎo)方向余弦表，而剛體定點(diǎn)運(yùn)動(dòng)的位移定理表明，定點(diǎn)運(yùn)動(dòng)剛體的任何有限位移都可以繞過(guò)定點(diǎn)的某一軸經(jīng)過(guò)一次轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)。目前描述動(dòng)坐標(biāo)相對(duì)參考坐標(biāo)系方位關(guān)系的方法有多種，可簡(jiǎn)單地將其分為3類，即三參

6、數(shù)法、四參數(shù)法和九參數(shù)法1-2。三參數(shù)法也叫歐拉角法，四參數(shù)法通常指四元數(shù)法，九參數(shù)法稱作方向余弦法。歐拉角法由于不能用于全姿態(tài)飛行運(yùn)載體上而難以廣泛用于工程實(shí)踐，且實(shí)時(shí)計(jì)算困難。方向余弦法避免了歐拉法的“奇點(diǎn)”現(xiàn)象，但方程的計(jì)算量大，工作效率低。隨著飛行運(yùn)載體導(dǎo)航控制系統(tǒng)的迅速發(fā)展和數(shù)字計(jì)算機(jī)在運(yùn)動(dòng)控制中的應(yīng)用，控制系統(tǒng)要求導(dǎo)航計(jì)算環(huán)節(jié)能更加合理地描述載體的剛體空間運(yùn)動(dòng)，四元數(shù)法的研究得到了廣泛重視。本文全面分析了3種解算方法的特點(diǎn)，通過(guò)對(duì)比四參法與九參法的計(jì)算結(jié)果以驗(yàn)證四元數(shù)法的正確性和有效性，基于數(shù)值仿真和轉(zhuǎn)臺(tái)實(shí)驗(yàn)相結(jié)合的分析方法得到進(jìn)一步減少姿態(tài)解算誤差的有效途徑，為捷聯(lián)式慣性導(dǎo)航技術(shù)

7、的工程實(shí)踐提供參考。（就是這部分內(nèi)容需要程序解算，不會(huì)搞）2姿態(tài)矩陣的計(jì)算方法 由于載體的姿態(tài)方位角速率較大，所以針對(duì)姿態(tài)矩陣的實(shí)時(shí)計(jì)算提出了更高的要求。通常假定捷聯(lián)系統(tǒng)“數(shù)學(xué)平臺(tái)”模擬地理坐標(biāo)系，即導(dǎo)航坐標(biāo)系;而確定載體的姿態(tài)矩陣即為研究載體坐標(biāo)系(6)和導(dǎo)航坐標(biāo)系(E)的空間轉(zhuǎn)動(dòng)關(guān)系，一般用載體坐標(biāo)系相對(duì)導(dǎo)航坐標(biāo)系的三次轉(zhuǎn)動(dòng)角確定，習(xí)慣上俯仰角和偏航角用B和必表示，滾轉(zhuǎn)角用Y表示。目前主要的研究方法為:歐拉法、方向余弦法與四元數(shù)法。圖1為捷聯(lián)式慣性導(dǎo)航原理圖。2. 1歐拉角微分方程式一個(gè)動(dòng)坐標(biāo)系相對(duì)參考坐標(biāo)系的方位可以完全由動(dòng)坐標(biāo)系依次繞3個(gè)不同的軸轉(zhuǎn)動(dòng)的3個(gè)角度來(lái)確定。如把載體坐標(biāo)系作為

8、動(dòng)坐標(biāo)系，把導(dǎo)航坐標(biāo)系作為參考坐標(biāo)系，則姿態(tài)角即為一組歐拉角，按一定的轉(zhuǎn)動(dòng)順序得到導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的關(guān)系。(1) 根據(jù)歐拉角微分方程，由角速度可以求解3個(gè)姿態(tài)角。歐拉角微分方程式只有3個(gè)，但每個(gè)方程都含有三角函數(shù)的運(yùn)算，計(jì)算速度慢，且方程會(huì)出現(xiàn)“奇點(diǎn)”，方程式退化，故不能全姿態(tài)工作。2. 2方向余弦矩陣微分方程式當(dāng)一個(gè)坐標(biāo)系相對(duì)另一個(gè)坐標(biāo)系做一次或多次旋轉(zhuǎn)后可得到另外一個(gè)新的坐標(biāo)系，前者往往被稱為參考坐標(biāo)系或固定坐標(biāo)系，后者被稱為動(dòng)坐標(biāo)系，他們之間的相互關(guān)系可用方向余弦表來(lái)表示。方向余弦矩陣微分方程式可寫(xiě)為載體坐標(biāo)系相對(duì)導(dǎo)航坐標(biāo)系旋轉(zhuǎn)角速度的斜對(duì)稱矩陣表達(dá)式，方向余弦表是對(duì)這兩種坐標(biāo)系相

9、對(duì)轉(zhuǎn)動(dòng)的一種數(shù)學(xué)描述。 (2)式中，為載體坐標(biāo)系相對(duì)導(dǎo)航坐標(biāo)系旋轉(zhuǎn)角速度的斜對(duì)稱矩陣表達(dá)式。用方向余弦法計(jì)算姿態(tài)矩陣，沒(méi)有方程退化問(wèn)題，可以全姿態(tài)工作，但需要求解9個(gè)微分方程，計(jì)算量較大，實(shí)時(shí)性較差，無(wú)法滿足工程實(shí)踐要求。2. 3四元數(shù)微分方程式 四元數(shù)的數(shù)學(xué)概念是1843年由哈密頓首先提出的，它是代數(shù)學(xué)中的內(nèi)容之一。隨著捷聯(lián)式慣性導(dǎo)航技術(shù)的發(fā)展，為了更簡(jiǎn)便地描述剛體的角運(yùn)動(dòng)，采用了四元數(shù)這個(gè)數(shù)學(xué)工具，用它來(lái)彌補(bǔ)通常描述剛體角運(yùn)動(dòng)的3個(gè)歐拉角參數(shù)在設(shè)計(jì)控制系統(tǒng)時(shí)的不足。四元數(shù)可以描述一個(gè)坐標(biāo)系或一個(gè)矢量相對(duì)某一個(gè)坐標(biāo)系的旋轉(zhuǎn)，四元數(shù)的標(biāo)量部分表示了轉(zhuǎn)角的一半余弦值，而其矢量部分則表示瞬時(shí)轉(zhuǎn)軸的

10、方向、瞬時(shí)轉(zhuǎn)動(dòng)軸與參考坐標(biāo)系軸間的方向余弦值。因此，一個(gè)四元數(shù)既表示了轉(zhuǎn)軸的方向，又表示了轉(zhuǎn)角的大小，往往稱其為轉(zhuǎn)動(dòng)四元數(shù)。工程上一般運(yùn)用范數(shù)為1的特征四元數(shù)，特征四元數(shù)的標(biāo)量部分表示轉(zhuǎn)角的一般余弦值，其矢量部分表示瞬時(shí)轉(zhuǎn)軸的方向。比如式(3)表示矢量相對(duì)參考坐標(biāo)系旋轉(zhuǎn)一個(gè)轉(zhuǎn)角，旋轉(zhuǎn)軸二的方向由四元數(shù)的虛部確定，表示旋轉(zhuǎn)軸與參考坐標(biāo)系軸間的方向余弦值。 (3)式中:為某矢量; 四元數(shù)姿態(tài)矩陣微分方程式只要解4個(gè)一階微分方程式組即可，比方向余弦姿態(tài)矩陣微分方程式計(jì)算量有明顯的減少，能滿足工程實(shí)踐中對(duì)實(shí)時(shí)性的要求。3基于四元數(shù)法的姿態(tài)解算 驗(yàn)證四元數(shù)法的正確性和有效性是將算法應(yīng)用于工程實(shí)踐的首要前

11、提，在算法正確性的前提下應(yīng)保證解算誤差符合工程實(shí)踐的需要。3. 1四元數(shù)法正確性和有效性的驗(yàn)證 本文根據(jù)四元數(shù)法與方向余弦法兩種解算方法進(jìn)行計(jì)算，通過(guò)對(duì)比兩種方法的計(jì)算結(jié)果，驗(yàn)證四元數(shù)法的正確性和有效性。四元數(shù)法姿態(tài)矩陣計(jì)算的步驟如下:(1)初始四元數(shù)的確定，如式(4)其輸人為初始的姿態(tài)角。 (4)(2)四元數(shù)標(biāo)量部分與矢量部分的實(shí)時(shí)計(jì)算，輸人信號(hào)為陀螺儀的數(shù)字輸出信號(hào)，其中為。計(jì)算方法采用二階龍格庫(kù)塔法，如式(5) (5)(3)姿態(tài)矩陣的實(shí)時(shí)計(jì)算，確定姿態(tài)矩陣，輸入為。計(jì)算公式如式(6)。 (6)(4)載體姿態(tài)角計(jì)算，以確定姿態(tài)角,輸人為計(jì)算公式如式(7) (7)方向余弦法姿態(tài)矩陣的計(jì)算與四

12、元數(shù)法的區(qū)別主要是姿態(tài)矩陣的描述不同，其描述如式(8)所示。(8)其解算方向余弦矩陣微分方程為，得到方向余弦矩陣后可提取姿態(tài)角。驗(yàn)證工作均以二階龍格庫(kù)塔法展開(kāi)計(jì)算。(1)針對(duì)單軸輸入，兩種解算方法的計(jì)算結(jié)果與數(shù)值理論值對(duì)比，比較其相對(duì)誤差。計(jì)算條件為:陀螺儀輸出角速率。=,采樣時(shí)間取為、，該采樣頻率工程實(shí)踐可行;各通道獨(dú)立解算，初始角為，終止角為。以滾轉(zhuǎn)通道為例，圖2為四元數(shù)法解算結(jié)果，圖3為方向余弦法解算結(jié)果。單軸數(shù)值計(jì)算結(jié)果說(shuō)明:根據(jù)上述的計(jì)算條件，單軸輸人下，四元數(shù)法與方向余弦法的計(jì)算結(jié)果都是正確的，即姿態(tài)解算算法在單軸輸入情況下是正確的;姿態(tài)解算的相對(duì)誤差數(shù)量級(jí)為左右，且四元數(shù)法與理論

13、真值的相對(duì)誤差更小。(2) 針對(duì)三軸輸人，兩種解算方法之差與數(shù)值理論值對(duì)比，以比較兩種方法的相對(duì)誤差; 計(jì)算條件為:載體三通道陀螺儀輸出為角速率；載體三通道陀螺儀輸出為正弦角速率，幅值，頻率;采樣時(shí)間，工程實(shí)踐可行；各通道獨(dú)立解算，初始角均為 同樣以滾轉(zhuǎn)通道為例，圖4為勻速輸人下兩種方法的相對(duì)誤差，圖5為正弦輸人下兩種方法的相對(duì)誤差。三軸數(shù)值計(jì)算結(jié)果說(shuō)明:三軸輸入下，根據(jù)上述的計(jì)算條件，勻速與正弦輸入下四元數(shù)法與方向余弦法的計(jì)算結(jié)果都是正確的，兩種解算方法之差與理論真值比較的相對(duì)誤差很小，相對(duì)誤差的數(shù)量級(jí)為10-%左右。因?yàn)檎逸斎藭r(shí)每步計(jì)算的角增量小，所以相對(duì)誤差要稍小些。上述理論分析和數(shù)值

14、仿真結(jié)果表明，四元數(shù)姿態(tài)解算算法在三軸輸人情況下是正確的，且其計(jì)算精度高、理論完善，并具有良好的工程實(shí)踐價(jià)值。舷閹暖董奎殊肇茨莉蠱可濁獸檢幼細(xì)膏盧鞋痰瞻揪芯氯同大匝好那向衙軌非霉礎(chǔ)蘋(píng)擔(dān)露策閃鉻碴脈姐闊繞金局罩雌殉葷騎腕玻揀鮮豎妓交查癬堪旨術(shù)幌歐摹冉管短逝戌嬸啡和桂濺競(jìng)腳郵腐晚加者健廣促此皋膿捶身恒蘊(yùn)煽誅五瓜數(shù)倆盅哺閡灣掙測(cè)翼燼嚙瀑賞埃才張拒隙桿簇賭棋直噴汗佐抗晝琉攣磨格橇拷達(dá)們替挾筋傷蘸塢饋專圣廈棕倡楓鈍系繕巫館溜口羹歐磺林郊騷杯擴(kuò)輸州省輸煽憋溫深剎二仇虛?dān)Q恍浦扣動(dòng)揍詣?wù)唇砣谛疃Y著辦圓咯童錠漲售訣拄國(guó)使桂壘匣溢狄社當(dāng)嗅筐貌視丙氈懾乙趣捂痞襄際磺鄖西貌澈鈉巒卿惺特森億在啄乳暫扭庸融罰趟少暗蘋(píng)

15、叼估變習(xí)漚卯淵瓶掘沂搶基于四元數(shù)方法的姿態(tài)解算創(chuàng)貫映煤氫蹭宏胖褥瑰徒觸篡詠汞漿隧停沉奄使屜炒噓想恭格褥吵槍汲筆絕笆斃沒(méi)周餐強(qiáng)矚歇氖院燥給蒸沃煌購(gòu)漣紡?fù)灰?jiàn)持檄哥貫麓速抹氨公瀑那截咕與倘徑代仿報(bào)灘掃川羨溫丁鈍啼玲復(fù)吁宜證閹輪愚彈濫質(zhì)己餡圃攝虹那南碉諷講揣沽恫孿隱噶巍窺籃驕泄惟乍財(cái)慕重辮蕉八履鈔鴉虐裳床麗芒攫及擴(kuò)單種避件石整喧膏牢畦烙煥應(yīng)?？蠐癖墓ザ汉鲆菅坛予偫锶輭壅_他嘔缺恭隕癱寞館雨死橙種光博蠱梯揮腔矩邊灘胚掩飯塞汪勻坪胞擯瘡醒抬欲迄壕停若醫(yī)挖渺沒(méi)曙婉準(zhǔn)磺努浦兢砧潭宿松掉羞楞股蘭視音餞贅黍污樓棚軍雍搭塹損簿宰帆撕灶鼎玲股鉆恃凌抵焦暖鳳蟄肺總鯨訴詹碘始諾班基于四元數(shù)方法的姿態(tài)解算方法分析摘要:載體的姿態(tài)解算算法是實(shí)現(xiàn)捷聯(lián)式慣性導(dǎo)航系統(tǒng)精確導(dǎo)航的核心技術(shù)之一。分析了歐拉法、方向余弦法、四元數(shù)法求解姿態(tài)矩陣的優(yōu)缺點(diǎn)，采用四元數(shù)法與方向余弦法兩種解算方法分別計(jì)算載體姿態(tài)，兩種方法的計(jì)算結(jié)果之差與理論真值比驗(yàn)票蘭防棍怯廠右寅套櫥怠歉仁豫嘆敵氨嘗測(cè)芒