月面著陸器INS/CNS深組合導(dǎo)航方法

孫洪馳，穆榮軍，李云天，崔乃剛

(哈爾濱工業(yè)大學(xué)航天工程系，哈爾濱 150001)

0 引 言

在月面著陸器下降過程中，需要快速調(diào)整姿態(tài)，要求導(dǎo)航系統(tǒng)具備高實(shí)時(shí)性、高精度和高可靠性[1-2]。當(dāng)前深空探測任務(wù)中姿態(tài)精度最高的導(dǎo)航方式為天文導(dǎo)航，其姿態(tài)精度可達(dá)角秒級[3-5]。為提高導(dǎo)航系統(tǒng)的可靠性，月面著陸器通常都攜帶慣導(dǎo)與星敏感器，采用慣性/天文組合導(dǎo)航方式進(jìn)行導(dǎo)航解算[6-7]。慣性/天文組合導(dǎo)航系統(tǒng)一般可分為松組合和緊組合兩種模式。松組合模式下慣導(dǎo)和星敏感器各自獨(dú)立地進(jìn)行導(dǎo)航解算，然后將導(dǎo)航誤差作為觀測量進(jìn)行組合[8]；緊組合模式下星敏感器不再進(jìn)行獨(dú)立解算，而是作為測量星點(diǎn)矢量的傳感器使用，根據(jù)星點(diǎn)矢量誤差構(gòu)建量測方程[9]。無論是松組合模式還是緊組合模式，星敏感器均需要采用星圖識別算法進(jìn)行導(dǎo)航星的識別，這一過程往往是天文導(dǎo)航所有流程中耗時(shí)最多的環(huán)節(jié)[10-11]。

針對上述問題本文提出一種慣性/天文深組合模型，能在不進(jìn)行星圖識別的前提下實(shí)現(xiàn)導(dǎo)航解算，有助于提高天文導(dǎo)航算法的計(jì)算效率，降低系統(tǒng)的功耗，減少對硬件資源的占用。同時(shí)在導(dǎo)航星數(shù)目不足3顆時(shí)仍可正常工作，可有效提高導(dǎo)航系統(tǒng)的實(shí)時(shí)性與可靠性。

1 月面著陸器運(yùn)動學(xué)模型

月面著陸器運(yùn)動學(xué)模型包括姿態(tài)微分方程、速度微分方程和位置微分方程。月面著陸器姿態(tài)采用四元數(shù)進(jìn)行描述，姿態(tài)微分方程可以表示為

(1)

(2)

對于月面著陸器，在固連系下的速度ve為

(3)

將速度ve轉(zhuǎn)到導(dǎo)航系中，得

(4)

進(jìn)一步整理，可得

(5)

(6)

式中:fb為體系下的比力；gn為導(dǎo)航系下的月球重力加速度。

記p=[L,λ,h]T，月面著陸器位置微分方程為

(7)

綜上所述，式(1)、式(6)和式(7)共同組成了月面著陸器運(yùn)動學(xué)模型。

2 星敏感器測量模型與慣性/天文松、緊組合模型

2.1 星敏感器測量模型

在星敏感器中，圖像是以像素的形式進(jìn)行采集和存儲的，為了能建立起星點(diǎn)矢量與像素坐標(biāo)之間的關(guān)系，引入視覺導(dǎo)航領(lǐng)域常用的內(nèi)參數(shù)矩陣構(gòu)建星敏感器的測量模型。假設(shè)某導(dǎo)航星sk在星敏感器坐標(biāo)系下的方向矢量為lsk=[X,Y,Z]T，則其在像平面中的坐標(biāo)為

(8)

式中:f為星敏感器焦距。將星點(diǎn)坐標(biāo)轉(zhuǎn)換到像素坐標(biāo)系下，得

(9)

式中:α和β描述了像平面坐標(biāo)和像素坐標(biāo)的縮放關(guān)系，物理含義是單位長度內(nèi)的像素?cái)?shù)目；cx和cy描述了像平面坐標(biāo)和像素坐標(biāo)的平移關(guān)系，單位為像素?cái)?shù)；fx=αf,fy=βf，作用是簡化模型。將式(9)寫成矩陣形式，得

(10)

式中:A即為星敏感器的內(nèi)參數(shù)矩陣，且各項(xiàng)參數(shù)出廠后是固定不變的。l′sk為lsk的歸一化矢量。

2.2 慣性/天文松組合模型

慣性/天文松組合是最常見的組合方式，該模式下慣導(dǎo)與星敏感器各自獨(dú)立工作，利用二者輸出的姿態(tài)之差構(gòu)建觀測量。狀態(tài)方程選取捷聯(lián)慣導(dǎo)系統(tǒng)的誤差傳播方程，具體推導(dǎo)過程參照文獻(xiàn)[12]。

姿態(tài)誤差傳播方程為

(11)

速度誤差傳播方程為

(12)

位置誤差傳播方程為

(13)

(14)

(15)

(16)

記δq=[δq0, (δqv)T]T，δq0和δqv代表δq的標(biāo)量部分和矢量部分。當(dāng)φ為小量時(shí)，令φ=|φ|，有

(17)

由式(17)可以得到觀測量為φ=2δqv。

選取姿態(tài)失準(zhǔn)角、速度誤差、位置誤差、三軸陀螺儀零偏和加速度計(jì)零偏作為狀態(tài)變量，松組合模式下狀態(tài)量、狀態(tài)方程和量測方程為

(18)

(19)

式中:W為陀螺角速度測量白噪聲和加速度計(jì)比力測量白噪聲；V為星敏感器測量白噪聲；F陣各元素取值參照式(11)致式(15)，G陣和H陣各元素取值如下

(20)

2.3 慣性/天文緊組合模型

慣性/天文緊組合模型以星點(diǎn)矢量誤差為觀測量，該模式下星敏感器不進(jìn)行姿態(tài)解算。緊組合狀態(tài)方程仍為慣導(dǎo)系統(tǒng)的誤差傳播方程，僅觀測方程有所不同。在緊組合模型中，星敏感器測量到的星點(diǎn)矢量ls可以表示為

(21)

式中:ln為該星點(diǎn)矢量在導(dǎo)航系下的投影，φ為導(dǎo)航系的姿態(tài)失準(zhǔn)角。

(22)

(23)

當(dāng)視場中存在N顆導(dǎo)航星時(shí)，觀測方程可以表示為

(24)

觀測矩陣H陣各元素取值如下

(25)

3 慣性/天文深組合模型

慣性/天文深組合模型根據(jù)慣導(dǎo)輸出的姿態(tài)，預(yù)測此姿態(tài)下理論上星敏感器應(yīng)觀測到的星圖(記為預(yù)測星圖)，然后利用預(yù)測星圖和實(shí)測星圖的灰度誤差進(jìn)行失準(zhǔn)角的估計(jì)，從而在像素層面構(gòu)建新的觀測模型而非對單個(gè)星點(diǎn)進(jìn)行處理，故深組合時(shí)不需要星圖識別。

3.1 基于李群李代數(shù)理論的灰度誤差函數(shù)

群是一種集合加一種運(yùn)算組成的代數(shù)結(jié)構(gòu)，李群指具有連續(xù)性質(zhì)的群。構(gòu)建灰度誤差函數(shù)需涉及到其中一個(gè)特殊的李群—特殊正交群SO(3)，是由姿態(tài)陣C構(gòu)成的，其數(shù)學(xué)定義為

SO(3)={C∈R3×3|CCT=I,det(C)=1}

(26)

對于每一個(gè)C，都有對應(yīng)的李代數(shù)，SO(3)的李代數(shù)so(3)定義為

so(3)={(φc×)∈R3×3|φc∈R3}

(27)

so(3)反映了SO(3)的導(dǎo)數(shù)性質(zhì)，對姿態(tài)陣C求微分，有

(28)

式中:φc的物理意義是姿態(tài)陣C對應(yīng)的等效旋轉(zhuǎn)矢量，式(28)為關(guān)于C的微分方程，求解后得到

C=exp(φc×)

(29)

圖1 灰度誤差函數(shù)構(gòu)建過程

如圖1所示，I1為星敏感器拍攝星圖，I2為慣導(dǎo)預(yù)測星圖，設(shè)p1為I1中第i個(gè)像素點(diǎn)，滿足

p1=Aps

(30)

(31)

(32)

(33)

ps和p′s可視為同一矢量的真值及慣導(dǎo)預(yù)測值，pn為該矢量在導(dǎo)航系下的投影，滿足

(34)

由式(34)即可得到φ與φs的關(guān)系式為

(35)

3.2 基于阻尼牛頓法的姿態(tài)優(yōu)化算法

圖2 深組合模型基本原理

根據(jù)灰度不變假設(shè)，式(32)可進(jìn)一步寫為

(36)

圖3 某星點(diǎn)窗口內(nèi)灰度誤差優(yōu)化方向

(37)

(38)

接下來分別計(jì)算這3項(xiàng)偏導(dǎo)數(shù)，?I2/??為I2在?處的像素梯度，記?像素坐標(biāo)為?=[ua,ub]T，則像素梯度為

(39)

記ρ=[Xρ,Yρ,Zρ]T，利用式(10)，可以求得??/?ρ的具體形式為

(40)

(41)

根據(jù)BCH公式，有

(42)

(43)

3.3 慣性/天文深組合濾波模型

為提高算法的計(jì)算效率，可針對月面環(huán)境特征對狀態(tài)方程做進(jìn)一步簡化。例如，月球自轉(zhuǎn)速度很小，且月面著陸器下降過程時(shí)間較短，所引起的導(dǎo)航系旋轉(zhuǎn)也很微弱，月面重力較弱，其誤差項(xiàng)δgn對速度誤差的影響也是小量。忽略狀態(tài)方程式(11)-式(15)中的小量后，可得簡化后的狀態(tài)方程為

(44)

量測方程與式(19)一致。狀態(tài)與量測方程都是線性模型，離散化后可采用標(biāo)準(zhǔn)卡爾曼濾波進(jìn)行狀態(tài)估計(jì)。

4 仿真校驗(yàn)

4.1 算法魯棒性驗(yàn)證

利用慣導(dǎo)姿態(tài)預(yù)測星敏光軸指向，估算的星點(diǎn)和實(shí)際觀測星點(diǎn)不一定完全匹配，慣導(dǎo)預(yù)測星圖會出現(xiàn)缺星和多星的現(xiàn)象。但根據(jù)式(33)可知，本文所提出的深組合算法，其優(yōu)化對象為所有導(dǎo)航星堆疊而成的全局灰度誤差，優(yōu)化結(jié)果為最小二乘意義下的最優(yōu)解，故理論上個(gè)別星點(diǎn)的不匹配不會對最終結(jié)果造成太大影響，接下來通過仿真進(jìn)行驗(yàn)證。

采用SAO星表數(shù)據(jù)進(jìn)行仿真驗(yàn)證，歲差、章動采用IAU1980模型進(jìn)行補(bǔ)償，光行差只考慮一階修正項(xiàng)，極移修正量由國際地球自轉(zhuǎn)服務(wù)(International earth rotation service，IERS)提供。星圖不確定度按低(不確定度：17%)、中(不確定度：50%)、高(不確定度：70%)三種情況進(jìn)行仿真驗(yàn)證。

圖4 低、中、高三種不確定度下姿態(tài)誤差曲線

從圖4(c)中可以看出，即使在高不確定度下，慣導(dǎo)僅正確預(yù)測30%的導(dǎo)航星，深組合算法依然收斂，三種工況僅精度略有不同。這說明深組合算法具有較強(qiáng)的魯棒性，慣導(dǎo)僅需要正確預(yù)測到其中的幾顆亮星即可進(jìn)行導(dǎo)航解算。

(45)

式(45)表明，對全部導(dǎo)航星做優(yōu)化等價(jià)于只優(yōu)化正確匹配的星點(diǎn)，不匹配的星點(diǎn)由于灰度誤差為常值，不影響優(yōu)化結(jié)果。

4.2 三種組合方式對比仿真

本文所提出的方法主要針對月面著陸器姿態(tài)調(diào)整段進(jìn)行設(shè)計(jì)，仿真軌跡設(shè)置為姿態(tài)調(diào)整段一條持續(xù)時(shí)間60 s的軌跡。著陸過程中著陸器姿態(tài)變化及星敏感器視場內(nèi)的星圖變化如圖5所示。

采用阻尼牛頓法一般迭代3～4次即可使姿態(tài)達(dá)到收斂。取第1、4次迭代優(yōu)化結(jié)果，對比其圖像灰度誤差，對比結(jié)果如圖6所示。

圖5 仿真工況設(shè)計(jì)

圖6 某星點(diǎn)窗口內(nèi)圖像灰度誤差迭代結(jié)果

接下來對比慣性/天文松、緊、深三種組合模式下的組合導(dǎo)航精度和算法計(jì)算耗時(shí)。緊組合模式只進(jìn)行星圖識別不進(jìn)行QUST解算，故算法耗時(shí)應(yīng)少于松組合。深組合模式則是采用圖像梯度優(yōu)化算法獲得姿態(tài)，一般迭代4次算法已經(jīng)收斂，平均每顆星占據(jù)3×3即9個(gè)像素點(diǎn)，故平均每顆星需要優(yōu)化算法迭代計(jì)算36次；而緊組合的星圖識別算法，采用星表分割后(極限星等+6Mv)平均每個(gè)子星表依舊含有196顆導(dǎo)航星，角距搜索空間大小為196×196/2=19208，即平均每顆星需要進(jìn)行的搜索次數(shù)為19208次，顯然這一過程的計(jì)算量要大于深組合的優(yōu)化算法。三種組合方式的解算精度和算法耗時(shí)統(tǒng)計(jì)仿真結(jié)果如圖7和圖8所示。

圖7 松、緊、深組合導(dǎo)航模式精度對比

圖8 松、緊、深組合導(dǎo)航模式計(jì)算耗時(shí)對比

分析圖中不同階段的三種導(dǎo)航模式精度曲線和計(jì)算耗時(shí)統(tǒng)計(jì)直方圖，可以得到以下分析結(jié)果：

1) 0～20 s內(nèi)導(dǎo)航星充足(≥3顆)，松、緊、深三種組合導(dǎo)航模式的姿態(tài)精度基本處于同一量級；

2) 20～40 s內(nèi)視場中可見導(dǎo)航星降為2顆，星圖識別算法失效，松、緊組合模式導(dǎo)航結(jié)果開始發(fā)散，深組合模式依舊可以進(jìn)行導(dǎo)航解算；

3) 40～60 s時(shí)視場內(nèi)可見導(dǎo)航星數(shù)目恢復(fù)到≥3顆，此時(shí)松、緊組合導(dǎo)航模式迅速收斂，三種導(dǎo)航模式的姿態(tài)精度基本處于同一量級；

4) 俯仰、滾轉(zhuǎn)軸姿態(tài)精度整體上優(yōu)于偏航軸，這是因?yàn)槠捷S與星敏感器光軸重合，星敏感器光軸方向的姿態(tài)精度較其余兩軸差一個(gè)量級；

5) 經(jīng)統(tǒng)計(jì)，松、緊、深三種組合模式的單次計(jì)算耗時(shí)均值分別為0.0074 s、0.0061 s、0.0029 s。深組合模式計(jì)算耗時(shí)最低。

5 結(jié) 論

本文針對月面著陸器設(shè)計(jì)了一種慣性/天文深組合導(dǎo)航方法。仿真結(jié)果表明，在導(dǎo)航星數(shù)目充足的情況下，松、緊、深三種導(dǎo)航方式姿態(tài)精度處于同一量級；在導(dǎo)航星數(shù)目不足時(shí)，松、緊組合方式無法實(shí)現(xiàn)導(dǎo)航星的識別，算法開始發(fā)散，而深組合模式依然可以正常工作；在計(jì)算耗時(shí)方面，深組合模式相較于緊組合時(shí)間縮減50%、相較于松組合時(shí)間縮減60%。深組合模式計(jì)算耗時(shí)短，有利于提高月面著陸器導(dǎo)航實(shí)時(shí)性，降低對硬件資源的占用，同時(shí)可保證在星敏感器視場內(nèi)星點(diǎn)數(shù)目不足時(shí)，導(dǎo)航結(jié)果不發(fā)散。將其應(yīng)用于月面著陸器等實(shí)時(shí)性需求較高的背景中，可以有效提升導(dǎo)航系統(tǒng)的快速響應(yīng)能力并節(jié)約系統(tǒng)功耗，為未來月面著陸器導(dǎo)航系統(tǒng)設(shè)計(jì)提供理論參考。