第1章概述

1.1空間飛行器GNC系統(tǒng)概述

1.1.1GNC系統(tǒng)含義

近地軌道、月球等載人探索和無人科學(xué)任務(wù)，火星等深空探測任務(wù)，都涉及GNC技術(shù)，包括自主交會對接（ARD）、自主精確著陸（APL）、進入下降著陸（EDL）等。GNC系統(tǒng)技術(shù)是空間探測關(guān)鍵需求和未來技術(shù)發(fā)展方向。

空間飛行器GNC系統(tǒng)涵蓋制導(dǎo)、導(dǎo)航與控制，典型的GNC系統(tǒng)框圖如圖11所示，GNC是一個內(nèi)部相互作用和耦合的系統(tǒng)，也是一個具有很強外部耦合作用的集成系統(tǒng)，耦合存在于飛行機械、電子設(shè)備、軟件、推進、飛行科學(xué)、載荷等相關(guān)分系統(tǒng)，也與載人/無人自主飛行操控交互作用。

圖11典型的GNC系統(tǒng)框圖

制導(dǎo)：一個運動的飛行器從當(dāng)前的位置/速度/姿態(tài)到一個期望的位置/速度/姿態(tài)狀態(tài)的路徑?jīng)Q策，同時滿足規(guī)定的制約因素，如推進劑消耗、安全、動態(tài)熱負(fù)荷和時間等約束。

導(dǎo)航：確定一個運動飛行器在指定參考系下的當(dāng)前運動狀態(tài)，包括姿態(tài)、位置、速度等。

控制：作用在一個運動飛行器上，能夠穩(wěn)定和調(diào)節(jié)飛行器的運動，使飛行器狀態(tài)朝向期望導(dǎo)引狀態(tài)的力和力矩控制指令，通常采用閉環(huán)控制方式。

1.1.2載人空間探索領(lǐng)域GNC

GNC系統(tǒng)是確保飛行器在上升段、逃逸段、在軌和再入任務(wù)階段可靠性和魯棒性的關(guān)鍵分系統(tǒng)，以支持航天員安全和任務(wù)成功為目標(biāo)，GNC系統(tǒng)相關(guān)技術(shù)包括：

1）自主交會、接近操作、對接和出艙，涉及自主交會對接系統(tǒng)敏感器、算法和地面驗證測試平臺及在軌技術(shù)驗證；

2）發(fā)射中斷逃逸系統(tǒng)穩(wěn)定性和控制；

3）柔性/撓性火箭魯棒自適應(yīng)控制；

4）載人飛行器高性能控制；

5）自主精確著陸和障礙規(guī)避；

6）模塊化實時飛行軟件架構(gòu)的開發(fā)、集成、地面和飛行測試，確保軟件執(zhí)行的安全性和運行的可靠性；

7）飛行器健康管理；

8）飛行全過程測試。

1.1.3空間科學(xué)探測領(lǐng)域GNC

把人類的探索范圍延伸到太空，通過地球軌道和深空放置天文臺，飛行器探訪月球和行星，探測器著陸、環(huán)繞和采樣返回等，飛行器可以位于低地球軌道、地球靜止軌道、高地球軌道、地月轉(zhuǎn)移軌道、繞月軌道、地球行星轉(zhuǎn)移軌道、繞行星軌道、拉格朗日點等，需要GNC具備高容錯性和高可靠性，相關(guān)的GNC系統(tǒng)技術(shù)包括：

1）天文級精密望遠(yuǎn)鏡捕獲、跟蹤和指向控制。哈勃望遠(yuǎn)鏡指向精度要求為0.007，撓性振蕩對韋伯望遠(yuǎn)鏡影響更大，但韋伯望遠(yuǎn)鏡指向精度約為哈勃望遠(yuǎn)鏡的1/18，韋伯望遠(yuǎn)鏡的指向精度比哈勃望遠(yuǎn)鏡更高。

2）行星大氣層減速，大氣捕獲，進入、下降和著陸控制，包括行星探測的定點著陸和自主障礙規(guī)避。

3）行星取樣返回自主交會對接。

4）多飛行器精確編隊飛行。涉及編隊飛行敏感器和執(zhí)行機構(gòu)，編隊飛行任務(wù)設(shè)計工具，地面驗證測試平臺和在軌技術(shù)演示驗證。

5）推進劑在軌貯存和補加。

6）用于測量行星結(jié)構(gòu)和自然資源的超高精度引力敏感器。

7）優(yōu)于10 cm精度的激光深空測距系統(tǒng)。光學(xué)跟蹤技術(shù)將用于深空探測網(wǎng)，以實現(xiàn)更高精度的三維測量。

8）先進的控制系統(tǒng)技術(shù)。研制安全可靠的新一代無人飛行控制系統(tǒng)，集成了先進的通信和指控技術(shù)，擴展了在極具挑戰(zhàn)性的太空環(huán)境中的操作能力。

9）高度集成的自主故障管理多功能模塊化GNC，可用于深空和近地探測。

10）小型化、低功耗GNC敏感器，用于提高載人和無人飛行器的安全性和可靠性，并可用于微小衛(wèi)星/納米衛(wèi)星。微技術(shù)和納米技術(shù)應(yīng)用于超低功耗電子產(chǎn)品，有助于減小飛行器的重量和體積。

11）可靠的長壽命執(zhí)行機構(gòu)，包括高靈敏度的控制力矩陀螺、先進的電推等。

1.1.4天文觀測和激光通信領(lǐng)域GNC

隨著對望遠(yuǎn)鏡指向控制和抖動抑制要求越來越高，高速激光通信需要光束捕獲、指向和穩(wěn)定跟蹤控制，指向傳感與控制相關(guān)的GNC系統(tǒng)技術(shù)如下：

1）高分辨率、低抖動導(dǎo)航敏感器，包括高精度相對導(dǎo)航敏感器和慣組。

2）高分辨率和高動態(tài)執(zhí)行機構(gòu)。

3）影像穩(wěn)定和防抖控制，包括慣性偽星參考穩(wěn)像技術(shù)，圖像匹配識別技術(shù)等。

4）波前傳感與控制，包括先進的光學(xué)波前傳感器、快速轉(zhuǎn)向反射鏡、高帶寬驅(qū)動、光路長度延遲線控制等。

5）振動/抖動主動傳感與控制。

6）多變量自適應(yīng)控制和自主重構(gòu)。開發(fā)自適應(yīng)飛行控制系統(tǒng)，根據(jù)操作環(huán)境的變化，對飛行器進行指令重新配置。

7）多學(xué)科優(yōu)化設(shè)計。

8）端到端集成的動態(tài)建模技術(shù)。研究光學(xué)、結(jié)構(gòu)、散熱、電源等相關(guān)系統(tǒng)之間的相互作用。

9）由傳感器、算法和執(zhí)行機構(gòu)組成的超精密抗干擾系統(tǒng)。

1.1.5GNC系統(tǒng)發(fā)展趨勢

隨著微電子和微機械應(yīng)用，星敏感器、跟瞄相機、慣組、太陽敏感器、反作用飛輪、動量輪、減振平臺、快反鏡、力矩陀螺、焦平面陣列、制冷機、推力器等空間飛行器GNC系統(tǒng)敏感器和執(zhí)行機構(gòu)不斷小型化。隨著集成芯片技術(shù)的發(fā)展，控制器計算速度不斷提高，未來GNC系統(tǒng)可能的發(fā)展方向如下：

1）編隊飛行和自主交會對接敏感器及執(zhí)行機構(gòu)小型化；

2）無人空間飛行器自適應(yīng)飛行控制；

3）抗干擾高低精度敏感器組合定姿和組合導(dǎo)航；

4）低功耗、輕質(zhì)、小型化、高可靠敏感器；

5）力矩陀螺、飛輪等高可靠、長壽命執(zhí)行機構(gòu)；

6）微振動抑制高精度指向、捕獲和跟蹤；

7）空間組網(wǎng)運行星座自主相位保持及快速機動；

8）深空探測自主導(dǎo)航和自主軌控。

1.2主要內(nèi)容

1.2.1數(shù)學(xué)基礎(chǔ)

GNC系統(tǒng)數(shù)學(xué)建模需要一定的數(shù)學(xué)基礎(chǔ)，主要包括坐標(biāo)系轉(zhuǎn)換、四元數(shù)乘法、不同轉(zhuǎn)序姿態(tài)解算、球面三角形公式、卡爾曼濾波和龍格庫塔積分等，這些數(shù)學(xué)公式是后續(xù)建模和仿真的基礎(chǔ)。

1.2.2仿真基礎(chǔ)

GNC系統(tǒng)數(shù)學(xué)建模也需要一定的工程基礎(chǔ)，主要包括姿態(tài)和軌道簡易動力學(xué)、章動角和章動角速率、格林尼治恒星時計算、微波和光學(xué)基本公式、單/雙目視覺、大氣密度、軌道參數(shù)轉(zhuǎn)換、日月軌道及日月指向等算法�？紤]到超實時仿真對動力學(xué)快速迭代推算的需求，結(jié)合前期發(fā)表的論文，給出撓性動力學(xué)模型解耦及局部迭代方法、撓性和晃動動力學(xué)模型解耦及局部迭代等算法。姿態(tài)和軌道動力學(xué)及運動學(xué)是姿態(tài)和軌道確定與控制的基礎(chǔ)，也是數(shù)學(xué)仿真的基礎(chǔ)。

1.2.3組合定姿

組合定姿包括常規(guī)的陀螺漂移與姿態(tài)四元數(shù)偏差關(guān)系、地磁場模型、基于卡爾曼濾波磁強計與陀螺組合定姿、雙矢量和多矢量定姿等姿態(tài)確定算法�？紤]到卡爾曼濾波算法相對復(fù)雜、物理概念不夠清晰，結(jié)合前期發(fā)表的論文，給出基于PI濾波陀螺與磁強計組合定姿，陀螺與星敏感器組合定姿及陀螺漂移估計，地平儀與陀螺軌道羅盤組合定姿，以及星敏外場觀星姿態(tài)及極性測試算法，并經(jīng)仿真和在軌驗證。組合定姿是GNC系統(tǒng)的關(guān)鍵，沒有姿態(tài)基準(zhǔn)，姿態(tài)控制、導(dǎo)航和軌控都將無法進行。

1.2.4組合導(dǎo)航

組合導(dǎo)航包括常規(guī)的軌道參數(shù)轉(zhuǎn)換、軌道平根與瞬根相互轉(zhuǎn)換、WGS84地固系與J2000慣性系相互轉(zhuǎn)換、WGS84位置/速度與經(jīng)緯高及地平系速度相互轉(zhuǎn)換、軌道根數(shù)與J2000慣性系位置/速度相互轉(zhuǎn)換、基于最小二乘地面靜態(tài)對準(zhǔn)、慣組建模及標(biāo)定、純慣導(dǎo)、GNSS兼容機與慣組卡爾曼濾波組合導(dǎo)航、基于跟瞄近程相對導(dǎo)航卡爾曼濾波算法等組合導(dǎo)航算法�？紤]到卡爾曼濾波算法相對復(fù)雜、物理概念不夠清晰，結(jié)合前期發(fā)表的論文，給出基于PI濾波估計加表漂移的GNSS兼容機與加表組合導(dǎo)航、偽距及偽距率與加表組合導(dǎo)航及漂移估計、基于跟瞄和加表PI濾波近程相對導(dǎo)航、基于光電測角相對導(dǎo)航等算法，以及基于星敏與地平儀的慣性天文組合導(dǎo)航，星光折射敏感器譜寬選擇及探測能力分析，基于星光折射敏感器和慣組組合導(dǎo)航，基于準(zhǔn)北東地系地面靜態(tài)對準(zhǔn)等算法，并經(jīng)仿真和在軌驗證。組合導(dǎo)航是軌道控制的基礎(chǔ)，地面測定軌為主逐步向星上自主定軌轉(zhuǎn)變。

1.2.5姿態(tài)控制

姿態(tài)控制也是GNC系統(tǒng)設(shè)計和研制的重要內(nèi)容之一，主要包括撓性動力學(xué)及模態(tài)增益、常規(guī)的干擾力矩分析、飛輪加磁穩(wěn)定控制、磁消旋及磁速率阻尼控制、力矩陀螺控制、六支腿Stewart減振平臺動力學(xué)、基于姿態(tài)偏差四元數(shù)姿態(tài)機動等姿控算法�？紤]到干擾力矩補償和部件故障時姿態(tài)控制的可靠性和安全性，結(jié)合前期發(fā)表的論文，給出低軌偏置動量衛(wèi)星氣動干擾力矩補償控制、近地零動量衛(wèi)星干擾力矩飛輪補償控制、非偏置動量單飛輪加磁控制、異軌交會過程高精度指向跟蹤控制、微振動抑制和高精度指向控制等算法，經(jīng)仿真和在軌驗證的穩(wěn)定平臺是軌控的基礎(chǔ)。

1.2.6軌道控制

軌道控制研究包括常規(guī)的地球非球形攝動分析、太陽同步等典型軌道分析、改進春分點根數(shù)轉(zhuǎn)換、軌道面內(nèi)軌控、軌道面外軌控、蘭伯特軌道轉(zhuǎn)移等軌控相關(guān)算法，考慮到載荷對軌道機動要求不斷提高，結(jié)合前期發(fā)表的論文，給出遠(yuǎn)程快速軌道機動、再入返回離軌時機及制動策略、基于再入誤差預(yù)測精確返回控制、近程自主伴飛及防撞控制等算法，并經(jīng)仿真和在軌驗證。隨著飛行器組網(wǎng)和編隊需求的增強，軌道機動控制越來越重要。

1.2.7深空探測GNC系統(tǒng)

深空探測越來越受到重視，結(jié)合火星探測研制經(jīng)驗，給出深空探測不同坐標(biāo)系及軌道轉(zhuǎn)換、行星軌道推算、行星探測器對地指向、火星時、火影、對火中繼、引力影響球、近火捕獲和再入制動策略、火星表面靜態(tài)對準(zhǔn)，以及小行星探測軌道轉(zhuǎn)移策略等GNC系統(tǒng)相關(guān)算法及仿真驗證結(jié)果。

1.3GNC系統(tǒng)建模及仿真

GNC系統(tǒng)工程研制涉及大量的模型及仿真，模型包括敏感器模型、姿態(tài)動力學(xué)模型、軌道動力學(xué)模型、執(zhí)行機構(gòu)模型等，仿真包括超實時數(shù)學(xué)仿真、實時仿真、半實物仿真和全物理仿真等。

航天早期研制過程由于缺乏經(jīng)驗和計算機技術(shù)限制，較多采用半實物仿真和全物理仿真，隨著計算機技術(shù)的發(fā)展，數(shù)學(xué)仿真和實時仿真的重要性越來越突出，基于數(shù)字單機的虛擬仿真在研制過程中的作用正在逐步增強。

GNC系統(tǒng)數(shù)學(xué)建模及仿真一方面有助于熟悉和了解GNC系統(tǒng)研制理論和特點，另一方面為后續(xù)進入工程研制打下建模和仿真基礎(chǔ)。

從工程應(yīng)用角度，在滿足研制需求的前提下GNC系統(tǒng)模型越簡單越好，精簡的模型不但有利于提高仿真效率，還便于分析和查找問題，模型的精簡需要一定的工程經(jīng)驗，特別是參數(shù)選取。

仿真模型的驗證至關(guān)重要，因為很多航天事故是由于模型不正確導(dǎo)致的，本書是在前期型號研制的基礎(chǔ)上編寫的，在符合工程研制需求的前提下盡量采用精簡的模型，這些模型在地面經(jīng)過了大量的復(fù)核，大部分模型已經(jīng)過在軌考核驗證。

本書提供了適合于GNC系統(tǒng)設(shè)計與研制的工程應(yīng)用算法，并經(jīng)數(shù)學(xué)仿真和在軌驗證。早期GNC系統(tǒng)設(shè)計與研制主要依靠物理仿真驗證，隨著計算機技術(shù)的發(fā)展和飛行器動力學(xué)及運動學(xué)建模準(zhǔn)確性的提高，GNC系統(tǒng)設(shè)計與研制越來越依靠數(shù)學(xué)仿真驗證，特別是超實時仿真，不但能加快研制速度，而且也能夠大幅降低研制成本。

1.4GNC系統(tǒng)研制關(guān)注重點

GNC系統(tǒng)研制關(guān)注重點如下：

（1）理解公式物理概念

姿態(tài)方面包括坐標(biāo)轉(zhuǎn)換、歐拉軸角與四元數(shù)關(guān)系、四元數(shù)非唯一性、偏差四元數(shù)與陀螺角速率關(guān)系、雙矢量定姿、剛體動力學(xué)、動不平衡力矩、靜不平衡力矩、磁場與磁矩、撓性動力學(xué)、模態(tài)增益、基頻與剛度關(guān)系等物理概念。

軌道方面包括質(zhì)點軌道動力學(xué)、力學(xué)時與世界時、跳秒、單/雙目視覺、跟瞄視線距和視線角、相對位置和相對姿態(tài)確定、星光折射、星等、天線、軌道六要素、太陽同步軌道、回歸軌道、凍結(jié)軌道等物理概念。

（2）建立基本模型庫

姿態(tài)模塊包括四元數(shù)乘法、龍格庫塔積分、四元數(shù)求312轉(zhuǎn)序歐拉角、四元數(shù)求321轉(zhuǎn)序歐拉角、基于四元數(shù)求轉(zhuǎn)換矩陣、基于312轉(zhuǎn)序歐拉角求轉(zhuǎn)換矩陣、基于321轉(zhuǎn)序歐拉角求轉(zhuǎn)換矩陣、慣組、飛輪、地平儀、磁場表等單機建模等。

軌道模塊包括地球非球形J2至J4項攝動、太陽章動和歲差參數(shù)、儒略日和儒略世紀(jì)數(shù)計算、格林尼治真恒星時計算、日月指向簡單計算、平流層和軌道空間大氣密度計算、軌道根數(shù)不同表示形式相互轉(zhuǎn)換、近點角MEf相互轉(zhuǎn)換、軌道根數(shù)與J2000慣性系位置/速度相互轉(zhuǎn)換、WGS84坐標(biāo)系與地平坐標(biāo)系相互轉(zhuǎn)換、軌道平根與瞬根相互轉(zhuǎn)換、軌道交點周期、再入點縱程與橫程計算等。

建模的關(guān)鍵是模型要正確，基于標(biāo)準(zhǔn)數(shù)據(jù)校驗?zāi)Ｐ驼�確性，如WGS84坐標(biāo)系與J2000慣性系位置/速度轉(zhuǎn)換、軌道平根與瞬根相互轉(zhuǎn)換等。

（3）學(xué)會基于仿真分析問題

姿態(tài)方面包括外場觀星數(shù)據(jù)正確性判別、基于地平儀軌道羅盤組合定姿仿真、基于磁強計與陀螺卡爾曼濾波組合定姿仿真、多矢量定姿、干擾力矩建模及對姿態(tài)影響仿真分析、零動量加磁卸載控制、偏置動量加磁章進動控制、雙自由度偏置動量加磁卸載控制、非偏置動量單飛輪加磁控制、基于偏差四元數(shù)姿態(tài)機動控制等。

軌道方面包括攝動力建模及對軌道影響仿真分析、靜態(tài)粗對準(zhǔn)和精對準(zhǔn)、兼容機輸出位置/速度與慣性組合導(dǎo)航、基于跟瞄輸出近程相對導(dǎo)航、小偏心率軌道推算、大偏心率軌道推算、軌道面內(nèi)控制、軌道面外控制、升交點赤經(jīng)漂移控制、再入點跡向與法向誤差分析等。

（4）提高解決建模和仿真問題能力

復(fù)雜的建模與仿真包括高階地球非球形攝動模型與STK誤差比對、基于地固系慣導(dǎo)與基于J2000慣性系慣導(dǎo)比對、地面重力加速度與引力和離心力關(guān)系、兼容機輸出偽距與慣性組合導(dǎo)航等，通過這些復(fù)雜建模和仿真提高解決問題的能力。

具有飛行器姿態(tài)和軌道控制基礎(chǔ)，且后續(xù)還將從事飛行器GNC系統(tǒng)設(shè)計與研制的讀者，可以結(jié)合習(xí)題仿真驗證模型，為后續(xù)從事該專業(yè)工作奠定基礎(chǔ)。

參 考 文 獻

［1］
王獻忠,湯敏蘭,董晉芳.一種撓性動力學(xué)模型解耦及局部迭代方法［J］.系統(tǒng)仿真學(xué)報，2015，27（6）：12041208.

［2］王獻忠,張肖.撓性和晃動動力學(xué)模型解耦及局部迭代算法［J］.航天控制，2021，38（6）：913，33.

［3］王獻忠.陀螺與磁強計組合定姿及工程應(yīng)用分析［J］.航天控制，2009，26（5）：4347.

［4］王獻忠,張肖,劉艷.陀螺與磁強計組合定姿及陀螺漂移估計［J］.航天控制，2017，34（4）：1519.

［5］王獻忠,張肖,劉艷.陀螺與磁強計組合定姿及漂移估計算法優(yōu)化［J］.航天控制，2020，37（4）：913.

［6］王獻忠,張肖.陀螺與星敏感器組合定姿及陀螺漂移估計［J］.航天控制，2018，35（4）：711.

［7］王獻忠,張肖,張麗敏，等.基于PI濾波估計加表漂移的GNSS兼容機與加表組合導(dǎo)航［J］.航天控制，2018，35（6）：3641.

［8］王獻忠,張肖,張麗敏，等.基于跟瞄和加表PI濾波近程相對導(dǎo)航方法［J］.上海航天，2019，36（1）：4347.

［9］王獻忠,張肖,張麗敏.基于地平儀的慣性天文組合導(dǎo)航［J］.導(dǎo)航定位學(xué)報，2016，22（3）：2630.

［10］王獻忠,張肖,張麗敏，等.偽距及偽距率與加表組合導(dǎo)航及漂移估計［J］.航天控制，2020，37（6）：38.

［11］王獻忠,劉禹,湯敏蘭，等.星光折射敏感器譜寬選擇及探測能力分析［J］.系統(tǒng)仿真學(xué)報，2018，30（2）：722730.

［12］王獻忠,張肖.基于準(zhǔn)北東地系地面靜態(tài)對準(zhǔn)算法［J］.航天控制，2022年，39（1）：3741.

［13］王獻忠,張肖,張麗敏.低軌偏置動量衛(wèi)星氣動干擾力矩補償控制研究［J］.上海航天，2017，34（2）：7478.

［14］王獻忠,張肖,張麗敏.近地零動量衛(wèi)星干擾力矩飛輪補償控制［J］.航天控制，2016，33（2）：6065.

［15］王獻忠,張肖.一種非偏置動量單飛輪加磁控制算法［J］.空間控制技術(shù)與應(yīng)用，2014，40（6）：4852.

［16］王獻忠,張肖,張國柱.撓性振蕩在軌自主識別算法［J］.控制工程，2017，S1：5658.

［17］王獻忠,湯敏蘭,張麗敏.再入返回離軌時機及制動策略［J］.航天控制，2016，33（4）：5358.

第2章數(shù)學(xué)基礎(chǔ)

第2章數(shù)學(xué)基礎(chǔ)

2.1坐標(biāo)系旋轉(zhuǎn)及歐拉軸角

2.1.1坐標(biāo)系旋轉(zhuǎn)

分別繞三軸旋轉(zhuǎn)角度的轉(zhuǎn)換矩陣如下：

Rx（）=1000cossin0-sincos（21）

Ry（）=cos0-sin010sin0cos（22）

Rz（）=cossin0-sincos0001（23）

設(shè)坐標(biāo)系繞X軸旋轉(zhuǎn)角度，原坐標(biāo)系中的矢量r在新坐標(biāo)系中的矢量rn為

rn=Rx（）·r=1000cossin0-sincos·r（24）

新坐標(biāo)系中的矢量rn在原坐標(biāo)系中的矢量r為

r=Rx（）T·rn=1000cos-sin0sincos·rn（25）

坐標(biāo)系繞Y軸和Z軸旋轉(zhuǎn)角度，計算過程類似。

2.1.2歐拉軸角

以歐拉軸角定義的姿態(tài)四元數(shù)如下：

q=q0q1q2q3=cos2exsin2eysin2ezsin2（26）

式中
e旋轉(zhuǎn)方向矢量,e=exeyezT;

繞旋轉(zhuǎn)軸旋轉(zhuǎn)角度，以歐拉軸角定義的姿態(tài)四元數(shù)具有清晰的姿態(tài)角含義。

繞e旋轉(zhuǎn)角度和繞-e旋轉(zhuǎn)角度2-得到的姿態(tài)是相同的，如圖21所示。

圖21姿態(tài)不同旋轉(zhuǎn)方向

··

··

從數(shù)學(xué)上計算如下：

q=cos2-2exsin2-2eysin2-2ezsin2-2=-cos2exsin2eysin2ezsin2=-q0q1q2q3=-q（27）

說明四元數(shù)非唯一性，一般對q0＜0進行正則化處理如下：

q=-q0q1q2q3（28）

正則化處理后的姿態(tài)四元數(shù)用于姿態(tài)機動轉(zhuǎn)動角度q0＜0情況，有利于快速姿態(tài)機動。

2.2四元數(shù)乘法

2.2.1四元數(shù)乘法形式

姿態(tài)從慣性坐標(biāo)系轉(zhuǎn)到軌道坐標(biāo)系，再從軌道坐標(biāo)系轉(zhuǎn)到星體坐標(biāo)系，姿態(tài)轉(zhuǎn)移矩陣采用左乘形式

Abi=Abo·Aoi（29）

姿態(tài)四元數(shù)乘法采用右乘形式，如圖22所示，初始姿態(tài)四元數(shù)p經(jīng)四元數(shù)q轉(zhuǎn)動后為姿態(tài)四元數(shù)r，姿態(tài)四元數(shù)采用右乘形式

r=p�醧（210）

圖22姿態(tài)旋轉(zhuǎn)

姿態(tài)轉(zhuǎn)換矩陣采用左乘形式比較直觀，姿態(tài)四元數(shù)乘法采用右乘形式比較直觀，應(yīng)用時姿態(tài)轉(zhuǎn)換矩陣一般采用左乘形式，姿態(tài)四元數(shù)乘法一般采用右乘形式。

2.2.2四元數(shù)乘法推導(dǎo)

基于右乘形式推導(dǎo)四元數(shù)乘法公式，四元數(shù)乘法可以看作將3維叉乘擴展到4維空間，令

p=p0Pi，P=p1p2p3（211）

q=q0Qi，Q=q1q2q3（212）

求得右乘形式四元數(shù)乘法公式為

p�醧=p0Pi�醧0Qi=p0·q0-P·Q（p0·Q q0·P PQ）i（213）

四元數(shù)書寫時一般省去矢量標(biāo)識i，得到四元數(shù)乘法如下

p�醧=p0P�醧0Q=p0·q0-P·Qp0·Q q0·P PQ

=p0-PTP［P］ p0·I33·q0Q

=q0-QTQ-［Q］ q0·I33·p0P（214）

其中

［Q］=0-q3q2q30-q1-q2q10

［P］=0-p3p2p30-p1-p2p10

同理求得

q�醦=q0Q�醦0P=q0·p0-Q·Pq0·P p0·Q QP

=q0-QTQ［Q］ q0·I33·p0P

=p0-PTP-［P］ p0·I33·q0Q（215）

3維叉乘QP=-PQ，由式（214）和式（215）得

q�醦=p�醧 20013031［Q］·p0P（216）

2.2.3姿態(tài)四元數(shù)運動學(xué)推導(dǎo)

設(shè)姿態(tài)角速率為，姿態(tài)推算時間為dt，則姿態(tài)角變化A為

A=·dt

姿態(tài)變化四元數(shù)qe為

qe=cos（/2）sin（/2）A（217）

其中

=A

推算時間短，較小，sin（/2）/2，姿態(tài)四元數(shù)qe近似為

qe1·dt/2（218）

令初始姿態(tài)四元數(shù)q為

q=q0Q（219）

求得新姿態(tài)四元數(shù)qn為

qn=q�醧e=q0Q��1·dt/2（220）

即

qn=q0-Q··dt/2Q q0·dt/2 Q·dt/2（221）

求得dq為

dq=qn-q=-Q··dt/2q0·dt/2 Q·dt/2（222）

求得q·為

q·=dqdt=-Q·/2q0/2 Q/2（223）

應(yīng)用四元數(shù)乘法形式把式（223）寫成

q·=12q0Q��0（224）

其中，0不是真正四元數(shù)，但滿足四元數(shù)乘法規(guī)則。

得到姿態(tài)四元數(shù)運動學(xué)方程如下：

q·=12q��0（225）

2.2.4姿態(tài)四元數(shù)應(yīng)用處理

實際應(yīng)用中要對姿態(tài)四元數(shù)進行歸一化處理，即

p=p0p1p2p3=1p20 p21 p22 p23·p0p1p2p3（226）

姿態(tài)四元數(shù)寫成歐拉軸角形式為

p=cos2sin2·e（227）

其中

|e|=1

繞歐拉軸e旋轉(zhuǎn)角與繞矢量軸-e旋轉(zhuǎn)2-結(jié)果是一樣的，姿態(tài)四元數(shù)不具有唯一性，即

p=p0P與p=-p0P=-p0-P

是等效的。

從姿態(tài)機動快捷性出發(fā)，姿態(tài)機動四元數(shù)選擇p00情況，應(yīng)用中需要對姿態(tài)四元數(shù)進行正則化處理，當(dāng)p0＜0時

p=|p0|sign（p0）·P（228）

姿態(tài)四元數(shù)的共軛四元數(shù)為

p*=p0-P（229）

2.3姿態(tài)四元數(shù)轉(zhuǎn)為姿態(tài)角

2.3.1姿態(tài)轉(zhuǎn)換矩陣

姿態(tài)四元數(shù)表示的姿態(tài)轉(zhuǎn)換矩陣如下：

A=q20 q21-q22-q232q0q3 2q1q2-2q0q2 2q1q3-2q0q3 2q1q2q20-q21 q22-q232q0q1 2q2q32q0q2 2q1q3-2q0q1 2q2q3q20-q21-q22 q23（230）

令

A=a11a12a13a21a22a23a31a32a33（231）

2.3.2姿態(tài)四元數(shù)按312轉(zhuǎn)序轉(zhuǎn)為姿態(tài)角

姿態(tài)按312轉(zhuǎn)序表示的姿態(tài)轉(zhuǎn)換矩陣如下：

A=cos0-sin010sin0cos·1000cossin0-sincos·cossin0-sincos0001（232）

A=cos0-sin010sin0cos·cossin0-cossincoscossinsinsin-sincoscos（233）

A=coscos-sinsinsincossin sinsincos-cossin-cossincoscossinsincos sincossinsinsin-sincoscoscoscos（234）

姿態(tài)四元數(shù)按312轉(zhuǎn)序求三軸姿態(tài)角，如果|a23|0.999 9，則

sin=a23，=arcsin（a23）（235）

tan=-a13/cosa33/cos，=arctan2-a13/cosa33/cos（236）

tan=-a21/cosa22/cos，=arctan2-a21/cosa22/cos（237）

否則

=arcsin（a23）（238）

=0（239）

=arctan2a12a11（240）

2.3.3姿態(tài)四元數(shù)按313轉(zhuǎn)序轉(zhuǎn)為姿態(tài)角

姿態(tài)按313轉(zhuǎn)序表示的姿態(tài)轉(zhuǎn)換矩陣如下：

A=cossin0-sincos0001·1000cossin0-sincos·cossin0-sincos0001（241）

A=cossin0-sincos0001·cossin0-cossincoscossinsinsin-sincoscos（242）

A=coscos-sincossincossin sincoscossinsin-sincos-coscossin-sinsin coscoscoscossinsinsin-sincoscos（243）

姿態(tài)四元數(shù)按313轉(zhuǎn)序求三軸姿態(tài)角，如果|a23|0.999 9，則

cos=a33，=arccos（a33）（244）

tan=a31/sin-a32/sin，=arctan2a31/sin-a32/sin（245）

tan=a13/sina23/sin，=arctan2a13/sina23/sin（246）

否則

=arctan2a12a11（247）

=arccos（a33）（248）

=0（249）

2.3.4姿態(tài)四元數(shù)按321轉(zhuǎn)序轉(zhuǎn)為姿態(tài)角

姿態(tài)按321轉(zhuǎn)序表示的姿態(tài)轉(zhuǎn)換矩陣如下：

A=1000cossin0-sincos·cos0-sin010sin0cos·cossin0-sincos0001（250）

A=coscoscossin-sinsinsincos-cossincoscos sinsinsinsincoscossincos sinsincossinsin-sincoscoscos（251）