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

1.北京控制工程研究所,北京 100190;2．空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190.

0 引 言

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

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

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

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

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

1.1 小天體探測器運(yùn)動(dòng)學(xué)模型與測量模型

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

圖1 坐標(biāo)系相對幾何關(guān)系示意圖Fig.1 Relative geometric relations of coordinate system

1) 著陸點(diǎn)坐標(biāo)系{l}：原點(diǎn)定義為著陸點(diǎn)，基準(zhǔn)平面取為當(dāng)?shù)厮矫妫瑇軸指向東，y軸指向北，z軸與x、y軸構(gòu)成右手坐標(biāo)系.

2) 本體坐標(biāo)系：原點(diǎn)定義為探測器質(zhì)心，x,y,z軸分別沿探測器三個(gè)慣性主軸方向構(gòu)成右手坐標(biāo)系.

3) 相機(jī)坐標(biāo)系{c}：原點(diǎn)定義為相機(jī)的光心，x,y軸平行于像平面，z軸與x，y軸構(gòu)成右手坐標(biāo)系.

1.1.1 小天體探測器運(yùn)動(dòng)學(xué)模型

利用慣性敏感器測量建立探測器著陸運(yùn)動(dòng)學(xué)方程為：

(1)

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

(2)

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

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

1.1.2 觀測方程

1) 導(dǎo)航相機(jī)測量模型

假設(shè)光學(xué)導(dǎo)航相機(jī)模型為理想的小孔成像模型，由射影變換可得

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

(3)

其中，第j個(gè)特征點(diǎn)測量量為mj=[ujvj]T=f[xj/zjyj/zj]T.

則導(dǎo)航測量方程為

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

2) 測速敏感器測量模型

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

(5)

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

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

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

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

(6)

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

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

第i時(shí)刻導(dǎo)航陸標(biāo)pj估計(jì)觀測量為：

(7)

則第i時(shí)刻的觀測量殘差為

(8)

測量方程為

(9)

式中：H1=[H1103×3H13]

其中：

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

(10)

其中測量敏感矩陣為：

2 基于改進(jìn)預(yù)測濾波算法的小天體探測器自主光學(xué)導(dǎo)航系統(tǒng)可觀度分析

2.1 改進(jìn)的NPF算法及其誤差模型推導(dǎo)

2.1.1 改進(jìn)的NPF算法

設(shè)一個(gè)非線性系統(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.

將輸出估計(jì)方程泰勒展開，并運(yùn)用李導(dǎo)數(shù)知識，得

(13)

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

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

(14)

式(14)中下標(biāo)k表示第k個(gè)采樣時(shí)刻，yk+1是tk+1時(shí)刻的測量輸出.預(yù)測輸出為：

(15)

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

(16)

其中，

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

2.1.2 誤差模型推導(dǎo)

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

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

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

=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)化簡為

將模型誤差的估計(jì)誤差[9]

TMkHkgwwk-Mkvk+1(23)

代入式(22)中，可得

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

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

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

式(25)中第二項(xiàng)為：

(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)中舍去包含模型誤差估計(jì)和狀態(tài)估計(jì)中的各種線性化、離散化以及泰勒展開高階舍入誤差，模型噪聲和測量噪聲的相關(guān)項(xiàng).

則改進(jìn)的預(yù)測濾波誤差模型為：

(29)

設(shè)濾波均方誤差陣

(Φ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 可觀度定義

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

(32)

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

(33)

(34)

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

對最小二乘估計(jì)，有

(35)

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

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

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

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

(36)

考慮其線性組合

(37)

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

(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)誤差方差陣進(jìn)行規(guī)范化處理.無量綱化的方差陣記為：

(44)

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

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

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

3 仿真驗(yàn)證

為驗(yàn)證本文提出的改進(jìn)預(yù)測濾波算法的小天體探測器自主光學(xué)導(dǎo)航系統(tǒng)的可觀度分析，取小行星Eros433作為目標(biāo)天體進(jìn)行數(shù)學(xué)仿真，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,加速度計(jì)零偏ba=[3×10-33×10-33×10-3]m/s2,仿真初始條件為位置各方向存在100 m的隨機(jī)誤差，速度各方向存在1 m/s隨機(jī)誤差，姿態(tài)各軸指向存在1°隨機(jī)誤差，導(dǎo)航陸標(biāo)位置各方向存在1 m的隨機(jī)誤差.

3.1 模型不確定性下的導(dǎo)航精度比較與分析

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

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

圖3 改進(jìn)的NPF濾波精度Fig.3 The Accuracy of improved NPF

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

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

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

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

利用2.2節(jié)的結(jié)論，可以得出，從最可觀的狀態(tài)到最不可觀的狀態(tài)順序?yàn)椋害铡取住鷙z→vy→vx→z→y→x.

圖4～6給出了探測器的位置和速度估計(jì)誤差，對應(yīng)t=100 s時(shí)刻，位置誤差從小到大的順序?yàn)閦→y→x，速度誤差從小到大的順序?yàn)関z→vy→vx,姿態(tài)角誤差從小到大的順序?yàn)棣铡取?這和表2給出的狀態(tài)可觀度順序是一致的，表明最小特征值和對應(yīng)的特征向量，反映了可觀度最大的狀態(tài)參數(shù)，其中較大的分量對應(yīng)的狀態(tài)參數(shù)φ的可觀度也較大，而最大的特征值和對應(yīng)的特征向量，反映了可觀度最小的狀態(tài)參數(shù)，特征向量中較大的分量對應(yīng)的狀態(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é)誤差方差陣的推導(dǎo)結(jié)果(28)～(29)可看出，Pk+1|k+1陣的大小與模型誤差，系統(tǒng)噪聲，測量噪聲相關(guān)，從而與導(dǎo)航系統(tǒng)的可觀度相關(guān)，表3～5給出各誤差值對狀態(tài)分量可觀度的影響，其中表3為當(dāng)引力不確定性導(dǎo)致的模型誤差增大時(shí)的影響，表4為陀螺噪聲增大時(shí)的影響，表5為圖像處理精度引起陸標(biāo)誤差減小時(shí)的影響.

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

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

表5 陸標(biāo)誤差減小時(shí)的可觀度順序Tab.5 Obervability order with landmark error

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

4 結(jié) 論

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