基于特征值分解的小天體著陸自主導航系統(tǒng)可觀度分析*

冀紅霞，宗 紅，黃翔宇,2

1.北京控制工程研究所,北京 100190;2．空間智能控制技術重點實驗室，北京 100190.

0 引 言

導航系統(tǒng)的基本任務是確定航天器在給定參考坐標系內的狀態(tài)，而導航系統(tǒng)的能觀性反映了系統(tǒng)在有限時間內測量系統(tǒng)的能力，由于深空天體與地球的距離遙遠，導致地面測控站所發(fā)出的測控信息的傳輸出現(xiàn)時滯,難以實現(xiàn)對著陸過程進行實時測控，因此深空探測著陸任務必須利用探測器上的敏感器和導航算法自主完成，因此自主導航系統(tǒng)的可觀性分析成為一個關鍵的問題.

由于自主導航系統(tǒng)對應的狀態(tài)方程和觀測方程的非線性導致分析其可觀性存在一定的困難，利用李導數(shù)定義的觀測矩陣[1]能分析一類非線性系統(tǒng)的局部弱可觀性，但是由于自主導航系統(tǒng)的復雜性(狀態(tài)維數(shù)高、非線性強)多數(shù)情況下很難直接利用現(xiàn)有的可觀性秩條件來確定其可觀性，文獻[2]和[3]基于Kalman濾波誤差協(xié)方差矩陣來定義能觀度,文獻[4]研究了基于星光角距和星光仰角的航天器自主天文導航方法，對相應導航系統(tǒng)的能觀性進行了分析，并以此為基礎說明了觀測量的選擇對導航性能的影響.文獻[5]系統(tǒng)地介紹了深空探測自主導航系統(tǒng)的幾種觀測模式及其數(shù)學模型.

非線性估計領域最常用的方法是擴展卡爾曼濾波(extended Kalman filter, EKF)[6],它具有收斂速度快、算法簡單等優(yōu)點.盡管EKF在建模準確且系統(tǒng)噪聲為零均值白噪聲的情況下，可以獲得良好的估計效果.但在實際應用中，難以獲得精確的系統(tǒng)模型，且最初建好的模型在系統(tǒng)運行過程中受環(huán)境變化的影響可能變得不準確，進而造成模型誤差，這時采用EKF將導致濾波精度下降甚至發(fā)散，非線性預測濾波算法(nonlinear predictive filter, NPF)[7]對不確定模型具有很強的適應性，但收斂速度較慢，該算法通過使預測輸出與測量輸出的誤差和模型誤差的加權和最小來估計模型誤差，進而修正狀態(tài)估計，形成一種遞推的在線估計算法，由于對模型誤差實時進行跟蹤，故收斂速度較慢.

針對NPF和EKF的局限性，本文結合EKF算法對NPF算法進行改進，首先利用NPF的預測輸出與實際測量輸出之間的方差最小的原則來估計模型誤差并修正系統(tǒng)模型，再利用EKF的時間更新和測量更新來進一步估計系統(tǒng)狀態(tài)，并采用改進的預測濾波方法與狀態(tài)估計誤差特征值分解方法，對小天體探測器著陸自主光學導航系統(tǒng)的可觀性進行度量，推導了改進的預測濾波的協(xié)方差矩陣的傳播模型，在誤差分析的基礎上，使用誤差協(xié)方差矩陣的特征值和特征向量進行可觀度分析，最后以小天體探測器著陸自主導航為例，對小天體引力變化引起的模型誤差下改進NPF與EKF的導航精度進行比較，分析狀態(tài)參數(shù)可觀度與導航精度之間的關系，并分析不同誤差因素對導航系統(tǒng)可觀度的影響.

1 小天體探測器自主光學導航系統(tǒng)

1.1 小天體探測器運動學模型與測量模型

探測器導航的任務就是確定探測器相對于所選定的參考坐標系的位置、速度和飛行姿態(tài)，需找到一個合適的參考坐標系.所以這里定義了涉及到的參考坐標系：著陸點坐標系、本體坐標系和相機坐標系.

圖1 坐標系相對幾何關系示意圖Fig.1 Relative geometric relations of coordinate system

1) 著陸點坐標系{l}：原點定義為著陸點，基準平面取為當?shù)厮矫?，x軸指向東，y軸指向北，z軸與x、y軸構成右手坐標系.

2) 本體坐標系：原點定義為探測器質心，x,y,z軸分別沿探測器三個慣性主軸方向構成右手坐標系.

3) 相機坐標系{c}：原點定義為相機的光心，x,y軸平行于像平面，z軸與x，y軸構成右手坐標系.

1.1.1 小天體探測器運動學模型

利用慣性敏感器測量建立探測器著陸運動學方程為：

(1)

加速度計和陀螺的測量模型分別表示為：

(2)

其中，ba和bw分別為加速度計零偏和陀螺零偏；

a為除引力外所有作用于探測器上的合力產生的加速度，na、nw分別是加速度計和陀螺測量噪聲，nwa、nwω分別為加速度計和陀螺偏差噪聲.

1.1.2 觀測方程

1) 導航相機測量模型

假設光學導航相機模型為理想的小孔成像模型，由射影變換可得

其中，[xjyjzj]T是相機固連系下探測器到第j個(j=1,2,3,…,N)特征點的位置矢量,N為識別的特征點個數(shù)，特征點在像平面中的位置坐標即為導航測量量，可表示如下:

(3)

其中，第j個特征點測量量為mj=[ujvj]T=f[xj/zjyj/zj]T.

則導航測量方程為

Z(tk)=[u1v1…uNvN]T=m(tk)(4)

2) 測速敏感器測量模型

測速敏感器輸出的相對速度的測量方程如下：

(5)

式中，nu為測速敏感器測量噪聲.

1.1.3 小天體探測器自主光學導航系統(tǒng)

導航系統(tǒng)狀態(tài)包括探測器當前位置、速度和姿態(tài)，陀螺漂移和加速度計漂移

線性化式(1)可得系統(tǒng)當前誤差狀態(tài)方程：

(6)

對于導航相機，以特征點在像平面的位置為觀測量，對于特征點由式(4)可得觀測方程為：

式中，vi為觀測噪聲，設其協(xié)方差陣為Rv.

第i時刻導航陸標pj估計觀測量為：

(7)

則第i時刻的觀測量殘差為

(8)

測量方程為

(9)

式中：H1=[H1103×3H13]

其中：

對于測距測速敏感器，由式(5)可得觀測方程為

(10)

其中測量敏感矩陣為：

2 基于改進預測濾波算法的小天體探測器自主光學導航系統(tǒng)可觀度分析

2.1 改進的NPF算法及其誤差模型推導

2.1.1 改進的NPF算法

設一個非線性系統(tǒng)模型為

(11)

式中：f(·)∈Rn和h∈Rm為連續(xù)可微的非線性函數(shù)向量；X(t)∈Rn為狀態(tài)向量，y(t)∈Rm為觀測向量；d(t)∈Rp為模型誤差向量；gd(·)∈Rn×p為模型誤差分布矩陣gw(·)∈Rn×q為噪聲分布矩陣；w(t)為系統(tǒng)噪聲，為零均值的高斯白噪聲，方差陣為E{w(t)wT(t)}=Q,v(t)為測量噪聲，為零均值的高斯白噪聲，方差陣為E{v(t)vT(t)}=R.

將輸出估計方程泰勒展開，并運用李導數(shù)知識，得

(13)

ri是與第i個輸出對應的相對階，為d(t)出現(xiàn)在hi[x(t)]的微分中的最低階數(shù)；Λ(T)∈Rm×m是對角矩陣，對角元λi=Ti/ri!,i=1,2,…,m；U(x)∈Rm×p為靈敏度矩陣.

基于預測輸出與實際測量輸出的殘差最小的思想來估計系統(tǒng)的模型誤差.由此建立的目標函數(shù)是由測量輸出與預測輸出間殘差以及模型誤差修正項的加權平方和組成的，即

(14)

式(14)中下標k表示第k個采樣時刻，yk+1是tk+1時刻的測量輸出.預測輸出為：

(15)

將式(15)代入式(14)，并令?J/?d(k)=0,可得模型誤差估計為：

(16)

其中，

本文提出的改進的NPF結合NPF與EKF的優(yōu)點，利用式(16)計算模型誤差并修正系統(tǒng)模型，再利用EKF[8]的時間更新和測量更新過程來進一步估計系統(tǒng)狀態(tài).

2.1.2 誤差模型推導

根據(jù)系統(tǒng)狀態(tài)方程和狀態(tài)估計方程，使用一階泰勒展開的離散形式進行狀態(tài)更新：

xk+1≈xk+T·f(xk)+T·gd·dk+T·gw·wk(17)

其中，T為采樣步長， 根據(jù)式(17)～(18)可以計算出一步預測的狀態(tài)估計的誤差.

=xk+T·f(xk)+T·gd·dk+T·gw·wk-

則式(20)化為

T·gw·wk(21)

令Φk+1|k=I+T·Fk，B=T·gd,C=T·gw.

則式(21)化簡為

將模型誤差的估計誤差[9]

TMkHkgwwk-Mkvk+1(23)

代入式(22)中，可得

BMkΛ(T)U(xk)dk+B·dk+

(I-BMkHk)·C·wk-BMkvk+1(24)

由式(17)和(19)可得出狀態(tài)估計的誤差式為：

式(25)中第二項為：

(26)

將式(26)代入式(25)得：

[I-Kk+1Hk+1|k]·[I-BkMkHk]C·wk-

[I-Kk+1Hk+1|k]BkMkvk+1-Kk+1vk+1+

[I-Kk+1Hk+1|k]Bk[I-MkΛ(T)U]·dk(28)

式(28)中舍去包含模型誤差估計和狀態(tài)估計中的各種線性化、離散化以及泰勒展開高階舍入誤差，模型噪聲和測量噪聲的相關項.

則改進的預測濾波誤差模型為：

(29)

設濾波均方誤差陣

(Φk+1,k-BkMkHk)Pk|k(Φk+1,k-BkMkHk)T+

(I-BkMkHk)·Ck·Qk·CkT(I-BkMkHk)T+

{I-MkΛ(T)U(xk)}T+

BkMk·R·(BkMk)T(30)

由卡爾曼濾波方程得濾波均方誤差陣

Pk+1|k+1=(I-Kk+1Hk+1)Pk+1|k(31)

2.2 基于特征值分解的可觀度分析

2.2.1 可觀度定義

考慮到模型誤差和觀測誤差，導航系統(tǒng)一般可以表示為如下非線性系統(tǒng)：

(32)

式中:X為狀態(tài)參數(shù)矢量；Z為觀測量；f,h為狀態(tài)參數(shù)的非線性函數(shù)；w(t),v(t)分別為導航系統(tǒng)模型誤差和觀測誤差矢量.通過在設計的標稱軌道上線性化和離散化，導航系統(tǒng)可轉化為如下形式：

(33)

(34)

式中，tr表示矩陣的跡，n為狀態(tài)參數(shù)的維數(shù).

對最小二乘估計，有

(35)

式中，Pk為狀態(tài)參數(shù)的誤差方差陣.

可以看出，導航系統(tǒng)可觀度越高，導航系統(tǒng)狀態(tài)估計誤差越小，即估計精度越高.說明了導航系統(tǒng)可觀度定義反映了狀態(tài)參數(shù)估計精度和導航系統(tǒng)可觀度之間的關系.可觀度定義描述了狀態(tài)參數(shù)確定的精度與導航系統(tǒng)的運動學模型和觀測模型及測量精度的依賴關系.

2.2.2 基于特征值分解的可觀度分析

雖然導航系統(tǒng)的可觀度定義能夠描述整個導航系統(tǒng)的性能，但是無法具體說明在一定的觀測模式下哪些狀態(tài)最可觀(估計精度高)和哪些狀態(tài)最不可觀(估計精度低).由式(35)可知，導航系統(tǒng)的可觀度直接與狀態(tài)誤差方差陣相關，所以可以利用狀態(tài)誤差方差陣的特征值和特征向量來分析軌道參數(shù)的可觀度，給出基于特征值和特征向量的軌道參數(shù)可觀度分析方法.

(36)

考慮其線性組合

(37)

式中，v=[v1,v2,…,vn]T為線性組合的系數(shù)，且vTv=1,定義w的相關方差為

(39)

即有

(?/?v)[vTPv-λ(vTv-1)]=0(40)

則可得

(P-λI)v=0(41)

由式(41)可以看出，λ為方差陣P的特征值.對式(41)兩邊左乘以vT,則可得到

vT(P-λI)v=vTPv-vTλv=0(42)

(43)

為了無量綱化和限定特征值的范圍，對系統(tǒng)狀態(tài)誤差方差陣進行規(guī)范化處理.無量綱化的方差陣記為：

(44)

式中，P0為初始狀態(tài)誤差方差陣，Pk為k時刻的狀態(tài)誤差方差陣.

通過分析導航系統(tǒng)的誤差方差陣Pk的特征值和特征向量，可以得到狀態(tài)參數(shù)的可觀度信息：1) 最小特征值和對應的特征向量，反映了可觀度最大的狀態(tài)參數(shù)，特征向量中較大的分量對應的狀態(tài)參數(shù)的可觀度也較大；2) 最大的特征值和對應的特征向量，反映了可觀度最小的狀態(tài)參數(shù)，特征向量中較大的分量對應的狀態(tài)參數(shù)的可觀度則較小.

用式(30)和(31)計算導航系統(tǒng)的一步預測狀態(tài)誤差的方差陣Pk+1|k和Pk+1|k+1,并對Pk+1|k+1進行特征值分解，從而利用基于誤差方差陣的可觀度分析方法對導航系統(tǒng)進行分析.

3 仿真驗證

為驗證本文提出的改進預測濾波算法的小天體探測器自主光學導航系統(tǒng)的可觀度分析，取小行星Eros433作為目標天體進行數(shù)學仿真，Eros433小天體物理參數(shù)如表1所示.

初始狀態(tài)參數(shù)為：位置初始值r0=[300 500 3 000]Tm,速度初始值v0=[-3 -2-20]Tm/s,姿態(tài)四元數(shù)初始值q=[0.993 8 0.060 85 0.069 39 0.060 85]T.

陀螺零偏bω=[1 1 1]T(°)/h,加速度計零偏ba=[3×10-33×10-33×10-3]m/s2,仿真初始條件為位置各方向存在100 m的隨機誤差，速度各方向存在1 m/s隨機誤差，姿態(tài)各軸指向存在1°隨機誤差，導航陸標位置各方向存在1 m的隨機誤差.

3.1 模型不確定性下的導航精度比較與分析

由于濾波模型中的小天體引力參數(shù)存在不確定性，這里對小天體引力場模型存在50%的不確定性進行了仿真，給出了導航濾波的誤差值，結果分別如下圖所示，圖2～3為模型參數(shù)存在50%不確定度時的濾波精度.

圖2 EKF濾波精度Fig.2 The Accuracy of EKF

圖3 改進的NPF濾波精度Fig.3 The Accuracy of improved NPF

從圖2和圖3可以看出，在模型誤差存在時，改進的NPF較EKF的濾波精度高，這是由于EKF只針對白噪聲的模型誤差進行處理導致精度下降，而改進的NPF對模型誤差做一步估計并進行補償，不限制模型誤差的類型，從而提高了估計精度.

3.2 狀態(tài)參數(shù)的可觀度順序分析

對于t=100 s時刻，規(guī)范化后的誤差方差陣特征值和對應特征向量如表2所示.

表2 誤差方差陣的特征值和對應特征向量Tab.2 Eigenvalues and associated eigenvectors oferror covariance matrix

利用2.2節(jié)的結論，可以得出，從最可觀的狀態(tài)到最不可觀的狀態(tài)順序為：φ→θ→ψ→vz→vy→vx→z→y→x.

圖4～6給出了探測器的位置和速度估計誤差，對應t=100 s時刻，位置誤差從小到大的順序為z→y→x，速度誤差從小到大的順序為vz→vy→vx,姿態(tài)角誤差從小到大的順序為φ→θ→ψ,這和表2給出的狀態(tài)可觀度順序是一致的，表明最小特征值和對應的特征向量，反映了可觀度最大的狀態(tài)參數(shù)，其中較大的分量對應的狀態(tài)參數(shù)φ的可觀度也較大，而最大的特征值和對應的特征向量，反映了可觀度最小的狀態(tài)參數(shù)，特征向量中較大的分量對應的狀態(tài)參數(shù)的可觀度則較小.

圖4 位置誤差Fig.4 The curve of location error

圖5 速度誤差Fig.5 The curve of velocity error

圖6 姿態(tài)誤差Fig.6 The curve of attitude error

3.3 不同誤差因素對可觀度的影響

由2.1.2節(jié)誤差方差陣的推導結果(28)～(29)可看出，Pk+1|k+1陣的大小與模型誤差，系統(tǒng)噪聲，測量噪聲相關，從而與導航系統(tǒng)的可觀度相關，表3～5給出各誤差值對狀態(tài)分量可觀度的影響，其中表3為當引力不確定性導致的模型誤差增大時的影響，表4為陀螺噪聲增大時的影響，表5為圖像處理精度引起陸標誤差減小時的影響.

表3 模型誤差增大時的可觀度順序Tab.3 Obervability order with model error

表4 陀螺噪聲增大時的可觀度順序Tab.4 Obervability order with gyro noise

表5 陸標誤差減小時的可觀度順序Tab.5 Obervability order with landmark error

由表3看出：在表1的仿真條件設置下，可觀度的順序為φ→θ→ψ→vz→vy→vx→z→y→x，模型誤差增大時，狀態(tài)可觀度順序為ψ→φ→θ→z→y→vy→x→vz→vx，可以看出：vz,vx由接近可觀的狀態(tài)變?yōu)樽畈豢捎^的狀態(tài)，因此處的模型誤差為小天體引力場引起的模型誤差，直接作用于動力學模型中速度分量，使得速度的可觀度下降，由表4可看出，當陀螺噪聲增大時，狀態(tài)可觀度順序為z→vy→vz→z→φ→θ→ψ→y→x，由于姿態(tài)角是由陀螺輸出角速度分量積分得出，故當陀螺噪聲增大時，直接影響姿態(tài)角的可觀度，由表5可看出，陸標誤差減小時，狀態(tài)可觀度的順序為z→x→vz→y→ψ→θ→φ→vx→vy，由于陸標誤差的減小，使得觀測誤差減小，從而使得位置可觀度提高.

4 結 論

本文建立基于改進的預測濾波的模型，并進一步推導了誤差方差陣的模型.針對非線性系統(tǒng)觀測矩陣的秩無法通過表達式直接確定的問題，給出了基于狀態(tài)誤差矩陣特征值和特征向量的導航系統(tǒng)可觀性分析方法，并用來分析了小天體探測器精確著陸過程自主導航系統(tǒng)的可觀性和狀態(tài)參數(shù)可觀度，提供了狀態(tài)參數(shù)的估計精度順序，通過仿真分析驗證了存在一定模型誤差時，改進的預測濾波方法可以有效地估計探測器的位置、速度和姿態(tài)信息，并分析了狀態(tài)參數(shù)的可觀度與引力不確定性、圖像處理精度、觀測不確定性之間的關系，可以為提高狀態(tài)的估計精度提供參考依據(jù).