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

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

引 言

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

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

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

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

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

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

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

1.1 深空探測器軌道動力學(xué)模型

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

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

可簡寫為

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

1.2 深空探測器量測模型

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

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

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

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

1.3 量測量幾何平面約束模型

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

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

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

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

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

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

則式(6)可以表示為

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

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

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

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

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

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

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

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

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

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

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

3 計算機仿真

3.1 仿真條件

本節(jié)采用2018 年火星探測任務(wù)為仿真對象，探測器軌道由Satellite Tool Kit Astrogator 產(chǎn)生，此次任務(wù)在J2000日心平黃道坐標系中，探測器發(fā)射時間為2018-05-20，預(yù)計抵達火星時間為2019-01-14，軌道歷元時間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，仿真時間為2019-01-12，18: 04: 36.650 UTCG 到 2019-01-14， 10: 25: 29.737 UTCG，接近火星時間為，2019-01-14，10:25:29.737 UCTG，近火點高度600 km，遠火點高度8萬km，軌道積分方法和步長RKF89/1 s，初始位置和速度的誤差為[1 000 km,1 000 km,1 000 km,1 m/s,1 m/s,1 m/s]T，火衛(wèi)一矢量方向測量誤差6″，火衛(wèi)二矢量方向測量誤差3″，非線性規(guī)劃方法最大迭代次數(shù)為400次，最優(yōu)化閾值1×10-6，步長閾值1×10-6。

3.2 仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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