一種平面約束輔助測(cè)量的深空探測(cè)器自主天文導(dǎo)航方法

馬 辛，寧曉琳，劉 勁，劉 剛

（1.北京航空航天大學(xué)前沿科學(xué)技術(shù)創(chuàng)新研究院，北京100191；2.北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191；3.武漢科技大學(xué)信息科學(xué)與工程學(xué)院，武漢430081）

引 言

21 世紀(jì)初，世界主要航天集團(tuán)都提出了未來(lái)的深空探測(cè)計(jì)劃。我國(guó)的火星探測(cè)計(jì)劃也已立項(xiàng)，即將于2020 年開始火星探測(cè)。自主天文導(dǎo)航已成為深空探測(cè)的關(guān)鍵技術(shù)。自主天文導(dǎo)航系統(tǒng)需要在同一時(shí)刻觀測(cè)至少兩個(gè)導(dǎo)航天體，通過(guò)兩個(gè)天體的位置信息確定探測(cè)器的位置和速度[1-5]。導(dǎo)航系統(tǒng)測(cè)量誤差是制約深空探測(cè)自主天文導(dǎo)航精度的重要影響因素，因此，有效抑制測(cè)量誤差是深空探測(cè)器高精度自主導(dǎo)航的主要問(wèn)題。

在實(shí)際深空任務(wù)中，深空探測(cè)器自主天文導(dǎo)航系統(tǒng)滿足一定的內(nèi)在系統(tǒng)約束。這些約束是實(shí)際復(fù)雜物理系統(tǒng)內(nèi)在的固有特性，對(duì)這些內(nèi)在約束建模將提高系統(tǒng)估計(jì)性能[8]。系統(tǒng)約束特性的建模與優(yōu)化已經(jīng)應(yīng)用于多個(gè)領(lǐng)域，如立體視覺(jué)匹配[9]、無(wú)源測(cè)向定位系統(tǒng)[10-11]等。已有深空探測(cè)器自主天文導(dǎo)航方法都尚未考慮導(dǎo)航系統(tǒng)量測(cè)量所滿足的多種約束條件，更未對(duì)約束條件建模優(yōu)化，導(dǎo)致系統(tǒng)模型中信息相互孤立，未能完全利用所含信息。因此需要對(duì)深空探測(cè)器自主天文導(dǎo)航系統(tǒng)量測(cè)量的平面約束條件進(jìn)行建模，并針對(duì)約束模型，利用約束模型規(guī)劃算法，輔助優(yōu)化測(cè)量信息，進(jìn)而可實(shí)現(xiàn)深空探測(cè)器高精度自主天文導(dǎo)航。

由于平面約束模型為非線性等式和不等式約束模型，因此需要采用非線性規(guī)劃方法，對(duì)模型中的測(cè)量信息進(jìn)行優(yōu)化。目前已有的非線性規(guī)劃方法包括：罰函數(shù)法（內(nèi)點(diǎn)、外點(diǎn)、混合）、信賴域反射法、有效集法、序列二次規(guī)劃（Sequential Quadratic Program‐ming，SQP）方法等。罰函數(shù)內(nèi)點(diǎn)法的基本思想是當(dāng)?shù)c(diǎn)靠近可行域邊界時(shí)，目標(biāo)函數(shù)值驟然增大，阻止迭代點(diǎn)穿越邊界。信賴域法的基本思想是給定一個(gè)信賴域半徑，以此半徑作為位移長(zhǎng)度的上界，并以當(dāng)前迭代點(diǎn)為中心以此上界為半徑確定信賴域區(qū)域，通過(guò)求解這個(gè)區(qū)域內(nèi)目標(biāo)函數(shù)二次近似模型的最優(yōu)點(diǎn)來(lái)確定候選位移，若候選位移能使目標(biāo)函數(shù)值有充分的下降量，則接受該候選位移作為新的位移，并保持或擴(kuò)大信賴域半徑，繼續(xù)新的迭代；否則，需要縮小信賴域半徑，再通過(guò)求解新的信賴域內(nèi)的子問(wèn)題得到新的候選位移，直到滿足迭代終止條件。有效集法的基本思想是將非線性優(yōu)化問(wèn)題轉(zhuǎn)化為求解一些列的二次規(guī)劃問(wèn)題，以目標(biāo)函數(shù)減少為原則，讓迭代點(diǎn)循著約束邊界前進(jìn)，直到達(dá)到問(wèn)題的最優(yōu)點(diǎn)。SQP方法的基本思想是建立原問(wèn)題的近似二次規(guī)劃子問(wèn)題，求解該子問(wèn)題得到搜索方向，再通過(guò)步長(zhǎng)規(guī)則得到合適的迭代步長(zhǎng)，求出下一個(gè)迭代點(diǎn)，進(jìn)入下一次迭代。如何選取優(yōu)化效果好、計(jì)算實(shí)時(shí)性高的非線性規(guī)劃方法是急需解決的關(guān)鍵問(wèn)題[12]。

本文提出一種基于SQP 優(yōu)化平面約束輔助的深空探測(cè)器自主天文導(dǎo)航方法，該方法在對(duì)系統(tǒng)非線性不等式幾何平面約束建模的基礎(chǔ)上，利用SQP 非線性規(guī)劃方法，對(duì)深空探測(cè)器自主天文導(dǎo)航系統(tǒng)的非線性不等式約束進(jìn)行非線性規(guī)劃，直接輔助減小深空探測(cè)器自主導(dǎo)航系統(tǒng)的量測(cè)誤差；利用Cubature卡爾曼濾波（Cubature Kalman Filter, CKF）-SQP 量測(cè)優(yōu)化非線性約束濾波方法，對(duì)深空探測(cè)器自主導(dǎo)航系統(tǒng)的狀態(tài)進(jìn)行估計(jì)，進(jìn)一步減小系統(tǒng)隨機(jī)誤差，實(shí)現(xiàn)深空探測(cè)器高精度自主導(dǎo)航。

1 深空探測(cè)器平面約束輔助自主天文導(dǎo)航系統(tǒng)

深空探測(cè)器平面約束輔助自主天文導(dǎo)航系統(tǒng)可分為敏感器子系統(tǒng)、系統(tǒng)模型子系統(tǒng)和濾波器子系統(tǒng)，如圖1 所示。其中，敏感器子系統(tǒng)主要包括用于測(cè)量姿態(tài)信息的星敏感器、測(cè)量火星衛(wèi)星信息的科學(xué)載荷和自主導(dǎo)航敏感器；系統(tǒng)模型子系統(tǒng)主要包括軌道動(dòng)力學(xué)模型、量測(cè)模型、量測(cè)量和量測(cè)量平面約束模型；濾波子系統(tǒng)主要包括平面約束非線性規(guī)劃、時(shí)間更新、量測(cè)更新，最終輸出探測(cè)器的位置和速度。

圖1 深空探測(cè)器平面約束輔助自主天文導(dǎo)航系統(tǒng)框圖Fig. 1 The block diagram of deep space probe autonomous celestial navigation system with coplanar constraints

1.1 深空探測(cè)器軌道動(dòng)力學(xué)模型

以火星探測(cè)為例，在J2000.0 火心赤道慣性坐標(biāo)系建立火星探測(cè)器的軌道動(dòng)力學(xué)模型[1-3]

其中：r=[x,y,z]T為探測(cè)器在火心慣性坐標(biāo)系中的位置矢量；v=[vx,vy,vz]T為探測(cè)器在火心慣性坐標(biāo)系中的速度矢量；μm為火星引力常數(shù)；μs為太陽(yáng)引力常數(shù)；rps為日心到探測(cè)器的位置矢量；rms為日心到火心的位置矢量；aJ為火星非球形引力攝動(dòng)加速度，as為太陽(yáng)輻射壓力加速度，ao為其他未建模攝動(dòng)加速度。

可簡(jiǎn)寫為

其中：X=[x,y,z,vx,vy,vz]T為天文測(cè)角導(dǎo)航系統(tǒng)的狀態(tài)矢量，f(?)為天文測(cè)角導(dǎo)航系統(tǒng)的非線性連續(xù)狀態(tài)轉(zhuǎn)移函數(shù)，w=[w1,w2,w3,w4,w5,w6]T為天文測(cè)角導(dǎo)航系統(tǒng)的狀態(tài)模型誤差。

1.2 深空探測(cè)器量測(cè)模型

以火衛(wèi)一和火衛(wèi)二的天體矢量方向作為天文測(cè)角導(dǎo)航系統(tǒng)的量測(cè)量，可由敏感器獲得的像元像線坐標(biāo)計(jì)算得到，則火衛(wèi)一、火衛(wèi)二天體矢量方向的量測(cè)模型可以表示為

其中：Lphobos和Ldeimos分別為火衛(wèi)一和火衛(wèi)二天體矢量方向，rphobos和rdeimos分別為火衛(wèi)一和火衛(wèi)二在火心慣性坐標(biāo)系中的位置矢量，為探測(cè)器與火衛(wèi)一和火衛(wèi)二之間的距離。

令Z=[LphobosT,LdeimosT]T， 量 測(cè) 噪 聲V=[vlphobosT,vldeimosT]T，vl1,vl2分別為L(zhǎng)phobos,Ldeimos的觀測(cè)誤差，則分別以火衛(wèi)一和火衛(wèi)二的天體矢量方向作為觀測(cè)量的量測(cè)模型可表示為

其中，H(?)為天文測(cè)角自主導(dǎo)航系統(tǒng)以火衛(wèi)一和火衛(wèi)二的天體矢量方向?yàn)橛^測(cè)量的量測(cè)方程。

1.3 量測(cè)量幾何平面約束模型

在同一時(shí)刻，兩個(gè)導(dǎo)航天體到探測(cè)器的矢量方向L0=[m0,n0,p0]T和L1=[m1,n1,p1]T不 共 線 (如 圖 2 所示)，即

式中：m0,n0,p0,m1,n1,p1為第1 個(gè)和第2 個(gè)導(dǎo)航天體到探測(cè)器的矢量方向。

圖2 天文測(cè)角幾何平面約束示意圖Fig.2 Geometric coplanar constraints in celestial navigation

由于L0和L1在同一平面內(nèi)，L01和L0在同一平面內(nèi)，且L01和L1也在同一平面內(nèi)，因此可以確定三個(gè)矢量方向在同一平面內(nèi)，如圖2所示，因此可得

對(duì)于實(shí)際天文測(cè)角自主導(dǎo)航系統(tǒng)，測(cè)量所得的導(dǎo)航天體到探測(cè)器的矢量方向與真實(shí)的導(dǎo)航天體到探測(cè)器的矢量方向存在誤差，則根據(jù)導(dǎo)航天體到探測(cè)器真實(shí)的矢量方向與其測(cè)量誤差的關(guān)系，測(cè)量所得的第i個(gè)導(dǎo)航天體到探測(cè)器的矢量方向可定義為

根據(jù)測(cè)量所得的導(dǎo)航天體到探測(cè)器的矢量方向的定義如式(7)所示，可得

則式(6)可以表示為

由于式(6)表示真實(shí)的2個(gè)導(dǎo)航天體與探測(cè)器之間各矢量的關(guān)系，因此無(wú)論探測(cè)器與2個(gè)導(dǎo)航天體之間的矢量方向測(cè)量誤差有多大，都不會(huì)影響式(6)所表示的平面約束關(guān)系。

天文測(cè)角導(dǎo)航測(cè)量誤差的平面約束，可以采用基于約束最優(yōu)問(wèn)題的最優(yōu)化方法對(duì)測(cè)量誤差進(jìn)行進(jìn)一步限制，從而減小測(cè)量誤差對(duì)導(dǎo)航精度的影響。本論文采用天文測(cè)角視線單位矢量方向誤差平方和作為目標(biāo)函數(shù)，以視線單位矢量方向共面作為約束條件，建立最優(yōu)化模型

其中：em0,en0,ep0,em1,en1,ep1為量測(cè)量天體視線矢量方向m0,n0,p0,m1,n1,p1的測(cè)量誤差，σj為第j個(gè)導(dǎo)航天體的測(cè)量誤差標(biāo)準(zhǔn)差。從式(10)可以看出，基于幾何平面約束的測(cè)量誤差最優(yōu)化問(wèn)題是一個(gè)非線性規(guī)劃問(wèn)題，其約束條件包含一個(gè)等式約束和一個(gè)不等式約束。

2 基于SQP優(yōu)化的約束Cubature卡爾曼濾波

基于SQP優(yōu)化的約束Cubature卡爾曼濾波的具體流程如圖3 所示，首先對(duì)計(jì)算Cubature 采樣點(diǎn)和權(quán)值，根據(jù)系統(tǒng)狀態(tài)模型計(jì)算各采樣點(diǎn)的一步預(yù)測(cè)值，再通過(guò)權(quán)值加權(quán)完成時(shí)間更新；其次利用SQP 非線性規(guī)劃算法，優(yōu)化測(cè)量誤差和量測(cè)量；最后根據(jù)系統(tǒng)量測(cè)模型和SQP 優(yōu)化后的量測(cè)值，進(jìn)行量測(cè)更新，最后得到系統(tǒng)的狀態(tài)估計(jì)和狀態(tài)誤差方差，輸出探測(cè)器的位置和速度。本節(jié)給出SQP 非線性規(guī)劃算法基本步驟[12]。

圖3 基于SQP優(yōu)化的約束Cubature卡爾曼濾波Fig. 3 Constraint Cubature Kalman Filter based on SQP

將量測(cè)量所滿足的平面約束（如式（10）所示）展開為拉格朗日二次逼近函數(shù)

其中，λi為第i個(gè)約束的拉格朗日乘子。

線性化非線性約束，可得二次規(guī)劃子問(wèn)題，其目標(biāo)函數(shù)為

其中：d為全變量搜索方向；Hs為拉格朗日函數(shù)Hes‐sian 矩陣的正定擬牛頓近似，其可以采用BFGS 方法更新，即

其中：ds為xs到xs+1的向量，步長(zhǎng)參數(shù)αs通過(guò)合適的線性搜索方式確定，使指標(biāo)函數(shù)值得到足夠的精度。

3 計(jì)算機(jī)仿真

3.1 仿真條件

本節(jié)采用2018 年火星探測(cè)任務(wù)為仿真對(duì)象，探測(cè)器軌道由Satellite Tool Kit Astrogator 產(chǎn)生，此次任務(wù)在J2000日心平黃道坐標(biāo)系中，探測(cè)器發(fā)射時(shí)間為2018-05-20，預(yù)計(jì)抵達(dá)火星時(shí)間為2019-01-14，軌道歷元時(shí)間2018-05-23，08:23:24.009 UTCG，初始軌道參數(shù)為[?70 597 583.471 308 km, ?134 137 866.280 822 km, 23 192.415 638 km, 28.251 309 km/s, ?15.795 096 km/s,0.055 146 km/s]T，仿真時(shí)間為2019-01-12，18: 04: 36.650 UTCG 到 2019-01-14， 10: 25: 29.737 UTCG，接近火星時(shí)間為，2019-01-14，10:25:29.737 UCTG，近火點(diǎn)高度600 km，遠(yuǎn)火點(diǎn)高度8萬(wàn)km，軌道積分方法和步長(zhǎng)RKF89/1 s，初始位置和速度的誤差為[1 000 km,1 000 km,1 000 km,1 m/s,1 m/s,1 m/s]T，火衛(wèi)一矢量方向測(cè)量誤差6″，火衛(wèi)二矢量方向測(cè)量誤差3″，非線性規(guī)劃方法最大迭代次數(shù)為400次，最優(yōu)化閾值1×10-6，步長(zhǎng)閾值1×10-6。

3.2 仿真結(jié)果

1）不同非線性約束規(guī)劃算法優(yōu)化結(jié)果

圖4給出了在相同初始條件下不同非線性約束規(guī)劃算法對(duì)量測(cè)量的優(yōu)化結(jié)果。由圖可以看出，信賴域反射法、內(nèi)點(diǎn)法、有限集法、序列二次規(guī)劃法都可有效減小精度較低的量測(cè)量測(cè)量誤差（火衛(wèi)一矢量方向測(cè)量誤差），但對(duì)精度較高的測(cè)量誤差抑制效果不明顯。表1給出了不同非線性約束規(guī)劃算法對(duì)量測(cè)量的詳細(xì)優(yōu)化結(jié)果。從表1可以看出，不同非線性約束規(guī)劃算法對(duì)精度較低的量測(cè)量測(cè)量誤差進(jìn)行了明顯的優(yōu)化，優(yōu)化后火衛(wèi)一和火衛(wèi)二測(cè)量信息可優(yōu)化至接近的精度，但精度較高的測(cè)量誤差與優(yōu)化前相比精度略有下降。這是由于平面約束方法的特性是將2個(gè)帶有誤差的矢量約束至同一平面內(nèi)，因此約束過(guò)程中，精度較差的矢量會(huì)對(duì)精度較高的矢量造成一定的影響。從表1還可以看出，信賴域反射法和內(nèi)點(diǎn)法量測(cè)優(yōu)化后的結(jié)果相同，優(yōu)化指標(biāo)函數(shù)值也相同，具有相同的優(yōu)化精度；有限集法可以得到最高的火衛(wèi)二矢量方向量測(cè)優(yōu)化精度，但其對(duì)火衛(wèi)一矢量方向量測(cè)優(yōu)化精度與其他方法相比最低；序列二次規(guī)劃法相比其他3種方法可以獲得最小的優(yōu)化指標(biāo)函數(shù)值，優(yōu)化效果最好，優(yōu)化后火衛(wèi)一的測(cè)量誤差為3.461 1″，火衛(wèi)二的測(cè)量誤差為3.409 8″。此外，表1還給出了4種非線性規(guī)劃方法計(jì)算時(shí)間的比較，序列二次規(guī)劃方法用時(shí)最少。

圖4 不同非線性約束規(guī)劃算法對(duì)量測(cè)量的優(yōu)化結(jié)果Fig. 4 Measurement optimization results for different nonlinear constrained programming algorithms

表1 不同非線性約束規(guī)劃算法對(duì)量測(cè)量的優(yōu)化結(jié)果對(duì)比表Table 1 Comparisons of measurement optimization result using different nonlinear constrained programming algorithms

2）導(dǎo)航天體不同測(cè)量誤差的優(yōu)化結(jié)果

圖5 給出了當(dāng)深空探測(cè)器火衛(wèi)一和火衛(wèi)二測(cè)量誤差分別為2 ″和1 ″時(shí)的測(cè)量?jī)?yōu)化結(jié)果。由圖可以看出，采用序列二次規(guī)劃法可有效減小精度較低的量測(cè)量測(cè)量誤差（火衛(wèi)一矢量方向測(cè)量誤差）。表2 給出了導(dǎo)航天體不同測(cè)量誤差的詳細(xì)優(yōu)化結(jié)果。從表2 可以看出，當(dāng)深空探測(cè)器火衛(wèi)一和火衛(wèi)二測(cè)量誤差分別為2 ″和1 ″時(shí)，優(yōu)化后火衛(wèi)一測(cè)量誤差為1.098 5″，火衛(wèi)二測(cè)量誤差為1.096 4″；當(dāng)深空探測(cè)器火衛(wèi)一和火衛(wèi)二測(cè)量誤差均為1 ″時(shí)，優(yōu)化后火衛(wèi)一的測(cè)量誤差為0.696 4″，火衛(wèi)二的測(cè)量誤差為0.698 1″；當(dāng)深空探測(cè)器火衛(wèi)一和火衛(wèi)二測(cè)量誤差均為3 ″時(shí)，優(yōu)化后火衛(wèi)一測(cè)量誤差為2.095 8″，火衛(wèi)二測(cè)量誤差為2.104 8″。當(dāng)2 個(gè)導(dǎo)航天體測(cè)量誤差精度相同時(shí)，優(yōu)化后火衛(wèi)一和火衛(wèi)二測(cè)量信息精度可提高接近30%。當(dāng)2 個(gè)導(dǎo)航天體測(cè)量誤差精度不同時(shí)，優(yōu)化后火衛(wèi)一和火衛(wèi)二測(cè)量信息可優(yōu)化至接近的精度，精度較差的導(dǎo)航天體的優(yōu)化結(jié)果更為明顯。這是由于本文所提方法采用了平面約束，將測(cè)量所得火衛(wèi)一矢量和火衛(wèi)二矢量約束至同一平面，因此精度較高的測(cè)量矢量精度決定了2 個(gè)矢量的測(cè)量精度。

3）平面約束輔助測(cè)量后的導(dǎo)航結(jié)果

圖6 和表3 給出了平面約束輔助量測(cè)優(yōu)化深空探測(cè)器自主天文導(dǎo)航結(jié)果，與無(wú)平面約束輔助優(yōu)化的深空探測(cè)器自主天文導(dǎo)航結(jié)果進(jìn)行了比較。由圖6和表3可以看出，濾波收斂后無(wú)平面約束輔助量測(cè)優(yōu)化的自主天文導(dǎo)航方法的平均位置誤差為11.305 6 km（RMS），最大位置誤差為21.947 9 km，平均速度誤差為0.269 8 m/s（RMS），最大速度誤差為0.609 5 m/s；基于平面約束輔助量測(cè)的自主天文導(dǎo)航方法平均位置誤差為7.665 2 km（RMS），最大位置誤差為18.426 2 km，平均速度誤差為0.208 7 m/s（RMS），最大速度誤差0.575 5 m/s。由此可以看出，平面約束可以減小矢量測(cè)量方向的隨機(jī)測(cè)量誤差，提高自主天文導(dǎo)航精度。

圖5 導(dǎo)航天體不同測(cè)量誤差的優(yōu)化結(jié)果Fig. 5 Measurement optimization results of different measurement accuracies

表2 導(dǎo)航天體不同測(cè)量誤差的優(yōu)化結(jié)果對(duì)比表Table 2 Comparisons of measurement optimization results of different measurement accuracies

圖6 平面約束輔助量測(cè)優(yōu)化深空探測(cè)器自主天文導(dǎo)航結(jié)果Fig. 6 Autonomous celestial navigation results with coplanar constraints aided measurement

表3 平面約束輔助量測(cè)優(yōu)化的深空探測(cè)器自主天文導(dǎo)航結(jié)果對(duì)比Table 3 Comparisons of autonomous celestial navigation results with coplanar constraints aided measurement

4 結(jié) 論

本文針對(duì)抑制深空探測(cè)器自主天文導(dǎo)航系統(tǒng)測(cè)量誤差這一問(wèn)題，提出了一種平面約束輔助測(cè)量的深空探測(cè)器自主天文導(dǎo)航方法，該方法在對(duì)系統(tǒng)非線性不等式幾何平面約束建模的基礎(chǔ)上，利用了SQP 非線性規(guī)劃方法，對(duì)深空探測(cè)器自主天文導(dǎo)航系統(tǒng)的非線性不等式約束進(jìn)行了非線性規(guī)劃，直接輔助減小了深空探測(cè)器自主導(dǎo)航系統(tǒng)的量測(cè)誤差；利用CKF-SQP量測(cè)優(yōu)化非線性約束濾波方法，對(duì)深空探測(cè)器自主導(dǎo)航系統(tǒng)的狀態(tài)進(jìn)行估計(jì)，進(jìn)一步減小了系統(tǒng)隨機(jī)誤差，提高了深空探測(cè)自主天文導(dǎo)航的精度。仿真結(jié)果表明，所提方法可以有效抑制了測(cè)量誤差，尤其對(duì)精度較低的測(cè)量誤差抑制效果顯著，實(shí)現(xiàn)了深空探測(cè)器高精度自主導(dǎo)航。