李云天，穆榮軍，單永志,2，崔乃剛

（1. 哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150001；2. 中國兵器工業(yè)集團航空彈藥研究院，哈爾濱 150001）

隨著嫦娥四號無人探測器在月球背面的成功著陸，中國探月工程即將進入收官階段，未來的月球探測任務(wù)將主要集中于月球資源的開發(fā)和載人登月方面[1]。

由于月球尚未建成完備可靠的絕對定位系統(tǒng)，地面測控站與著陸器之間又存在較大的通信延遲，目前已成功實現(xiàn)月面著陸的月球探測任務(wù)中，著陸器慣性測量單元（Inertial Measurement Unit, IMU）的累積誤差修正大多依賴于視覺導(dǎo)航系統(tǒng)（Visual Navigation System, VNS）。VNS 雖具有自主性強、造價低、重量輕、功耗小、可同時獲取著陸器位姿等優(yōu)勢[2]，但其需要進行復(fù)雜的系統(tǒng)初始化且需要具有良好的視線（line of sight, LOS）才能工作。同時，VNS 的定位精度受制于圖像數(shù)據(jù)庫分辨率、相機視角、自身陰影、月面揚塵等因素，應(yīng)用場景受到極大限制。

雖然相較于 VNS，無線電導(dǎo)航系統(tǒng)（Radio Navigation System, RNS）能提供的量測信息有限（大多數(shù)只具有測距/測速功能，少數(shù)可同時測角），但RNS沒有復(fù)雜的初始化過程，只需在月面布置若干無線電信標即可工作；同時，RNS 對LOS 的依賴相對較弱，抗月塵干擾能力更強。

上述優(yōu)勢使RNS 成為VNS 的首選替代方案，國內(nèi)外陸續(xù)有基于月面信標的相關(guān)研究成果面世。同釗等[3]研究了基于測距和測速信息的多信標定位方法；Bora 等[4]分別研究了1-3 個信標數(shù)目對導(dǎo)航精度和魯棒性的影響；Zhao 等[5]則基于Fisher 可觀測性矩陣研究了月面信標的分布優(yōu)化方法。上述研究成果均基于一個重要假設(shè)，即信標位置精確已知。實際上，由于月面沒有建成類似GPS 的精確定位系統(tǒng)，目前信標自身位置的確定大多需要依賴深空探測網(wǎng)絡(luò)或環(huán)繞軌道器，其中深空探測網(wǎng)絡(luò)定位誤差較大（百米級）且無法直接測量位于月球背面的信標位置；環(huán)繞軌道器的定位精度則依賴于自身的軌道精度且耗時較長，兩者均會使得信標的位置存在一定的不確定性，無法滿足未來登月任務(wù)高精度、快響應(yīng)和自適應(yīng)需求。

此外，由于量測函數(shù)是非線性的，大多數(shù)RNS均基于以擴展卡爾曼濾波（Extended Kalman Filter，EKF）為代表的非線性濾波方法實現(xiàn)[6,7]，這類濾波算法由于需要對協(xié)方差矩陣求逆，算法復(fù)雜度為（n為系統(tǒng)狀態(tài)矢量的維數(shù)，下同）[8]。當(dāng)月面信標位置已知時，系統(tǒng)待估計狀態(tài)量的維數(shù)較小，上述算法可在估計精度和估計效率之間取得較好的平衡。而當(dāng)月面信標位置不確定時，每一個信標的三維位置均會被歸入待估計狀態(tài)量中，進而使得狀態(tài)矢量的維數(shù)出現(xiàn)顯著增加。此時仍使用上述協(xié)方差形式的濾波算法極易引發(fā)“維數(shù)災(zāi)難”，算法的計算耗時將急劇增加。

作為協(xié)方差矩陣的對偶形式，信息矩陣天然的稀疏性使以稀疏擴展信息濾波（Sparse Extended Information Filter, SEIF）為代表的信息形式濾波算法在大規(guī)模估計問題中得到了廣泛應(yīng)用[9-11]。雖然信息形式的濾波算法具有較快的計算速度，但算法中所有的后驗概率都將以信息矢量和信息矩陣的形式表示，這使得均值矢量的獲?。ㄓ绕涫窃诋惒搅繙y下）變得較為困難。當(dāng)各導(dǎo)航傳感器的量測為異步時（如IMU數(shù)據(jù)更新頻率遠高于其他傳感器且量測存在時延），濾波器的兩次量測更新之間需要進行多次狀態(tài)預(yù)測過程。而每次狀態(tài)預(yù)測過程都需要從信息矢量中反解出均值矢量無疑會帶來許多不必要的計算負擔(dān)，降低算法的實時性。

針對上述問題和背景需求，本文提出一種基于稀疏擴展混合濾波（Sparse Extended Hybrid Filter，SEHF）的信標輔助月面著陸導(dǎo)航方法，所提出方法所具有的優(yōu)勢如下：

1）不需要事先對月面信標的位置進行精確標定，在月面信標位置不確定的條件下亦可工作。

2）采用SEIF 處理量測更新過程，實現(xiàn)著陸器位速和信標位置的聯(lián)合估計，有效增強了算法實時性和收斂速度。

3）采用“均值矢量+信息矩陣”的混合形式處理異步量測下的狀態(tài)預(yù)測過程，進一步提高了算法的計算效率。

1 信標輔助月面著陸導(dǎo)航系統(tǒng)原理

1.1 信標輔助月面著陸導(dǎo)航系統(tǒng)概覽

信標輔助月面著陸導(dǎo)航系統(tǒng)主要工作在主減速段之后，此階段的飛行時間和距離均較短（例如嫦娥-4號探測器的飛行時間和距離約為 200 s 和5 km～10 km），同時月球的自轉(zhuǎn)角速度較小，不失一般性地，可選擇著陸坐標系作為導(dǎo)航坐標系[5][7]，該坐標系原點on位于預(yù)定著陸場，xn、yn和zn軸分別指向東-北-天（E-N-U）；本體系坐標原點ob位于著陸器質(zhì)心，xb、yb和zb軸分別指向前-左-上（F-L-U）。

圖1 給出了信標輔助月面著陸導(dǎo)航系統(tǒng)概覽，在執(zhí)行著陸任務(wù)前，事先由無人探測任務(wù)或月面基地在設(shè)計好的著陸軌跡兩側(cè)布置一定數(shù)量的無線電測距信標。不失一般性地，可假設(shè)所有月面信標位于同一水平面上。這一假設(shè)雖然會在一定程度上弱化高程通道的可觀測性，但由于月面著陸過程中高程通常由高度計輔助修正，因此影響并不顯著。

圖2 則進一步給出了信標輔助月面著陸導(dǎo)航系統(tǒng)工作流程。在離開主減速段后，著陸器上的信標接收機開始工作，搜索探測范圍內(nèi)的所有可見信標，并將著陸器當(dāng)前的位置信息和包含所有可見信標ID 及對應(yīng)距離信息的信標列表（List of Beacons，LBN）發(fā)送給所有可見信標。所有信標初始工作在“待機”模式，在接收到著陸器的信息后，待機信標（圖2 中虛線圓）即轉(zhuǎn)入“初始化”模式。“初始化”模式下信標并不參與導(dǎo)航定位，而是根據(jù)接收到的著陸器位置和測距信息進行自身位置的粗估計。當(dāng)信標完成自身位置的初始化工作后，即轉(zhuǎn)入“定位”工作模式。在此模式下，定位信標（圖2 中實線圓）基于自身和著陸器之間的測距信息持續(xù)估計自身對應(yīng)的信息矢量和信息矩陣[9]并發(fā)送給著陸器。當(dāng)定位信標的數(shù)目大于一定值時（通常為3 個），著陸器上的導(dǎo)航濾波器即根據(jù)接收到的所有信息矢量和信息矩陣進行量測更新過程，進行著陸器當(dāng)前位速及月面信標位置的聯(lián)合估計。

圖1 信標輔助月面著陸導(dǎo)航系統(tǒng)示意圖Fig.1 Illustration of the beacon-supported navigation scheme for lunar landing

圖2 信標輔助月面著陸導(dǎo)航系統(tǒng)工作流程圖Fig.2 Working flow of the beacon-supported navigation scheme for lunar landing

1.2 信標輔助月面著陸導(dǎo)航系統(tǒng)模型

記t時刻待估計狀態(tài)矢量為其中xt=[xt,y t,z t,vt x,vt y,vtz]T為t時刻著陸器在導(dǎo)航坐標系下的三軸位置和速度，為所有信標點在導(dǎo)航坐標系下的三維位置。同時，記t時刻系統(tǒng)的控制量為著陸坐標系下的三軸加速度，即且加速度量測噪聲服從高斯分布wt～ N( 0,Qt)，則著陸器對應(yīng)的狀態(tài)方程為：

式中In×n為n維單位矩陣，Δt為IMU 的量測步長。

記t+ 1時刻的量測量為其中hri為第i個無線電信標和著陸器之間的距離量測，即：

式中[xl y lzl]和[xmiymizmi]分別表示著陸器和第i個信標在著陸坐標系下的三維位置。

2 基于SEIF 的信標輔助月面著陸導(dǎo)航系統(tǒng)模型

在貝葉斯框架下，狀態(tài)估計問題本質(zhì)上是求解極大后驗估計的過程，因此選擇合適的參數(shù)化形式，對于算法的求解精度和效率至關(guān)重要。EKF 的估計過程是基于均值矢量μ和協(xié)方差矩陣Σ實現(xiàn)的，該參數(shù)化形式稱為高斯分布的標準型（Standard Form）；而SEIF的估計過程則是基于信息矢量η和信息矩陣Λ實現(xiàn)的，該參數(shù)化形式稱為高斯分布的規(guī)范型（Canonical Form），兩者之間的關(guān)系如下：

除狀態(tài)預(yù)測和量測更新以外，SEIF 還存在用于維護信息矩陣稀疏性的稀疏化過程和用于恢復(fù)均值矢量的狀態(tài)恢復(fù)過程。SEIF 雖然在計算形式上更為復(fù)雜，但其在大規(guī)模估計問題上相較EKF 具有更高的計算效率和可擴展性[12]。

2.1 SEIF 的狀態(tài)預(yù)測過程

SIEF 的狀態(tài)預(yù)測過程分為狀態(tài)增廣和狀態(tài)邊緣化兩步，假設(shè)ξt服從如下形式的高斯分布：

式(9)的計算會為信息矩陣引入較多的填充項（fill-in），在量測數(shù)目較大的應(yīng)用中（例如在視覺同步定位與建圖中特征點數(shù)目通常為上百個）會嚴重破壞信息矩陣的稀疏性，需要進行條件稀疏化操作以維護信息矩陣的稀疏性。但在所提出方法中，由于信標數(shù)目不會太多（通常為十幾個），條件稀疏化過程可暫且省略。

2.2 SEIF 的量測更新過程

作為EKF 的對偶形式，SEIF 同樣采用一階泰勒展開方式對非線性量測函數(shù)進行線性近似，即：

將式(3)帶入上式有：

式中所有未標明元素均為0?？梢钥闯觯瑹o論是量測Jacobian 矩陣本身還是其子塊都具有良好的稀疏性，非零項只存在于和著陸器當(dāng)前狀態(tài)以及對應(yīng)的量測信標相關(guān)的位置處。得益于稀疏性的存在，式(11)的計算復(fù)雜度與HTR-1H一致，均為計算效率相較于協(xié)方差形式的濾波算法得到了有效提高。

2.3 均值矢量的恢復(fù)

SEIF 雖保證了算法的計算效率，卻難以直接獲取著陸器的狀態(tài)均值矢量μt。一方面μt所代表的著陸器位速才是著陸器導(dǎo)航制導(dǎo)與控制系統(tǒng)所需要的，另一方面非線性函數(shù)的線性化亦依賴于μt。由于信息矩陣Λt是對稱正定（Symmetric Positive Definite，SPD）矩陣，因此的求解可等效為如下形式的最小二乘估計問題：

該式可采用共軛梯度法（Conjugate Gradient， CG）迭代求解，但傳統(tǒng)CG 方法要求系數(shù)矩陣的條件數(shù)不能過大，否則迭代收斂速度將急劇下降。針對這一問題，預(yù)處理共軛梯度（Preconditioned Conjugate Gradient, PCG）算法應(yīng)運而生。PCG 引入SPD 矩陣M對系數(shù)矩陣進行預(yù)處理以實現(xiàn)對系數(shù)矩陣條件數(shù)的抑制，進而提高算法的收斂速度[13]。

3 異步量測處理方法

3.1 量測信息的時間對齊

異步量測是多傳感器組合導(dǎo)航系統(tǒng)中普遍存在的問題，主要包括兩方面：1）不同傳感器的數(shù)據(jù)更新異步；2）不同傳感器對同一狀態(tài)的量測異步。信標輔助月面著陸導(dǎo)航系統(tǒng)的傳感器量測時序如圖3 所示，可以看出在兩次信標量測［t k,tk+1］間存在多次IMU 量測，且IMU 量測和信標量測之間存在一定的偏差。

記tk前后的兩個IMU 量測分別為Itj和Itj+1，則tk時刻的IMU 量測可通過如下的線性插值得到：

圖3 傳感器異步量測時序示意圖Fig.3 Illustration of asynchronous sensor measurements

3.2 混合狀態(tài)預(yù)測

4 仿真分析

4.1 仿真場景設(shè)計

月面著陸仿真場景參照嫦娥-4 號著陸器的著陸軌跡[14]和表1 所示各項參數(shù)設(shè)計，得到的三維模擬著陸軌跡和月面信標分布如圖4 所示，對應(yīng)的加速度和角速度剖面如圖5 和圖6 所示。

圖4 三維著陸軌跡及信標分布Fig.4 3D landing trajectory and beacon distribution

表1 各項仿真場景參數(shù)Tab.1 Parameters of simulation scenario

圖5 著陸軌跡加速度剖面Fig.5 Acceleration profile of landing trajectory

圖6 著陸軌跡角速度剖面Fig.6 Angular rate profile of landing trajectory

著陸導(dǎo)航系統(tǒng)所使用的導(dǎo)航傳感器包括IMU、無線電信標及激光高度計三種[4]，各傳感器的相關(guān)仿真參數(shù)（1σ）如表2 所示。

表2 各導(dǎo)航傳感器仿真參數(shù)（1σ）Tab.2 Simulation parameters of all navigation sensors(1σ )

4.2 仿真結(jié)果分析

為驗證本文所提出方法的有效性，在4.1 所構(gòu)建的仿真場景下分別使用EKF 和SEHF 對著陸軌跡進行估計。所有仿真程序均基于C++開發(fā)并運行在硬件配置為Intel(R) Core(TM) i5-8300@2.3GHz CPU+8GB RAM 的Linux 平臺上。為保證算法耗時比較的準確性，仿真程序中未使用任何多線程或GPU 加速，全部運算均由CPU 獨立完成。

兩種濾波算法的位置估計誤差如圖7 所示（放大后的穩(wěn)態(tài)估計誤差對比如圖8 所示），速度估計誤差如圖9 所示。可以看出，在信標位置不確定的情況下，SEHF 無論是收斂速度還是穩(wěn)態(tài)估計誤差均優(yōu)于EKF。EKF 在位置估計方面收斂耗時較長（大于100 s），且最終的穩(wěn)態(tài)估計誤差大于SEHF（兩者位置估計誤差穩(wěn)態(tài)值如表3 所示）；而在速度估計方面，EKF 則僅有東向速度估計誤差在100 s 左右收斂。

圖7 EKF 與SEHF 位置估計誤差對比Fig.7 Comparison of error in position estimation between EKF and SEHF

圖8 EKF 與SEHF 位置估計誤差穩(wěn)態(tài)值對比Fig.8 Comparison of steady-state error in position estimation between EKF and SEHF

圖9 EKF 與SEHF 速度估計誤差對比Fig.9 Comparison of error in velocity estimation between EKF and SEHF

表3 EKF 與SEHF 位置估計誤差穩(wěn)態(tài)值對比Tab.3 Comparison of steady-state error in position estimation between EKF and SEHF

為進一步驗證SEHF 算法的穩(wěn)定性，對SEHF 進行100 次Monte-Carlo 打靶得到的最終著陸點位置估計結(jié)果如圖10 所示?？梢钥闯?，所有估計著陸點精度均優(yōu)于100 m，且圓概率誤差（Circle Error Probable，CEP）小于50 m，平均落點誤差為[32.54,17.23]m。

圖10 100 次Monte-Carlo 試驗最終著陸點散布Fig.10 Distribution of final landing site with 100 Monte Carlo runs

最后，分別對原始SEIF 算法和SEHF 算法的單次狀態(tài)預(yù)測耗時和估計耗時進行了比較，結(jié)果如圖11和表4 所示。仿真結(jié)果表明，SEHF 的外點數(shù)目更少，在計算耗時方面相比SEIF 具有更好的一致性；同時，在單次狀態(tài)預(yù)測耗時和估計耗時方面，SEHF 相較于SEIF 分別縮短了16.6%和16.3%，有效提高了算法的實時性。

圖11 SEIF 和SEHF 單次估計耗時對比Fig.11 Comparison of one-step computational cost between SEIF and SEHF

表4 SEIF 與SEHF 計算耗時統(tǒng)計Tab.4 Computational cost of SEIF and SEHF

5 結(jié) 論

為進一步提高初始位置不確定條件下，信標輔助月面著陸導(dǎo)航系統(tǒng)的估計精度和效率，基于稀疏擴展混合濾波，本文提出了一種改進的信標輔助月面著陸導(dǎo)航方法。該方法在基于SEIF 實現(xiàn)著陸器自身位速和月面信標位置聯(lián)合估計的基礎(chǔ)上，進一步采用“均值矢量+信息矩陣”的混合形式進行狀態(tài)預(yù)測，可有效提高算法的計算效率。

仿真結(jié)果表明，在收斂速度和估計精度方面，所提出方法均優(yōu)于EKF，同時相較于原始SEIF 可縮短16%左右的計算耗時；100 次Monte-Carlo 仿真則進一步表明，所提出方法可在信標位置存在150 m（1σ）初始誤差時取得優(yōu)于50 m（CEP）的精度。