第1章概述

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

1.1.1GNC系統(tǒng)含義

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

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

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

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

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

控制：作用在一個(gè)運(yùn)動(dòng)飛行器上，能夠穩(wěn)定和調(diào)節(jié)飛行器的運(yùn)動(dòng)，使飛行器狀態(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）自主交會(huì)、接近操作、對(duì)接和出艙，涉及自主交會(huì)對(duì)接系統(tǒng)敏感器、算法和地面驗(yàn)證測(cè)試平臺(tái)及在軌技術(shù)驗(yàn)證；

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

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

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

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

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

7）飛行器健康管理；

8）飛行全過(guò)程測(cè)試。

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

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

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

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

3）行星取樣返回自主交會(huì)對(duì)接。

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

5）推進(jìn)劑在軌貯存和補(bǔ)加。

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

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

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

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

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

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

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

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

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

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

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

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

5）振動(dòng)/抖動(dòng)主動(dòng)傳感與控制。

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

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

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

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

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

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

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

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

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

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

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

6）微振動(dòng)抑制高精度指向、捕獲和跟蹤；

7）空間組網(wǎng)運(yùn)行星座自主相位保持及快速機(jī)動(dòng)；

8）深空探測(cè)自主導(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)解算、球面三角形公式、卡爾曼濾波和龍格庫(kù)塔積分等，這些數(shù)學(xué)公式是后續(xù)建模和仿真的基礎(chǔ)。

1.2.2仿真基礎(chǔ)

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

1.2.3組合定姿

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

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)對(duì)準(zhǔn)、慣組建模及標(biāo)定、純慣導(dǎo)、GNSS兼容機(jī)與慣組卡爾曼濾波組合導(dǎo)航、基于跟瞄近程相對(duì)導(dǎo)航卡爾曼濾波算法等組合導(dǎo)航算法�？紤]到卡爾曼濾波算法相對(duì)復(fù)雜、物理概念不夠清晰，結(jié)合前期發(fā)表的論文，給出基于PI濾波估計(jì)加表漂移的GNSS兼容機(jī)與加表組合導(dǎo)航、偽距及偽距率與加表組合導(dǎo)航及漂移估計(jì)、基于跟瞄和加表PI濾波近程相對(duì)導(dǎo)航、基于光電測(cè)角相對(duì)導(dǎo)航等算法，以及基于星敏與地平儀的慣性天文組合導(dǎo)航，星光折射敏感器譜寬選擇及探測(cè)能力分析，基于星光折射敏感器和慣組組合導(dǎo)航，基于準(zhǔn)北東地系地面靜態(tài)對(duì)準(zhǔn)等算法，并經(jīng)仿真和在軌驗(yàn)證。組合導(dǎo)航是軌道控制的基礎(chǔ)，地面測(cè)定軌為主逐步向星上自主定軌轉(zhuǎn)變。

1.2.5姿態(tài)控制

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

1.2.6軌道控制

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

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

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

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

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

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

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

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

仿真模型的驗(yàn)證至關(guān)重要，因?yàn)楹芏嗪教焓鹿适怯捎谀Ｐ筒徽�確導(dǎo)致的，本書是在前期型號(hào)研制的基礎(chǔ)上編寫的，在符合工程研制需求的前提下盡量采用精簡(jiǎn)的模型，這些模型在地面經(jīng)過(guò)了大量的復(fù)核，大部分模型已經(jīng)過(guò)在軌考核驗(yàn)證。

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

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

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

（1）理解公式物理概念

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

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

（2）建立基本模型庫(kù)

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

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

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

（3）學(xué)會(huì)基于仿真分析問(wèn)題

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

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

（4）提高解決建模和仿真問(wèn)題能力

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

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

參 考 文 獻(xiàn)

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

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

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

［4］王獻(xiàn)忠,張肖,劉艷.陀螺與磁強(qiáng)計(jì)組合定姿及陀螺漂移估計(jì)［J］.航天控制，2017，34（4）：1519.

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

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

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

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

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

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

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

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

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

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

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

［16］王獻(xiàn)忠,張肖,張國(guó)柱.撓性振蕩在軌自主識(shí)別算法［J］.控制工程，2017，S1：5658.

［17］王獻(xiàn)忠,湯敏蘭,張麗敏.再入返回離軌時(shí)機(jī)及制動(dòng)策略［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)角度，計(jì)算過(guò)程類似。

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é)上計(jì)算如下：

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

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

q=-q0q1q2q3（28）

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

2.2四元數(shù)乘法

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

姿態(tài)從慣性坐標(biāo)系轉(zhuǎn)到軌道坐標(biāo)系，再?gòu)能壍雷鴺?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)動(dòng)后為姿態(tài)四元數(shù)r，姿態(tài)四元數(shù)采用右乘形式

r=p�醧（210）

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

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

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

基于右乘形式推導(dǎo)四元數(shù)乘法公式，四元數(shù)乘法可以看作將3維叉乘擴(kuò)展到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ù)書寫時(shí)一般省去矢量標(biāo)識(shí)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ù)運(yùn)動(dòng)學(xué)推導(dǎo)

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

A=·dt

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

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

其中

=A

推算時(shí)間短，較小，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ù)運(yùn)動(dòng)學(xué)方程如下：

q·=12q��0（225）

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

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

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)機(jī)動(dòng)快捷性出發(fā)，姿態(tài)機(jī)動(dòng)四元數(shù)選擇p00情況，應(yīng)用中需要對(duì)姿態(tài)四元數(shù)進(jìn)行正則化處理，當(dāng)p0＜0時(shí)

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）