基于特征值分解的小天体着陆自主导航系统可观度分析*

冀红霞，宗 红，黄翔宇,2

1.北京控制工程研究所,北京 100190;2．空间智能控制技术重点实验室，北京 100190.

0 引 言

导航系统的基本任务是确定航天器在给定参考坐标系内的状态，而导航系统的能观性反映了系统在有限时间内测量系统的能力，由于深空天体与地球的距离遥远，导致地面测控站所发出的测控信息的传输出现时滞,难以实现对着陆过程进行实时测控，因此深空探测着陆任务必须利用探测器上的敏感器和导航算法自主完成，因此自主导航系统的可观性分析成为一个关键的问题.

由于自主导航系统对应的状态方程和观测方程的非线性导致分析其可观性存在一定的困难，利用李导数定义的观测矩阵[1]能分析一类非线性系统的局部弱可观性，但是由于自主导航系统的复杂性(状态维数高、非线性强)多数情况下很难直接利用现有的可观性秩条件来确定其可观性，文献[2]和[3]基于Kalman滤波误差协方差矩阵来定义能观度,文献[4]研究了基于星光角距和星光仰角的航天器自主天文导航方法，对相应导航系统的能观性进行了分析，并以此为基础说明了观测量的选择对导航性能的影响.文献[5]系统地介绍了深空探测自主导航系统的几种观测模式及其数学模型.

非线性估计领域最常用的方法是扩展卡尔曼滤波(extended Kalman filter, EKF)[6],它具有收敛速度快、算法简单等优点.尽管EKF在建模准确且系统噪声为零均值白噪声的情况下，可以获得良好的估计效果.但在实际应用中，难以获得精确的系统模型，且最初建好的模型在系统运行过程中受环境变化的影响可能变得不准确，进而造成模型误差，这时采用EKF将导致滤波精度下降甚至发散，非线性预测滤波算法(nonlinear predictive filter, NPF)[7]对不确定模型具有很强的适应性，但收敛速度较慢，该算法通过使预测输出与测量输出的误差和模型误差的加权和最小来估计模型误差，进而修正状态估计，形成一种递推的在线估计算法，由于对模型误差实时进行跟踪，故收敛速度较慢.

针对NPF和EKF的局限性，本文结合EKF算法对NPF算法进行改进，首先利用NPF的预测输出与实际测量输出之间的方差最小的原则来估计模型误差并修正系统模型，再利用EKF的时间更新和测量更新来进一步估计系统状态，并采用改进的预测滤波方法与状态估计误差特征值分解方法，对小天体探测器着陆自主光学导航系统的可观性进行度量，推导了改进的预测滤波的协方差矩阵的传播模型，在误差分析的基础上，使用误差协方差矩阵的特征值和特征向量进行可观度分析，最后以小天体探测器着陆自主导航为例，对小天体引力变化引起的模型误差下改进NPF与EKF的导航精度进行比较，分析状态参数可观度与导航精度之间的关系，并分析不同误差因素对导航系统可观度的影响.

1 小天体探测器自主光学导航系统

1.1 小天体探测器运动学模型与测量模型

探测器导航的任务就是确定探测器相对于所选定的参考坐标系的位置、速度和飞行姿态，需找到一个合适的参考坐标系.所以这里定义了涉及到的参考坐标系：着陆点坐标系、本体坐标系和相机坐标系.

图1 坐标系相对几何关系示意图Fig.1 Relative geometric relations of coordinate system

1) 着陆点坐标系{l}：原点定义为着陆点，基准平面取为当地水平面，x轴指向东，y轴指向北，z轴与x、y轴构成右手坐标系.

2) 本体坐标系{b}：原点定义为探测器质心，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为识别的特征点个数，特征点在像平面中的位置坐标即为导航测量量，可表示如下:

(3)

其中，第j个特征点测量量为mj=[ujvj]T=f[xj/zjyj/zj]T.

则导航测量方程为

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

2) 测速敏感器测量模型

测速敏感器输出的相对速度的测量方程如下：

(5)

式中，nu为测速敏感器测量噪声.

1.1.3 小天体探测器自主光学导航系统

导航系统状态包括探测器当前位置、速度和姿态，陀螺漂移和加速度计漂移

线性化式(1)可得系统当前误差状态方程：

(6)

对于导航相机，以特征点在像平面的位置为观测量，对于特征点由式(4)可得观测方程为：

式中，vi为观测噪声，设其协方差阵为Rv.

第i时刻导航陆标pj估计观测量为：

(7)

则第i时刻的观测量残差为

(8)

测量方程为

(9)

式中：H1=[H1103×3H13]

其中：

对于测距测速敏感器，由式(5)可得观测方程为

(10)

其中测量敏感矩阵为：

2 基于改进预测滤波算法的小天体探测器自主光学导航系统可观度分析

2.1 改进的NPF算法及其误差模型推导

2.1.1 改进的NPF算法

设一个非线性系统模型为

(11)

式中：f(·)∈Rn和h∈Rm为连续可微的非线性函数向量；X(t)∈Rn为状态向量，y(t)∈Rm为观测向量；d(t)∈Rp为模型误差向量；gd(·)∈Rn×p为模型误差分布矩阵gw(·)∈Rn×q为噪声分布矩阵；w(t)为系统噪声，为零均值的高斯白噪声，方差阵为E{w(t)wT(t)}=Q,v(t)为测量噪声，为零均值的高斯白噪声，方差阵为E{v(t)vT(t)}=R.

将输出估计方程泰勒展开，并运用李导数知识，得

(13)

ri是与第i个输出对应的相对阶，为d(t)出现在hi[x(t)]的微分中的最低阶数；Λ(T)∈Rm×m是对角矩阵，对角元λi=Ti/ri!,i=1,2,…,m；U(x)∈Rm×p为灵敏度矩阵.

基于预测输出与实际测量输出的残差最小的思想来估计系统的模型误差.由此建立的目标函数是由测量输出与预测输出间残差以及模型误差修正项的加权平方和组成的，即

(14)

式(14)中下标k表示第k个采样时刻，yk+1是tk+1时刻的测量输出.预测输出为：

(15)

将式(15)代入式(14)，并令∂J/∂d(k)=0,可得模型误差估计为：

(16)

其中，

本文提出的改进的NPF结合NPF与EKF的优点，利用式(16)计算模型误差并修正系统模型，再利用EKF[8]的时间更新和测量更新过程来进一步估计系统状态.

2.1.2 误差模型推导

根据系统状态方程和状态估计方程，使用一阶泰勒展开的离散形式进行状态更新：

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

其中，T为采样步长， 根据式(17)～(18)可以计算出一步预测的状态估计的误差.

=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)可得出状态估计的误差式为：

式(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)中舍去包含模型误差估计和状态估计中的各种线性化、离散化以及泰勒展开高阶舍入误差，模型噪声和测量噪声的相关项.

则改进的预测滤波误差模型为：

(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 可观度定义

考虑到模型误差和观测误差，导航系统一般可以表示为如下非线性系统：

(32)

式中:X为状态参数矢量；Z为观测量；f,h为状态参数的非线性函数；w(t),v(t)分别为导航系统模型误差和观测误差矢量.通过在设计的标称轨道上线性化和离散化，导航系统可转化为如下形式：

(33)

(34)

式中，tr表示矩阵的迹，n为状态参数的维数.

对最小二乘估计，有

(35)

式中，Pk为状态参数的误差方差阵.

可以看出，导航系统可观度越高，导航系统状态估计误差越小，即估计精度越高.说明了导航系统可观度定义反映了状态参数估计精度和导航系统可观度之间的关系.可观度定义描述了状态参数确定的精度与导航系统的运动学模型和观测模型及测量精度的依赖关系.

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

虽然导航系统的可观度定义能够描述整个导航系统的性能，但是无法具体说明在一定的观测模式下哪些状态最可观(估计精度高)和哪些状态最不可观(估计精度低).由式(35)可知，导航系统的可观度直接与状态误差方差阵相关，所以可以利用状态误差方差阵的特征值和特征向量来分析轨道参数的可观度，给出基于特征值和特征向量的轨道参数可观度分析方法.

(36)

考虑其线性组合

(37)

式中，v=[v1,v2,…,vn]T为线性组合的系数，且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)

为了无量纲化和限定特征值的范围，对系统状态误差方差阵进行规范化处理.无量纲化的方差阵记为：

(44)

式中，P0为初始状态误差方差阵，Pk为k时刻的状态误差方差阵.

通过分析导航系统的误差方差阵Pk的特征值和特征向量，可以得到状态参数的可观度信息：1) 最小特征值和对应的特征向量，反映了可观度最大的状态参数，特征向量中较大的分量对应的状态参数的可观度也较大；2) 最大的特征值和对应的特征向量，反映了可观度最小的状态参数，特征向量中较大的分量对应的状态参数的可观度则较小.

用式(30)和(31)计算导航系统的一步预测状态误差的方差阵Pk+1|k和Pk+1|k+1,并对Pk+1|k+1进行特征值分解，从而利用基于误差方差阵的可观度分析方法对导航系统进行分析.

3 仿真验证

为验证本文提出的改进预测滤波算法的小天体探测器自主光学导航系统的可观度分析，取小行星Eros433作为目标天体进行数学仿真，Eros433小天体物理参数如表1所示.

初始状态参数为：位置初始值r0=[300 500 3 000]Tm,速度初始值v0=[-3 -2-20]Tm/s,姿态四元数初始值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随机误差，姿态各轴指向存在1°随机误差，导航陆标位置各方向存在1 m的随机误差.

3.1 模型不确定性下的导航精度比较与分析

由于滤波模型中的小天体引力参数存在不确定性，这里对小天体引力场模型存在50%的不确定性进行了仿真，给出了导航滤波的误差值，结果分别如下图所示，图2～3为模型参数存在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=100 s时刻，规范化后的误差方差阵特征值和对应特征向量如表2所示.

表2 误差方差阵的特征值和对应特征向量Tab.2 Eigenvalues and associated eigenvectors oferror covariance matrix

利用2.2节的结论，可以得出，从最可观的状态到最不可观的状态顺序为：φ→θ→ψ→vz→vy→vx→z→y→x.

图4～6给出了探测器的位置和速度估计误差，对应t=100 s时刻，位置误差从小到大的顺序为z→y→x，速度误差从小到大的顺序为vz→vy→vx,姿态角误差从小到大的顺序为φ→θ→ψ,这和表2给出的状态可观度顺序是一致的，表明最小特征值和对应的特征向量，反映了可观度最大的状态参数，其中较大的分量对应的状态参数φ的可观度也较大，而最大的特征值和对应的特征向量，反映了可观度最小的状态参数，特征向量中较大的分量对应的状态参数的可观度则较小.

图4 位置误差Fig.4 The curve of location error

图5 速度误差Fig.5 The curve of velocity error

图6 姿态误差Fig.6 The curve of attitude error

3.3 不同误差因素对可观度的影响

由2.1.2节误差方差阵的推导结果(28)～(29)可看出，Pk+1|k+1阵的大小与模型误差，系统噪声，测量噪声相关，从而与导航系统的可观度相关，表3～5给出各误差值对状态分量可观度的影响，其中表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，模型误差增大时，状态可观度顺序为ψ→φ→θ→z→y→vy→x→vz→vx，可以看出：vz,vx由接近可观的状态变为最不可观的状态，因此处的模型误差为小天体引力场引起的模型误差，直接作用于动力学模型中速度分量，使得速度的可观度下降，由表4可看出，当陀螺噪声增大时，状态可观度顺序为z→vy→vz→z→φ→θ→ψ→y→x，由于姿态角是由陀螺输出角速度分量积分得出，故当陀螺噪声增大时，直接影响姿态角的可观度，由表5可看出，陆标误差减小时，状态可观度的顺序为z→x→vz→y→ψ→θ→φ→vx→vy，由于陆标误差的减小，使得观测误差减小，从而使得位置可观度提高.

4 结 论

本文建立基于改进的预测滤波的模型，并进一步推导了误差方差阵的模型.针对非线性系统观测矩阵的秩无法通过表达式直接确定的问题，给出了基于状态误差矩阵特征值和特征向量的导航系统可观性分析方法，并用来分析了小天体探测器精确着陆过程自主导航系统的可观性和状态参数可观度，提供了状态参数的估计精度顺序，通过仿真分析验证了存在一定模型误差时，改进的预测滤波方法可以有效地估计探测器的位置、速度和姿态信息，并分析了状态参数的可观度与引力不确定性、图像处理精度、观测不确定性之间的关系，可以为提高状态的估计精度提供参考依据.